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