Subversion Repositories Kolibri OS

Rev

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