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