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