Subversion Repositories Kolibri OS

Rev

Rev 5060 | Rev 6088 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | Download | RSS feed

  1. /*
  2.  * Copyright © 2014 Red Hat
  3.  *
  4.  * Permission to use, copy, modify, distribute, and sell this software and its
  5.  * documentation for any purpose is hereby granted without fee, provided that
  6.  * the above copyright notice appear in all copies and that both that copyright
  7.  * notice and this permission notice appear in supporting documentation, and
  8.  * that the name of the copyright holders not be used in advertising or
  9.  * publicity pertaining to distribution of the software without specific,
  10.  * written prior permission.  The copyright holders make no representations
  11.  * about the suitability of this software for any purpose.  It is provided "as
  12.  * is" without express or implied warranty.
  13.  *
  14.  * THE COPYRIGHT HOLDERS DISCLAIM ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
  15.  * INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO
  16.  * EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE FOR ANY SPECIAL, INDIRECT OR
  17.  * CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE,
  18.  * DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER
  19.  * TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
  20.  * OF THIS SOFTWARE.
  21.  */
  22.  
  23. #include <linux/kernel.h>
  24. #include <linux/mutex.h>
  25. #include <linux/kref.h>
  26. #include <linux/wait.h>
  27. #include <linux/workqueue.h>
  28. #include <linux/delay.h>
  29. #include <linux/errno.h>
  30. #include <linux/sched.h>
  31. #include <linux/seq_file.h>
  32. #include <linux/i2c.h>
  33. #include <drm/drm_dp_mst_helper.h>
  34. #include <drm/drmP.h>
  35.  
  36. #include <drm/drm_fixed.h>
  37.  
  38. u64 get_jiffies_64(void)
  39. {
  40.     return jiffies;
  41. }
  42. /**
  43.  * DOC: dp mst helper
  44.  *
  45.  * These functions contain parts of the DisplayPort 1.2a MultiStream Transport
  46.  * protocol. The helpers contain a topology manager and bandwidth manager.
  47.  * The helpers encapsulate the sending and received of sideband msgs.
  48.  */
  49. static bool dump_dp_payload_table(struct drm_dp_mst_topology_mgr *mgr,
  50.                                   char *buf);
  51. static int test_calc_pbn_mode(void);
  52.  
  53. static void drm_dp_put_port(struct drm_dp_mst_port *port);
  54.  
  55. static int drm_dp_dpcd_write_payload(struct drm_dp_mst_topology_mgr *mgr,
  56.                                      int id,
  57.                                      struct drm_dp_payload *payload);
  58.  
  59. static int drm_dp_send_dpcd_write(struct drm_dp_mst_topology_mgr *mgr,
  60.                                   struct drm_dp_mst_port *port,
  61.                                   int offset, int size, u8 *bytes);
  62.  
  63. static int drm_dp_send_link_address(struct drm_dp_mst_topology_mgr *mgr,
  64.                                     struct drm_dp_mst_branch *mstb);
  65. static int drm_dp_send_enum_path_resources(struct drm_dp_mst_topology_mgr *mgr,
  66.                                            struct drm_dp_mst_branch *mstb,
  67.                                            struct drm_dp_mst_port *port);
  68. static bool drm_dp_validate_guid(struct drm_dp_mst_topology_mgr *mgr,
  69.                                  u8 *guid);
  70.  
  71. static int drm_dp_mst_register_i2c_bus(struct drm_dp_aux *aux);
  72. static void drm_dp_mst_unregister_i2c_bus(struct drm_dp_aux *aux);
  73. static void drm_dp_mst_kick_tx(struct drm_dp_mst_topology_mgr *mgr);
  74. /* sideband msg handling */
  75. static u8 drm_dp_msg_header_crc4(const uint8_t *data, size_t num_nibbles)
  76. {
  77.         u8 bitmask = 0x80;
  78.         u8 bitshift = 7;
  79.         u8 array_index = 0;
  80.         int number_of_bits = num_nibbles * 4;
  81.         u8 remainder = 0;
  82.  
  83.         while (number_of_bits != 0) {
  84.                 number_of_bits--;
  85.                 remainder <<= 1;
  86.                 remainder |= (data[array_index] & bitmask) >> bitshift;
  87.                 bitmask >>= 1;
  88.                 bitshift--;
  89.                 if (bitmask == 0) {
  90.                         bitmask = 0x80;
  91.                         bitshift = 7;
  92.                         array_index++;
  93.                 }
  94.                 if ((remainder & 0x10) == 0x10)
  95.                         remainder ^= 0x13;
  96.         }
  97.  
  98.         number_of_bits = 4;
  99.         while (number_of_bits != 0) {
  100.                 number_of_bits--;
  101.                 remainder <<= 1;
  102.                 if ((remainder & 0x10) != 0)
  103.                         remainder ^= 0x13;
  104.         }
  105.  
  106.         return remainder;
  107. }
  108.  
  109. static u8 drm_dp_msg_data_crc4(const uint8_t *data, u8 number_of_bytes)
  110. {
  111.         u8 bitmask = 0x80;
  112.         u8 bitshift = 7;
  113.         u8 array_index = 0;
  114.         int number_of_bits = number_of_bytes * 8;
  115.         u16 remainder = 0;
  116.  
  117.         while (number_of_bits != 0) {
  118.                 number_of_bits--;
  119.                 remainder <<= 1;
  120.                 remainder |= (data[array_index] & bitmask) >> bitshift;
  121.                 bitmask >>= 1;
  122.                 bitshift--;
  123.                 if (bitmask == 0) {
  124.                         bitmask = 0x80;
  125.                         bitshift = 7;
  126.                         array_index++;
  127.                 }
  128.                 if ((remainder & 0x100) == 0x100)
  129.                         remainder ^= 0xd5;
  130.         }
  131.  
  132.         number_of_bits = 8;
  133.         while (number_of_bits != 0) {
  134.                 number_of_bits--;
  135.                 remainder <<= 1;
  136.                 if ((remainder & 0x100) != 0)
  137.                         remainder ^= 0xd5;
  138.         }
  139.  
  140.         return remainder & 0xff;
  141. }
  142. static inline u8 drm_dp_calc_sb_hdr_size(struct drm_dp_sideband_msg_hdr *hdr)
  143. {
  144.         u8 size = 3;
  145.         size += (hdr->lct / 2);
  146.         return size;
  147. }
  148.  
  149. static void drm_dp_encode_sideband_msg_hdr(struct drm_dp_sideband_msg_hdr *hdr,
  150.                                            u8 *buf, int *len)
  151. {
  152.         int idx = 0;
  153.         int i;
  154.         u8 crc4;
  155.         buf[idx++] = ((hdr->lct & 0xf) << 4) | (hdr->lcr & 0xf);
  156.         for (i = 0; i < (hdr->lct / 2); i++)
  157.                 buf[idx++] = hdr->rad[i];
  158.         buf[idx++] = (hdr->broadcast << 7) | (hdr->path_msg << 6) |
  159.                 (hdr->msg_len & 0x3f);
  160.         buf[idx++] = (hdr->somt << 7) | (hdr->eomt << 6) | (hdr->seqno << 4);
  161.  
  162.         crc4 = drm_dp_msg_header_crc4(buf, (idx * 2) - 1);
  163.         buf[idx - 1] |= (crc4 & 0xf);
  164.  
  165.         *len = idx;
  166. }
  167.  
  168. static bool drm_dp_decode_sideband_msg_hdr(struct drm_dp_sideband_msg_hdr *hdr,
  169.                                            u8 *buf, int buflen, u8 *hdrlen)
  170. {
  171.         u8 crc4;
  172.         u8 len;
  173.         int i;
  174.         u8 idx;
  175.         if (buf[0] == 0)
  176.                 return false;
  177.         len = 3;
  178.         len += ((buf[0] & 0xf0) >> 4) / 2;
  179.         if (len > buflen)
  180.                 return false;
  181.         crc4 = drm_dp_msg_header_crc4(buf, (len * 2) - 1);
  182.  
  183.         if ((crc4 & 0xf) != (buf[len - 1] & 0xf)) {
  184.                 DRM_DEBUG_KMS("crc4 mismatch 0x%x 0x%x\n", crc4, buf[len - 1]);
  185.                 return false;
  186.         }
  187.  
  188.         hdr->lct = (buf[0] & 0xf0) >> 4;
  189.         hdr->lcr = (buf[0] & 0xf);
  190.         idx = 1;
  191.         for (i = 0; i < (hdr->lct / 2); i++)
  192.                 hdr->rad[i] = buf[idx++];
  193.         hdr->broadcast = (buf[idx] >> 7) & 0x1;
  194.         hdr->path_msg = (buf[idx] >> 6) & 0x1;
  195.         hdr->msg_len = buf[idx] & 0x3f;
  196.         idx++;
  197.         hdr->somt = (buf[idx] >> 7) & 0x1;
  198.         hdr->eomt = (buf[idx] >> 6) & 0x1;
  199.         hdr->seqno = (buf[idx] >> 4) & 0x1;
  200.         idx++;
  201.         *hdrlen = idx;
  202.         return true;
  203. }
  204.  
  205. static void drm_dp_encode_sideband_req(struct drm_dp_sideband_msg_req_body *req,
  206.                                        struct drm_dp_sideband_msg_tx *raw)
  207. {
  208.         int idx = 0;
  209.         int i;
  210.         u8 *buf = raw->msg;
  211.         buf[idx++] = req->req_type & 0x7f;
  212.  
  213.         switch (req->req_type) {
  214.         case DP_ENUM_PATH_RESOURCES:
  215.                 buf[idx] = (req->u.port_num.port_number & 0xf) << 4;
  216.                 idx++;
  217.                 break;
  218.         case DP_ALLOCATE_PAYLOAD:
  219.                 buf[idx] = (req->u.allocate_payload.port_number & 0xf) << 4 |
  220.                         (req->u.allocate_payload.number_sdp_streams & 0xf);
  221.                 idx++;
  222.                 buf[idx] = (req->u.allocate_payload.vcpi & 0x7f);
  223.                 idx++;
  224.                 buf[idx] = (req->u.allocate_payload.pbn >> 8);
  225.                 idx++;
  226.                 buf[idx] = (req->u.allocate_payload.pbn & 0xff);
  227.                 idx++;
  228.                 for (i = 0; i < req->u.allocate_payload.number_sdp_streams / 2; i++) {
  229.                         buf[idx] = ((req->u.allocate_payload.sdp_stream_sink[i * 2] & 0xf) << 4) |
  230.                                 (req->u.allocate_payload.sdp_stream_sink[i * 2 + 1] & 0xf);
  231.                         idx++;
  232.                 }
  233.                 if (req->u.allocate_payload.number_sdp_streams & 1) {
  234.                         i = req->u.allocate_payload.number_sdp_streams - 1;
  235.                         buf[idx] = (req->u.allocate_payload.sdp_stream_sink[i] & 0xf) << 4;
  236.                         idx++;
  237.                 }
  238.                 break;
  239.         case DP_QUERY_PAYLOAD:
  240.                 buf[idx] = (req->u.query_payload.port_number & 0xf) << 4;
  241.                 idx++;
  242.                 buf[idx] = (req->u.query_payload.vcpi & 0x7f);
  243.                 idx++;
  244.                 break;
  245.         case DP_REMOTE_DPCD_READ:
  246.                 buf[idx] = (req->u.dpcd_read.port_number & 0xf) << 4;
  247.                 buf[idx] |= ((req->u.dpcd_read.dpcd_address & 0xf0000) >> 16) & 0xf;
  248.                 idx++;
  249.                 buf[idx] = (req->u.dpcd_read.dpcd_address & 0xff00) >> 8;
  250.                 idx++;
  251.                 buf[idx] = (req->u.dpcd_read.dpcd_address & 0xff);
  252.                 idx++;
  253.                 buf[idx] = (req->u.dpcd_read.num_bytes);
  254.                 idx++;
  255.                 break;
  256.  
  257.         case DP_REMOTE_DPCD_WRITE:
  258.                 buf[idx] = (req->u.dpcd_write.port_number & 0xf) << 4;
  259.                 buf[idx] |= ((req->u.dpcd_write.dpcd_address & 0xf0000) >> 16) & 0xf;
  260.                 idx++;
  261.                 buf[idx] = (req->u.dpcd_write.dpcd_address & 0xff00) >> 8;
  262.                 idx++;
  263.                 buf[idx] = (req->u.dpcd_write.dpcd_address & 0xff);
  264.                 idx++;
  265.                 buf[idx] = (req->u.dpcd_write.num_bytes);
  266.                 idx++;
  267.                 memcpy(&buf[idx], req->u.dpcd_write.bytes, req->u.dpcd_write.num_bytes);
  268.                 idx += req->u.dpcd_write.num_bytes;
  269.                 break;
  270.         case DP_REMOTE_I2C_READ:
  271.                 buf[idx] = (req->u.i2c_read.port_number & 0xf) << 4;
  272.                 buf[idx] |= (req->u.i2c_read.num_transactions & 0x3);
  273.                 idx++;
  274.                 for (i = 0; i < (req->u.i2c_read.num_transactions & 0x3); i++) {
  275.                         buf[idx] = req->u.i2c_read.transactions[i].i2c_dev_id & 0x7f;
  276.                         idx++;
  277.                         buf[idx] = req->u.i2c_read.transactions[i].num_bytes;
  278.                         idx++;
  279.                         memcpy(&buf[idx], req->u.i2c_read.transactions[i].bytes, req->u.i2c_read.transactions[i].num_bytes);
  280.                         idx += req->u.i2c_read.transactions[i].num_bytes;
  281.  
  282.                         buf[idx] = (req->u.i2c_read.transactions[i].no_stop_bit & 0x1) << 5;
  283.                         buf[idx] |= (req->u.i2c_read.transactions[i].i2c_transaction_delay & 0xf);
  284.                         idx++;
  285.                 }
  286.                 buf[idx] = (req->u.i2c_read.read_i2c_device_id) & 0x7f;
  287.                 idx++;
  288.                 buf[idx] = (req->u.i2c_read.num_bytes_read);
  289.                 idx++;
  290.                 break;
  291.  
  292.         case DP_REMOTE_I2C_WRITE:
  293.                 buf[idx] = (req->u.i2c_write.port_number & 0xf) << 4;
  294.                 idx++;
  295.                 buf[idx] = (req->u.i2c_write.write_i2c_device_id) & 0x7f;
  296.                 idx++;
  297.                 buf[idx] = (req->u.i2c_write.num_bytes);
  298.                 idx++;
  299.                 memcpy(&buf[idx], req->u.i2c_write.bytes, req->u.i2c_write.num_bytes);
  300.                 idx += req->u.i2c_write.num_bytes;
  301.                 break;
  302.         }
  303.         raw->cur_len = idx;
  304. }
  305.  
  306. static void drm_dp_crc_sideband_chunk_req(u8 *msg, u8 len)
  307. {
  308.         u8 crc4;
  309.         crc4 = drm_dp_msg_data_crc4(msg, len);
  310.         msg[len] = crc4;
  311. }
  312.  
  313. static void drm_dp_encode_sideband_reply(struct drm_dp_sideband_msg_reply_body *rep,
  314.                                          struct drm_dp_sideband_msg_tx *raw)
  315. {
  316.         int idx = 0;
  317.         u8 *buf = raw->msg;
  318.  
  319.         buf[idx++] = (rep->reply_type & 0x1) << 7 | (rep->req_type & 0x7f);
  320.  
  321.         raw->cur_len = idx;
  322. }
  323.  
  324. /* this adds a chunk of msg to the builder to get the final msg */
  325. static bool drm_dp_sideband_msg_build(struct drm_dp_sideband_msg_rx *msg,
  326.                                       u8 *replybuf, u8 replybuflen, bool hdr)
  327. {
  328.         int ret;
  329.         u8 crc4;
  330.  
  331.         if (hdr) {
  332.                 u8 hdrlen;
  333.                 struct drm_dp_sideband_msg_hdr recv_hdr;
  334.                 ret = drm_dp_decode_sideband_msg_hdr(&recv_hdr, replybuf, replybuflen, &hdrlen);
  335.                 if (ret == false) {
  336.                         print_hex_dump(KERN_DEBUG, "failed hdr", DUMP_PREFIX_NONE, 16, 1, replybuf, replybuflen, false);
  337.                         return false;
  338.                 }
  339.  
  340.                 /* get length contained in this portion */
  341.                 msg->curchunk_len = recv_hdr.msg_len;
  342.                 msg->curchunk_hdrlen = hdrlen;
  343.  
  344.                 /* we have already gotten an somt - don't bother parsing */
  345.                 if (recv_hdr.somt && msg->have_somt)
  346.                         return false;
  347.  
  348.                 if (recv_hdr.somt) {
  349.                         memcpy(&msg->initial_hdr, &recv_hdr, sizeof(struct drm_dp_sideband_msg_hdr));
  350.                         msg->have_somt = true;
  351.                 }
  352.                 if (recv_hdr.eomt)
  353.                         msg->have_eomt = true;
  354.  
  355.                 /* copy the bytes for the remainder of this header chunk */
  356.                 msg->curchunk_idx = min(msg->curchunk_len, (u8)(replybuflen - hdrlen));
  357.                 memcpy(&msg->chunk[0], replybuf + hdrlen, msg->curchunk_idx);
  358.         } else {
  359.                 memcpy(&msg->chunk[msg->curchunk_idx], replybuf, replybuflen);
  360.                 msg->curchunk_idx += replybuflen;
  361.         }
  362.  
  363.         if (msg->curchunk_idx >= msg->curchunk_len) {
  364.                 /* do CRC */
  365.                 crc4 = drm_dp_msg_data_crc4(msg->chunk, msg->curchunk_len - 1);
  366.                 /* copy chunk into bigger msg */
  367.                 memcpy(&msg->msg[msg->curlen], msg->chunk, msg->curchunk_len - 1);
  368.                 msg->curlen += msg->curchunk_len - 1;
  369.         }
  370.         return true;
  371. }
  372.  
  373. static bool drm_dp_sideband_parse_link_address(struct drm_dp_sideband_msg_rx *raw,
  374.                                                struct drm_dp_sideband_msg_reply_body *repmsg)
  375. {
  376.         int idx = 1;
  377.         int i;
  378.         memcpy(repmsg->u.link_addr.guid, &raw->msg[idx], 16);
  379.         idx += 16;
  380.         repmsg->u.link_addr.nports = raw->msg[idx] & 0xf;
  381.         idx++;
  382.         if (idx > raw->curlen)
  383.                 goto fail_len;
  384.         for (i = 0; i < repmsg->u.link_addr.nports; i++) {
  385.                 if (raw->msg[idx] & 0x80)
  386.                         repmsg->u.link_addr.ports[i].input_port = 1;
  387.  
  388.                 repmsg->u.link_addr.ports[i].peer_device_type = (raw->msg[idx] >> 4) & 0x7;
  389.                 repmsg->u.link_addr.ports[i].port_number = (raw->msg[idx] & 0xf);
  390.  
  391.                 idx++;
  392.                 if (idx > raw->curlen)
  393.                         goto fail_len;
  394.                 repmsg->u.link_addr.ports[i].mcs = (raw->msg[idx] >> 7) & 0x1;
  395.                 repmsg->u.link_addr.ports[i].ddps = (raw->msg[idx] >> 6) & 0x1;
  396.                 if (repmsg->u.link_addr.ports[i].input_port == 0)
  397.                         repmsg->u.link_addr.ports[i].legacy_device_plug_status = (raw->msg[idx] >> 5) & 0x1;
  398.                 idx++;
  399.                 if (idx > raw->curlen)
  400.                         goto fail_len;
  401.                 if (repmsg->u.link_addr.ports[i].input_port == 0) {
  402.                         repmsg->u.link_addr.ports[i].dpcd_revision = (raw->msg[idx]);
  403.                         idx++;
  404.                         if (idx > raw->curlen)
  405.                                 goto fail_len;
  406.                         memcpy(repmsg->u.link_addr.ports[i].peer_guid, &raw->msg[idx], 16);
  407.                         idx += 16;
  408.                         if (idx > raw->curlen)
  409.                                 goto fail_len;
  410.                         repmsg->u.link_addr.ports[i].num_sdp_streams = (raw->msg[idx] >> 4) & 0xf;
  411.                         repmsg->u.link_addr.ports[i].num_sdp_stream_sinks = (raw->msg[idx] & 0xf);
  412.                         idx++;
  413.  
  414.                 }
  415.                 if (idx > raw->curlen)
  416.                         goto fail_len;
  417.         }
  418.  
  419.         return true;
  420. fail_len:
  421.         DRM_DEBUG_KMS("link address reply parse length fail %d %d\n", idx, raw->curlen);
  422.         return false;
  423. }
  424.  
  425. static bool drm_dp_sideband_parse_remote_dpcd_read(struct drm_dp_sideband_msg_rx *raw,
  426.                                                    struct drm_dp_sideband_msg_reply_body *repmsg)
  427. {
  428.         int idx = 1;
  429.         repmsg->u.remote_dpcd_read_ack.port_number = raw->msg[idx] & 0xf;
  430.         idx++;
  431.         if (idx > raw->curlen)
  432.                 goto fail_len;
  433.         repmsg->u.remote_dpcd_read_ack.num_bytes = raw->msg[idx];
  434.         if (idx > raw->curlen)
  435.                 goto fail_len;
  436.  
  437.         memcpy(repmsg->u.remote_dpcd_read_ack.bytes, &raw->msg[idx], repmsg->u.remote_dpcd_read_ack.num_bytes);
  438.         return true;
  439. fail_len:
  440.         DRM_DEBUG_KMS("link address reply parse length fail %d %d\n", idx, raw->curlen);
  441.         return false;
  442. }
  443.  
  444. static bool drm_dp_sideband_parse_remote_dpcd_write(struct drm_dp_sideband_msg_rx *raw,
  445.                                                       struct drm_dp_sideband_msg_reply_body *repmsg)
  446. {
  447.         int idx = 1;
  448.         repmsg->u.remote_dpcd_write_ack.port_number = raw->msg[idx] & 0xf;
  449.         idx++;
  450.         if (idx > raw->curlen)
  451.                 goto fail_len;
  452.         return true;
  453. fail_len:
  454.         DRM_DEBUG_KMS("parse length fail %d %d\n", idx, raw->curlen);
  455.         return false;
  456. }
  457.  
  458. static bool drm_dp_sideband_parse_remote_i2c_read_ack(struct drm_dp_sideband_msg_rx *raw,
  459.                                                       struct drm_dp_sideband_msg_reply_body *repmsg)
  460. {
  461.         int idx = 1;
  462.  
  463.         repmsg->u.remote_i2c_read_ack.port_number = (raw->msg[idx] & 0xf);
  464.         idx++;
  465.         if (idx > raw->curlen)
  466.                 goto fail_len;
  467.         repmsg->u.remote_i2c_read_ack.num_bytes = raw->msg[idx];
  468.         idx++;
  469.         /* TODO check */
  470.         memcpy(repmsg->u.remote_i2c_read_ack.bytes, &raw->msg[idx], repmsg->u.remote_i2c_read_ack.num_bytes);
  471.         return true;
  472. fail_len:
  473.         DRM_DEBUG_KMS("remote i2c reply parse length fail %d %d\n", idx, raw->curlen);
  474.         return false;
  475. }
  476.  
  477. static bool drm_dp_sideband_parse_enum_path_resources_ack(struct drm_dp_sideband_msg_rx *raw,
  478.                                                           struct drm_dp_sideband_msg_reply_body *repmsg)
  479. {
  480.         int idx = 1;
  481.         repmsg->u.path_resources.port_number = (raw->msg[idx] >> 4) & 0xf;
  482.         idx++;
  483.         if (idx > raw->curlen)
  484.                 goto fail_len;
  485.         repmsg->u.path_resources.full_payload_bw_number = (raw->msg[idx] << 8) | (raw->msg[idx+1]);
  486.         idx += 2;
  487.         if (idx > raw->curlen)
  488.                 goto fail_len;
  489.         repmsg->u.path_resources.avail_payload_bw_number = (raw->msg[idx] << 8) | (raw->msg[idx+1]);
  490.         idx += 2;
  491.         if (idx > raw->curlen)
  492.                 goto fail_len;
  493.         return true;
  494. fail_len:
  495.         DRM_DEBUG_KMS("enum resource parse length fail %d %d\n", idx, raw->curlen);
  496.         return false;
  497. }
  498.  
  499. static bool drm_dp_sideband_parse_allocate_payload_ack(struct drm_dp_sideband_msg_rx *raw,
  500.                                                           struct drm_dp_sideband_msg_reply_body *repmsg)
  501. {
  502.         int idx = 1;
  503.         repmsg->u.allocate_payload.port_number = (raw->msg[idx] >> 4) & 0xf;
  504.         idx++;
  505.         if (idx > raw->curlen)
  506.                 goto fail_len;
  507.         repmsg->u.allocate_payload.vcpi = raw->msg[idx];
  508.         idx++;
  509.         if (idx > raw->curlen)
  510.                 goto fail_len;
  511.         repmsg->u.allocate_payload.allocated_pbn = (raw->msg[idx] << 8) | (raw->msg[idx+1]);
  512.         idx += 2;
  513.         if (idx > raw->curlen)
  514.                 goto fail_len;
  515.         return true;
  516. fail_len:
  517.         DRM_DEBUG_KMS("allocate payload parse length fail %d %d\n", idx, raw->curlen);
  518.         return false;
  519. }
  520.  
  521. static bool drm_dp_sideband_parse_query_payload_ack(struct drm_dp_sideband_msg_rx *raw,
  522.                                                     struct drm_dp_sideband_msg_reply_body *repmsg)
  523. {
  524.         int idx = 1;
  525.         repmsg->u.query_payload.port_number = (raw->msg[idx] >> 4) & 0xf;
  526.         idx++;
  527.         if (idx > raw->curlen)
  528.                 goto fail_len;
  529.         repmsg->u.query_payload.allocated_pbn = (raw->msg[idx] << 8) | (raw->msg[idx + 1]);
  530.         idx += 2;
  531.         if (idx > raw->curlen)
  532.                 goto fail_len;
  533.         return true;
  534. fail_len:
  535.         DRM_DEBUG_KMS("query payload parse length fail %d %d\n", idx, raw->curlen);
  536.         return false;
  537. }
  538.  
  539. static bool drm_dp_sideband_parse_reply(struct drm_dp_sideband_msg_rx *raw,
  540.                                         struct drm_dp_sideband_msg_reply_body *msg)
  541. {
  542.         memset(msg, 0, sizeof(*msg));
  543.         msg->reply_type = (raw->msg[0] & 0x80) >> 7;
  544.         msg->req_type = (raw->msg[0] & 0x7f);
  545.  
  546.         if (msg->reply_type) {
  547.                 memcpy(msg->u.nak.guid, &raw->msg[1], 16);
  548.                 msg->u.nak.reason = raw->msg[17];
  549.                 msg->u.nak.nak_data = raw->msg[18];
  550.                 return false;
  551.         }
  552.  
  553.         switch (msg->req_type) {
  554.         case DP_LINK_ADDRESS:
  555.                 return drm_dp_sideband_parse_link_address(raw, msg);
  556.         case DP_QUERY_PAYLOAD:
  557.                 return drm_dp_sideband_parse_query_payload_ack(raw, msg);
  558.         case DP_REMOTE_DPCD_READ:
  559.                 return drm_dp_sideband_parse_remote_dpcd_read(raw, msg);
  560.         case DP_REMOTE_DPCD_WRITE:
  561.                 return drm_dp_sideband_parse_remote_dpcd_write(raw, msg);
  562.         case DP_REMOTE_I2C_READ:
  563.                 return drm_dp_sideband_parse_remote_i2c_read_ack(raw, msg);
  564.         case DP_ENUM_PATH_RESOURCES:
  565.                 return drm_dp_sideband_parse_enum_path_resources_ack(raw, msg);
  566.         case DP_ALLOCATE_PAYLOAD:
  567.                 return drm_dp_sideband_parse_allocate_payload_ack(raw, msg);
  568.         default:
  569.                 DRM_ERROR("Got unknown reply 0x%02x\n", msg->req_type);
  570.                 return false;
  571.         }
  572. }
  573.  
  574. static bool drm_dp_sideband_parse_connection_status_notify(struct drm_dp_sideband_msg_rx *raw,
  575.                                                            struct drm_dp_sideband_msg_req_body *msg)
  576. {
  577.         int idx = 1;
  578.  
  579.         msg->u.conn_stat.port_number = (raw->msg[idx] & 0xf0) >> 4;
  580.         idx++;
  581.         if (idx > raw->curlen)
  582.                 goto fail_len;
  583.  
  584.         memcpy(msg->u.conn_stat.guid, &raw->msg[idx], 16);
  585.         idx += 16;
  586.         if (idx > raw->curlen)
  587.                 goto fail_len;
  588.  
  589.         msg->u.conn_stat.legacy_device_plug_status = (raw->msg[idx] >> 6) & 0x1;
  590.         msg->u.conn_stat.displayport_device_plug_status = (raw->msg[idx] >> 5) & 0x1;
  591.         msg->u.conn_stat.message_capability_status = (raw->msg[idx] >> 4) & 0x1;
  592.         msg->u.conn_stat.input_port = (raw->msg[idx] >> 3) & 0x1;
  593.         msg->u.conn_stat.peer_device_type = (raw->msg[idx] & 0x7);
  594.         idx++;
  595.         return true;
  596. fail_len:
  597.         DRM_DEBUG_KMS("connection status reply parse length fail %d %d\n", idx, raw->curlen);
  598.         return false;
  599. }
  600.  
  601. static bool drm_dp_sideband_parse_resource_status_notify(struct drm_dp_sideband_msg_rx *raw,
  602.                                                            struct drm_dp_sideband_msg_req_body *msg)
  603. {
  604.         int idx = 1;
  605.  
  606.         msg->u.resource_stat.port_number = (raw->msg[idx] & 0xf0) >> 4;
  607.         idx++;
  608.         if (idx > raw->curlen)
  609.                 goto fail_len;
  610.  
  611.         memcpy(msg->u.resource_stat.guid, &raw->msg[idx], 16);
  612.         idx += 16;
  613.         if (idx > raw->curlen)
  614.                 goto fail_len;
  615.  
  616.         msg->u.resource_stat.available_pbn = (raw->msg[idx] << 8) | (raw->msg[idx + 1]);
  617.         idx++;
  618.         return true;
  619. fail_len:
  620.         DRM_DEBUG_KMS("resource status reply parse length fail %d %d\n", idx, raw->curlen);
  621.         return false;
  622. }
  623.  
  624. static bool drm_dp_sideband_parse_req(struct drm_dp_sideband_msg_rx *raw,
  625.                                       struct drm_dp_sideband_msg_req_body *msg)
  626. {
  627.         memset(msg, 0, sizeof(*msg));
  628.         msg->req_type = (raw->msg[0] & 0x7f);
  629.  
  630.         switch (msg->req_type) {
  631.         case DP_CONNECTION_STATUS_NOTIFY:
  632.                 return drm_dp_sideband_parse_connection_status_notify(raw, msg);
  633.         case DP_RESOURCE_STATUS_NOTIFY:
  634.                 return drm_dp_sideband_parse_resource_status_notify(raw, msg);
  635.         default:
  636.                 DRM_ERROR("Got unknown request 0x%02x\n", msg->req_type);
  637.                 return false;
  638.         }
  639. }
  640.  
  641. static int build_dpcd_write(struct drm_dp_sideband_msg_tx *msg, u8 port_num, u32 offset, u8 num_bytes, u8 *bytes)
  642. {
  643.         struct drm_dp_sideband_msg_req_body req;
  644.  
  645.         req.req_type = DP_REMOTE_DPCD_WRITE;
  646.         req.u.dpcd_write.port_number = port_num;
  647.         req.u.dpcd_write.dpcd_address = offset;
  648.         req.u.dpcd_write.num_bytes = num_bytes;
  649.         req.u.dpcd_write.bytes = bytes;
  650.         drm_dp_encode_sideband_req(&req, msg);
  651.  
  652.         return 0;
  653. }
  654.  
  655. static int build_link_address(struct drm_dp_sideband_msg_tx *msg)
  656. {
  657.         struct drm_dp_sideband_msg_req_body req;
  658.  
  659.         req.req_type = DP_LINK_ADDRESS;
  660.         drm_dp_encode_sideband_req(&req, msg);
  661.         return 0;
  662. }
  663.  
  664. static int build_enum_path_resources(struct drm_dp_sideband_msg_tx *msg, int port_num)
  665. {
  666.         struct drm_dp_sideband_msg_req_body req;
  667.  
  668.         req.req_type = DP_ENUM_PATH_RESOURCES;
  669.         req.u.port_num.port_number = port_num;
  670.         drm_dp_encode_sideband_req(&req, msg);
  671.         msg->path_msg = true;
  672.         return 0;
  673. }
  674.  
  675. static int build_allocate_payload(struct drm_dp_sideband_msg_tx *msg, int port_num,
  676.                                   u8 vcpi, uint16_t pbn)
  677. {
  678.         struct drm_dp_sideband_msg_req_body req;
  679.         memset(&req, 0, sizeof(req));
  680.         req.req_type = DP_ALLOCATE_PAYLOAD;
  681.         req.u.allocate_payload.port_number = port_num;
  682.         req.u.allocate_payload.vcpi = vcpi;
  683.         req.u.allocate_payload.pbn = pbn;
  684.         drm_dp_encode_sideband_req(&req, msg);
  685.         msg->path_msg = true;
  686.         return 0;
  687. }
  688.  
  689. static int drm_dp_mst_assign_payload_id(struct drm_dp_mst_topology_mgr *mgr,
  690.                                         struct drm_dp_vcpi *vcpi)
  691. {
  692.         int ret, vcpi_ret;
  693.  
  694.         mutex_lock(&mgr->payload_lock);
  695.         ret = find_first_zero_bit(&mgr->payload_mask, mgr->max_payloads + 1);
  696.         if (ret > mgr->max_payloads) {
  697.                 ret = -EINVAL;
  698.                 DRM_DEBUG_KMS("out of payload ids %d\n", ret);
  699.                 goto out_unlock;
  700.         }
  701.  
  702.         vcpi_ret = find_first_zero_bit(&mgr->vcpi_mask, mgr->max_payloads + 1);
  703.         if (vcpi_ret > mgr->max_payloads) {
  704.                 ret = -EINVAL;
  705.                 DRM_DEBUG_KMS("out of vcpi ids %d\n", ret);
  706.                 goto out_unlock;
  707.         }
  708.  
  709.         set_bit(ret, &mgr->payload_mask);
  710.         set_bit(vcpi_ret, &mgr->vcpi_mask);
  711.         vcpi->vcpi = vcpi_ret + 1;
  712.         mgr->proposed_vcpis[ret - 1] = vcpi;
  713. out_unlock:
  714.         mutex_unlock(&mgr->payload_lock);
  715.         return ret;
  716. }
  717.  
  718. static void drm_dp_mst_put_payload_id(struct drm_dp_mst_topology_mgr *mgr,
  719.                                       int vcpi)
  720. {
  721.         int i;
  722.         if (vcpi == 0)
  723.                 return;
  724.  
  725.         mutex_lock(&mgr->payload_lock);
  726.         DRM_DEBUG_KMS("putting payload %d\n", vcpi);
  727.         clear_bit(vcpi - 1, &mgr->vcpi_mask);
  728.  
  729.         for (i = 0; i < mgr->max_payloads; i++) {
  730.                 if (mgr->proposed_vcpis[i])
  731.                         if (mgr->proposed_vcpis[i]->vcpi == vcpi) {
  732.                                 mgr->proposed_vcpis[i] = NULL;
  733.                                 clear_bit(i + 1, &mgr->payload_mask);
  734.                         }
  735.         }
  736.         mutex_unlock(&mgr->payload_lock);
  737. }
  738.  
  739. static bool check_txmsg_state(struct drm_dp_mst_topology_mgr *mgr,
  740.                               struct drm_dp_sideband_msg_tx *txmsg)
  741. {
  742.         bool ret;
  743.         mutex_lock(&mgr->qlock);
  744.         ret = (txmsg->state == DRM_DP_SIDEBAND_TX_RX ||
  745.                txmsg->state == DRM_DP_SIDEBAND_TX_TIMEOUT);
  746.         mutex_unlock(&mgr->qlock);
  747.         return ret;
  748. }
  749.  
  750. static int drm_dp_mst_wait_tx_reply(struct drm_dp_mst_branch *mstb,
  751.                                     struct drm_dp_sideband_msg_tx *txmsg)
  752. {
  753.         struct drm_dp_mst_topology_mgr *mgr = mstb->mgr;
  754.         int ret;
  755.  
  756.         ret = wait_event_timeout(mgr->tx_waitq,
  757.                                  check_txmsg_state(mgr, txmsg),
  758.                                  (4 * HZ));
  759.         mutex_lock(&mstb->mgr->qlock);
  760.         if (ret > 0) {
  761.                 if (txmsg->state == DRM_DP_SIDEBAND_TX_TIMEOUT) {
  762.                         ret = -EIO;
  763.                         goto out;
  764.                 }
  765.         } else {
  766.                 DRM_DEBUG_KMS("timedout msg send %p %d %d\n", txmsg, txmsg->state, txmsg->seqno);
  767.  
  768.                 /* dump some state */
  769.                 ret = -EIO;
  770.  
  771.                 /* remove from q */
  772.                 if (txmsg->state == DRM_DP_SIDEBAND_TX_QUEUED ||
  773.                     txmsg->state == DRM_DP_SIDEBAND_TX_START_SEND) {
  774.                         list_del(&txmsg->next);
  775.                 }
  776.  
  777.                 if (txmsg->state == DRM_DP_SIDEBAND_TX_START_SEND ||
  778.                     txmsg->state == DRM_DP_SIDEBAND_TX_SENT) {
  779.                         mstb->tx_slots[txmsg->seqno] = NULL;
  780.                 }
  781.         }
  782. out:
  783.         mutex_unlock(&mgr->qlock);
  784.  
  785.         return ret;
  786. }
  787.  
  788. static struct drm_dp_mst_branch *drm_dp_add_mst_branch_device(u8 lct, u8 *rad)
  789. {
  790.         struct drm_dp_mst_branch *mstb;
  791.  
  792.         mstb = kzalloc(sizeof(*mstb), GFP_KERNEL);
  793.         if (!mstb)
  794.                 return NULL;
  795.  
  796.         mstb->lct = lct;
  797.         if (lct > 1)
  798.                 memcpy(mstb->rad, rad, lct / 2);
  799.         INIT_LIST_HEAD(&mstb->ports);
  800.         kref_init(&mstb->kref);
  801.         return mstb;
  802. }
  803.  
  804. static void drm_dp_destroy_mst_branch_device(struct kref *kref)
  805. {
  806.         struct drm_dp_mst_branch *mstb = container_of(kref, struct drm_dp_mst_branch, kref);
  807.         struct drm_dp_mst_port *port, *tmp;
  808.         bool wake_tx = false;
  809.  
  810.         cancel_work_sync(&mstb->mgr->work);
  811.  
  812.         /*
  813.          * destroy all ports - don't need lock
  814.          * as there are no more references to the mst branch
  815.          * device at this point.
  816.          */
  817.         list_for_each_entry_safe(port, tmp, &mstb->ports, next) {
  818.                 list_del(&port->next);
  819.                 drm_dp_put_port(port);
  820.         }
  821.  
  822.         /* drop any tx slots msg */
  823.         mutex_lock(&mstb->mgr->qlock);
  824.         if (mstb->tx_slots[0]) {
  825.                 mstb->tx_slots[0]->state = DRM_DP_SIDEBAND_TX_TIMEOUT;
  826.                 mstb->tx_slots[0] = NULL;
  827.                 wake_tx = true;
  828.         }
  829.         if (mstb->tx_slots[1]) {
  830.                 mstb->tx_slots[1]->state = DRM_DP_SIDEBAND_TX_TIMEOUT;
  831.                 mstb->tx_slots[1] = NULL;
  832.                 wake_tx = true;
  833.         }
  834.         mutex_unlock(&mstb->mgr->qlock);
  835.  
  836. //   if (wake_tx)
  837. //       wake_up(&mstb->mgr->tx_waitq);
  838.         kfree(mstb);
  839. }
  840.  
  841. static void drm_dp_put_mst_branch_device(struct drm_dp_mst_branch *mstb)
  842. {
  843.         kref_put(&mstb->kref, drm_dp_destroy_mst_branch_device);
  844. }
  845.  
  846.  
  847. static void drm_dp_port_teardown_pdt(struct drm_dp_mst_port *port, int old_pdt)
  848. {
  849.         struct drm_dp_mst_branch *mstb;
  850.  
  851.         switch (old_pdt) {
  852.         case DP_PEER_DEVICE_DP_LEGACY_CONV:
  853.         case DP_PEER_DEVICE_SST_SINK:
  854.                 /* remove i2c over sideband */
  855.                 drm_dp_mst_unregister_i2c_bus(&port->aux);
  856.                 break;
  857.         case DP_PEER_DEVICE_MST_BRANCHING:
  858.                 mstb = port->mstb;
  859.                 port->mstb = NULL;
  860.                 drm_dp_put_mst_branch_device(mstb);
  861.                 break;
  862.         }
  863. }
  864.  
  865. static void drm_dp_destroy_port(struct kref *kref)
  866. {
  867.         struct drm_dp_mst_port *port = container_of(kref, struct drm_dp_mst_port, kref);
  868.         struct drm_dp_mst_topology_mgr *mgr = port->mgr;
  869.         if (!port->input) {
  870.                 port->vcpi.num_slots = 0;
  871.  
  872.                 kfree(port->cached_edid);
  873.                 if (port->connector)
  874.                         (*port->mgr->cbs->destroy_connector)(mgr, port->connector);
  875.                 drm_dp_port_teardown_pdt(port, port->pdt);
  876.  
  877.                 if (!port->input && port->vcpi.vcpi > 0)
  878.                         drm_dp_mst_put_payload_id(mgr, port->vcpi.vcpi);
  879.         }
  880.         kfree(port);
  881.  
  882.         (*mgr->cbs->hotplug)(mgr);
  883. }
  884.  
  885. static void drm_dp_put_port(struct drm_dp_mst_port *port)
  886. {
  887.         kref_put(&port->kref, drm_dp_destroy_port);
  888. }
  889.  
  890. static struct drm_dp_mst_branch *drm_dp_mst_get_validated_mstb_ref_locked(struct drm_dp_mst_branch *mstb, struct drm_dp_mst_branch *to_find)
  891. {
  892.         struct drm_dp_mst_port *port;
  893.         struct drm_dp_mst_branch *rmstb;
  894.         if (to_find == mstb) {
  895.                 kref_get(&mstb->kref);
  896.                 return mstb;
  897.         }
  898.         list_for_each_entry(port, &mstb->ports, next) {
  899.                 if (port->mstb) {
  900.                         rmstb = drm_dp_mst_get_validated_mstb_ref_locked(port->mstb, to_find);
  901.                         if (rmstb)
  902.                                 return rmstb;
  903.                 }
  904.         }
  905.         return NULL;
  906. }
  907.  
  908. static struct drm_dp_mst_branch *drm_dp_get_validated_mstb_ref(struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_branch *mstb)
  909. {
  910.         struct drm_dp_mst_branch *rmstb = NULL;
  911.         mutex_lock(&mgr->lock);
  912.         if (mgr->mst_primary)
  913.                 rmstb = drm_dp_mst_get_validated_mstb_ref_locked(mgr->mst_primary, mstb);
  914.         mutex_unlock(&mgr->lock);
  915.         return rmstb;
  916. }
  917.  
  918. static struct drm_dp_mst_port *drm_dp_mst_get_port_ref_locked(struct drm_dp_mst_branch *mstb, struct drm_dp_mst_port *to_find)
  919. {
  920.         struct drm_dp_mst_port *port, *mport;
  921.  
  922.         list_for_each_entry(port, &mstb->ports, next) {
  923.                 if (port == to_find) {
  924.                         kref_get(&port->kref);
  925.                         return port;
  926.                 }
  927.                 if (port->mstb) {
  928.                         mport = drm_dp_mst_get_port_ref_locked(port->mstb, to_find);
  929.                         if (mport)
  930.                                 return mport;
  931.                 }
  932.         }
  933.         return NULL;
  934. }
  935.  
  936. static struct drm_dp_mst_port *drm_dp_get_validated_port_ref(struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port)
  937. {
  938.         struct drm_dp_mst_port *rport = NULL;
  939.         mutex_lock(&mgr->lock);
  940.         if (mgr->mst_primary)
  941.                 rport = drm_dp_mst_get_port_ref_locked(mgr->mst_primary, port);
  942.         mutex_unlock(&mgr->lock);
  943.         return rport;
  944. }
  945.  
  946. static struct drm_dp_mst_port *drm_dp_get_port(struct drm_dp_mst_branch *mstb, u8 port_num)
  947. {
  948.         struct drm_dp_mst_port *port;
  949.  
  950.         list_for_each_entry(port, &mstb->ports, next) {
  951.                 if (port->port_num == port_num) {
  952.                         kref_get(&port->kref);
  953.                         return port;
  954.                 }
  955.         }
  956.  
  957.         return NULL;
  958. }
  959.  
  960. /*
  961.  * calculate a new RAD for this MST branch device
  962.  * if parent has an LCT of 2 then it has 1 nibble of RAD,
  963.  * if parent has an LCT of 3 then it has 2 nibbles of RAD,
  964.  */
  965. static u8 drm_dp_calculate_rad(struct drm_dp_mst_port *port,
  966.                                  u8 *rad)
  967. {
  968.         int lct = port->parent->lct;
  969.         int shift = 4;
  970.         int idx = lct / 2;
  971.         if (lct > 1) {
  972.                 memcpy(rad, port->parent->rad, idx);
  973.                 shift = (lct % 2) ? 4 : 0;
  974.         } else
  975.                 rad[0] = 0;
  976.  
  977.         rad[idx] |= port->port_num << shift;
  978.         return lct + 1;
  979. }
  980.  
  981. /*
  982.  * return sends link address for new mstb
  983.  */
  984. static bool drm_dp_port_setup_pdt(struct drm_dp_mst_port *port)
  985. {
  986.         int ret;
  987.         u8 rad[6], lct;
  988.         bool send_link = false;
  989.         switch (port->pdt) {
  990.         case DP_PEER_DEVICE_DP_LEGACY_CONV:
  991.         case DP_PEER_DEVICE_SST_SINK:
  992.                 /* add i2c over sideband */
  993.                 ret = drm_dp_mst_register_i2c_bus(&port->aux);
  994.                 break;
  995.         case DP_PEER_DEVICE_MST_BRANCHING:
  996.                 lct = drm_dp_calculate_rad(port, rad);
  997.  
  998.                 port->mstb = drm_dp_add_mst_branch_device(lct, rad);
  999.                 port->mstb->mgr = port->mgr;
  1000.                 port->mstb->port_parent = port;
  1001.  
  1002.                 send_link = true;
  1003.                 break;
  1004.         }
  1005.         return send_link;
  1006. }
  1007.  
  1008. static void drm_dp_check_port_guid(struct drm_dp_mst_branch *mstb,
  1009.                                    struct drm_dp_mst_port *port)
  1010. {
  1011.         int ret;
  1012.         if (port->dpcd_rev >= 0x12) {
  1013.                 port->guid_valid = drm_dp_validate_guid(mstb->mgr, port->guid);
  1014.                 if (!port->guid_valid) {
  1015.                         ret = drm_dp_send_dpcd_write(mstb->mgr,
  1016.                                                      port,
  1017.                                                      DP_GUID,
  1018.                                                      16, port->guid);
  1019.                         port->guid_valid = true;
  1020.                 }
  1021.         }
  1022. }
  1023.  
  1024. static void build_mst_prop_path(struct drm_dp_mst_port *port,
  1025.                                 struct drm_dp_mst_branch *mstb,
  1026.                                 char *proppath,
  1027.                                 size_t proppath_size)
  1028. {
  1029.         int i;
  1030.         char temp[8];
  1031.         snprintf(proppath, proppath_size, "mst:%d", mstb->mgr->conn_base_id);
  1032.         for (i = 0; i < (mstb->lct - 1); i++) {
  1033.                 int shift = (i % 2) ? 0 : 4;
  1034.                 int port_num = mstb->rad[i / 2] >> shift;
  1035.                 snprintf(temp, sizeof(temp), "-%d", port_num);
  1036.                 strlcat(proppath, temp, proppath_size);
  1037.         }
  1038.         snprintf(temp, sizeof(temp), "-%d", port->port_num);
  1039.         strlcat(proppath, temp, proppath_size);
  1040. }
  1041.  
  1042. static void drm_dp_add_port(struct drm_dp_mst_branch *mstb,
  1043.                             struct device *dev,
  1044.                             struct drm_dp_link_addr_reply_port *port_msg)
  1045. {
  1046.         struct drm_dp_mst_port *port;
  1047.         bool ret;
  1048.         bool created = false;
  1049.         int old_pdt = 0;
  1050.         int old_ddps = 0;
  1051.         port = drm_dp_get_port(mstb, port_msg->port_number);
  1052.         if (!port) {
  1053.                 port = kzalloc(sizeof(*port), GFP_KERNEL);
  1054.                 if (!port)
  1055.                         return;
  1056.                 kref_init(&port->kref);
  1057.                 port->parent = mstb;
  1058.                 port->port_num = port_msg->port_number;
  1059.                 port->mgr = mstb->mgr;
  1060.                 port->aux.name = "DPMST";
  1061.                 port->aux.dev = dev;
  1062.                 created = true;
  1063.         } else {
  1064.                 old_pdt = port->pdt;
  1065.                 old_ddps = port->ddps;
  1066.         }
  1067.  
  1068.         port->pdt = port_msg->peer_device_type;
  1069.         port->input = port_msg->input_port;
  1070.         port->mcs = port_msg->mcs;
  1071.         port->ddps = port_msg->ddps;
  1072.         port->ldps = port_msg->legacy_device_plug_status;
  1073.         port->dpcd_rev = port_msg->dpcd_revision;
  1074.         port->num_sdp_streams = port_msg->num_sdp_streams;
  1075.         port->num_sdp_stream_sinks = port_msg->num_sdp_stream_sinks;
  1076.         memcpy(port->guid, port_msg->peer_guid, 16);
  1077.  
  1078.         /* manage mstb port lists with mgr lock - take a reference
  1079.            for this list */
  1080.         if (created) {
  1081.                 mutex_lock(&mstb->mgr->lock);
  1082.                 kref_get(&port->kref);
  1083.                 list_add(&port->next, &mstb->ports);
  1084.                 mutex_unlock(&mstb->mgr->lock);
  1085.         }
  1086.  
  1087.         if (old_ddps != port->ddps) {
  1088.                 if (port->ddps) {
  1089.                         drm_dp_check_port_guid(mstb, port);
  1090.                         if (!port->input)
  1091.                                 drm_dp_send_enum_path_resources(mstb->mgr, mstb, port);
  1092.                 } else {
  1093.                         port->guid_valid = false;
  1094.                         port->available_pbn = 0;
  1095.                         }
  1096.         }
  1097.  
  1098.         if (old_pdt != port->pdt && !port->input) {
  1099.                 drm_dp_port_teardown_pdt(port, old_pdt);
  1100.  
  1101.                 ret = drm_dp_port_setup_pdt(port);
  1102.                 if (ret == true) {
  1103.                         drm_dp_send_link_address(mstb->mgr, port->mstb);
  1104.                         port->mstb->link_address_sent = true;
  1105.                 }
  1106.         }
  1107.  
  1108.         if (created && !port->input) {
  1109.                 char proppath[255];
  1110.                 build_mst_prop_path(port, mstb, proppath, sizeof(proppath));
  1111.                 port->connector = (*mstb->mgr->cbs->add_connector)(mstb->mgr, port, proppath);
  1112.  
  1113.                 if (port->port_num >= 8) {
  1114.                         port->cached_edid = drm_get_edid(port->connector, &port->aux.ddc);
  1115.                 }
  1116.         }
  1117.  
  1118.         /* put reference to this port */
  1119.         drm_dp_put_port(port);
  1120. }
  1121.  
  1122. static void drm_dp_update_port(struct drm_dp_mst_branch *mstb,
  1123.                                struct drm_dp_connection_status_notify *conn_stat)
  1124. {
  1125.         struct drm_dp_mst_port *port;
  1126.         int old_pdt;
  1127.         int old_ddps;
  1128.         bool dowork = false;
  1129.         port = drm_dp_get_port(mstb, conn_stat->port_number);
  1130.         if (!port)
  1131.                 return;
  1132.  
  1133.         old_ddps = port->ddps;
  1134.         old_pdt = port->pdt;
  1135.         port->pdt = conn_stat->peer_device_type;
  1136.         port->mcs = conn_stat->message_capability_status;
  1137.         port->ldps = conn_stat->legacy_device_plug_status;
  1138.         port->ddps = conn_stat->displayport_device_plug_status;
  1139.  
  1140.         if (old_ddps != port->ddps) {
  1141.                 if (port->ddps) {
  1142.                         drm_dp_check_port_guid(mstb, port);
  1143.                         dowork = true;
  1144.                 } else {
  1145.                         port->guid_valid = false;
  1146.                         port->available_pbn = 0;
  1147.                 }
  1148.         }
  1149.         if (old_pdt != port->pdt && !port->input) {
  1150.                 drm_dp_port_teardown_pdt(port, old_pdt);
  1151.  
  1152.                 if (drm_dp_port_setup_pdt(port))
  1153.                         dowork = true;
  1154.         }
  1155.  
  1156.         drm_dp_put_port(port);
  1157. //   if (dowork)
  1158. //       queue_work(system_long_wq, &mstb->mgr->work);
  1159.  
  1160. }
  1161.  
  1162. static struct drm_dp_mst_branch *drm_dp_get_mst_branch_device(struct drm_dp_mst_topology_mgr *mgr,
  1163.                                                                u8 lct, u8 *rad)
  1164. {
  1165.         struct drm_dp_mst_branch *mstb;
  1166.         struct drm_dp_mst_port *port;
  1167.         int i;
  1168.         /* find the port by iterating down */
  1169.         mstb = mgr->mst_primary;
  1170.  
  1171.         for (i = 0; i < lct - 1; i++) {
  1172.                 int shift = (i % 2) ? 0 : 4;
  1173.                 int port_num = rad[i / 2] >> shift;
  1174.  
  1175.                 list_for_each_entry(port, &mstb->ports, next) {
  1176.                         if (port->port_num == port_num) {
  1177.                                 if (!port->mstb) {
  1178.                                         DRM_ERROR("failed to lookup MSTB with lct %d, rad %02x\n", lct, rad[0]);
  1179.                                         return NULL;
  1180.                                 }
  1181.  
  1182.                                 mstb = port->mstb;
  1183.                                 break;
  1184.                         }
  1185.                 }
  1186.         }
  1187.         kref_get(&mstb->kref);
  1188.         return mstb;
  1189. }
  1190.  
  1191. static void drm_dp_check_and_send_link_address(struct drm_dp_mst_topology_mgr *mgr,
  1192.                                                struct drm_dp_mst_branch *mstb)
  1193. {
  1194.         struct drm_dp_mst_port *port;
  1195.  
  1196.         if (!mstb->link_address_sent) {
  1197.                 drm_dp_send_link_address(mgr, mstb);
  1198.                 mstb->link_address_sent = true;
  1199.         }
  1200.         list_for_each_entry(port, &mstb->ports, next) {
  1201.                 if (port->input)
  1202.                         continue;
  1203.  
  1204.                 if (!port->ddps)
  1205.                         continue;
  1206.  
  1207.                 if (!port->available_pbn)
  1208.                         drm_dp_send_enum_path_resources(mgr, mstb, port);
  1209.  
  1210.                 if (port->mstb)
  1211.                         drm_dp_check_and_send_link_address(mgr, port->mstb);
  1212.         }
  1213. }
  1214.  
  1215. static void drm_dp_mst_link_probe_work(struct work_struct *work)
  1216. {
  1217.         struct drm_dp_mst_topology_mgr *mgr = container_of(work, struct drm_dp_mst_topology_mgr, work);
  1218.  
  1219.         drm_dp_check_and_send_link_address(mgr, mgr->mst_primary);
  1220.  
  1221. }
  1222.  
  1223. static bool drm_dp_validate_guid(struct drm_dp_mst_topology_mgr *mgr,
  1224.                                  u8 *guid)
  1225. {
  1226.         static u8 zero_guid[16];
  1227.  
  1228.         if (!memcmp(guid, zero_guid, 16)) {
  1229.                 u64 salt = get_jiffies_64();
  1230.                 memcpy(&guid[0], &salt, sizeof(u64));
  1231.                 memcpy(&guid[8], &salt, sizeof(u64));
  1232.                 return false;
  1233.         }
  1234.         return true;
  1235. }
  1236.  
  1237. #if 0
  1238. static int build_dpcd_read(struct drm_dp_sideband_msg_tx *msg, u8 port_num, u32 offset, u8 num_bytes)
  1239. {
  1240.         struct drm_dp_sideband_msg_req_body req;
  1241.  
  1242.         req.req_type = DP_REMOTE_DPCD_READ;
  1243.         req.u.dpcd_read.port_number = port_num;
  1244.         req.u.dpcd_read.dpcd_address = offset;
  1245.         req.u.dpcd_read.num_bytes = num_bytes;
  1246.         drm_dp_encode_sideband_req(&req, msg);
  1247.  
  1248.         return 0;
  1249. }
  1250. #endif
  1251.  
  1252. static int drm_dp_send_sideband_msg(struct drm_dp_mst_topology_mgr *mgr,
  1253.                                     bool up, u8 *msg, int len)
  1254. {
  1255.         int ret;
  1256.         int regbase = up ? DP_SIDEBAND_MSG_UP_REP_BASE : DP_SIDEBAND_MSG_DOWN_REQ_BASE;
  1257.         int tosend, total, offset;
  1258.         int retries = 0;
  1259.  
  1260. retry:
  1261.         total = len;
  1262.         offset = 0;
  1263.         do {
  1264.                 tosend = min3(mgr->max_dpcd_transaction_bytes, 16, total);
  1265.  
  1266.                 ret = drm_dp_dpcd_write(mgr->aux, regbase + offset,
  1267.                                         &msg[offset],
  1268.                                         tosend);
  1269.                 if (ret != tosend) {
  1270.                         if (ret == -EIO && retries < 5) {
  1271.                                 retries++;
  1272.                                 goto retry;
  1273.                         }
  1274.                         DRM_DEBUG_KMS("failed to dpcd write %d %d\n", tosend, ret);
  1275.                         WARN(1, "fail\n");
  1276.  
  1277.                         return -EIO;
  1278.                 }
  1279.                 offset += tosend;
  1280.                 total -= tosend;
  1281.         } while (total > 0);
  1282.         return 0;
  1283. }
  1284.  
  1285. static int set_hdr_from_dst_qlock(struct drm_dp_sideband_msg_hdr *hdr,
  1286.                                   struct drm_dp_sideband_msg_tx *txmsg)
  1287. {
  1288.         struct drm_dp_mst_branch *mstb = txmsg->dst;
  1289.  
  1290.         /* both msg slots are full */
  1291.         if (txmsg->seqno == -1) {
  1292.                 if (mstb->tx_slots[0] && mstb->tx_slots[1]) {
  1293.                         DRM_DEBUG_KMS("%s: failed to find slot\n", __func__);
  1294.                         return -EAGAIN;
  1295.                 }
  1296.                 if (mstb->tx_slots[0] == NULL && mstb->tx_slots[1] == NULL) {
  1297.                         txmsg->seqno = mstb->last_seqno;
  1298.                         mstb->last_seqno ^= 1;
  1299.                 } else if (mstb->tx_slots[0] == NULL)
  1300.                         txmsg->seqno = 0;
  1301.                 else
  1302.                         txmsg->seqno = 1;
  1303.                 mstb->tx_slots[txmsg->seqno] = txmsg;
  1304.         }
  1305.         hdr->broadcast = 0;
  1306.         hdr->path_msg = txmsg->path_msg;
  1307.         hdr->lct = mstb->lct;
  1308.         hdr->lcr = mstb->lct - 1;
  1309.         if (mstb->lct > 1)
  1310.                 memcpy(hdr->rad, mstb->rad, mstb->lct / 2);
  1311.         hdr->seqno = txmsg->seqno;
  1312.         return 0;
  1313. }
  1314. /*
  1315.  * process a single block of the next message in the sideband queue
  1316.  */
  1317. static int process_single_tx_qlock(struct drm_dp_mst_topology_mgr *mgr,
  1318.                                    struct drm_dp_sideband_msg_tx *txmsg,
  1319.                                    bool up)
  1320. {
  1321.         u8 chunk[48];
  1322.         struct drm_dp_sideband_msg_hdr hdr;
  1323.         int len, space, idx, tosend;
  1324.         int ret;
  1325.  
  1326.         memset(&hdr, 0, sizeof(struct drm_dp_sideband_msg_hdr));
  1327.  
  1328.         if (txmsg->state == DRM_DP_SIDEBAND_TX_QUEUED) {
  1329.                 txmsg->seqno = -1;
  1330.                 txmsg->state = DRM_DP_SIDEBAND_TX_START_SEND;
  1331.         }
  1332.  
  1333.         /* make hdr from dst mst - for replies use seqno
  1334.            otherwise assign one */
  1335.         ret = set_hdr_from_dst_qlock(&hdr, txmsg);
  1336.         if (ret < 0)
  1337.                 return ret;
  1338.  
  1339.         /* amount left to send in this message */
  1340.         len = txmsg->cur_len - txmsg->cur_offset;
  1341.  
  1342.         /* 48 - sideband msg size - 1 byte for data CRC, x header bytes */
  1343.         space = 48 - 1 - drm_dp_calc_sb_hdr_size(&hdr);
  1344.  
  1345.         tosend = min(len, space);
  1346.         if (len == txmsg->cur_len)
  1347.                 hdr.somt = 1;
  1348.         if (space >= len)
  1349.                 hdr.eomt = 1;
  1350.  
  1351.  
  1352.         hdr.msg_len = tosend + 1;
  1353.         drm_dp_encode_sideband_msg_hdr(&hdr, chunk, &idx);
  1354.         memcpy(&chunk[idx], &txmsg->msg[txmsg->cur_offset], tosend);
  1355.         /* add crc at end */
  1356.         drm_dp_crc_sideband_chunk_req(&chunk[idx], tosend);
  1357.         idx += tosend + 1;
  1358.  
  1359.         ret = drm_dp_send_sideband_msg(mgr, up, chunk, idx);
  1360.         if (ret) {
  1361.                 DRM_DEBUG_KMS("sideband msg failed to send\n");
  1362.                 return ret;
  1363.         }
  1364.  
  1365.         txmsg->cur_offset += tosend;
  1366.         if (txmsg->cur_offset == txmsg->cur_len) {
  1367.                 txmsg->state = DRM_DP_SIDEBAND_TX_SENT;
  1368.                 return 1;
  1369.         }
  1370.         return 0;
  1371. }
  1372.  
  1373. /* must be called holding qlock */
  1374. static void process_single_down_tx_qlock(struct drm_dp_mst_topology_mgr *mgr)
  1375. {
  1376.         struct drm_dp_sideband_msg_tx *txmsg;
  1377.         int ret;
  1378.  
  1379.         /* construct a chunk from the first msg in the tx_msg queue */
  1380.         if (list_empty(&mgr->tx_msg_downq)) {
  1381.                 mgr->tx_down_in_progress = false;
  1382.                 return;
  1383.         }
  1384.         mgr->tx_down_in_progress = true;
  1385.  
  1386.         txmsg = list_first_entry(&mgr->tx_msg_downq, struct drm_dp_sideband_msg_tx, next);
  1387.         ret = process_single_tx_qlock(mgr, txmsg, false);
  1388.         if (ret == 1) {
  1389.                 /* txmsg is sent it should be in the slots now */
  1390.                 list_del(&txmsg->next);
  1391.         } else if (ret) {
  1392.                 DRM_DEBUG_KMS("failed to send msg in q %d\n", ret);
  1393.                 list_del(&txmsg->next);
  1394.                 if (txmsg->seqno != -1)
  1395.                         txmsg->dst->tx_slots[txmsg->seqno] = NULL;
  1396.                 txmsg->state = DRM_DP_SIDEBAND_TX_TIMEOUT;
  1397. //       wake_up(&mgr->tx_waitq);
  1398.         }
  1399.         if (list_empty(&mgr->tx_msg_downq)) {
  1400.                 mgr->tx_down_in_progress = false;
  1401.                 return;
  1402.         }
  1403. }
  1404.  
  1405. /* called holding qlock */
  1406. static void process_single_up_tx_qlock(struct drm_dp_mst_topology_mgr *mgr)
  1407. {
  1408.         struct drm_dp_sideband_msg_tx *txmsg;
  1409.         int ret;
  1410.  
  1411.         /* construct a chunk from the first msg in the tx_msg queue */
  1412.         if (list_empty(&mgr->tx_msg_upq)) {
  1413.                 mgr->tx_up_in_progress = false;
  1414.                 return;
  1415.         }
  1416.  
  1417.         txmsg = list_first_entry(&mgr->tx_msg_upq, struct drm_dp_sideband_msg_tx, next);
  1418.         ret = process_single_tx_qlock(mgr, txmsg, true);
  1419.         if (ret == 1) {
  1420.                 /* up txmsgs aren't put in slots - so free after we send it */
  1421.                 list_del(&txmsg->next);
  1422.                 kfree(txmsg);
  1423.         } else if (ret)
  1424.                 DRM_DEBUG_KMS("failed to send msg in q %d\n", ret);
  1425.         mgr->tx_up_in_progress = true;
  1426. }
  1427.  
  1428. static void drm_dp_queue_down_tx(struct drm_dp_mst_topology_mgr *mgr,
  1429.                                  struct drm_dp_sideband_msg_tx *txmsg)
  1430. {
  1431.         mutex_lock(&mgr->qlock);
  1432.         list_add_tail(&txmsg->next, &mgr->tx_msg_downq);
  1433.         if (!mgr->tx_down_in_progress)
  1434.                 process_single_down_tx_qlock(mgr);
  1435.         mutex_unlock(&mgr->qlock);
  1436. }
  1437.  
  1438. static int drm_dp_send_link_address(struct drm_dp_mst_topology_mgr *mgr,
  1439.                                     struct drm_dp_mst_branch *mstb)
  1440. {
  1441.         int len;
  1442.         struct drm_dp_sideband_msg_tx *txmsg;
  1443.         int ret;
  1444.  
  1445.         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
  1446.         if (!txmsg)
  1447.                 return -ENOMEM;
  1448.  
  1449.         txmsg->dst = mstb;
  1450.         len = build_link_address(txmsg);
  1451.  
  1452.         drm_dp_queue_down_tx(mgr, txmsg);
  1453.  
  1454.         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
  1455.         if (ret > 0) {
  1456.                 int i;
  1457.  
  1458.                 if (txmsg->reply.reply_type == 1)
  1459.                         DRM_DEBUG_KMS("link address nak received\n");
  1460.                 else {
  1461.                         DRM_DEBUG_KMS("link address reply: %d\n", txmsg->reply.u.link_addr.nports);
  1462.                         for (i = 0; i < txmsg->reply.u.link_addr.nports; i++) {
  1463.                                 DRM_DEBUG_KMS("port %d: input %d, pdt: %d, pn: %d, dpcd_rev: %02x, mcs: %d, ddps: %d, ldps %d, sdp %d/%d\n", i,
  1464.                                        txmsg->reply.u.link_addr.ports[i].input_port,
  1465.                                        txmsg->reply.u.link_addr.ports[i].peer_device_type,
  1466.                                        txmsg->reply.u.link_addr.ports[i].port_number,
  1467.                                        txmsg->reply.u.link_addr.ports[i].dpcd_revision,
  1468.                                        txmsg->reply.u.link_addr.ports[i].mcs,
  1469.                                        txmsg->reply.u.link_addr.ports[i].ddps,
  1470.                                        txmsg->reply.u.link_addr.ports[i].legacy_device_plug_status,
  1471.                                        txmsg->reply.u.link_addr.ports[i].num_sdp_streams,
  1472.                                        txmsg->reply.u.link_addr.ports[i].num_sdp_stream_sinks);
  1473.                         }
  1474.                         for (i = 0; i < txmsg->reply.u.link_addr.nports; i++) {
  1475.                                 drm_dp_add_port(mstb, mgr->dev, &txmsg->reply.u.link_addr.ports[i]);
  1476.                         }
  1477.                         (*mgr->cbs->hotplug)(mgr);
  1478.                 }
  1479.         } else
  1480.                 DRM_DEBUG_KMS("link address failed %d\n", ret);
  1481.  
  1482.         kfree(txmsg);
  1483.         return 0;
  1484. }
  1485.  
  1486. static int drm_dp_send_enum_path_resources(struct drm_dp_mst_topology_mgr *mgr,
  1487.                                            struct drm_dp_mst_branch *mstb,
  1488.                                            struct drm_dp_mst_port *port)
  1489. {
  1490.         int len;
  1491.         struct drm_dp_sideband_msg_tx *txmsg;
  1492.         int ret;
  1493.  
  1494.         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
  1495.         if (!txmsg)
  1496.                 return -ENOMEM;
  1497.  
  1498.         txmsg->dst = mstb;
  1499.         len = build_enum_path_resources(txmsg, port->port_num);
  1500.  
  1501.         drm_dp_queue_down_tx(mgr, txmsg);
  1502.  
  1503.         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
  1504.         if (ret > 0) {
  1505.                 if (txmsg->reply.reply_type == 1)
  1506.                         DRM_DEBUG_KMS("enum path resources nak received\n");
  1507.                 else {
  1508.                         if (port->port_num != txmsg->reply.u.path_resources.port_number)
  1509.                                 DRM_ERROR("got incorrect port in response\n");
  1510.                         DRM_DEBUG_KMS("enum path resources %d: %d %d\n", txmsg->reply.u.path_resources.port_number, txmsg->reply.u.path_resources.full_payload_bw_number,
  1511.                                txmsg->reply.u.path_resources.avail_payload_bw_number);
  1512.                         port->available_pbn = txmsg->reply.u.path_resources.avail_payload_bw_number;
  1513.                 }
  1514.         }
  1515.  
  1516.         kfree(txmsg);
  1517.         return 0;
  1518. }
  1519.  
  1520. static int drm_dp_payload_send_msg(struct drm_dp_mst_topology_mgr *mgr,
  1521.                                    struct drm_dp_mst_port *port,
  1522.                                    int id,
  1523.                                    int pbn)
  1524. {
  1525.         struct drm_dp_sideband_msg_tx *txmsg;
  1526.         struct drm_dp_mst_branch *mstb;
  1527.         int len, ret;
  1528.  
  1529.         mstb = drm_dp_get_validated_mstb_ref(mgr, port->parent);
  1530.         if (!mstb)
  1531.                 return -EINVAL;
  1532.  
  1533.         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
  1534.         if (!txmsg) {
  1535.                 ret = -ENOMEM;
  1536.                 goto fail_put;
  1537.         }
  1538.  
  1539.         txmsg->dst = mstb;
  1540.         len = build_allocate_payload(txmsg, port->port_num,
  1541.                                      id,
  1542.                                      pbn);
  1543.  
  1544.         drm_dp_queue_down_tx(mgr, txmsg);
  1545.  
  1546.         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
  1547.         if (ret > 0) {
  1548.                 if (txmsg->reply.reply_type == 1) {
  1549.                         ret = -EINVAL;
  1550.                 } else
  1551.                         ret = 0;
  1552.         }
  1553.         kfree(txmsg);
  1554. fail_put:
  1555.         drm_dp_put_mst_branch_device(mstb);
  1556.         return ret;
  1557. }
  1558.  
  1559. static int drm_dp_create_payload_step1(struct drm_dp_mst_topology_mgr *mgr,
  1560.                                        int id,
  1561.                                        struct drm_dp_payload *payload)
  1562. {
  1563.         int ret;
  1564.  
  1565.         ret = drm_dp_dpcd_write_payload(mgr, id, payload);
  1566.         if (ret < 0) {
  1567.                 payload->payload_state = 0;
  1568.                 return ret;
  1569.         }
  1570.         payload->payload_state = DP_PAYLOAD_LOCAL;
  1571.         return 0;
  1572. }
  1573.  
  1574. static int drm_dp_create_payload_step2(struct drm_dp_mst_topology_mgr *mgr,
  1575.                                        struct drm_dp_mst_port *port,
  1576.                                        int id,
  1577.                                        struct drm_dp_payload *payload)
  1578. {
  1579.         int ret;
  1580.         ret = drm_dp_payload_send_msg(mgr, port, id, port->vcpi.pbn);
  1581.         if (ret < 0)
  1582.                 return ret;
  1583.         payload->payload_state = DP_PAYLOAD_REMOTE;
  1584.         return ret;
  1585. }
  1586.  
  1587. static int drm_dp_destroy_payload_step1(struct drm_dp_mst_topology_mgr *mgr,
  1588.                                         struct drm_dp_mst_port *port,
  1589.                                         int id,
  1590.                                         struct drm_dp_payload *payload)
  1591. {
  1592.         DRM_DEBUG_KMS("\n");
  1593.         /* its okay for these to fail */
  1594.         if (port) {
  1595.                 drm_dp_payload_send_msg(mgr, port, id, 0);
  1596.         }
  1597.  
  1598.         drm_dp_dpcd_write_payload(mgr, id, payload);
  1599.         payload->payload_state = DP_PAYLOAD_DELETE_LOCAL;
  1600.         return 0;
  1601. }
  1602.  
  1603. static int drm_dp_destroy_payload_step2(struct drm_dp_mst_topology_mgr *mgr,
  1604.                                         int id,
  1605.                                         struct drm_dp_payload *payload)
  1606. {
  1607.         payload->payload_state = 0;
  1608.         return 0;
  1609. }
  1610.  
  1611. /**
  1612.  * drm_dp_update_payload_part1() - Execute payload update part 1
  1613.  * @mgr: manager to use.
  1614.  *
  1615.  * This iterates over all proposed virtual channels, and tries to
  1616.  * allocate space in the link for them. For 0->slots transitions,
  1617.  * this step just writes the VCPI to the MST device. For slots->0
  1618.  * transitions, this writes the updated VCPIs and removes the
  1619.  * remote VC payloads.
  1620.  *
  1621.  * after calling this the driver should generate ACT and payload
  1622.  * packets.
  1623.  */
  1624. int drm_dp_update_payload_part1(struct drm_dp_mst_topology_mgr *mgr)
  1625. {
  1626.         int i, j;
  1627.         int cur_slots = 1;
  1628.         struct drm_dp_payload req_payload;
  1629.         struct drm_dp_mst_port *port;
  1630.  
  1631.         mutex_lock(&mgr->payload_lock);
  1632.         for (i = 0; i < mgr->max_payloads; i++) {
  1633.                 /* solve the current payloads - compare to the hw ones
  1634.                    - update the hw view */
  1635.                 req_payload.start_slot = cur_slots;
  1636.                 if (mgr->proposed_vcpis[i]) {
  1637.                         port = container_of(mgr->proposed_vcpis[i], struct drm_dp_mst_port, vcpi);
  1638.                         req_payload.num_slots = mgr->proposed_vcpis[i]->num_slots;
  1639.                 } else {
  1640.                         port = NULL;
  1641.                         req_payload.num_slots = 0;
  1642.                 }
  1643.  
  1644.                 if (mgr->payloads[i].start_slot != req_payload.start_slot) {
  1645.                         mgr->payloads[i].start_slot = req_payload.start_slot;
  1646.                 }
  1647.                 /* work out what is required to happen with this payload */
  1648.                 if (mgr->payloads[i].num_slots != req_payload.num_slots) {
  1649.  
  1650.                         /* need to push an update for this payload */
  1651.                         if (req_payload.num_slots) {
  1652.                                 drm_dp_create_payload_step1(mgr, mgr->proposed_vcpis[i]->vcpi, &req_payload);
  1653.                                 mgr->payloads[i].num_slots = req_payload.num_slots;
  1654.                         } else if (mgr->payloads[i].num_slots) {
  1655.                                 mgr->payloads[i].num_slots = 0;
  1656.                                 drm_dp_destroy_payload_step1(mgr, port, port->vcpi.vcpi, &mgr->payloads[i]);
  1657.                                 req_payload.payload_state = mgr->payloads[i].payload_state;
  1658.                                 mgr->payloads[i].start_slot = 0;
  1659.                         }
  1660.                         mgr->payloads[i].payload_state = req_payload.payload_state;
  1661.                 }
  1662.                 cur_slots += req_payload.num_slots;
  1663.         }
  1664.  
  1665.         for (i = 0; i < mgr->max_payloads; i++) {
  1666.                 if (mgr->payloads[i].payload_state == DP_PAYLOAD_DELETE_LOCAL) {
  1667.                         DRM_DEBUG_KMS("removing payload %d\n", i);
  1668.                         for (j = i; j < mgr->max_payloads - 1; j++) {
  1669.                                 memcpy(&mgr->payloads[j], &mgr->payloads[j + 1], sizeof(struct drm_dp_payload));
  1670.                                 mgr->proposed_vcpis[j] = mgr->proposed_vcpis[j + 1];
  1671.                                 if (mgr->proposed_vcpis[j] && mgr->proposed_vcpis[j]->num_slots) {
  1672.                                         set_bit(j + 1, &mgr->payload_mask);
  1673.                                 } else {
  1674.                                         clear_bit(j + 1, &mgr->payload_mask);
  1675.                                 }
  1676.                         }
  1677.                         memset(&mgr->payloads[mgr->max_payloads - 1], 0, sizeof(struct drm_dp_payload));
  1678.                         mgr->proposed_vcpis[mgr->max_payloads - 1] = NULL;
  1679.                         clear_bit(mgr->max_payloads, &mgr->payload_mask);
  1680.  
  1681.                 }
  1682.         }
  1683.         mutex_unlock(&mgr->payload_lock);
  1684.  
  1685.         return 0;
  1686. }
  1687. EXPORT_SYMBOL(drm_dp_update_payload_part1);
  1688.  
  1689. /**
  1690.  * drm_dp_update_payload_part2() - Execute payload update part 2
  1691.  * @mgr: manager to use.
  1692.  *
  1693.  * This iterates over all proposed virtual channels, and tries to
  1694.  * allocate space in the link for them. For 0->slots transitions,
  1695.  * this step writes the remote VC payload commands. For slots->0
  1696.  * this just resets some internal state.
  1697.  */
  1698. int drm_dp_update_payload_part2(struct drm_dp_mst_topology_mgr *mgr)
  1699. {
  1700.         struct drm_dp_mst_port *port;
  1701.         int i;
  1702.         int ret = 0;
  1703.         mutex_lock(&mgr->payload_lock);
  1704.         for (i = 0; i < mgr->max_payloads; i++) {
  1705.  
  1706.                 if (!mgr->proposed_vcpis[i])
  1707.                         continue;
  1708.  
  1709.                 port = container_of(mgr->proposed_vcpis[i], struct drm_dp_mst_port, vcpi);
  1710.  
  1711.                 DRM_DEBUG_KMS("payload %d %d\n", i, mgr->payloads[i].payload_state);
  1712.                 if (mgr->payloads[i].payload_state == DP_PAYLOAD_LOCAL) {
  1713.                         ret = drm_dp_create_payload_step2(mgr, port, mgr->proposed_vcpis[i]->vcpi, &mgr->payloads[i]);
  1714.                 } else if (mgr->payloads[i].payload_state == DP_PAYLOAD_DELETE_LOCAL) {
  1715.                         ret = drm_dp_destroy_payload_step2(mgr, mgr->proposed_vcpis[i]->vcpi, &mgr->payloads[i]);
  1716.                 }
  1717.                 if (ret) {
  1718.                         mutex_unlock(&mgr->payload_lock);
  1719.                         return ret;
  1720.                 }
  1721.         }
  1722.         mutex_unlock(&mgr->payload_lock);
  1723.         return 0;
  1724. }
  1725. EXPORT_SYMBOL(drm_dp_update_payload_part2);
  1726.  
  1727. #if 0 /* unused as of yet */
  1728. static int drm_dp_send_dpcd_read(struct drm_dp_mst_topology_mgr *mgr,
  1729.                                  struct drm_dp_mst_port *port,
  1730.                                  int offset, int size)
  1731. {
  1732.         int len;
  1733.         struct drm_dp_sideband_msg_tx *txmsg;
  1734.  
  1735.         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
  1736.         if (!txmsg)
  1737.                 return -ENOMEM;
  1738.  
  1739.         len = build_dpcd_read(txmsg, port->port_num, 0, 8);
  1740.         txmsg->dst = port->parent;
  1741.  
  1742.         drm_dp_queue_down_tx(mgr, txmsg);
  1743.  
  1744.         return 0;
  1745. }
  1746. #endif
  1747.  
  1748. static int drm_dp_send_dpcd_write(struct drm_dp_mst_topology_mgr *mgr,
  1749.                                   struct drm_dp_mst_port *port,
  1750.                                   int offset, int size, u8 *bytes)
  1751. {
  1752.         int len;
  1753.         int ret;
  1754.         struct drm_dp_sideband_msg_tx *txmsg;
  1755.         struct drm_dp_mst_branch *mstb;
  1756.  
  1757.         mstb = drm_dp_get_validated_mstb_ref(mgr, port->parent);
  1758.         if (!mstb)
  1759.                 return -EINVAL;
  1760.  
  1761.         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
  1762.         if (!txmsg) {
  1763.                 ret = -ENOMEM;
  1764.                 goto fail_put;
  1765.         }
  1766.  
  1767.         len = build_dpcd_write(txmsg, port->port_num, offset, size, bytes);
  1768.         txmsg->dst = mstb;
  1769.  
  1770.         drm_dp_queue_down_tx(mgr, txmsg);
  1771.  
  1772.         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
  1773.         if (ret > 0) {
  1774.                 if (txmsg->reply.reply_type == 1) {
  1775.                         ret = -EINVAL;
  1776.                 } else
  1777.                         ret = 0;
  1778.         }
  1779.         kfree(txmsg);
  1780. fail_put:
  1781.         drm_dp_put_mst_branch_device(mstb);
  1782.         return ret;
  1783. }
  1784.  
  1785. static int drm_dp_encode_up_ack_reply(struct drm_dp_sideband_msg_tx *msg, u8 req_type)
  1786. {
  1787.         struct drm_dp_sideband_msg_reply_body reply;
  1788.  
  1789.         reply.reply_type = 1;
  1790.         reply.req_type = req_type;
  1791.         drm_dp_encode_sideband_reply(&reply, msg);
  1792.         return 0;
  1793. }
  1794.  
  1795. static int drm_dp_send_up_ack_reply(struct drm_dp_mst_topology_mgr *mgr,
  1796.                                     struct drm_dp_mst_branch *mstb,
  1797.                                     int req_type, int seqno, bool broadcast)
  1798. {
  1799.         struct drm_dp_sideband_msg_tx *txmsg;
  1800.  
  1801.         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
  1802.         if (!txmsg)
  1803.                 return -ENOMEM;
  1804.  
  1805.         txmsg->dst = mstb;
  1806.         txmsg->seqno = seqno;
  1807.         drm_dp_encode_up_ack_reply(txmsg, req_type);
  1808.  
  1809.         mutex_lock(&mgr->qlock);
  1810.         list_add_tail(&txmsg->next, &mgr->tx_msg_upq);
  1811.         if (!mgr->tx_up_in_progress) {
  1812.                 process_single_up_tx_qlock(mgr);
  1813.         }
  1814.         mutex_unlock(&mgr->qlock);
  1815.         return 0;
  1816. }
  1817.  
  1818. static bool drm_dp_get_vc_payload_bw(int dp_link_bw,
  1819.                                      int dp_link_count,
  1820.                                      int *out)
  1821. {
  1822.         switch (dp_link_bw) {
  1823.         default:
  1824.                 DRM_DEBUG_KMS("invalid link bandwidth in DPCD: %x (link count: %d)\n",
  1825.                               dp_link_bw, dp_link_count);
  1826.                 return false;
  1827.  
  1828.         case DP_LINK_BW_1_62:
  1829.                 *out = 3 * dp_link_count;
  1830.                 break;
  1831.         case DP_LINK_BW_2_7:
  1832.                 *out = 5 * dp_link_count;
  1833.                 break;
  1834.         case DP_LINK_BW_5_4:
  1835.                 *out = 10 * dp_link_count;
  1836.                 break;
  1837.         }
  1838.         return true;
  1839. }
  1840.  
  1841. /**
  1842.  * drm_dp_mst_topology_mgr_set_mst() - Set the MST state for a topology manager
  1843.  * @mgr: manager to set state for
  1844.  * @mst_state: true to enable MST on this connector - false to disable.
  1845.  *
  1846.  * This is called by the driver when it detects an MST capable device plugged
  1847.  * into a DP MST capable port, or when a DP MST capable device is unplugged.
  1848.  */
  1849. int drm_dp_mst_topology_mgr_set_mst(struct drm_dp_mst_topology_mgr *mgr, bool mst_state)
  1850. {
  1851.         int ret = 0;
  1852.         struct drm_dp_mst_branch *mstb = NULL;
  1853.  
  1854.         mutex_lock(&mgr->lock);
  1855.         if (mst_state == mgr->mst_state)
  1856.                 goto out_unlock;
  1857.  
  1858.         mgr->mst_state = mst_state;
  1859.         /* set the device into MST mode */
  1860.         if (mst_state) {
  1861.                 WARN_ON(mgr->mst_primary);
  1862.  
  1863.                 /* get dpcd info */
  1864.                 ret = drm_dp_dpcd_read(mgr->aux, DP_DPCD_REV, mgr->dpcd, DP_RECEIVER_CAP_SIZE);
  1865.                 if (ret != DP_RECEIVER_CAP_SIZE) {
  1866.                         DRM_DEBUG_KMS("failed to read DPCD\n");
  1867.                         goto out_unlock;
  1868.                 }
  1869.  
  1870.                 if (!drm_dp_get_vc_payload_bw(mgr->dpcd[1],
  1871.                                               mgr->dpcd[2] & DP_MAX_LANE_COUNT_MASK,
  1872.                                               &mgr->pbn_div)) {
  1873.                         ret = -EINVAL;
  1874.                         goto out_unlock;
  1875.                 }
  1876.  
  1877.                 mgr->total_pbn = 2560;
  1878.                 mgr->total_slots = DIV_ROUND_UP(mgr->total_pbn, mgr->pbn_div);
  1879.                 mgr->avail_slots = mgr->total_slots;
  1880.  
  1881.                 /* add initial branch device at LCT 1 */
  1882.                 mstb = drm_dp_add_mst_branch_device(1, NULL);
  1883.                 if (mstb == NULL) {
  1884.                         ret = -ENOMEM;
  1885.                         goto out_unlock;
  1886.                 }
  1887.                 mstb->mgr = mgr;
  1888.  
  1889.                 /* give this the main reference */
  1890.                 mgr->mst_primary = mstb;
  1891.                 kref_get(&mgr->mst_primary->kref);
  1892.  
  1893.                 {
  1894.                         struct drm_dp_payload reset_pay;
  1895.                         reset_pay.start_slot = 0;
  1896.                         reset_pay.num_slots = 0x3f;
  1897.                         drm_dp_dpcd_write_payload(mgr, 0, &reset_pay);
  1898.                 }
  1899.  
  1900.                 ret = drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL,
  1901.                                          DP_MST_EN | DP_UP_REQ_EN | DP_UPSTREAM_IS_SRC);
  1902.                 if (ret < 0) {
  1903.                         goto out_unlock;
  1904.                 }
  1905.  
  1906.  
  1907.                 /* sort out guid */
  1908.                 ret = drm_dp_dpcd_read(mgr->aux, DP_GUID, mgr->guid, 16);
  1909.                 if (ret != 16) {
  1910.                         DRM_DEBUG_KMS("failed to read DP GUID %d\n", ret);
  1911.                         goto out_unlock;
  1912.                 }
  1913.  
  1914.                 mgr->guid_valid = drm_dp_validate_guid(mgr, mgr->guid);
  1915.                 if (!mgr->guid_valid) {
  1916.                         ret = drm_dp_dpcd_write(mgr->aux, DP_GUID, mgr->guid, 16);
  1917.                         mgr->guid_valid = true;
  1918.                 }
  1919.  
  1920. //       queue_work(system_long_wq, &mgr->work);
  1921.  
  1922.                 ret = 0;
  1923.         } else {
  1924.                 /* disable MST on the device */
  1925.                 mstb = mgr->mst_primary;
  1926.                 mgr->mst_primary = NULL;
  1927.                 /* this can fail if the device is gone */
  1928.                 drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL, 0);
  1929.                 ret = 0;
  1930.                 memset(mgr->payloads, 0, mgr->max_payloads * sizeof(struct drm_dp_payload));
  1931.                 mgr->payload_mask = 0;
  1932.                 set_bit(0, &mgr->payload_mask);
  1933.                 mgr->vcpi_mask = 0;
  1934.         }
  1935.  
  1936. out_unlock:
  1937.         mutex_unlock(&mgr->lock);
  1938.         if (mstb)
  1939.                 drm_dp_put_mst_branch_device(mstb);
  1940.         return ret;
  1941.  
  1942. }
  1943. EXPORT_SYMBOL(drm_dp_mst_topology_mgr_set_mst);
  1944.  
  1945. /**
  1946.  * drm_dp_mst_topology_mgr_suspend() - suspend the MST manager
  1947.  * @mgr: manager to suspend
  1948.  *
  1949.  * This function tells the MST device that we can't handle UP messages
  1950.  * anymore. This should stop it from sending any since we are suspended.
  1951.  */
  1952. void drm_dp_mst_topology_mgr_suspend(struct drm_dp_mst_topology_mgr *mgr)
  1953. {
  1954.         mutex_lock(&mgr->lock);
  1955.         drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL,
  1956.                            DP_MST_EN | DP_UPSTREAM_IS_SRC);
  1957.         mutex_unlock(&mgr->lock);
  1958. }
  1959. EXPORT_SYMBOL(drm_dp_mst_topology_mgr_suspend);
  1960.  
  1961. /**
  1962.  * drm_dp_mst_topology_mgr_resume() - resume the MST manager
  1963.  * @mgr: manager to resume
  1964.  *
  1965.  * This will fetch DPCD and see if the device is still there,
  1966.  * if it is, it will rewrite the MSTM control bits, and return.
  1967.  *
  1968.  * if the device fails this returns -1, and the driver should do
  1969.  * a full MST reprobe, in case we were undocked.
  1970.  */
  1971. int drm_dp_mst_topology_mgr_resume(struct drm_dp_mst_topology_mgr *mgr)
  1972. {
  1973.         int ret = 0;
  1974.  
  1975.         mutex_lock(&mgr->lock);
  1976.  
  1977.         if (mgr->mst_primary) {
  1978.                 int sret;
  1979.                 sret = drm_dp_dpcd_read(mgr->aux, DP_DPCD_REV, mgr->dpcd, DP_RECEIVER_CAP_SIZE);
  1980.                 if (sret != DP_RECEIVER_CAP_SIZE) {
  1981.                         DRM_DEBUG_KMS("dpcd read failed - undocked during suspend?\n");
  1982.                         ret = -1;
  1983.                         goto out_unlock;
  1984.                 }
  1985.  
  1986.                 ret = drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL,
  1987.                                          DP_MST_EN | DP_UP_REQ_EN | DP_UPSTREAM_IS_SRC);
  1988.                 if (ret < 0) {
  1989.                         DRM_DEBUG_KMS("mst write failed - undocked during suspend?\n");
  1990.                         ret = -1;
  1991.                         goto out_unlock;
  1992.                 }
  1993.                 ret = 0;
  1994.         } else
  1995.                 ret = -1;
  1996.  
  1997. out_unlock:
  1998.         mutex_unlock(&mgr->lock);
  1999.         return ret;
  2000. }
  2001. EXPORT_SYMBOL(drm_dp_mst_topology_mgr_resume);
  2002.  
  2003. static void drm_dp_get_one_sb_msg(struct drm_dp_mst_topology_mgr *mgr, bool up)
  2004. {
  2005.         int len;
  2006.         u8 replyblock[32];
  2007.         int replylen, origlen, curreply;
  2008.         int ret;
  2009.         struct drm_dp_sideband_msg_rx *msg;
  2010.         int basereg = up ? DP_SIDEBAND_MSG_UP_REQ_BASE : DP_SIDEBAND_MSG_DOWN_REP_BASE;
  2011.         msg = up ? &mgr->up_req_recv : &mgr->down_rep_recv;
  2012.  
  2013.         len = min(mgr->max_dpcd_transaction_bytes, 16);
  2014.         ret = drm_dp_dpcd_read(mgr->aux, basereg,
  2015.                                replyblock, len);
  2016.         if (ret != len) {
  2017.                 DRM_DEBUG_KMS("failed to read DPCD down rep %d %d\n", len, ret);
  2018.                 return;
  2019.         }
  2020.         ret = drm_dp_sideband_msg_build(msg, replyblock, len, true);
  2021.         if (!ret) {
  2022.                 DRM_DEBUG_KMS("sideband msg build failed %d\n", replyblock[0]);
  2023.                 return;
  2024.         }
  2025.         replylen = msg->curchunk_len + msg->curchunk_hdrlen;
  2026.  
  2027.         origlen = replylen;
  2028.         replylen -= len;
  2029.         curreply = len;
  2030.         while (replylen > 0) {
  2031.                 len = min3(replylen, mgr->max_dpcd_transaction_bytes, 16);
  2032.                 ret = drm_dp_dpcd_read(mgr->aux, basereg + curreply,
  2033.                                     replyblock, len);
  2034.                 if (ret != len) {
  2035.                         DRM_DEBUG_KMS("failed to read a chunk\n");
  2036.                 }
  2037.                 ret = drm_dp_sideband_msg_build(msg, replyblock, len, false);
  2038.                 if (ret == false)
  2039.                         DRM_DEBUG_KMS("failed to build sideband msg\n");
  2040.                 curreply += len;
  2041.                 replylen -= len;
  2042.         }
  2043. }
  2044.  
  2045. static int drm_dp_mst_handle_down_rep(struct drm_dp_mst_topology_mgr *mgr)
  2046. {
  2047.         int ret = 0;
  2048.  
  2049.         drm_dp_get_one_sb_msg(mgr, false);
  2050.  
  2051.         if (mgr->down_rep_recv.have_eomt) {
  2052.                 struct drm_dp_sideband_msg_tx *txmsg;
  2053.                 struct drm_dp_mst_branch *mstb;
  2054.                 int slot = -1;
  2055.                 mstb = drm_dp_get_mst_branch_device(mgr,
  2056.                                                     mgr->down_rep_recv.initial_hdr.lct,
  2057.                                                     mgr->down_rep_recv.initial_hdr.rad);
  2058.  
  2059.                 if (!mstb) {
  2060.                         DRM_DEBUG_KMS("Got MST reply from unknown device %d\n", mgr->down_rep_recv.initial_hdr.lct);
  2061.                         memset(&mgr->down_rep_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
  2062.                         return 0;
  2063.                 }
  2064.  
  2065.                 /* find the message */
  2066.                 slot = mgr->down_rep_recv.initial_hdr.seqno;
  2067.                 mutex_lock(&mgr->qlock);
  2068.                 txmsg = mstb->tx_slots[slot];
  2069.                 /* remove from slots */
  2070.                 mutex_unlock(&mgr->qlock);
  2071.  
  2072.                 if (!txmsg) {
  2073.                         DRM_DEBUG_KMS("Got MST reply with no msg %p %d %d %02x %02x\n",
  2074.                                mstb,
  2075.                                mgr->down_rep_recv.initial_hdr.seqno,
  2076.                                mgr->down_rep_recv.initial_hdr.lct,
  2077.                                       mgr->down_rep_recv.initial_hdr.rad[0],
  2078.                                       mgr->down_rep_recv.msg[0]);
  2079.                         drm_dp_put_mst_branch_device(mstb);
  2080.                         memset(&mgr->down_rep_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
  2081.                         return 0;
  2082.                 }
  2083.  
  2084.                 drm_dp_sideband_parse_reply(&mgr->down_rep_recv, &txmsg->reply);
  2085.                 if (txmsg->reply.reply_type == 1) {
  2086.                         DRM_DEBUG_KMS("Got NAK reply: req 0x%02x, reason 0x%02x, nak data 0x%02x\n", txmsg->reply.req_type, txmsg->reply.u.nak.reason, txmsg->reply.u.nak.nak_data);
  2087.                 }
  2088.  
  2089.                 memset(&mgr->down_rep_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
  2090.                 drm_dp_put_mst_branch_device(mstb);
  2091.  
  2092.                 mutex_lock(&mgr->qlock);
  2093.                 txmsg->state = DRM_DP_SIDEBAND_TX_RX;
  2094.                 mstb->tx_slots[slot] = NULL;
  2095.                 mutex_unlock(&mgr->qlock);
  2096.  
  2097. //       wake_up(&mgr->tx_waitq);
  2098.         }
  2099.         return ret;
  2100. }
  2101.  
  2102. static int drm_dp_mst_handle_up_req(struct drm_dp_mst_topology_mgr *mgr)
  2103. {
  2104.         int ret = 0;
  2105.         drm_dp_get_one_sb_msg(mgr, true);
  2106.  
  2107.         if (mgr->up_req_recv.have_eomt) {
  2108.                 struct drm_dp_sideband_msg_req_body msg;
  2109.                 struct drm_dp_mst_branch *mstb;
  2110.                 bool seqno;
  2111.                 mstb = drm_dp_get_mst_branch_device(mgr,
  2112.                                                     mgr->up_req_recv.initial_hdr.lct,
  2113.                                                     mgr->up_req_recv.initial_hdr.rad);
  2114.                 if (!mstb) {
  2115.                         DRM_DEBUG_KMS("Got MST reply from unknown device %d\n", mgr->up_req_recv.initial_hdr.lct);
  2116.                         memset(&mgr->up_req_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
  2117.                         return 0;
  2118.                 }
  2119.  
  2120.                 seqno = mgr->up_req_recv.initial_hdr.seqno;
  2121.                 drm_dp_sideband_parse_req(&mgr->up_req_recv, &msg);
  2122.  
  2123.                 if (msg.req_type == DP_CONNECTION_STATUS_NOTIFY) {
  2124.                         drm_dp_send_up_ack_reply(mgr, mstb, msg.req_type, seqno, false);
  2125.                         drm_dp_update_port(mstb, &msg.u.conn_stat);
  2126.                         DRM_DEBUG_KMS("Got CSN: pn: %d ldps:%d ddps: %d mcs: %d ip: %d pdt: %d\n", msg.u.conn_stat.port_number, msg.u.conn_stat.legacy_device_plug_status, msg.u.conn_stat.displayport_device_plug_status, msg.u.conn_stat.message_capability_status, msg.u.conn_stat.input_port, msg.u.conn_stat.peer_device_type);
  2127.                         (*mgr->cbs->hotplug)(mgr);
  2128.  
  2129.                 } else if (msg.req_type == DP_RESOURCE_STATUS_NOTIFY) {
  2130.                         drm_dp_send_up_ack_reply(mgr, mstb, msg.req_type, seqno, false);
  2131.                         DRM_DEBUG_KMS("Got RSN: pn: %d avail_pbn %d\n", msg.u.resource_stat.port_number, msg.u.resource_stat.available_pbn);
  2132.                 }
  2133.  
  2134.                 drm_dp_put_mst_branch_device(mstb);
  2135.                 memset(&mgr->up_req_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
  2136.         }
  2137.         return ret;
  2138. }
  2139.  
  2140. /**
  2141.  * drm_dp_mst_hpd_irq() - MST hotplug IRQ notify
  2142.  * @mgr: manager to notify irq for.
  2143.  * @esi: 4 bytes from SINK_COUNT_ESI
  2144.  * @handled: whether the hpd interrupt was consumed or not
  2145.  *
  2146.  * This should be called from the driver when it detects a short IRQ,
  2147.  * along with the value of the DEVICE_SERVICE_IRQ_VECTOR_ESI0. The
  2148.  * topology manager will process the sideband messages received as a result
  2149.  * of this.
  2150.  */
  2151. int drm_dp_mst_hpd_irq(struct drm_dp_mst_topology_mgr *mgr, u8 *esi, bool *handled)
  2152. {
  2153.         int ret = 0;
  2154.         int sc;
  2155.         *handled = false;
  2156.         sc = esi[0] & 0x3f;
  2157.  
  2158.         if (sc != mgr->sink_count) {
  2159.                 mgr->sink_count = sc;
  2160.                 *handled = true;
  2161.         }
  2162.  
  2163.         if (esi[1] & DP_DOWN_REP_MSG_RDY) {
  2164.                 ret = drm_dp_mst_handle_down_rep(mgr);
  2165.                 *handled = true;
  2166.         }
  2167.  
  2168.         if (esi[1] & DP_UP_REQ_MSG_RDY) {
  2169.                 ret |= drm_dp_mst_handle_up_req(mgr);
  2170.                 *handled = true;
  2171.         }
  2172.  
  2173.         drm_dp_mst_kick_tx(mgr);
  2174.         return ret;
  2175. }
  2176. EXPORT_SYMBOL(drm_dp_mst_hpd_irq);
  2177.  
  2178. /**
  2179.  * drm_dp_mst_detect_port() - get connection status for an MST port
  2180.  * @mgr: manager for this port
  2181.  * @port: unverified pointer to a port
  2182.  *
  2183.  * This returns the current connection state for a port. It validates the
  2184.  * port pointer still exists so the caller doesn't require a reference
  2185.  */
  2186. enum drm_connector_status drm_dp_mst_detect_port(struct drm_connector *connector,
  2187.                                                  struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port)
  2188. {
  2189.         enum drm_connector_status status = connector_status_disconnected;
  2190.  
  2191.         /* we need to search for the port in the mgr in case its gone */
  2192.         port = drm_dp_get_validated_port_ref(mgr, port);
  2193.         if (!port)
  2194.                 return connector_status_disconnected;
  2195.  
  2196.         if (!port->ddps)
  2197.                 goto out;
  2198.  
  2199.         switch (port->pdt) {
  2200.         case DP_PEER_DEVICE_NONE:
  2201.         case DP_PEER_DEVICE_MST_BRANCHING:
  2202.                 break;
  2203.  
  2204.         case DP_PEER_DEVICE_SST_SINK:
  2205.                 status = connector_status_connected;
  2206.                 /* for logical ports - cache the EDID */
  2207.                 if (port->port_num >= 8 && !port->cached_edid) {
  2208.                         port->cached_edid = drm_get_edid(connector, &port->aux.ddc);
  2209.                 }
  2210.                 break;
  2211.         case DP_PEER_DEVICE_DP_LEGACY_CONV:
  2212.                 if (port->ldps)
  2213.                         status = connector_status_connected;
  2214.                 break;
  2215.         }
  2216. out:
  2217.         drm_dp_put_port(port);
  2218.         return status;
  2219. }
  2220. EXPORT_SYMBOL(drm_dp_mst_detect_port);
  2221.  
  2222. /**
  2223.  * drm_dp_mst_get_edid() - get EDID for an MST port
  2224.  * @connector: toplevel connector to get EDID for
  2225.  * @mgr: manager for this port
  2226.  * @port: unverified pointer to a port.
  2227.  *
  2228.  * This returns an EDID for the port connected to a connector,
  2229.  * It validates the pointer still exists so the caller doesn't require a
  2230.  * reference.
  2231.  */
  2232. struct edid *drm_dp_mst_get_edid(struct drm_connector *connector, struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port)
  2233. {
  2234.         struct edid *edid = NULL;
  2235.  
  2236.         /* we need to search for the port in the mgr in case its gone */
  2237.         port = drm_dp_get_validated_port_ref(mgr, port);
  2238.         if (!port)
  2239.                 return NULL;
  2240.  
  2241.         if (port->cached_edid)
  2242.                 edid = drm_edid_duplicate(port->cached_edid);
  2243.         else
  2244.         edid = drm_get_edid(connector, &port->aux.ddc);
  2245.  
  2246.         drm_mode_connector_set_tile_property(connector);
  2247.         drm_dp_put_port(port);
  2248.         return edid;
  2249. }
  2250. EXPORT_SYMBOL(drm_dp_mst_get_edid);
  2251.  
  2252. /**
  2253.  * drm_dp_find_vcpi_slots() - find slots for this PBN value
  2254.  * @mgr: manager to use
  2255.  * @pbn: payload bandwidth to convert into slots.
  2256.  */
  2257. int drm_dp_find_vcpi_slots(struct drm_dp_mst_topology_mgr *mgr,
  2258.                            int pbn)
  2259. {
  2260.         int num_slots;
  2261.  
  2262.         num_slots = DIV_ROUND_UP(pbn, mgr->pbn_div);
  2263.  
  2264.         if (num_slots > mgr->avail_slots)
  2265.                 return -ENOSPC;
  2266.         return num_slots;
  2267. }
  2268. EXPORT_SYMBOL(drm_dp_find_vcpi_slots);
  2269.  
  2270. static int drm_dp_init_vcpi(struct drm_dp_mst_topology_mgr *mgr,
  2271.                             struct drm_dp_vcpi *vcpi, int pbn)
  2272. {
  2273.         int num_slots;
  2274.         int ret;
  2275.  
  2276.         num_slots = DIV_ROUND_UP(pbn, mgr->pbn_div);
  2277.  
  2278.         if (num_slots > mgr->avail_slots)
  2279.                 return -ENOSPC;
  2280.  
  2281.         vcpi->pbn = pbn;
  2282.         vcpi->aligned_pbn = num_slots * mgr->pbn_div;
  2283.         vcpi->num_slots = num_slots;
  2284.  
  2285.         ret = drm_dp_mst_assign_payload_id(mgr, vcpi);
  2286.         if (ret < 0)
  2287.                 return ret;
  2288.         return 0;
  2289. }
  2290.  
  2291. /**
  2292.  * drm_dp_mst_allocate_vcpi() - Allocate a virtual channel
  2293.  * @mgr: manager for this port
  2294.  * @port: port to allocate a virtual channel for.
  2295.  * @pbn: payload bandwidth number to request
  2296.  * @slots: returned number of slots for this PBN.
  2297.  */
  2298. bool drm_dp_mst_allocate_vcpi(struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port, int pbn, int *slots)
  2299. {
  2300.         int ret;
  2301.  
  2302.         port = drm_dp_get_validated_port_ref(mgr, port);
  2303.         if (!port)
  2304.                 return false;
  2305.  
  2306.         if (port->vcpi.vcpi > 0) {
  2307.                 DRM_DEBUG_KMS("payload: vcpi %d already allocated for pbn %d - requested pbn %d\n", port->vcpi.vcpi, port->vcpi.pbn, pbn);
  2308.                 if (pbn == port->vcpi.pbn) {
  2309.                         *slots = port->vcpi.num_slots;
  2310.                         return true;
  2311.                 }
  2312.         }
  2313.  
  2314.         ret = drm_dp_init_vcpi(mgr, &port->vcpi, pbn);
  2315.         if (ret) {
  2316.                 DRM_DEBUG_KMS("failed to init vcpi %d %d %d\n", DIV_ROUND_UP(pbn, mgr->pbn_div), mgr->avail_slots, ret);
  2317.                 goto out;
  2318.         }
  2319.         DRM_DEBUG_KMS("initing vcpi for %d %d\n", pbn, port->vcpi.num_slots);
  2320.         *slots = port->vcpi.num_slots;
  2321.  
  2322.         drm_dp_put_port(port);
  2323.         return true;
  2324. out:
  2325.         return false;
  2326. }
  2327. EXPORT_SYMBOL(drm_dp_mst_allocate_vcpi);
  2328.  
  2329. /**
  2330.  * drm_dp_mst_reset_vcpi_slots() - Reset number of slots to 0 for VCPI
  2331.  * @mgr: manager for this port
  2332.  * @port: unverified pointer to a port.
  2333.  *
  2334.  * This just resets the number of slots for the ports VCPI for later programming.
  2335.  */
  2336. void drm_dp_mst_reset_vcpi_slots(struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port)
  2337. {
  2338.         port = drm_dp_get_validated_port_ref(mgr, port);
  2339.         if (!port)
  2340.                 return;
  2341.         port->vcpi.num_slots = 0;
  2342.         drm_dp_put_port(port);
  2343. }
  2344. EXPORT_SYMBOL(drm_dp_mst_reset_vcpi_slots);
  2345.  
  2346. /**
  2347.  * drm_dp_mst_deallocate_vcpi() - deallocate a VCPI
  2348.  * @mgr: manager for this port
  2349.  * @port: unverified port to deallocate vcpi for
  2350.  */
  2351. void drm_dp_mst_deallocate_vcpi(struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port)
  2352. {
  2353.         port = drm_dp_get_validated_port_ref(mgr, port);
  2354.         if (!port)
  2355.                 return;
  2356.  
  2357.         drm_dp_mst_put_payload_id(mgr, port->vcpi.vcpi);
  2358.         port->vcpi.num_slots = 0;
  2359.         port->vcpi.pbn = 0;
  2360.         port->vcpi.aligned_pbn = 0;
  2361.         port->vcpi.vcpi = 0;
  2362.         drm_dp_put_port(port);
  2363. }
  2364. EXPORT_SYMBOL(drm_dp_mst_deallocate_vcpi);
  2365.  
  2366. static int drm_dp_dpcd_write_payload(struct drm_dp_mst_topology_mgr *mgr,
  2367.                                      int id, struct drm_dp_payload *payload)
  2368. {
  2369.         u8 payload_alloc[3], status;
  2370.         int ret;
  2371.         int retries = 0;
  2372.  
  2373.         drm_dp_dpcd_writeb(mgr->aux, DP_PAYLOAD_TABLE_UPDATE_STATUS,
  2374.                            DP_PAYLOAD_TABLE_UPDATED);
  2375.  
  2376.         payload_alloc[0] = id;
  2377.         payload_alloc[1] = payload->start_slot;
  2378.         payload_alloc[2] = payload->num_slots;
  2379.  
  2380.         ret = drm_dp_dpcd_write(mgr->aux, DP_PAYLOAD_ALLOCATE_SET, payload_alloc, 3);
  2381.         if (ret != 3) {
  2382.                 DRM_DEBUG_KMS("failed to write payload allocation %d\n", ret);
  2383.                 goto fail;
  2384.         }
  2385.  
  2386. retry:
  2387.         ret = drm_dp_dpcd_readb(mgr->aux, DP_PAYLOAD_TABLE_UPDATE_STATUS, &status);
  2388.         if (ret < 0) {
  2389.                 DRM_DEBUG_KMS("failed to read payload table status %d\n", ret);
  2390.                 goto fail;
  2391.         }
  2392.  
  2393.         if (!(status & DP_PAYLOAD_TABLE_UPDATED)) {
  2394.                 retries++;
  2395.                 if (retries < 20) {
  2396.                         usleep_range(10000, 20000);
  2397.                         goto retry;
  2398.                 }
  2399.                 DRM_DEBUG_KMS("status not set after read payload table status %d\n", status);
  2400.                 ret = -EINVAL;
  2401.                 goto fail;
  2402.         }
  2403.         ret = 0;
  2404. fail:
  2405.         return ret;
  2406. }
  2407.  
  2408.  
  2409. /**
  2410.  * drm_dp_check_act_status() - Check ACT handled status.
  2411.  * @mgr: manager to use
  2412.  *
  2413.  * Check the payload status bits in the DPCD for ACT handled completion.
  2414.  */
  2415. int drm_dp_check_act_status(struct drm_dp_mst_topology_mgr *mgr)
  2416. {
  2417.         u8 status;
  2418.         int ret;
  2419.         int count = 0;
  2420.  
  2421.         do {
  2422.                 ret = drm_dp_dpcd_readb(mgr->aux, DP_PAYLOAD_TABLE_UPDATE_STATUS, &status);
  2423.  
  2424.                 if (ret < 0) {
  2425.                         DRM_DEBUG_KMS("failed to read payload table status %d\n", ret);
  2426.                         goto fail;
  2427.                 }
  2428.  
  2429.                 if (status & DP_PAYLOAD_ACT_HANDLED)
  2430.                         break;
  2431.                 count++;
  2432.                 udelay(100);
  2433.  
  2434.         } while (count < 30);
  2435.  
  2436.         if (!(status & DP_PAYLOAD_ACT_HANDLED)) {
  2437.                 DRM_DEBUG_KMS("failed to get ACT bit %d after %d retries\n", status, count);
  2438.                 ret = -EINVAL;
  2439.                 goto fail;
  2440.         }
  2441.         return 0;
  2442. fail:
  2443.         return ret;
  2444. }
  2445. EXPORT_SYMBOL(drm_dp_check_act_status);
  2446.  
  2447. /**
  2448.  * drm_dp_calc_pbn_mode() - Calculate the PBN for a mode.
  2449.  * @clock: dot clock for the mode
  2450.  * @bpp: bpp for the mode.
  2451.  *
  2452.  * This uses the formula in the spec to calculate the PBN value for a mode.
  2453.  */
  2454. int drm_dp_calc_pbn_mode(int clock, int bpp)
  2455. {
  2456.         fixed20_12 pix_bw;
  2457.         fixed20_12 fbpp;
  2458.         fixed20_12 result;
  2459.         fixed20_12 margin, tmp;
  2460.         u32 res;
  2461.  
  2462.         pix_bw.full = dfixed_const(clock);
  2463.         fbpp.full = dfixed_const(bpp);
  2464.         tmp.full = dfixed_const(8);
  2465.         fbpp.full = dfixed_div(fbpp, tmp);
  2466.  
  2467.         result.full = dfixed_mul(pix_bw, fbpp);
  2468.         margin.full = dfixed_const(54);
  2469.         tmp.full = dfixed_const(64);
  2470.         margin.full = dfixed_div(margin, tmp);
  2471.         result.full = dfixed_div(result, margin);
  2472.  
  2473.         margin.full = dfixed_const(1006);
  2474.         tmp.full = dfixed_const(1000);
  2475.         margin.full = dfixed_div(margin, tmp);
  2476.         result.full = dfixed_mul(result, margin);
  2477.  
  2478.         result.full = dfixed_div(result, tmp);
  2479.         result.full = dfixed_ceil(result);
  2480.         res = dfixed_trunc(result);
  2481.         return res;
  2482. }
  2483. EXPORT_SYMBOL(drm_dp_calc_pbn_mode);
  2484.  
  2485. static int test_calc_pbn_mode(void)
  2486. {
  2487.         int ret;
  2488.         ret = drm_dp_calc_pbn_mode(154000, 30);
  2489.         if (ret != 689)
  2490.                 return -EINVAL;
  2491.         ret = drm_dp_calc_pbn_mode(234000, 30);
  2492.         if (ret != 1047)
  2493.                 return -EINVAL;
  2494.         return 0;
  2495. }
  2496.  
  2497. /* we want to kick the TX after we've ack the up/down IRQs. */
  2498. static void drm_dp_mst_kick_tx(struct drm_dp_mst_topology_mgr *mgr)
  2499. {
  2500. //   queue_work(system_long_wq, &mgr->tx_work);
  2501. }
  2502.  
  2503. static void drm_dp_mst_dump_mstb(struct seq_file *m,
  2504.                                  struct drm_dp_mst_branch *mstb)
  2505. {
  2506.         struct drm_dp_mst_port *port;
  2507.         int tabs = mstb->lct;
  2508.         char prefix[10];
  2509.         int i;
  2510.  
  2511.         for (i = 0; i < tabs; i++)
  2512.                 prefix[i] = '\t';
  2513.         prefix[i] = '\0';
  2514.  
  2515. //   seq_printf(m, "%smst: %p, %d\n", prefix, mstb, mstb->num_ports);
  2516. //   list_for_each_entry(port, &mstb->ports, next) {
  2517. //       seq_printf(m, "%sport: %d: ddps: %d ldps: %d, %p, conn: %p\n", prefix, port->port_num, port->ddps, port->ldps, port, port->connector);
  2518. //       if (port->mstb)
  2519. //           drm_dp_mst_dump_mstb(m, port->mstb);
  2520. //   }
  2521. }
  2522.  
  2523. static bool dump_dp_payload_table(struct drm_dp_mst_topology_mgr *mgr,
  2524.                                   char *buf)
  2525. {
  2526.         int ret;
  2527.         int i;
  2528.         for (i = 0; i < 4; i++) {
  2529.                 ret = drm_dp_dpcd_read(mgr->aux, DP_PAYLOAD_TABLE_UPDATE_STATUS + (i * 16), &buf[i * 16], 16);
  2530.                 if (ret != 16)
  2531.                         break;
  2532.         }
  2533.         if (i == 4)
  2534.                 return true;
  2535.         return false;
  2536. }
  2537.  
  2538. /**
  2539.  * drm_dp_mst_dump_topology(): dump topology to seq file.
  2540.  * @m: seq_file to dump output to
  2541.  * @mgr: manager to dump current topology for.
  2542.  *
  2543.  * helper to dump MST topology to a seq file for debugfs.
  2544.  */
  2545. void drm_dp_mst_dump_topology(struct seq_file *m,
  2546.                               struct drm_dp_mst_topology_mgr *mgr)
  2547. {
  2548.         int i;
  2549.         struct drm_dp_mst_port *port;
  2550.         mutex_lock(&mgr->lock);
  2551.         if (mgr->mst_primary)
  2552.                 drm_dp_mst_dump_mstb(m, mgr->mst_primary);
  2553.  
  2554.         /* dump VCPIs */
  2555.         mutex_unlock(&mgr->lock);
  2556.  
  2557.  
  2558.  
  2559. }
  2560. EXPORT_SYMBOL(drm_dp_mst_dump_topology);
  2561.  
  2562. static void drm_dp_tx_work(struct work_struct *work)
  2563. {
  2564.         struct drm_dp_mst_topology_mgr *mgr = container_of(work, struct drm_dp_mst_topology_mgr, tx_work);
  2565.  
  2566.         mutex_lock(&mgr->qlock);
  2567.         if (mgr->tx_down_in_progress)
  2568.                 process_single_down_tx_qlock(mgr);
  2569.         mutex_unlock(&mgr->qlock);
  2570. }
  2571.  
  2572. /**
  2573.  * drm_dp_mst_topology_mgr_init - initialise a topology manager
  2574.  * @mgr: manager struct to initialise
  2575.  * @dev: device providing this structure - for i2c addition.
  2576.  * @aux: DP helper aux channel to talk to this device
  2577.  * @max_dpcd_transaction_bytes: hw specific DPCD transaction limit
  2578.  * @max_payloads: maximum number of payloads this GPU can source
  2579.  * @conn_base_id: the connector object ID the MST device is connected to.
  2580.  *
  2581.  * Return 0 for success, or negative error code on failure
  2582.  */
  2583. int drm_dp_mst_topology_mgr_init(struct drm_dp_mst_topology_mgr *mgr,
  2584.                                  struct device *dev, struct drm_dp_aux *aux,
  2585.                                  int max_dpcd_transaction_bytes,
  2586.                                  int max_payloads, int conn_base_id)
  2587. {
  2588.         mutex_init(&mgr->lock);
  2589.         mutex_init(&mgr->qlock);
  2590.         mutex_init(&mgr->payload_lock);
  2591.         INIT_LIST_HEAD(&mgr->tx_msg_upq);
  2592.         INIT_LIST_HEAD(&mgr->tx_msg_downq);
  2593.         INIT_WORK(&mgr->work, drm_dp_mst_link_probe_work);
  2594.         INIT_WORK(&mgr->tx_work, drm_dp_tx_work);
  2595. //   init_waitqueue_head(&mgr->tx_waitq);
  2596.         mgr->dev = dev;
  2597.         mgr->aux = aux;
  2598.         mgr->max_dpcd_transaction_bytes = max_dpcd_transaction_bytes;
  2599.         mgr->max_payloads = max_payloads;
  2600.         mgr->conn_base_id = conn_base_id;
  2601.         mgr->payloads = kcalloc(max_payloads, sizeof(struct drm_dp_payload), GFP_KERNEL);
  2602.         if (!mgr->payloads)
  2603.                 return -ENOMEM;
  2604.         mgr->proposed_vcpis = kcalloc(max_payloads, sizeof(struct drm_dp_vcpi *), GFP_KERNEL);
  2605.         if (!mgr->proposed_vcpis)
  2606.                 return -ENOMEM;
  2607.         set_bit(0, &mgr->payload_mask);
  2608.         test_calc_pbn_mode();
  2609.         return 0;
  2610. }
  2611. EXPORT_SYMBOL(drm_dp_mst_topology_mgr_init);
  2612.  
  2613. /**
  2614.  * drm_dp_mst_topology_mgr_destroy() - destroy topology manager.
  2615.  * @mgr: manager to destroy
  2616.  */
  2617. void drm_dp_mst_topology_mgr_destroy(struct drm_dp_mst_topology_mgr *mgr)
  2618. {
  2619.         mutex_lock(&mgr->payload_lock);
  2620.         kfree(mgr->payloads);
  2621.         mgr->payloads = NULL;
  2622.         kfree(mgr->proposed_vcpis);
  2623.         mgr->proposed_vcpis = NULL;
  2624.         mutex_unlock(&mgr->payload_lock);
  2625.         mgr->dev = NULL;
  2626.         mgr->aux = NULL;
  2627. }
  2628. EXPORT_SYMBOL(drm_dp_mst_topology_mgr_destroy);
  2629.  
  2630. /* I2C device */
  2631. static int drm_dp_mst_i2c_xfer(struct i2c_adapter *adapter, struct i2c_msg *msgs,
  2632.                                int num)
  2633. {
  2634.         struct drm_dp_aux *aux = adapter->algo_data;
  2635.         struct drm_dp_mst_port *port = container_of(aux, struct drm_dp_mst_port, aux);
  2636.         struct drm_dp_mst_branch *mstb;
  2637.         struct drm_dp_mst_topology_mgr *mgr = port->mgr;
  2638.         unsigned int i;
  2639.         bool reading = false;
  2640.         struct drm_dp_sideband_msg_req_body msg;
  2641.         struct drm_dp_sideband_msg_tx *txmsg = NULL;
  2642.         int ret;
  2643.  
  2644.         mstb = drm_dp_get_validated_mstb_ref(mgr, port->parent);
  2645.         if (!mstb)
  2646.                 return -EREMOTEIO;
  2647.  
  2648.         /* construct i2c msg */
  2649.         /* see if last msg is a read */
  2650.         if (msgs[num - 1].flags & I2C_M_RD)
  2651.                 reading = true;
  2652.  
  2653.         if (!reading) {
  2654.                 DRM_DEBUG_KMS("Unsupported I2C transaction for MST device\n");
  2655.                 ret = -EIO;
  2656.                 goto out;
  2657.         }
  2658.  
  2659.         msg.req_type = DP_REMOTE_I2C_READ;
  2660.         msg.u.i2c_read.num_transactions = num - 1;
  2661.         msg.u.i2c_read.port_number = port->port_num;
  2662.         for (i = 0; i < num - 1; i++) {
  2663.                 msg.u.i2c_read.transactions[i].i2c_dev_id = msgs[i].addr;
  2664.                 msg.u.i2c_read.transactions[i].num_bytes = msgs[i].len;
  2665.                 msg.u.i2c_read.transactions[i].bytes = msgs[i].buf;
  2666.         }
  2667.         msg.u.i2c_read.read_i2c_device_id = msgs[num - 1].addr;
  2668.         msg.u.i2c_read.num_bytes_read = msgs[num - 1].len;
  2669.  
  2670.         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
  2671.         if (!txmsg) {
  2672.                 ret = -ENOMEM;
  2673.                 goto out;
  2674.         }
  2675.  
  2676.         txmsg->dst = mstb;
  2677.         drm_dp_encode_sideband_req(&msg, txmsg);
  2678.  
  2679.         drm_dp_queue_down_tx(mgr, txmsg);
  2680.  
  2681.         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
  2682.         if (ret > 0) {
  2683.  
  2684.                 if (txmsg->reply.reply_type == 1) { /* got a NAK back */
  2685.                         ret = -EREMOTEIO;
  2686.                         goto out;
  2687.                 }
  2688.                 if (txmsg->reply.u.remote_i2c_read_ack.num_bytes != msgs[num - 1].len) {
  2689.                         ret = -EIO;
  2690.                         goto out;
  2691.                 }
  2692.                 memcpy(msgs[num - 1].buf, txmsg->reply.u.remote_i2c_read_ack.bytes, msgs[num - 1].len);
  2693.                 ret = num;
  2694.         }
  2695. out:
  2696.         kfree(txmsg);
  2697.         drm_dp_put_mst_branch_device(mstb);
  2698.         return ret;
  2699. }
  2700.  
  2701. static u32 drm_dp_mst_i2c_functionality(struct i2c_adapter *adapter)
  2702. {
  2703.         return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL |
  2704.                I2C_FUNC_SMBUS_READ_BLOCK_DATA |
  2705.                I2C_FUNC_SMBUS_BLOCK_PROC_CALL |
  2706.                I2C_FUNC_10BIT_ADDR;
  2707. }
  2708.  
  2709. static const struct i2c_algorithm drm_dp_mst_i2c_algo = {
  2710.         .functionality = drm_dp_mst_i2c_functionality,
  2711.         .master_xfer = drm_dp_mst_i2c_xfer,
  2712. };
  2713.  
  2714. /**
  2715.  * drm_dp_mst_register_i2c_bus() - register an I2C adapter for I2C-over-AUX
  2716.  * @aux: DisplayPort AUX channel
  2717.  *
  2718.  * Returns 0 on success or a negative error code on failure.
  2719.  */
  2720. static int drm_dp_mst_register_i2c_bus(struct drm_dp_aux *aux)
  2721. {
  2722.         aux->ddc.algo = &drm_dp_mst_i2c_algo;
  2723.         aux->ddc.algo_data = aux;
  2724.         aux->ddc.retries = 3;
  2725.  
  2726.         aux->ddc.class = I2C_CLASS_DDC;
  2727.         aux->ddc.owner = THIS_MODULE;
  2728.         aux->ddc.dev.parent = aux->dev;
  2729.  
  2730.         return i2c_add_adapter(&aux->ddc);
  2731. }
  2732.  
  2733. /**
  2734.  * drm_dp_mst_unregister_i2c_bus() - unregister an I2C-over-AUX adapter
  2735.  * @aux: DisplayPort AUX channel
  2736.  */
  2737. static void drm_dp_mst_unregister_i2c_bus(struct drm_dp_aux *aux)
  2738. {
  2739.         i2c_del_adapter(&aux->ddc);
  2740. }
  2741.