drivers/gpu/drm/drm_dp_mst_topology.c

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