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_mst_topology_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->topology_kref);
 854         kref_init(&mstb->malloc_kref);
 855         return mstb;
 856 }
 857
 858 static void drm_dp_free_mst_branch_device(struct kref *kref)
 859 {
 860         struct drm_dp_mst_branch *mstb =
 861                 container_of(kref, struct drm_dp_mst_branch, malloc_kref);
 862
 863         if (mstb->port_parent)
 864                 drm_dp_mst_put_port_malloc(mstb->port_parent);
 865
 866         kfree(mstb);
 867 }
 868
 869 /**
 870  * DOC: Branch device and port refcounting
 871  *
 872  * Topology refcount overview
 873  * ~~~~~~~~~~~~~~~~~~~~~~~~~~
 874  *
 875  * The refcounting schemes for &struct drm_dp_mst_branch and &struct
 876  * drm_dp_mst_port are somewhat unusual. Both ports and branch devices have
 877  * two different kinds of refcounts: topology refcounts, and malloc refcounts.
 878  *
 879  * Topology refcounts are not exposed to drivers, and are handled internally
 880  * by the DP MST helpers. The helpers use them in order to prevent the
 881  * in-memory topology state from being changed in the middle of critical
 882  * operations like changing the internal state of payload allocations. This
 883  * means each branch and port will be considered to be connected to the rest
 884  * of the topology until it's topology refcount reaches zero. Additionally,
 885  * for ports this means that their associated &struct drm_connector will stay
 886  * registered with userspace until the port's refcount reaches 0.
 887  *
 888  * Malloc refcount overview
 889  * ~~~~~~~~~~~~~~~~~~~~~~~~
 890  *
 891  * Malloc references are used to keep a &struct drm_dp_mst_port or &struct
 892  * drm_dp_mst_branch allocated even after all of its topology references have
 893  * been dropped, so that the driver or MST helpers can safely access each
 894  * branch's last known state before it was disconnected from the topology.
 895  * When the malloc refcount of a port or branch reaches 0, the memory
 896  * allocation containing the &struct drm_dp_mst_branch or &struct
 897  * drm_dp_mst_port respectively will be freed.
 898  *
 899  * For &struct drm_dp_mst_branch, malloc refcounts are not currently exposed
 900  * to drivers. As of writing this documentation, there are no drivers that
 901  * have a usecase for accessing &struct drm_dp_mst_branch outside of the MST
 902  * helpers. Exposing this API to drivers in a race-free manner would take more
 903  * tweaking of the refcounting scheme, however patches are welcome provided
 904  * there is a legitimate driver usecase for this.
 905  *
 906  * Refcount relationships in a topology
 907  * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 908  *
 909  * Let's take a look at why the relationship between topology and malloc
 910  * refcounts is designed the way it is.
 911  *
 912  * .. kernel-figure:: dp-mst/topology-figure-1.dot
 913  *
 914  *    An example of topology and malloc refs in a DP MST topology with two
 915  *    active payloads. Topology refcount increments are indicated by solid
 916  *    lines, and malloc refcount increments are indicated by dashed lines.
 917  *    Each starts from the branch which incremented the refcount, and ends at
 918  *    the branch to which the refcount belongs to, i.e. the arrow points the
 919  *    same way as the C pointers used to reference a structure.
 920  *
 921  * As you can see in the above figure, every branch increments the topology
 922  * refcount of it's children, and increments the malloc refcount of it's
 923  * parent. Additionally, every payload increments the malloc refcount of it's
 924  * assigned port by 1.
 925  *
 926  * So, what would happen if MSTB #3 from the above figure was unplugged from
 927  * the system, but the driver hadn't yet removed payload #2 from port #3? The
 928  * topology would start to look like the figure below.
 929  *
 930  * .. kernel-figure:: dp-mst/topology-figure-2.dot
 931  *
 932  *    Ports and branch devices which have been released from memory are
 933  *    colored grey, and references which have been removed are colored red.
 934  *
 935  * Whenever a port or branch device's topology refcount reaches zero, it will
 936  * decrement the topology refcounts of all its children, the malloc refcount
 937  * of its parent, and finally its own malloc refcount. For MSTB #4 and port
 938  * #4, this means they both have been disconnected from the topology and freed
 939  * from memory. But, because payload #2 is still holding a reference to port
 940  * #3, port #3 is removed from the topology but it's &struct drm_dp_mst_port
 941  * is still accessible from memory. This also means port #3 has not yet
 942  * decremented the malloc refcount of MSTB #3, so it's &struct
 943  * drm_dp_mst_branch will also stay allocated in memory until port #3's
 944  * malloc refcount reaches 0.
 945  *
 946  * This relationship is necessary because in order to release payload #2, we
 947  * need to be able to figure out the last relative of port #3 that's still
 948  * connected to the topology. In this case, we would travel up the topology as
 949  * shown below.
 950  *
 951  * .. kernel-figure:: dp-mst/topology-figure-3.dot
 952  *
 953  * And finally, remove payload #2 by communicating with port #2 through
 954  * sideband transactions.
 955  */
 956
 957 /**
 958  * drm_dp_mst_get_mstb_malloc() - Increment the malloc refcount of a branch
 959  * device
 960  * @mstb: The &struct drm_dp_mst_branch to increment the malloc refcount of
 961  *
 962  * Increments &drm_dp_mst_branch.malloc_kref. When
 963  * &drm_dp_mst_branch.malloc_kref reaches 0, the memory allocation for @mstb
 964  * will be released and @mstb may no longer be used.
 965  *
 966  * See also: drm_dp_mst_put_mstb_malloc()
 967  */
 968 static void
 969 drm_dp_mst_get_mstb_malloc(struct drm_dp_mst_branch *mstb)
 970 {
 971         kref_get(&mstb->malloc_kref);
 972         DRM_DEBUG("mstb %p (%d)\n", mstb, kref_read(&mstb->malloc_kref));
 973 }
 974
 975 /**
 976  * drm_dp_mst_put_mstb_malloc() - Decrement the malloc refcount of a branch
 977  * device
 978  * @mstb: The &struct drm_dp_mst_branch to decrement the malloc refcount of
 979  *
 980  * Decrements &drm_dp_mst_branch.malloc_kref. When
 981  * &drm_dp_mst_branch.malloc_kref reaches 0, the memory allocation for @mstb
 982  * will be released and @mstb may no longer be used.
 983  *
 984  * See also: drm_dp_mst_get_mstb_malloc()
 985  */
 986 static void
 987 drm_dp_mst_put_mstb_malloc(struct drm_dp_mst_branch *mstb)
 988 {
 989         DRM_DEBUG("mstb %p (%d)\n", mstb, kref_read(&mstb->malloc_kref) - 1);
 990         kref_put(&mstb->malloc_kref, drm_dp_free_mst_branch_device);
 991 }
 992
 993 static void drm_dp_free_mst_port(struct kref *kref)
 994 {
 995         struct drm_dp_mst_port *port =
 996                 container_of(kref, struct drm_dp_mst_port, malloc_kref);
 997
 998         drm_dp_mst_put_mstb_malloc(port->parent);
 999         kfree(port);
1000 }
1001
1002 /**
1003  * drm_dp_mst_get_port_malloc() - Increment the malloc refcount of an MST port
1004  * @port: The &struct drm_dp_mst_port to increment the malloc refcount of
1005  *
1006  * Increments &drm_dp_mst_port.malloc_kref. When &drm_dp_mst_port.malloc_kref
1007  * reaches 0, the memory allocation for @port will be released and @port may
1008  * no longer be used.
1009  *
1010  * Because @port could potentially be freed at any time by the DP MST helpers
1011  * if &drm_dp_mst_port.malloc_kref reaches 0, including during a call to this
1012  * function, drivers that which to make use of &struct drm_dp_mst_port should
1013  * ensure that they grab at least one main malloc reference to their MST ports
1014  * in &drm_dp_mst_topology_cbs.add_connector. This callback is called before
1015  * there is any chance for &drm_dp_mst_port.malloc_kref to reach 0.
1016  *
1017  * See also: drm_dp_mst_put_port_malloc()
1018  */
1019 void
1020 drm_dp_mst_get_port_malloc(struct drm_dp_mst_port *port)
1021 {
1022         kref_get(&port->malloc_kref);
1023         DRM_DEBUG("port %p (%d)\n", port, kref_read(&port->malloc_kref));
1024 }
1025 EXPORT_SYMBOL(drm_dp_mst_get_port_malloc);
1026
1027 /**
1028  * drm_dp_mst_put_port_malloc() - Decrement the malloc refcount of an MST port
1029  * @port: The &struct drm_dp_mst_port to decrement the malloc refcount of
1030  *
1031  * Decrements &drm_dp_mst_port.malloc_kref. When &drm_dp_mst_port.malloc_kref
1032  * reaches 0, the memory allocation for @port will be released and @port may
1033  * no longer be used.
1034  *
1035  * See also: drm_dp_mst_get_port_malloc()
1036  */
1037 void
1038 drm_dp_mst_put_port_malloc(struct drm_dp_mst_port *port)
1039 {
1040         DRM_DEBUG("port %p (%d)\n", port, kref_read(&port->malloc_kref) - 1);
1041         kref_put(&port->malloc_kref, drm_dp_free_mst_port);
1042 }
1043 EXPORT_SYMBOL(drm_dp_mst_put_port_malloc);
1044
1045 static void drm_dp_destroy_mst_branch_device(struct kref *kref)
1046 {
1047         struct drm_dp_mst_branch *mstb =
1048                 container_of(kref, struct drm_dp_mst_branch, topology_kref);
1049         struct drm_dp_mst_topology_mgr *mgr = mstb->mgr;
1050         struct drm_dp_mst_port *port, *tmp;
1051         bool wake_tx = false;
1052
1053         mutex_lock(&mgr->lock);
1054         list_for_each_entry_safe(port, tmp, &mstb->ports, next) {
1055                 list_del(&port->next);
1056                 drm_dp_mst_topology_put_port(port);
1057         }
1058         mutex_unlock(&mgr->lock);
1059
1060         /* drop any tx slots msg */
1061         mutex_lock(&mstb->mgr->qlock);
1062         if (mstb->tx_slots[0]) {
1063                 mstb->tx_slots[0]->state = DRM_DP_SIDEBAND_TX_TIMEOUT;
1064                 mstb->tx_slots[0] = NULL;
1065                 wake_tx = true;
1066         }
1067         if (mstb->tx_slots[1]) {
1068                 mstb->tx_slots[1]->state = DRM_DP_SIDEBAND_TX_TIMEOUT;
1069                 mstb->tx_slots[1] = NULL;
1070                 wake_tx = true;
1071         }
1072         mutex_unlock(&mstb->mgr->qlock);
1073
1074         if (wake_tx)
1075                 wake_up_all(&mstb->mgr->tx_waitq);
1076
1077         drm_dp_mst_put_mstb_malloc(mstb);
1078 }
1079
1080 /**
1081  * drm_dp_mst_topology_try_get_mstb() - Increment the topology refcount of a
1082  * branch device unless its zero
1083  * @mstb: &struct drm_dp_mst_branch to increment the topology refcount of
1084  *
1085  * Attempts to grab a topology reference to @mstb, if it hasn't yet been
1086  * removed from the topology (e.g. &drm_dp_mst_branch.topology_kref has
1087  * reached 0). Holding a topology reference implies that a malloc reference
1088  * will be held to @mstb as long as the user holds the topology reference.
1089  *
1090  * Care should be taken to ensure that the user has at least one malloc
1091  * reference to @mstb. If you already have a topology reference to @mstb, you
1092  * should use drm_dp_mst_topology_get_mstb() instead.
1093  *
1094  * See also:
1095  * drm_dp_mst_topology_get_mstb()
1096  * drm_dp_mst_topology_put_mstb()
1097  *
1098  * Returns:
1099  * * 1: A topology reference was grabbed successfully
1100  * * 0: @port is no longer in the topology, no reference was grabbed
1101  */
1102 static int __must_check
1103 drm_dp_mst_topology_try_get_mstb(struct drm_dp_mst_branch *mstb)
1104 {
1105         int ret = kref_get_unless_zero(&mstb->topology_kref);
1106
1107         if (ret)
1108                 DRM_DEBUG("mstb %p (%d)\n", mstb,
1109                           kref_read(&mstb->topology_kref));
1110
1111         return ret;
1112 }
1113
1114 /**
1115  * drm_dp_mst_topology_get_mstb() - Increment the topology refcount of a
1116  * branch device
1117  * @mstb: The &struct drm_dp_mst_branch to increment the topology refcount of
1118  *
1119  * Increments &drm_dp_mst_branch.topology_refcount without checking whether or
1120  * not it's already reached 0. This is only valid to use in scenarios where
1121  * you are already guaranteed to have at least one active topology reference
1122  * to @mstb. Otherwise, drm_dp_mst_topology_try_get_mstb() must be used.
1123  *
1124  * See also:
1125  * drm_dp_mst_topology_try_get_mstb()
1126  * drm_dp_mst_topology_put_mstb()
1127  */
1128 static void drm_dp_mst_topology_get_mstb(struct drm_dp_mst_branch *mstb)
1129 {
1130         WARN_ON(kref_read(&mstb->topology_kref) == 0);
1131         kref_get(&mstb->topology_kref);
1132         DRM_DEBUG("mstb %p (%d)\n", mstb, kref_read(&mstb->topology_kref));
1133 }
1134
1135 /**
1136  * drm_dp_mst_topology_put_mstb() - release a topology reference to a branch
1137  * device
1138  * @mstb: The &struct drm_dp_mst_branch to release the topology reference from
1139  *
1140  * Releases a topology reference from @mstb by decrementing
1141  * &drm_dp_mst_branch.topology_kref.
1142  *
1143  * See also:
1144  * drm_dp_mst_topology_try_get_mstb()
1145  * drm_dp_mst_topology_get_mstb()
1146  */
1147 static void
1148 drm_dp_mst_topology_put_mstb(struct drm_dp_mst_branch *mstb)
1149 {
1150         DRM_DEBUG("mstb %p (%d)\n",
1151                   mstb, kref_read(&mstb->topology_kref) - 1);
1152         kref_put(&mstb->topology_kref, drm_dp_destroy_mst_branch_device);
1153 }
1154
1155 static void drm_dp_port_teardown_pdt(struct drm_dp_mst_port *port, int old_pdt)
1156 {
1157         struct drm_dp_mst_branch *mstb;
1158
1159         switch (old_pdt) {
1160         case DP_PEER_DEVICE_DP_LEGACY_CONV:
1161         case DP_PEER_DEVICE_SST_SINK:
1162                 /* remove i2c over sideband */
1163                 drm_dp_mst_unregister_i2c_bus(&port->aux);
1164                 break;
1165         case DP_PEER_DEVICE_MST_BRANCHING:
1166                 mstb = port->mstb;
1167                 port->mstb = NULL;
1168                 drm_dp_mst_topology_put_mstb(mstb);
1169                 break;
1170         }
1171 }
1172
1173 static void drm_dp_destroy_port(struct kref *kref)
1174 {
1175         struct drm_dp_mst_port *port =
1176                 container_of(kref, struct drm_dp_mst_port, topology_kref);
1177         struct drm_dp_mst_topology_mgr *mgr = port->mgr;
1178
1179         if (!port->input) {
1180                 kfree(port->cached_edid);
1181
1182                 /*
1183                  * The only time we don't have a connector
1184                  * on an output port is if the connector init
1185                  * fails.
1186                  */
1187                 if (port->connector) {
1188                         /* we can't destroy the connector here, as
1189                          * we might be holding the mode_config.mutex
1190                          * from an EDID retrieval */
1191
1192                         mutex_lock(&mgr->destroy_connector_lock);
1193                         list_add(&port->next, &mgr->destroy_connector_list);
1194                         mutex_unlock(&mgr->destroy_connector_lock);
1195                         schedule_work(&mgr->destroy_connector_work);
1196                         return;
1197                 }
1198                 /* no need to clean up vcpi
1199                  * as if we have no connector we never setup a vcpi */
1200                 drm_dp_port_teardown_pdt(port, port->pdt);
1201                 port->pdt = DP_PEER_DEVICE_NONE;
1202         }
1203         drm_dp_mst_put_port_malloc(port);
1204 }
1205
1206 /**
1207  * drm_dp_mst_topology_try_get_port() - Increment the topology refcount of a
1208  * port unless its zero
1209  * @port: &struct drm_dp_mst_port to increment the topology refcount of
1210  *
1211  * Attempts to grab a topology reference to @port, if it hasn't yet been
1212  * removed from the topology (e.g. &drm_dp_mst_port.topology_kref has reached
1213  * 0). Holding a topology reference implies that a malloc reference will be
1214  * held to @port as long as the user holds the topology reference.
1215  *
1216  * Care should be taken to ensure that the user has at least one malloc
1217  * reference to @port. If you already have a topology reference to @port, you
1218  * should use drm_dp_mst_topology_get_port() instead.
1219  *
1220  * See also:
1221  * drm_dp_mst_topology_get_port()
1222  * drm_dp_mst_topology_put_port()
1223  *
1224  * Returns:
1225  * * 1: A topology reference was grabbed successfully
1226  * * 0: @port is no longer in the topology, no reference was grabbed
1227  */
1228 static int __must_check
1229 drm_dp_mst_topology_try_get_port(struct drm_dp_mst_port *port)
1230 {
1231         int ret = kref_get_unless_zero(&port->topology_kref);
1232
1233         if (ret)
1234                 DRM_DEBUG("port %p (%d)\n", port,
1235                           kref_read(&port->topology_kref));
1236
1237         return ret;
1238 }
1239
1240 /**
1241  * drm_dp_mst_topology_get_port() - Increment the topology refcount of a port
1242  * @port: The &struct drm_dp_mst_port to increment the topology refcount of
1243  *
1244  * Increments &drm_dp_mst_port.topology_refcount without checking whether or
1245  * not it's already reached 0. This is only valid to use in scenarios where
1246  * you are already guaranteed to have at least one active topology reference
1247  * to @port. Otherwise, drm_dp_mst_topology_try_get_port() must be used.
1248  *
1249  * See also:
1250  * drm_dp_mst_topology_try_get_port()
1251  * drm_dp_mst_topology_put_port()
1252  */
1253 static void drm_dp_mst_topology_get_port(struct drm_dp_mst_port *port)
1254 {
1255         WARN_ON(kref_read(&port->topology_kref) == 0);
1256         kref_get(&port->topology_kref);
1257         DRM_DEBUG("port %p (%d)\n", port, kref_read(&port->topology_kref));
1258 }
1259
1260 /**
1261  * drm_dp_mst_topology_put_port() - release a topology reference to a port
1262  * @port: The &struct drm_dp_mst_port to release the topology reference from
1263  *
1264  * Releases a topology reference from @port by decrementing
1265  * &drm_dp_mst_port.topology_kref.
1266  *
1267  * See also:
1268  * drm_dp_mst_topology_try_get_port()
1269  * drm_dp_mst_topology_get_port()
1270  */
1271 static void drm_dp_mst_topology_put_port(struct drm_dp_mst_port *port)
1272 {
1273         DRM_DEBUG("port %p (%d)\n",
1274                   port, kref_read(&port->topology_kref) - 1);
1275         kref_put(&port->topology_kref, drm_dp_destroy_port);
1276 }
1277
1278 static struct drm_dp_mst_branch *
1279 drm_dp_mst_topology_get_mstb_validated_locked(struct drm_dp_mst_branch *mstb,
1280                                               struct drm_dp_mst_branch *to_find)
1281 {
1282         struct drm_dp_mst_port *port;
1283         struct drm_dp_mst_branch *rmstb;
1284
1285         if (to_find == mstb)
1286                 return mstb;
1287
1288         list_for_each_entry(port, &mstb->ports, next) {
1289                 if (port->mstb) {
1290                         rmstb = drm_dp_mst_topology_get_mstb_validated_locked(
1291                             port->mstb, to_find);
1292                         if (rmstb)
1293                                 return rmstb;
1294                 }
1295         }
1296         return NULL;
1297 }
1298
1299 static struct drm_dp_mst_branch *
1300 drm_dp_mst_topology_get_mstb_validated(struct drm_dp_mst_topology_mgr *mgr,
1301                                        struct drm_dp_mst_branch *mstb)
1302 {
1303         struct drm_dp_mst_branch *rmstb = NULL;
1304
1305         mutex_lock(&mgr->lock);
1306         if (mgr->mst_primary) {
1307                 rmstb = drm_dp_mst_topology_get_mstb_validated_locked(
1308                     mgr->mst_primary, mstb);
1309
1310                 if (rmstb && !drm_dp_mst_topology_try_get_mstb(rmstb))
1311                         rmstb = NULL;
1312         }
1313         mutex_unlock(&mgr->lock);
1314         return rmstb;
1315 }
1316
1317 static struct drm_dp_mst_port *
1318 drm_dp_mst_topology_get_port_validated_locked(struct drm_dp_mst_branch *mstb,
1319                                               struct drm_dp_mst_port *to_find)
1320 {
1321         struct drm_dp_mst_port *port, *mport;
1322
1323         list_for_each_entry(port, &mstb->ports, next) {
1324                 if (port == to_find)
1325                         return port;
1326
1327                 if (port->mstb) {
1328                         mport = drm_dp_mst_topology_get_port_validated_locked(
1329                             port->mstb, to_find);
1330                         if (mport)
1331                                 return mport;
1332                 }
1333         }
1334         return NULL;
1335 }
1336
1337 static struct drm_dp_mst_port *
1338 drm_dp_mst_topology_get_port_validated(struct drm_dp_mst_topology_mgr *mgr,
1339                                        struct drm_dp_mst_port *port)
1340 {
1341         struct drm_dp_mst_port *rport = NULL;
1342
1343         mutex_lock(&mgr->lock);
1344         if (mgr->mst_primary) {
1345                 rport = drm_dp_mst_topology_get_port_validated_locked(
1346                     mgr->mst_primary, port);
1347
1348                 if (rport && !drm_dp_mst_topology_try_get_port(rport))
1349                         rport = NULL;
1350         }
1351         mutex_unlock(&mgr->lock);
1352         return rport;
1353 }
1354
1355 static struct drm_dp_mst_port *drm_dp_get_port(struct drm_dp_mst_branch *mstb, u8 port_num)
1356 {
1357         struct drm_dp_mst_port *port;
1358         int ret;
1359
1360         list_for_each_entry(port, &mstb->ports, next) {
1361                 if (port->port_num == port_num) {
1362                         ret = drm_dp_mst_topology_try_get_port(port);
1363                         return ret ? port : NULL;
1364                 }
1365         }
1366
1367         return NULL;
1368 }
1369
1370 /*
1371  * calculate a new RAD for this MST branch device
1372  * if parent has an LCT of 2 then it has 1 nibble of RAD,
1373  * if parent has an LCT of 3 then it has 2 nibbles of RAD,
1374  */
1375 static u8 drm_dp_calculate_rad(struct drm_dp_mst_port *port,
1376                                  u8 *rad)
1377 {
1378         int parent_lct = port->parent->lct;
1379         int shift = 4;
1380         int idx = (parent_lct - 1) / 2;
1381         if (parent_lct > 1) {
1382                 memcpy(rad, port->parent->rad, idx + 1);
1383                 shift = (parent_lct % 2) ? 4 : 0;
1384         } else
1385                 rad[0] = 0;
1386
1387         rad[idx] |= port->port_num << shift;
1388         return parent_lct + 1;
1389 }
1390
1391 /*
1392  * return sends link address for new mstb
1393  */
1394 static bool drm_dp_port_setup_pdt(struct drm_dp_mst_port *port)
1395 {
1396         int ret;
1397         u8 rad[6], lct;
1398         bool send_link = false;
1399         switch (port->pdt) {
1400         case DP_PEER_DEVICE_DP_LEGACY_CONV:
1401         case DP_PEER_DEVICE_SST_SINK:
1402                 /* add i2c over sideband */
1403                 ret = drm_dp_mst_register_i2c_bus(&port->aux);
1404                 break;
1405         case DP_PEER_DEVICE_MST_BRANCHING:
1406                 lct = drm_dp_calculate_rad(port, rad);
1407
1408                 port->mstb = drm_dp_add_mst_branch_device(lct, rad);
1409                 if (port->mstb) {
1410                         port->mstb->mgr = port->mgr;
1411                         port->mstb->port_parent = port;
1412                         /*
1413                          * Make sure this port's memory allocation stays
1414                          * around until it's child MSTB releases it
1415                          */
1416                         drm_dp_mst_get_port_malloc(port);
1417
1418                         send_link = true;
1419                 }
1420                 break;
1421         }
1422         return send_link;
1423 }
1424
1425 static void drm_dp_check_mstb_guid(struct drm_dp_mst_branch *mstb, u8 *guid)
1426 {
1427         int ret;
1428
1429         memcpy(mstb->guid, guid, 16);
1430
1431         if (!drm_dp_validate_guid(mstb->mgr, mstb->guid)) {
1432                 if (mstb->port_parent) {
1433                         ret = drm_dp_send_dpcd_write(
1434                                         mstb->mgr,
1435                                         mstb->port_parent,
1436                                         DP_GUID,
1437                                         16,
1438                                         mstb->guid);
1439                 } else {
1440
1441                         ret = drm_dp_dpcd_write(
1442                                         mstb->mgr->aux,
1443                                         DP_GUID,
1444                                         mstb->guid,
1445                                         16);
1446                 }
1447         }
1448 }
1449
1450 static void build_mst_prop_path(const struct drm_dp_mst_branch *mstb,
1451                                 int pnum,
1452                                 char *proppath,
1453                                 size_t proppath_size)
1454 {
1455         int i;
1456         char temp[8];
1457         snprintf(proppath, proppath_size, "mst:%d", mstb->mgr->conn_base_id);
1458         for (i = 0; i < (mstb->lct - 1); i++) {
1459                 int shift = (i % 2) ? 0 : 4;
1460                 int port_num = (mstb->rad[i / 2] >> shift) & 0xf;
1461                 snprintf(temp, sizeof(temp), "-%d", port_num);
1462                 strlcat(proppath, temp, proppath_size);
1463         }
1464         snprintf(temp, sizeof(temp), "-%d", pnum);
1465         strlcat(proppath, temp, proppath_size);
1466 }
1467
1468 static void drm_dp_add_port(struct drm_dp_mst_branch *mstb,
1469                             struct drm_device *dev,
1470                             struct drm_dp_link_addr_reply_port *port_msg)
1471 {
1472         struct drm_dp_mst_port *port;
1473         bool ret;
1474         bool created = false;
1475         int old_pdt = 0;
1476         int old_ddps = 0;
1477
1478         port = drm_dp_get_port(mstb, port_msg->port_number);
1479         if (!port) {
1480                 port = kzalloc(sizeof(*port), GFP_KERNEL);
1481                 if (!port)
1482                         return;
1483                 kref_init(&port->topology_kref);
1484                 kref_init(&port->malloc_kref);
1485                 port->parent = mstb;
1486                 port->port_num = port_msg->port_number;
1487                 port->mgr = mstb->mgr;
1488                 port->aux.name = "DPMST";
1489                 port->aux.dev = dev->dev;
1490
1491                 /*
1492                  * Make sure the memory allocation for our parent branch stays
1493                  * around until our own memory allocation is released
1494                  */
1495                 drm_dp_mst_get_mstb_malloc(mstb);
1496
1497                 created = true;
1498         } else {
1499                 old_pdt = port->pdt;
1500                 old_ddps = port->ddps;
1501         }
1502
1503         port->pdt = port_msg->peer_device_type;
1504         port->input = port_msg->input_port;
1505         port->mcs = port_msg->mcs;
1506         port->ddps = port_msg->ddps;
1507         port->ldps = port_msg->legacy_device_plug_status;
1508         port->dpcd_rev = port_msg->dpcd_revision;
1509         port->num_sdp_streams = port_msg->num_sdp_streams;
1510         port->num_sdp_stream_sinks = port_msg->num_sdp_stream_sinks;
1511
1512         /* manage mstb port lists with mgr lock - take a reference
1513            for this list */
1514         if (created) {
1515                 mutex_lock(&mstb->mgr->lock);
1516                 drm_dp_mst_topology_get_port(port);
1517                 list_add(&port->next, &mstb->ports);
1518                 mutex_unlock(&mstb->mgr->lock);
1519         }
1520
1521         if (old_ddps != port->ddps) {
1522                 if (port->ddps) {
1523                         if (!port->input) {
1524                                 drm_dp_send_enum_path_resources(mstb->mgr,
1525                                                                 mstb, port);
1526                         }
1527                 } else {
1528                         port->available_pbn = 0;
1529                 }
1530         }
1531
1532         if (old_pdt != port->pdt && !port->input) {
1533                 drm_dp_port_teardown_pdt(port, old_pdt);
1534
1535                 ret = drm_dp_port_setup_pdt(port);
1536                 if (ret == true)
1537                         drm_dp_send_link_address(mstb->mgr, port->mstb);
1538         }
1539
1540         if (created && !port->input) {
1541                 char proppath[255];
1542
1543                 build_mst_prop_path(mstb, port->port_num, proppath,
1544                                     sizeof(proppath));
1545                 port->connector = (*mstb->mgr->cbs->add_connector)(mstb->mgr,
1546                                                                    port,
1547                                                                    proppath);
1548                 if (!port->connector) {
1549                         /* remove it from the port list */
1550                         mutex_lock(&mstb->mgr->lock);
1551                         list_del(&port->next);
1552                         mutex_unlock(&mstb->mgr->lock);
1553                         /* drop port list reference */
1554                         drm_dp_mst_topology_put_port(port);
1555                         goto out;
1556                 }
1557                 if ((port->pdt == DP_PEER_DEVICE_DP_LEGACY_CONV ||
1558                      port->pdt == DP_PEER_DEVICE_SST_SINK) &&
1559                     port->port_num >= DP_MST_LOGICAL_PORT_0) {
1560                         port->cached_edid = drm_get_edid(port->connector,
1561                                                          &port->aux.ddc);
1562                         drm_connector_set_tile_property(port->connector);
1563                 }
1564                 (*mstb->mgr->cbs->register_connector)(port->connector);
1565         }
1566
1567 out:
1568         /* put reference to this port */
1569         drm_dp_mst_topology_put_port(port);
1570 }
1571
1572 static void drm_dp_update_port(struct drm_dp_mst_branch *mstb,
1573                                struct drm_dp_connection_status_notify *conn_stat)
1574 {
1575         struct drm_dp_mst_port *port;
1576         int old_pdt;
1577         int old_ddps;
1578         bool dowork = false;
1579         port = drm_dp_get_port(mstb, conn_stat->port_number);
1580         if (!port)
1581                 return;
1582
1583         old_ddps = port->ddps;
1584         old_pdt = port->pdt;
1585         port->pdt = conn_stat->peer_device_type;
1586         port->mcs = conn_stat->message_capability_status;
1587         port->ldps = conn_stat->legacy_device_plug_status;
1588         port->ddps = conn_stat->displayport_device_plug_status;
1589
1590         if (old_ddps != port->ddps) {
1591                 if (port->ddps) {
1592                         dowork = true;
1593                 } else {
1594                         port->available_pbn = 0;
1595                 }
1596         }
1597         if (old_pdt != port->pdt && !port->input) {
1598                 drm_dp_port_teardown_pdt(port, old_pdt);
1599
1600                 if (drm_dp_port_setup_pdt(port))
1601                         dowork = true;
1602         }
1603
1604         drm_dp_mst_topology_put_port(port);
1605         if (dowork)
1606                 queue_work(system_long_wq, &mstb->mgr->work);
1607
1608 }
1609
1610 static struct drm_dp_mst_branch *drm_dp_get_mst_branch_device(struct drm_dp_mst_topology_mgr *mgr,
1611                                                                u8 lct, u8 *rad)
1612 {
1613         struct drm_dp_mst_branch *mstb;
1614         struct drm_dp_mst_port *port;
1615         int i, ret;
1616         /* find the port by iterating down */
1617
1618         mutex_lock(&mgr->lock);
1619         mstb = mgr->mst_primary;
1620
1621         if (!mstb)
1622                 goto out;
1623
1624         for (i = 0; i < lct - 1; i++) {
1625                 int shift = (i % 2) ? 0 : 4;
1626                 int port_num = (rad[i / 2] >> shift) & 0xf;
1627
1628                 list_for_each_entry(port, &mstb->ports, next) {
1629                         if (port->port_num == port_num) {
1630                                 mstb = port->mstb;
1631                                 if (!mstb) {
1632                                         DRM_ERROR("failed to lookup MSTB with lct %d, rad %02x\n", lct, rad[0]);
1633                                         goto out;
1634                                 }
1635
1636                                 break;
1637                         }
1638                 }
1639         }
1640         ret = drm_dp_mst_topology_try_get_mstb(mstb);
1641         if (!ret)
1642                 mstb = NULL;
1643 out:
1644         mutex_unlock(&mgr->lock);
1645         return mstb;
1646 }
1647
1648 static struct drm_dp_mst_branch *get_mst_branch_device_by_guid_helper(
1649         struct drm_dp_mst_branch *mstb,
1650         uint8_t *guid)
1651 {
1652         struct drm_dp_mst_branch *found_mstb;
1653         struct drm_dp_mst_port *port;
1654
1655         if (memcmp(mstb->guid, guid, 16) == 0)
1656                 return mstb;
1657
1658
1659         list_for_each_entry(port, &mstb->ports, next) {
1660                 if (!port->mstb)
1661                         continue;
1662
1663                 found_mstb = get_mst_branch_device_by_guid_helper(port->mstb, guid);
1664
1665                 if (found_mstb)
1666                         return found_mstb;
1667         }
1668
1669         return NULL;
1670 }
1671
1672 static struct drm_dp_mst_branch *
1673 drm_dp_get_mst_branch_device_by_guid(struct drm_dp_mst_topology_mgr *mgr,
1674                                      uint8_t *guid)
1675 {
1676         struct drm_dp_mst_branch *mstb;
1677         int ret;
1678
1679         /* find the port by iterating down */
1680         mutex_lock(&mgr->lock);
1681
1682         mstb = get_mst_branch_device_by_guid_helper(mgr->mst_primary, guid);
1683         if (mstb) {
1684                 ret = drm_dp_mst_topology_try_get_mstb(mstb);
1685                 if (!ret)
1686                         mstb = NULL;
1687         }
1688
1689         mutex_unlock(&mgr->lock);
1690         return mstb;
1691 }
1692
1693 static void drm_dp_check_and_send_link_address(struct drm_dp_mst_topology_mgr *mgr,
1694                                                struct drm_dp_mst_branch *mstb)
1695 {
1696         struct drm_dp_mst_port *port;
1697         struct drm_dp_mst_branch *mstb_child;
1698         if (!mstb->link_address_sent)
1699                 drm_dp_send_link_address(mgr, mstb);
1700
1701         list_for_each_entry(port, &mstb->ports, next) {
1702                 if (port->input)
1703                         continue;
1704
1705                 if (!port->ddps)
1706                         continue;
1707
1708                 if (!port->available_pbn)
1709                         drm_dp_send_enum_path_resources(mgr, mstb, port);
1710
1711                 if (port->mstb) {
1712                         mstb_child = drm_dp_mst_topology_get_mstb_validated(
1713                             mgr, port->mstb);
1714                         if (mstb_child) {
1715                                 drm_dp_check_and_send_link_address(mgr, mstb_child);
1716                                 drm_dp_mst_topology_put_mstb(mstb_child);
1717                         }
1718                 }
1719         }
1720 }
1721
1722 static void drm_dp_mst_link_probe_work(struct work_struct *work)
1723 {
1724         struct drm_dp_mst_topology_mgr *mgr = container_of(work, struct drm_dp_mst_topology_mgr, work);
1725         struct drm_dp_mst_branch *mstb;
1726         int ret;
1727
1728         mutex_lock(&mgr->lock);
1729         mstb = mgr->mst_primary;
1730         if (mstb) {
1731                 ret = drm_dp_mst_topology_try_get_mstb(mstb);
1732                 if (!ret)
1733                         mstb = NULL;
1734         }
1735         mutex_unlock(&mgr->lock);
1736         if (mstb) {
1737                 drm_dp_check_and_send_link_address(mgr, mstb);
1738                 drm_dp_mst_topology_put_mstb(mstb);
1739         }
1740 }
1741
1742 static bool drm_dp_validate_guid(struct drm_dp_mst_topology_mgr *mgr,
1743                                  u8 *guid)
1744 {
1745         u64 salt;
1746
1747         if (memchr_inv(guid, 0, 16))
1748                 return true;
1749
1750         salt = get_jiffies_64();
1751
1752         memcpy(&guid[0], &salt, sizeof(u64));
1753         memcpy(&guid[8], &salt, sizeof(u64));
1754
1755         return false;
1756 }
1757
1758 #if 0
1759 static int build_dpcd_read(struct drm_dp_sideband_msg_tx *msg, u8 port_num, u32 offset, u8 num_bytes)
1760 {
1761         struct drm_dp_sideband_msg_req_body req;
1762
1763         req.req_type = DP_REMOTE_DPCD_READ;
1764         req.u.dpcd_read.port_number = port_num;
1765         req.u.dpcd_read.dpcd_address = offset;
1766         req.u.dpcd_read.num_bytes = num_bytes;
1767         drm_dp_encode_sideband_req(&req, msg);
1768
1769         return 0;
1770 }
1771 #endif
1772
1773 static int drm_dp_send_sideband_msg(struct drm_dp_mst_topology_mgr *mgr,
1774                                     bool up, u8 *msg, int len)
1775 {
1776         int ret;
1777         int regbase = up ? DP_SIDEBAND_MSG_UP_REP_BASE : DP_SIDEBAND_MSG_DOWN_REQ_BASE;
1778         int tosend, total, offset;
1779         int retries = 0;
1780
1781 retry:
1782         total = len;
1783         offset = 0;
1784         do {
1785                 tosend = min3(mgr->max_dpcd_transaction_bytes, 16, total);
1786
1787                 ret = drm_dp_dpcd_write(mgr->aux, regbase + offset,
1788                                         &msg[offset],
1789                                         tosend);
1790                 if (ret != tosend) {
1791                         if (ret == -EIO && retries < 5) {
1792                                 retries++;
1793                                 goto retry;
1794                         }
1795                         DRM_DEBUG_KMS("failed to dpcd write %d %d\n", tosend, ret);
1796
1797                         return -EIO;
1798                 }
1799                 offset += tosend;
1800                 total -= tosend;
1801         } while (total > 0);
1802         return 0;
1803 }
1804
1805 static int set_hdr_from_dst_qlock(struct drm_dp_sideband_msg_hdr *hdr,
1806                                   struct drm_dp_sideband_msg_tx *txmsg)
1807 {
1808         struct drm_dp_mst_branch *mstb = txmsg->dst;
1809         u8 req_type;
1810
1811         /* both msg slots are full */
1812         if (txmsg->seqno == -1) {
1813                 if (mstb->tx_slots[0] && mstb->tx_slots[1]) {
1814                         DRM_DEBUG_KMS("%s: failed to find slot\n", __func__);
1815                         return -EAGAIN;
1816                 }
1817                 if (mstb->tx_slots[0] == NULL && mstb->tx_slots[1] == NULL) {
1818                         txmsg->seqno = mstb->last_seqno;
1819                         mstb->last_seqno ^= 1;
1820                 } else if (mstb->tx_slots[0] == NULL)
1821                         txmsg->seqno = 0;
1822                 else
1823                         txmsg->seqno = 1;
1824                 mstb->tx_slots[txmsg->seqno] = txmsg;
1825         }
1826
1827         req_type = txmsg->msg[0] & 0x7f;
1828         if (req_type == DP_CONNECTION_STATUS_NOTIFY ||
1829                 req_type == DP_RESOURCE_STATUS_NOTIFY)
1830                 hdr->broadcast = 1;
1831         else
1832                 hdr->broadcast = 0;
1833         hdr->path_msg = txmsg->path_msg;
1834         hdr->lct = mstb->lct;
1835         hdr->lcr = mstb->lct - 1;
1836         if (mstb->lct > 1)
1837                 memcpy(hdr->rad, mstb->rad, mstb->lct / 2);
1838         hdr->seqno = txmsg->seqno;
1839         return 0;
1840 }
1841 /*
1842  * process a single block of the next message in the sideband queue
1843  */
1844 static int process_single_tx_qlock(struct drm_dp_mst_topology_mgr *mgr,
1845                                    struct drm_dp_sideband_msg_tx *txmsg,
1846                                    bool up)
1847 {
1848         u8 chunk[48];
1849         struct drm_dp_sideband_msg_hdr hdr;
1850         int len, space, idx, tosend;
1851         int ret;
1852
1853         memset(&hdr, 0, sizeof(struct drm_dp_sideband_msg_hdr));
1854
1855         if (txmsg->state == DRM_DP_SIDEBAND_TX_QUEUED) {
1856                 txmsg->seqno = -1;
1857                 txmsg->state = DRM_DP_SIDEBAND_TX_START_SEND;
1858         }
1859
1860         /* make hdr from dst mst - for replies use seqno
1861            otherwise assign one */
1862         ret = set_hdr_from_dst_qlock(&hdr, txmsg);
1863         if (ret < 0)
1864                 return ret;
1865
1866         /* amount left to send in this message */
1867         len = txmsg->cur_len - txmsg->cur_offset;
1868
1869         /* 48 - sideband msg size - 1 byte for data CRC, x header bytes */
1870         space = 48 - 1 - drm_dp_calc_sb_hdr_size(&hdr);
1871
1872         tosend = min(len, space);
1873         if (len == txmsg->cur_len)
1874                 hdr.somt = 1;
1875         if (space >= len)
1876                 hdr.eomt = 1;
1877
1878
1879         hdr.msg_len = tosend + 1;
1880         drm_dp_encode_sideband_msg_hdr(&hdr, chunk, &idx);
1881         memcpy(&chunk[idx], &txmsg->msg[txmsg->cur_offset], tosend);
1882         /* add crc at end */
1883         drm_dp_crc_sideband_chunk_req(&chunk[idx], tosend);
1884         idx += tosend + 1;
1885
1886         ret = drm_dp_send_sideband_msg(mgr, up, chunk, idx);
1887         if (ret) {
1888                 DRM_DEBUG_KMS("sideband msg failed to send\n");
1889                 return ret;
1890         }
1891
1892         txmsg->cur_offset += tosend;
1893         if (txmsg->cur_offset == txmsg->cur_len) {
1894                 txmsg->state = DRM_DP_SIDEBAND_TX_SENT;
1895                 return 1;
1896         }
1897         return 0;
1898 }
1899
1900 static void process_single_down_tx_qlock(struct drm_dp_mst_topology_mgr *mgr)
1901 {
1902         struct drm_dp_sideband_msg_tx *txmsg;
1903         int ret;
1904
1905         WARN_ON(!mutex_is_locked(&mgr->qlock));
1906
1907         /* construct a chunk from the first msg in the tx_msg queue */
1908         if (list_empty(&mgr->tx_msg_downq))
1909                 return;
1910
1911         txmsg = list_first_entry(&mgr->tx_msg_downq, struct drm_dp_sideband_msg_tx, next);
1912         ret = process_single_tx_qlock(mgr, txmsg, false);
1913         if (ret == 1) {
1914                 /* txmsg is sent it should be in the slots now */
1915                 list_del(&txmsg->next);
1916         } else if (ret) {
1917                 DRM_DEBUG_KMS("failed to send msg in q %d\n", ret);
1918                 list_del(&txmsg->next);
1919                 if (txmsg->seqno != -1)
1920                         txmsg->dst->tx_slots[txmsg->seqno] = NULL;
1921                 txmsg->state = DRM_DP_SIDEBAND_TX_TIMEOUT;
1922                 wake_up_all(&mgr->tx_waitq);
1923         }
1924 }
1925
1926 /* called holding qlock */
1927 static void process_single_up_tx_qlock(struct drm_dp_mst_topology_mgr *mgr,
1928                                        struct drm_dp_sideband_msg_tx *txmsg)
1929 {
1930         int ret;
1931
1932         /* construct a chunk from the first msg in the tx_msg queue */
1933         ret = process_single_tx_qlock(mgr, txmsg, true);
1934
1935         if (ret != 1)
1936                 DRM_DEBUG_KMS("failed to send msg in q %d\n", ret);
1937
1938         txmsg->dst->tx_slots[txmsg->seqno] = NULL;
1939 }
1940
1941 static void drm_dp_queue_down_tx(struct drm_dp_mst_topology_mgr *mgr,
1942                                  struct drm_dp_sideband_msg_tx *txmsg)
1943 {
1944         mutex_lock(&mgr->qlock);
1945         list_add_tail(&txmsg->next, &mgr->tx_msg_downq);
1946         if (list_is_singular(&mgr->tx_msg_downq))
1947                 process_single_down_tx_qlock(mgr);
1948         mutex_unlock(&mgr->qlock);
1949 }
1950
1951 static void drm_dp_send_link_address(struct drm_dp_mst_topology_mgr *mgr,
1952                                      struct drm_dp_mst_branch *mstb)
1953 {
1954         int len;
1955         struct drm_dp_sideband_msg_tx *txmsg;
1956         int ret;
1957
1958         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
1959         if (!txmsg)
1960                 return;
1961
1962         txmsg->dst = mstb;
1963         len = build_link_address(txmsg);
1964
1965         mstb->link_address_sent = true;
1966         drm_dp_queue_down_tx(mgr, txmsg);
1967
1968         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
1969         if (ret > 0) {
1970                 int i;
1971
1972                 if (txmsg->reply.reply_type == 1)
1973                         DRM_DEBUG_KMS("link address nak received\n");
1974                 else {
1975                         DRM_DEBUG_KMS("link address reply: %d\n", txmsg->reply.u.link_addr.nports);
1976                         for (i = 0; i < txmsg->reply.u.link_addr.nports; i++) {
1977                                 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,
1978                                        txmsg->reply.u.link_addr.ports[i].input_port,
1979                                        txmsg->reply.u.link_addr.ports[i].peer_device_type,
1980                                        txmsg->reply.u.link_addr.ports[i].port_number,
1981                                        txmsg->reply.u.link_addr.ports[i].dpcd_revision,
1982                                        txmsg->reply.u.link_addr.ports[i].mcs,
1983                                        txmsg->reply.u.link_addr.ports[i].ddps,
1984                                        txmsg->reply.u.link_addr.ports[i].legacy_device_plug_status,
1985                                        txmsg->reply.u.link_addr.ports[i].num_sdp_streams,
1986                                        txmsg->reply.u.link_addr.ports[i].num_sdp_stream_sinks);
1987                         }
1988
1989                         drm_dp_check_mstb_guid(mstb, txmsg->reply.u.link_addr.guid);
1990
1991                         for (i = 0; i < txmsg->reply.u.link_addr.nports; i++) {
1992                                 drm_dp_add_port(mstb, mgr->dev, &txmsg->reply.u.link_addr.ports[i]);
1993                         }
1994                         drm_kms_helper_hotplug_event(mgr->dev);
1995                 }
1996         } else {
1997                 mstb->link_address_sent = false;
1998                 DRM_DEBUG_KMS("link address failed %d\n", ret);
1999         }
2000
2001         kfree(txmsg);
2002 }
2003
2004 static int drm_dp_send_enum_path_resources(struct drm_dp_mst_topology_mgr *mgr,
2005                                            struct drm_dp_mst_branch *mstb,
2006                                            struct drm_dp_mst_port *port)
2007 {
2008         int len;
2009         struct drm_dp_sideband_msg_tx *txmsg;
2010         int ret;
2011
2012         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
2013         if (!txmsg)
2014                 return -ENOMEM;
2015
2016         txmsg->dst = mstb;
2017         len = build_enum_path_resources(txmsg, port->port_num);
2018
2019         drm_dp_queue_down_tx(mgr, txmsg);
2020
2021         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
2022         if (ret > 0) {
2023                 if (txmsg->reply.reply_type == 1)
2024                         DRM_DEBUG_KMS("enum path resources nak received\n");
2025                 else {
2026                         if (port->port_num != txmsg->reply.u.path_resources.port_number)
2027                                 DRM_ERROR("got incorrect port in response\n");
2028                         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,
2029                                txmsg->reply.u.path_resources.avail_payload_bw_number);
2030                         port->available_pbn = txmsg->reply.u.path_resources.avail_payload_bw_number;
2031                 }
2032         }
2033
2034         kfree(txmsg);
2035         return 0;
2036 }
2037
2038 static struct drm_dp_mst_port *drm_dp_get_last_connected_port_to_mstb(struct drm_dp_mst_branch *mstb)
2039 {
2040         if (!mstb->port_parent)
2041                 return NULL;
2042
2043         if (mstb->port_parent->mstb != mstb)
2044                 return mstb->port_parent;
2045
2046         return drm_dp_get_last_connected_port_to_mstb(mstb->port_parent->parent);
2047 }
2048
2049 /*
2050  * Searches upwards in the topology starting from mstb to try to find the
2051  * closest available parent of mstb that's still connected to the rest of the
2052  * topology. This can be used in order to perform operations like releasing
2053  * payloads, where the branch device which owned the payload may no longer be
2054  * around and thus would require that the payload on the last living relative
2055  * be freed instead.
2056  */
2057 static struct drm_dp_mst_branch *
2058 drm_dp_get_last_connected_port_and_mstb(struct drm_dp_mst_topology_mgr *mgr,
2059                                         struct drm_dp_mst_branch *mstb,
2060                                         int *port_num)
2061 {
2062         struct drm_dp_mst_branch *rmstb = NULL;
2063         struct drm_dp_mst_port *found_port;
2064
2065         mutex_lock(&mgr->lock);
2066         if (!mgr->mst_primary)
2067                 goto out;
2068
2069         do {
2070                 found_port = drm_dp_get_last_connected_port_to_mstb(mstb);
2071                 if (!found_port)
2072                         break;
2073
2074                 if (drm_dp_mst_topology_try_get_mstb(found_port->parent)) {
2075                         rmstb = found_port->parent;
2076                         *port_num = found_port->port_num;
2077                 } else {
2078                         /* Search again, starting from this parent */
2079                         mstb = found_port->parent;
2080                 }
2081         } while (!rmstb);
2082 out:
2083         mutex_unlock(&mgr->lock);
2084         return rmstb;
2085 }
2086
2087 static int drm_dp_payload_send_msg(struct drm_dp_mst_topology_mgr *mgr,
2088                                    struct drm_dp_mst_port *port,
2089                                    int id,
2090                                    int pbn)
2091 {
2092         struct drm_dp_sideband_msg_tx *txmsg;
2093         struct drm_dp_mst_branch *mstb;
2094         int len, ret, port_num;
2095         u8 sinks[DRM_DP_MAX_SDP_STREAMS];
2096         int i;
2097
2098         port_num = port->port_num;
2099         mstb = drm_dp_mst_topology_get_mstb_validated(mgr, port->parent);
2100         if (!mstb) {
2101                 mstb = drm_dp_get_last_connected_port_and_mstb(mgr,
2102                                                                port->parent,
2103                                                                &port_num);
2104
2105                 if (!mstb)
2106                         return -EINVAL;
2107         }
2108
2109         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
2110         if (!txmsg) {
2111                 ret = -ENOMEM;
2112                 goto fail_put;
2113         }
2114
2115         for (i = 0; i < port->num_sdp_streams; i++)
2116                 sinks[i] = i;
2117
2118         txmsg->dst = mstb;
2119         len = build_allocate_payload(txmsg, port_num,
2120                                      id,
2121                                      pbn, port->num_sdp_streams, sinks);
2122
2123         drm_dp_queue_down_tx(mgr, txmsg);
2124
2125         /*
2126          * FIXME: there is a small chance that between getting the last
2127          * connected mstb and sending the payload message, the last connected
2128          * mstb could also be removed from the topology. In the future, this
2129          * needs to be fixed by restarting the
2130          * drm_dp_get_last_connected_port_and_mstb() search in the event of a
2131          * timeout if the topology is still connected to the system.
2132          */
2133         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
2134         if (ret > 0) {
2135                 if (txmsg->reply.reply_type == 1)
2136                         ret = -EINVAL;
2137                 else
2138                         ret = 0;
2139         }
2140         kfree(txmsg);
2141 fail_put:
2142         drm_dp_mst_topology_put_mstb(mstb);
2143         return ret;
2144 }
2145
2146 int drm_dp_send_power_updown_phy(struct drm_dp_mst_topology_mgr *mgr,
2147                                  struct drm_dp_mst_port *port, bool power_up)
2148 {
2149         struct drm_dp_sideband_msg_tx *txmsg;
2150         int len, ret;
2151
2152         port = drm_dp_mst_topology_get_port_validated(mgr, port);
2153         if (!port)
2154                 return -EINVAL;
2155
2156         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
2157         if (!txmsg) {
2158                 drm_dp_mst_topology_put_port(port);
2159                 return -ENOMEM;
2160         }
2161
2162         txmsg->dst = port->parent;
2163         len = build_power_updown_phy(txmsg, port->port_num, power_up);
2164         drm_dp_queue_down_tx(mgr, txmsg);
2165
2166         ret = drm_dp_mst_wait_tx_reply(port->parent, txmsg);
2167         if (ret > 0) {
2168                 if (txmsg->reply.reply_type == 1)
2169                         ret = -EINVAL;
2170                 else
2171                         ret = 0;
2172         }
2173         kfree(txmsg);
2174         drm_dp_mst_topology_put_port(port);
2175
2176         return ret;
2177 }
2178 EXPORT_SYMBOL(drm_dp_send_power_updown_phy);
2179
2180 static int drm_dp_create_payload_step1(struct drm_dp_mst_topology_mgr *mgr,
2181                                        int id,
2182                                        struct drm_dp_payload *payload)
2183 {
2184         int ret;
2185
2186         ret = drm_dp_dpcd_write_payload(mgr, id, payload);
2187         if (ret < 0) {
2188                 payload->payload_state = 0;
2189                 return ret;
2190         }
2191         payload->payload_state = DP_PAYLOAD_LOCAL;
2192         return 0;
2193 }
2194
2195 static int drm_dp_create_payload_step2(struct drm_dp_mst_topology_mgr *mgr,
2196                                        struct drm_dp_mst_port *port,
2197                                        int id,
2198                                        struct drm_dp_payload *payload)
2199 {
2200         int ret;
2201         ret = drm_dp_payload_send_msg(mgr, port, id, port->vcpi.pbn);
2202         if (ret < 0)
2203                 return ret;
2204         payload->payload_state = DP_PAYLOAD_REMOTE;
2205         return ret;
2206 }
2207
2208 static int drm_dp_destroy_payload_step1(struct drm_dp_mst_topology_mgr *mgr,
2209                                         struct drm_dp_mst_port *port,
2210                                         int id,
2211                                         struct drm_dp_payload *payload)
2212 {
2213         DRM_DEBUG_KMS("\n");
2214         /* its okay for these to fail */
2215         if (port) {
2216                 drm_dp_payload_send_msg(mgr, port, id, 0);
2217         }
2218
2219         drm_dp_dpcd_write_payload(mgr, id, payload);
2220         payload->payload_state = DP_PAYLOAD_DELETE_LOCAL;
2221         return 0;
2222 }
2223
2224 static int drm_dp_destroy_payload_step2(struct drm_dp_mst_topology_mgr *mgr,
2225                                         int id,
2226                                         struct drm_dp_payload *payload)
2227 {
2228         payload->payload_state = 0;
2229         return 0;
2230 }
2231
2232 /**
2233  * drm_dp_update_payload_part1() - Execute payload update part 1
2234  * @mgr: manager to use.
2235  *
2236  * This iterates over all proposed virtual channels, and tries to
2237  * allocate space in the link for them. For 0->slots transitions,
2238  * this step just writes the VCPI to the MST device. For slots->0
2239  * transitions, this writes the updated VCPIs and removes the
2240  * remote VC payloads.
2241  *
2242  * after calling this the driver should generate ACT and payload
2243  * packets.
2244  */
2245 int drm_dp_update_payload_part1(struct drm_dp_mst_topology_mgr *mgr)
2246 {
2247         struct drm_dp_payload req_payload;
2248         struct drm_dp_mst_port *port;
2249         int i, j;
2250         int cur_slots = 1;
2251
2252         mutex_lock(&mgr->payload_lock);
2253         for (i = 0; i < mgr->max_payloads; i++) {
2254                 struct drm_dp_vcpi *vcpi = mgr->proposed_vcpis[i];
2255                 struct drm_dp_payload *payload = &mgr->payloads[i];
2256                 bool put_port = false;
2257
2258                 /* solve the current payloads - compare to the hw ones
2259                    - update the hw view */
2260                 req_payload.start_slot = cur_slots;
2261                 if (vcpi) {
2262                         port = container_of(vcpi, struct drm_dp_mst_port,
2263                                             vcpi);
2264
2265                         /* Validated ports don't matter if we're releasing
2266                          * VCPI
2267                          */
2268                         if (vcpi->num_slots) {
2269                                 port = drm_dp_mst_topology_get_port_validated(
2270                                     mgr, port);
2271                                 if (!port) {
2272                                         mutex_unlock(&mgr->payload_lock);
2273                                         return -EINVAL;
2274                                 }
2275                                 put_port = true;
2276                         }
2277
2278                         req_payload.num_slots = vcpi->num_slots;
2279                         req_payload.vcpi = vcpi->vcpi;
2280                 } else {
2281                         port = NULL;
2282                         req_payload.num_slots = 0;
2283                 }
2284
2285                 payload->start_slot = req_payload.start_slot;
2286                 /* work out what is required to happen with this payload */
2287                 if (payload->num_slots != req_payload.num_slots) {
2288
2289                         /* need to push an update for this payload */
2290                         if (req_payload.num_slots) {
2291                                 drm_dp_create_payload_step1(mgr, vcpi->vcpi,
2292                                                             &req_payload);
2293                                 payload->num_slots = req_payload.num_slots;
2294                                 payload->vcpi = req_payload.vcpi;
2295
2296                         } else if (payload->num_slots) {
2297                                 payload->num_slots = 0;
2298                                 drm_dp_destroy_payload_step1(mgr, port,
2299                                                              payload->vcpi,
2300                                                              payload);
2301                                 req_payload.payload_state =
2302                                         payload->payload_state;
2303                                 payload->start_slot = 0;
2304                         }
2305                         payload->payload_state = req_payload.payload_state;
2306                 }
2307                 cur_slots += req_payload.num_slots;
2308
2309                 if (put_port)
2310                         drm_dp_mst_topology_put_port(port);
2311         }
2312
2313         for (i = 0; i < mgr->max_payloads; i++) {
2314                 if (mgr->payloads[i].payload_state != DP_PAYLOAD_DELETE_LOCAL)
2315                         continue;
2316
2317                 DRM_DEBUG_KMS("removing payload %d\n", i);
2318                 for (j = i; j < mgr->max_payloads - 1; j++) {
2319                         mgr->payloads[j] = mgr->payloads[j + 1];
2320                         mgr->proposed_vcpis[j] = mgr->proposed_vcpis[j + 1];
2321
2322                         if (mgr->proposed_vcpis[j] &&
2323                             mgr->proposed_vcpis[j]->num_slots) {
2324                                 set_bit(j + 1, &mgr->payload_mask);
2325                         } else {
2326                                 clear_bit(j + 1, &mgr->payload_mask);
2327                         }
2328                 }
2329
2330                 memset(&mgr->payloads[mgr->max_payloads - 1], 0,
2331                        sizeof(struct drm_dp_payload));
2332                 mgr->proposed_vcpis[mgr->max_payloads - 1] = NULL;
2333                 clear_bit(mgr->max_payloads, &mgr->payload_mask);
2334         }
2335         mutex_unlock(&mgr->payload_lock);
2336
2337         return 0;
2338 }
2339 EXPORT_SYMBOL(drm_dp_update_payload_part1);
2340
2341 /**
2342  * drm_dp_update_payload_part2() - Execute payload update part 2
2343  * @mgr: manager to use.
2344  *
2345  * This iterates over all proposed virtual channels, and tries to
2346  * allocate space in the link for them. For 0->slots transitions,
2347  * this step writes the remote VC payload commands. For slots->0
2348  * this just resets some internal state.
2349  */
2350 int drm_dp_update_payload_part2(struct drm_dp_mst_topology_mgr *mgr)
2351 {
2352         struct drm_dp_mst_port *port;
2353         int i;
2354         int ret = 0;
2355         mutex_lock(&mgr->payload_lock);
2356         for (i = 0; i < mgr->max_payloads; i++) {
2357
2358                 if (!mgr->proposed_vcpis[i])
2359                         continue;
2360
2361                 port = container_of(mgr->proposed_vcpis[i], struct drm_dp_mst_port, vcpi);
2362
2363                 DRM_DEBUG_KMS("payload %d %d\n", i, mgr->payloads[i].payload_state);
2364                 if (mgr->payloads[i].payload_state == DP_PAYLOAD_LOCAL) {
2365                         ret = drm_dp_create_payload_step2(mgr, port, mgr->proposed_vcpis[i]->vcpi, &mgr->payloads[i]);
2366                 } else if (mgr->payloads[i].payload_state == DP_PAYLOAD_DELETE_LOCAL) {
2367                         ret = drm_dp_destroy_payload_step2(mgr, mgr->proposed_vcpis[i]->vcpi, &mgr->payloads[i]);
2368                 }
2369                 if (ret) {
2370                         mutex_unlock(&mgr->payload_lock);
2371                         return ret;
2372                 }
2373         }
2374         mutex_unlock(&mgr->payload_lock);
2375         return 0;
2376 }
2377 EXPORT_SYMBOL(drm_dp_update_payload_part2);
2378
2379 #if 0 /* unused as of yet */
2380 static int drm_dp_send_dpcd_read(struct drm_dp_mst_topology_mgr *mgr,
2381                                  struct drm_dp_mst_port *port,
2382                                  int offset, int size)
2383 {
2384         int len;
2385         struct drm_dp_sideband_msg_tx *txmsg;
2386
2387         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
2388         if (!txmsg)
2389                 return -ENOMEM;
2390
2391         len = build_dpcd_read(txmsg, port->port_num, 0, 8);
2392         txmsg->dst = port->parent;
2393
2394         drm_dp_queue_down_tx(mgr, txmsg);
2395
2396         return 0;
2397 }
2398 #endif
2399
2400 static int drm_dp_send_dpcd_write(struct drm_dp_mst_topology_mgr *mgr,
2401                                   struct drm_dp_mst_port *port,
2402                                   int offset, int size, u8 *bytes)
2403 {
2404         int len;
2405         int ret;
2406         struct drm_dp_sideband_msg_tx *txmsg;
2407         struct drm_dp_mst_branch *mstb;
2408
2409         mstb = drm_dp_mst_topology_get_mstb_validated(mgr, port->parent);
2410         if (!mstb)
2411                 return -EINVAL;
2412
2413         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
2414         if (!txmsg) {
2415                 ret = -ENOMEM;
2416                 goto fail_put;
2417         }
2418
2419         len = build_dpcd_write(txmsg, port->port_num, offset, size, bytes);
2420         txmsg->dst = mstb;
2421
2422         drm_dp_queue_down_tx(mgr, txmsg);
2423
2424         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
2425         if (ret > 0) {
2426                 if (txmsg->reply.reply_type == 1) {
2427                         ret = -EINVAL;
2428                 } else
2429                         ret = 0;
2430         }
2431         kfree(txmsg);
2432 fail_put:
2433         drm_dp_mst_topology_put_mstb(mstb);
2434         return ret;
2435 }
2436
2437 static int drm_dp_encode_up_ack_reply(struct drm_dp_sideband_msg_tx *msg, u8 req_type)
2438 {
2439         struct drm_dp_sideband_msg_reply_body reply;
2440
2441         reply.reply_type = 0;
2442         reply.req_type = req_type;
2443         drm_dp_encode_sideband_reply(&reply, msg);
2444         return 0;
2445 }
2446
2447 static int drm_dp_send_up_ack_reply(struct drm_dp_mst_topology_mgr *mgr,
2448                                     struct drm_dp_mst_branch *mstb,
2449                                     int req_type, int seqno, bool broadcast)
2450 {
2451         struct drm_dp_sideband_msg_tx *txmsg;
2452
2453         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
2454         if (!txmsg)
2455                 return -ENOMEM;
2456
2457         txmsg->dst = mstb;
2458         txmsg->seqno = seqno;
2459         drm_dp_encode_up_ack_reply(txmsg, req_type);
2460
2461         mutex_lock(&mgr->qlock);
2462
2463         process_single_up_tx_qlock(mgr, txmsg);
2464
2465         mutex_unlock(&mgr->qlock);
2466
2467         kfree(txmsg);
2468         return 0;
2469 }
2470
2471 static bool drm_dp_get_vc_payload_bw(int dp_link_bw,
2472                                      int dp_link_count,
2473                                      int *out)
2474 {
2475         switch (dp_link_bw) {
2476         default:
2477                 DRM_DEBUG_KMS("invalid link bandwidth in DPCD: %x (link count: %d)\n",
2478                               dp_link_bw, dp_link_count);
2479                 return false;
2480
2481         case DP_LINK_BW_1_62:
2482                 *out = 3 * dp_link_count;
2483                 break;
2484         case DP_LINK_BW_2_7:
2485                 *out = 5 * dp_link_count;
2486                 break;
2487         case DP_LINK_BW_5_4:
2488                 *out = 10 * dp_link_count;
2489                 break;
2490         case DP_LINK_BW_8_1:
2491                 *out = 15 * dp_link_count;
2492                 break;
2493         }
2494         return true;
2495 }
2496
2497 /**
2498  * drm_dp_mst_topology_mgr_set_mst() - Set the MST state for a topology manager
2499  * @mgr: manager to set state for
2500  * @mst_state: true to enable MST on this connector - false to disable.
2501  *
2502  * This is called by the driver when it detects an MST capable device plugged
2503  * into a DP MST capable port, or when a DP MST capable device is unplugged.
2504  */
2505 int drm_dp_mst_topology_mgr_set_mst(struct drm_dp_mst_topology_mgr *mgr, bool mst_state)
2506 {
2507         int ret = 0;
2508         struct drm_dp_mst_branch *mstb = NULL;
2509
2510         mutex_lock(&mgr->lock);
2511         if (mst_state == mgr->mst_state)
2512                 goto out_unlock;
2513
2514         mgr->mst_state = mst_state;
2515         /* set the device into MST mode */
2516         if (mst_state) {
2517                 WARN_ON(mgr->mst_primary);
2518
2519                 /* get dpcd info */
2520                 ret = drm_dp_dpcd_read(mgr->aux, DP_DPCD_REV, mgr->dpcd, DP_RECEIVER_CAP_SIZE);
2521                 if (ret != DP_RECEIVER_CAP_SIZE) {
2522                         DRM_DEBUG_KMS("failed to read DPCD\n");
2523                         goto out_unlock;
2524                 }
2525
2526                 if (!drm_dp_get_vc_payload_bw(mgr->dpcd[1],
2527                                               mgr->dpcd[2] & DP_MAX_LANE_COUNT_MASK,
2528                                               &mgr->pbn_div)) {
2529                         ret = -EINVAL;
2530                         goto out_unlock;
2531                 }
2532
2533                 /* add initial branch device at LCT 1 */
2534                 mstb = drm_dp_add_mst_branch_device(1, NULL);
2535                 if (mstb == NULL) {
2536                         ret = -ENOMEM;
2537                         goto out_unlock;
2538                 }
2539                 mstb->mgr = mgr;
2540
2541                 /* give this the main reference */
2542                 mgr->mst_primary = mstb;
2543                 drm_dp_mst_topology_get_mstb(mgr->mst_primary);
2544
2545                 ret = drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL,
2546                                                          DP_MST_EN | DP_UP_REQ_EN | DP_UPSTREAM_IS_SRC);
2547                 if (ret < 0) {
2548                         goto out_unlock;
2549                 }
2550
2551                 {
2552                         struct drm_dp_payload reset_pay;
2553                         reset_pay.start_slot = 0;
2554                         reset_pay.num_slots = 0x3f;
2555                         drm_dp_dpcd_write_payload(mgr, 0, &reset_pay);
2556                 }
2557
2558                 queue_work(system_long_wq, &mgr->work);
2559
2560                 ret = 0;
2561         } else {
2562                 /* disable MST on the device */
2563                 mstb = mgr->mst_primary;
2564                 mgr->mst_primary = NULL;
2565                 /* this can fail if the device is gone */
2566                 drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL, 0);
2567                 ret = 0;
2568                 memset(mgr->payloads, 0, mgr->max_payloads * sizeof(struct drm_dp_payload));
2569                 mgr->payload_mask = 0;
2570                 set_bit(0, &mgr->payload_mask);
2571                 mgr->vcpi_mask = 0;
2572         }
2573
2574 out_unlock:
2575         mutex_unlock(&mgr->lock);
2576         if (mstb)
2577                 drm_dp_mst_topology_put_mstb(mstb);
2578         return ret;
2579
2580 }
2581 EXPORT_SYMBOL(drm_dp_mst_topology_mgr_set_mst);
2582
2583 /**
2584  * drm_dp_mst_topology_mgr_suspend() - suspend the MST manager
2585  * @mgr: manager to suspend
2586  *
2587  * This function tells the MST device that we can't handle UP messages
2588  * anymore. This should stop it from sending any since we are suspended.
2589  */
2590 void drm_dp_mst_topology_mgr_suspend(struct drm_dp_mst_topology_mgr *mgr)
2591 {
2592         mutex_lock(&mgr->lock);
2593         drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL,
2594                            DP_MST_EN | DP_UPSTREAM_IS_SRC);
2595         mutex_unlock(&mgr->lock);
2596         flush_work(&mgr->work);
2597         flush_work(&mgr->destroy_connector_work);
2598 }
2599 EXPORT_SYMBOL(drm_dp_mst_topology_mgr_suspend);
2600
2601 /**
2602  * drm_dp_mst_topology_mgr_resume() - resume the MST manager
2603  * @mgr: manager to resume
2604  *
2605  * This will fetch DPCD and see if the device is still there,
2606  * if it is, it will rewrite the MSTM control bits, and return.
2607  *
2608  * if the device fails this returns -1, and the driver should do
2609  * a full MST reprobe, in case we were undocked.
2610  */
2611 int drm_dp_mst_topology_mgr_resume(struct drm_dp_mst_topology_mgr *mgr)
2612 {
2613         int ret = 0;
2614
2615         mutex_lock(&mgr->lock);
2616
2617         if (mgr->mst_primary) {
2618                 int sret;
2619                 u8 guid[16];
2620
2621                 sret = drm_dp_dpcd_read(mgr->aux, DP_DPCD_REV, mgr->dpcd, DP_RECEIVER_CAP_SIZE);
2622                 if (sret != DP_RECEIVER_CAP_SIZE) {
2623                         DRM_DEBUG_KMS("dpcd read failed - undocked during suspend?\n");
2624                         ret = -1;
2625                         goto out_unlock;
2626                 }
2627
2628                 ret = drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL,
2629                                          DP_MST_EN | DP_UP_REQ_EN | DP_UPSTREAM_IS_SRC);
2630                 if (ret < 0) {
2631                         DRM_DEBUG_KMS("mst write failed - undocked during suspend?\n");
2632                         ret = -1;
2633                         goto out_unlock;
2634                 }
2635
2636                 /* Some hubs forget their guids after they resume */
2637                 sret = drm_dp_dpcd_read(mgr->aux, DP_GUID, guid, 16);
2638                 if (sret != 16) {
2639                         DRM_DEBUG_KMS("dpcd read failed - undocked during suspend?\n");
2640                         ret = -1;
2641                         goto out_unlock;
2642                 }
2643                 drm_dp_check_mstb_guid(mgr->mst_primary, guid);
2644
2645                 ret = 0;
2646         } else
2647                 ret = -1;
2648
2649 out_unlock:
2650         mutex_unlock(&mgr->lock);
2651         return ret;
2652 }
2653 EXPORT_SYMBOL(drm_dp_mst_topology_mgr_resume);
2654
2655 static bool drm_dp_get_one_sb_msg(struct drm_dp_mst_topology_mgr *mgr, bool up)
2656 {
2657         int len;
2658         u8 replyblock[32];
2659         int replylen, origlen, curreply;
2660         int ret;
2661         struct drm_dp_sideband_msg_rx *msg;
2662         int basereg = up ? DP_SIDEBAND_MSG_UP_REQ_BASE : DP_SIDEBAND_MSG_DOWN_REP_BASE;
2663         msg = up ? &mgr->up_req_recv : &mgr->down_rep_recv;
2664
2665         len = min(mgr->max_dpcd_transaction_bytes, 16);
2666         ret = drm_dp_dpcd_read(mgr->aux, basereg,
2667                                replyblock, len);
2668         if (ret != len) {
2669                 DRM_DEBUG_KMS("failed to read DPCD down rep %d %d\n", len, ret);
2670                 return false;
2671         }
2672         ret = drm_dp_sideband_msg_build(msg, replyblock, len, true);
2673         if (!ret) {
2674                 DRM_DEBUG_KMS("sideband msg build failed %d\n", replyblock[0]);
2675                 return false;
2676         }
2677         replylen = msg->curchunk_len + msg->curchunk_hdrlen;
2678
2679         origlen = replylen;
2680         replylen -= len;
2681         curreply = len;
2682         while (replylen > 0) {
2683                 len = min3(replylen, mgr->max_dpcd_transaction_bytes, 16);
2684                 ret = drm_dp_dpcd_read(mgr->aux, basereg + curreply,
2685                                     replyblock, len);
2686                 if (ret != len) {
2687                         DRM_DEBUG_KMS("failed to read a chunk (len %d, ret %d)\n",
2688                                       len, ret);
2689                         return false;
2690                 }
2691
2692                 ret = drm_dp_sideband_msg_build(msg, replyblock, len, false);
2693                 if (!ret) {
2694                         DRM_DEBUG_KMS("failed to build sideband msg\n");
2695                         return false;
2696                 }
2697
2698                 curreply += len;
2699                 replylen -= len;
2700         }
2701         return true;
2702 }
2703
2704 static int drm_dp_mst_handle_down_rep(struct drm_dp_mst_topology_mgr *mgr)
2705 {
2706         int ret = 0;
2707
2708         if (!drm_dp_get_one_sb_msg(mgr, false)) {
2709                 memset(&mgr->down_rep_recv, 0,
2710                        sizeof(struct drm_dp_sideband_msg_rx));
2711                 return 0;
2712         }
2713
2714         if (mgr->down_rep_recv.have_eomt) {
2715                 struct drm_dp_sideband_msg_tx *txmsg;
2716                 struct drm_dp_mst_branch *mstb;
2717                 int slot = -1;
2718                 mstb = drm_dp_get_mst_branch_device(mgr,
2719                                                     mgr->down_rep_recv.initial_hdr.lct,
2720                                                     mgr->down_rep_recv.initial_hdr.rad);
2721
2722                 if (!mstb) {
2723                         DRM_DEBUG_KMS("Got MST reply from unknown device %d\n", mgr->down_rep_recv.initial_hdr.lct);
2724                         memset(&mgr->down_rep_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
2725                         return 0;
2726                 }
2727
2728                 /* find the message */
2729                 slot = mgr->down_rep_recv.initial_hdr.seqno;
2730                 mutex_lock(&mgr->qlock);
2731                 txmsg = mstb->tx_slots[slot];
2732                 /* remove from slots */
2733                 mutex_unlock(&mgr->qlock);
2734
2735                 if (!txmsg) {
2736                         DRM_DEBUG_KMS("Got MST reply with no msg %p %d %d %02x %02x\n",
2737                                mstb,
2738                                mgr->down_rep_recv.initial_hdr.seqno,
2739                                mgr->down_rep_recv.initial_hdr.lct,
2740                                       mgr->down_rep_recv.initial_hdr.rad[0],
2741                                       mgr->down_rep_recv.msg[0]);
2742                         drm_dp_mst_topology_put_mstb(mstb);
2743                         memset(&mgr->down_rep_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
2744                         return 0;
2745                 }
2746
2747                 drm_dp_sideband_parse_reply(&mgr->down_rep_recv, &txmsg->reply);
2748                 if (txmsg->reply.reply_type == 1) {
2749                         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);
2750                 }
2751
2752                 memset(&mgr->down_rep_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
2753                 drm_dp_mst_topology_put_mstb(mstb);
2754
2755                 mutex_lock(&mgr->qlock);
2756                 txmsg->state = DRM_DP_SIDEBAND_TX_RX;
2757                 mstb->tx_slots[slot] = NULL;
2758                 mutex_unlock(&mgr->qlock);
2759
2760                 wake_up_all(&mgr->tx_waitq);
2761         }
2762         return ret;
2763 }
2764
2765 static int drm_dp_mst_handle_up_req(struct drm_dp_mst_topology_mgr *mgr)
2766 {
2767         int ret = 0;
2768
2769         if (!drm_dp_get_one_sb_msg(mgr, true)) {
2770                 memset(&mgr->up_req_recv, 0,
2771                        sizeof(struct drm_dp_sideband_msg_rx));
2772                 return 0;
2773         }
2774
2775         if (mgr->up_req_recv.have_eomt) {
2776                 struct drm_dp_sideband_msg_req_body msg;
2777                 struct drm_dp_mst_branch *mstb = NULL;
2778                 bool seqno;
2779
2780                 if (!mgr->up_req_recv.initial_hdr.broadcast) {
2781                         mstb = drm_dp_get_mst_branch_device(mgr,
2782                                                             mgr->up_req_recv.initial_hdr.lct,
2783                                                             mgr->up_req_recv.initial_hdr.rad);
2784                         if (!mstb) {
2785                                 DRM_DEBUG_KMS("Got MST reply from unknown device %d\n", mgr->up_req_recv.initial_hdr.lct);
2786                                 memset(&mgr->up_req_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
2787                                 return 0;
2788                         }
2789                 }
2790
2791                 seqno = mgr->up_req_recv.initial_hdr.seqno;
2792                 drm_dp_sideband_parse_req(&mgr->up_req_recv, &msg);
2793
2794                 if (msg.req_type == DP_CONNECTION_STATUS_NOTIFY) {
2795                         drm_dp_send_up_ack_reply(mgr, mgr->mst_primary, msg.req_type, seqno, false);
2796
2797                         if (!mstb)
2798                                 mstb = drm_dp_get_mst_branch_device_by_guid(mgr, msg.u.conn_stat.guid);
2799
2800                         if (!mstb) {
2801                                 DRM_DEBUG_KMS("Got MST reply from unknown device %d\n", mgr->up_req_recv.initial_hdr.lct);
2802                                 memset(&mgr->up_req_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
2803                                 return 0;
2804                         }
2805
2806                         drm_dp_update_port(mstb, &msg.u.conn_stat);
2807
2808                         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);
2809                         drm_kms_helper_hotplug_event(mgr->dev);
2810
2811                 } else if (msg.req_type == DP_RESOURCE_STATUS_NOTIFY) {
2812                         drm_dp_send_up_ack_reply(mgr, mgr->mst_primary, msg.req_type, seqno, false);
2813                         if (!mstb)
2814                                 mstb = drm_dp_get_mst_branch_device_by_guid(mgr, msg.u.resource_stat.guid);
2815
2816                         if (!mstb) {
2817                                 DRM_DEBUG_KMS("Got MST reply from unknown device %d\n", mgr->up_req_recv.initial_hdr.lct);
2818                                 memset(&mgr->up_req_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
2819                                 return 0;
2820                         }
2821
2822                         DRM_DEBUG_KMS("Got RSN: pn: %d avail_pbn %d\n", msg.u.resource_stat.port_number, msg.u.resource_stat.available_pbn);
2823                 }
2824
2825                 if (mstb)
2826                         drm_dp_mst_topology_put_mstb(mstb);
2827
2828                 memset(&mgr->up_req_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
2829         }
2830         return ret;
2831 }
2832
2833 /**
2834  * drm_dp_mst_hpd_irq() - MST hotplug IRQ notify
2835  * @mgr: manager to notify irq for.
2836  * @esi: 4 bytes from SINK_COUNT_ESI
2837  * @handled: whether the hpd interrupt was consumed or not
2838  *
2839  * This should be called from the driver when it detects a short IRQ,
2840  * along with the value of the DEVICE_SERVICE_IRQ_VECTOR_ESI0. The
2841  * topology manager will process the sideband messages received as a result
2842  * of this.
2843  */
2844 int drm_dp_mst_hpd_irq(struct drm_dp_mst_topology_mgr *mgr, u8 *esi, bool *handled)
2845 {
2846         int ret = 0;
2847         int sc;
2848         *handled = false;
2849         sc = esi[0] & 0x3f;
2850
2851         if (sc != mgr->sink_count) {
2852                 mgr->sink_count = sc;
2853                 *handled = true;
2854         }
2855
2856         if (esi[1] & DP_DOWN_REP_MSG_RDY) {
2857                 ret = drm_dp_mst_handle_down_rep(mgr);
2858                 *handled = true;
2859         }
2860
2861         if (esi[1] & DP_UP_REQ_MSG_RDY) {
2862                 ret |= drm_dp_mst_handle_up_req(mgr);
2863                 *handled = true;
2864         }
2865
2866         drm_dp_mst_kick_tx(mgr);
2867         return ret;
2868 }
2869 EXPORT_SYMBOL(drm_dp_mst_hpd_irq);
2870
2871 /**
2872  * drm_dp_mst_detect_port() - get connection status for an MST port
2873  * @connector: DRM connector for this port
2874  * @mgr: manager for this port
2875  * @port: unverified pointer to a port
2876  *
2877  * This returns the current connection state for a port. It validates the
2878  * port pointer still exists so the caller doesn't require a reference
2879  */
2880 enum drm_connector_status drm_dp_mst_detect_port(struct drm_connector *connector,
2881                                                  struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port)
2882 {
2883         enum drm_connector_status status = connector_status_disconnected;
2884
2885         /* we need to search for the port in the mgr in case its gone */
2886         port = drm_dp_mst_topology_get_port_validated(mgr, port);
2887         if (!port)
2888                 return connector_status_disconnected;
2889
2890         if (!port->ddps)
2891                 goto out;
2892
2893         switch (port->pdt) {
2894         case DP_PEER_DEVICE_NONE:
2895         case DP_PEER_DEVICE_MST_BRANCHING:
2896                 break;
2897
2898         case DP_PEER_DEVICE_SST_SINK:
2899                 status = connector_status_connected;
2900                 /* for logical ports - cache the EDID */
2901                 if (port->port_num >= 8 && !port->cached_edid) {
2902                         port->cached_edid = drm_get_edid(connector, &port->aux.ddc);
2903                 }
2904                 break;
2905         case DP_PEER_DEVICE_DP_LEGACY_CONV:
2906                 if (port->ldps)
2907                         status = connector_status_connected;
2908                 break;
2909         }
2910 out:
2911         drm_dp_mst_topology_put_port(port);
2912         return status;
2913 }
2914 EXPORT_SYMBOL(drm_dp_mst_detect_port);
2915
2916 /**
2917  * drm_dp_mst_port_has_audio() - Check whether port has audio capability or not
2918  * @mgr: manager for this port
2919  * @port: unverified pointer to a port.
2920  *
2921  * This returns whether the port supports audio or not.
2922  */
2923 bool drm_dp_mst_port_has_audio(struct drm_dp_mst_topology_mgr *mgr,
2924                                         struct drm_dp_mst_port *port)
2925 {
2926         bool ret = false;
2927
2928         port = drm_dp_mst_topology_get_port_validated(mgr, port);
2929         if (!port)
2930                 return ret;
2931         ret = port->has_audio;
2932         drm_dp_mst_topology_put_port(port);
2933         return ret;
2934 }
2935 EXPORT_SYMBOL(drm_dp_mst_port_has_audio);
2936
2937 /**
2938  * drm_dp_mst_get_edid() - get EDID for an MST port
2939  * @connector: toplevel connector to get EDID for
2940  * @mgr: manager for this port
2941  * @port: unverified pointer to a port.
2942  *
2943  * This returns an EDID for the port connected to a connector,
2944  * It validates the pointer still exists so the caller doesn't require a
2945  * reference.
2946  */
2947 struct edid *drm_dp_mst_get_edid(struct drm_connector *connector, struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port)
2948 {
2949         struct edid *edid = NULL;
2950
2951         /* we need to search for the port in the mgr in case its gone */
2952         port = drm_dp_mst_topology_get_port_validated(mgr, port);
2953         if (!port)
2954                 return NULL;
2955
2956         if (port->cached_edid)
2957                 edid = drm_edid_duplicate(port->cached_edid);
2958         else {
2959                 edid = drm_get_edid(connector, &port->aux.ddc);
2960                 drm_connector_set_tile_property(connector);
2961         }
2962         port->has_audio = drm_detect_monitor_audio(edid);
2963         drm_dp_mst_topology_put_port(port);
2964         return edid;
2965 }
2966 EXPORT_SYMBOL(drm_dp_mst_get_edid);
2967
2968 /**
2969  * drm_dp_find_vcpi_slots() - Find VCPI slots for this PBN value
2970  * @mgr: manager to use
2971  * @pbn: payload bandwidth to convert into slots.
2972  *
2973  * Calculate the number of VCPI slots that will be required for the given PBN
2974  * value. This function is deprecated, and should not be used in atomic
2975  * drivers.
2976  *
2977  * RETURNS:
2978  * The total slots required for this port, or error.
2979  */
2980 int drm_dp_find_vcpi_slots(struct drm_dp_mst_topology_mgr *mgr,
2981                            int pbn)
2982 {
2983         int num_slots;
2984
2985         num_slots = DIV_ROUND_UP(pbn, mgr->pbn_div);
2986
2987         /* max. time slots - one slot for MTP header */
2988         if (num_slots > 63)
2989                 return -ENOSPC;
2990         return num_slots;
2991 }
2992 EXPORT_SYMBOL(drm_dp_find_vcpi_slots);
2993
2994 static int drm_dp_init_vcpi(struct drm_dp_mst_topology_mgr *mgr,
2995                             struct drm_dp_vcpi *vcpi, int pbn, int slots)
2996 {
2997         int ret;
2998
2999         /* max. time slots - one slot for MTP header */
3000         if (slots > 63)
3001                 return -ENOSPC;
3002
3003         vcpi->pbn = pbn;
3004         vcpi->aligned_pbn = slots * mgr->pbn_div;
3005         vcpi->num_slots = slots;
3006
3007         ret = drm_dp_mst_assign_payload_id(mgr, vcpi);
3008         if (ret < 0)
3009                 return ret;
3010         return 0;
3011 }
3012
3013 /**
3014  * drm_dp_atomic_find_vcpi_slots() - Find and add vcpi slots to the state
3015  * @state: global atomic state
3016  * @mgr: MST topology manager for the port
3017  * @port: port to find vcpi slots for
3018  * @pbn: bandwidth required for the mode in PBN
3019  *
3020  * RETURNS:
3021  * Total slots in the atomic state assigned for this port or error
3022  */
3023 int drm_dp_atomic_find_vcpi_slots(struct drm_atomic_state *state,
3024                                   struct drm_dp_mst_topology_mgr *mgr,
3025                                   struct drm_dp_mst_port *port, int pbn)
3026 {
3027         struct drm_dp_mst_topology_state *topology_state;
3028         int req_slots;
3029
3030         topology_state = drm_atomic_get_mst_topology_state(state, mgr);
3031         if (IS_ERR(topology_state))
3032                 return PTR_ERR(topology_state);
3033
3034         port = drm_dp_mst_topology_get_port_validated(mgr, port);
3035         if (port == NULL)
3036                 return -EINVAL;
3037         req_slots = DIV_ROUND_UP(pbn, mgr->pbn_div);
3038         DRM_DEBUG_KMS("vcpi slots req=%d, avail=%d\n",
3039                         req_slots, topology_state->avail_slots);
3040
3041         if (req_slots > topology_state->avail_slots) {
3042                 drm_dp_mst_topology_put_port(port);
3043                 return -ENOSPC;
3044         }
3045
3046         topology_state->avail_slots -= req_slots;
3047         DRM_DEBUG_KMS("vcpi slots avail=%d", topology_state->avail_slots);
3048
3049         drm_dp_mst_topology_put_port(port);
3050         return req_slots;
3051 }
3052 EXPORT_SYMBOL(drm_dp_atomic_find_vcpi_slots);
3053
3054 /**
3055  * drm_dp_atomic_release_vcpi_slots() - Release allocated vcpi slots
3056  * @state: global atomic state
3057  * @mgr: MST topology manager for the port
3058  * @slots: number of vcpi slots to release
3059  *
3060  * RETURNS:
3061  * 0 if @slots were added back to &drm_dp_mst_topology_state->avail_slots or
3062  * negative error code
3063  */
3064 int drm_dp_atomic_release_vcpi_slots(struct drm_atomic_state *state,
3065                                      struct drm_dp_mst_topology_mgr *mgr,
3066                                      int slots)
3067 {
3068         struct drm_dp_mst_topology_state *topology_state;
3069
3070         topology_state = drm_atomic_get_mst_topology_state(state, mgr);
3071         if (IS_ERR(topology_state))
3072                 return PTR_ERR(topology_state);
3073
3074         /* We cannot rely on port->vcpi.num_slots to update
3075          * topology_state->avail_slots as the port may not exist if the parent
3076          * branch device was unplugged. This should be fixed by tracking
3077          * per-port slot allocation in drm_dp_mst_topology_state instead of
3078          * depending on the caller to tell us how many slots to release.
3079          */
3080         topology_state->avail_slots += slots;
3081         DRM_DEBUG_KMS("vcpi slots released=%d, avail=%d\n",
3082                         slots, topology_state->avail_slots);
3083
3084         return 0;
3085 }
3086 EXPORT_SYMBOL(drm_dp_atomic_release_vcpi_slots);
3087
3088 /**
3089  * drm_dp_mst_allocate_vcpi() - Allocate a virtual channel
3090  * @mgr: manager for this port
3091  * @port: port to allocate a virtual channel for.
3092  * @pbn: payload bandwidth number to request
3093  * @slots: returned number of slots for this PBN.
3094  */
3095 bool drm_dp_mst_allocate_vcpi(struct drm_dp_mst_topology_mgr *mgr,
3096                               struct drm_dp_mst_port *port, int pbn, int slots)
3097 {
3098         int ret;
3099
3100         port = drm_dp_mst_topology_get_port_validated(mgr, port);
3101         if (!port)
3102                 return false;
3103
3104         if (slots < 0)
3105                 return false;
3106
3107         if (port->vcpi.vcpi > 0) {
3108                 DRM_DEBUG_KMS("payload: vcpi %d already allocated for pbn %d - requested pbn %d\n",
3109                               port->vcpi.vcpi, port->vcpi.pbn, pbn);
3110                 if (pbn == port->vcpi.pbn) {
3111                         drm_dp_mst_topology_put_port(port);
3112                         return true;
3113                 }
3114         }
3115
3116         ret = drm_dp_init_vcpi(mgr, &port->vcpi, pbn, slots);
3117         if (ret) {
3118                 DRM_DEBUG_KMS("failed to init vcpi slots=%d max=63 ret=%d\n",
3119                               DIV_ROUND_UP(pbn, mgr->pbn_div), ret);
3120                 goto out;
3121         }
3122         DRM_DEBUG_KMS("initing vcpi for pbn=%d slots=%d\n",
3123                       pbn, port->vcpi.num_slots);
3124
3125         /* Keep port allocated until it's payload has been removed */
3126         drm_dp_mst_get_port_malloc(port);
3127         drm_dp_mst_topology_put_port(port);
3128         return true;
3129 out:
3130         return false;
3131 }
3132 EXPORT_SYMBOL(drm_dp_mst_allocate_vcpi);
3133
3134 int drm_dp_mst_get_vcpi_slots(struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port)
3135 {
3136         int slots = 0;
3137         port = drm_dp_mst_topology_get_port_validated(mgr, port);
3138         if (!port)
3139                 return slots;
3140
3141         slots = port->vcpi.num_slots;
3142         drm_dp_mst_topology_put_port(port);
3143         return slots;
3144 }
3145 EXPORT_SYMBOL(drm_dp_mst_get_vcpi_slots);
3146
3147 /**
3148  * drm_dp_mst_reset_vcpi_slots() - Reset number of slots to 0 for VCPI
3149  * @mgr: manager for this port
3150  * @port: unverified pointer to a port.
3151  *
3152  * This just resets the number of slots for the ports VCPI for later programming.
3153  */
3154 void drm_dp_mst_reset_vcpi_slots(struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port)
3155 {
3156         /*
3157          * A port with VCPI will remain allocated until it's VCPI is
3158          * released, no verified ref needed
3159          */
3160
3161         port->vcpi.num_slots = 0;
3162 }
3163 EXPORT_SYMBOL(drm_dp_mst_reset_vcpi_slots);
3164
3165 /**
3166  * drm_dp_mst_deallocate_vcpi() - deallocate a VCPI
3167  * @mgr: manager for this port
3168  * @port: unverified port to deallocate vcpi for
3169  */
3170 void drm_dp_mst_deallocate_vcpi(struct drm_dp_mst_topology_mgr *mgr,
3171                                 struct drm_dp_mst_port *port)
3172 {
3173         /*
3174          * A port with VCPI will remain allocated until it's VCPI is
3175          * released, no verified ref needed
3176          */
3177
3178         drm_dp_mst_put_payload_id(mgr, port->vcpi.vcpi);
3179         port->vcpi.num_slots = 0;
3180         port->vcpi.pbn = 0;
3181         port->vcpi.aligned_pbn = 0;
3182         port->vcpi.vcpi = 0;
3183         drm_dp_mst_put_port_malloc(port);
3184 }
3185 EXPORT_SYMBOL(drm_dp_mst_deallocate_vcpi);
3186
3187 static int drm_dp_dpcd_write_payload(struct drm_dp_mst_topology_mgr *mgr,
3188                                      int id, struct drm_dp_payload *payload)
3189 {
3190         u8 payload_alloc[3], status;
3191         int ret;
3192         int retries = 0;
3193
3194         drm_dp_dpcd_writeb(mgr->aux, DP_PAYLOAD_TABLE_UPDATE_STATUS,
3195                            DP_PAYLOAD_TABLE_UPDATED);
3196
3197         payload_alloc[0] = id;
3198         payload_alloc[1] = payload->start_slot;
3199         payload_alloc[2] = payload->num_slots;
3200
3201         ret = drm_dp_dpcd_write(mgr->aux, DP_PAYLOAD_ALLOCATE_SET, payload_alloc, 3);
3202         if (ret != 3) {
3203                 DRM_DEBUG_KMS("failed to write payload allocation %d\n", ret);
3204                 goto fail;
3205         }
3206
3207 retry:
3208         ret = drm_dp_dpcd_readb(mgr->aux, DP_PAYLOAD_TABLE_UPDATE_STATUS, &status);
3209         if (ret < 0) {
3210                 DRM_DEBUG_KMS("failed to read payload table status %d\n", ret);
3211                 goto fail;
3212         }
3213
3214         if (!(status & DP_PAYLOAD_TABLE_UPDATED)) {
3215                 retries++;
3216                 if (retries < 20) {
3217                         usleep_range(10000, 20000);
3218                         goto retry;
3219                 }
3220                 DRM_DEBUG_KMS("status not set after read payload table status %d\n", status);
3221                 ret = -EINVAL;
3222                 goto fail;
3223         }
3224         ret = 0;
3225 fail:
3226         return ret;
3227 }
3228
3229
3230 /**
3231  * drm_dp_check_act_status() - Check ACT handled status.
3232  * @mgr: manager to use
3233  *
3234  * Check the payload status bits in the DPCD for ACT handled completion.
3235  */
3236 int drm_dp_check_act_status(struct drm_dp_mst_topology_mgr *mgr)
3237 {
3238         u8 status;
3239         int ret;
3240         int count = 0;
3241
3242         do {
3243                 ret = drm_dp_dpcd_readb(mgr->aux, DP_PAYLOAD_TABLE_UPDATE_STATUS, &status);
3244
3245                 if (ret < 0) {
3246                         DRM_DEBUG_KMS("failed to read payload table status %d\n", ret);
3247                         goto fail;
3248                 }
3249
3250                 if (status & DP_PAYLOAD_ACT_HANDLED)
3251                         break;
3252                 count++;
3253                 udelay(100);
3254
3255         } while (count < 30);
3256
3257         if (!(status & DP_PAYLOAD_ACT_HANDLED)) {
3258                 DRM_DEBUG_KMS("failed to get ACT bit %d after %d retries\n", status, count);
3259                 ret = -EINVAL;
3260                 goto fail;
3261         }
3262         return 0;
3263 fail:
3264         return ret;
3265 }
3266 EXPORT_SYMBOL(drm_dp_check_act_status);
3267
3268 /**
3269  * drm_dp_calc_pbn_mode() - Calculate the PBN for a mode.
3270  * @clock: dot clock for the mode
3271  * @bpp: bpp for the mode.
3272  *
3273  * This uses the formula in the spec to calculate the PBN value for a mode.
3274  */
3275 int drm_dp_calc_pbn_mode(int clock, int bpp)
3276 {
3277         u64 kbps;
3278         s64 peak_kbps;
3279         u32 numerator;
3280         u32 denominator;
3281
3282         kbps = clock * bpp;
3283
3284         /*
3285          * margin 5300ppm + 300ppm ~ 0.6% as per spec, factor is 1.006
3286          * The unit of 54/64Mbytes/sec is an arbitrary unit chosen based on
3287          * common multiplier to render an integer PBN for all link rate/lane
3288          * counts combinations
3289          * calculate
3290          * peak_kbps *= (1006/1000)
3291          * peak_kbps *= (64/54)
3292          * peak_kbps *= 8    convert to bytes
3293          */
3294
3295         numerator = 64 * 1006;
3296         denominator = 54 * 8 * 1000 * 1000;
3297
3298         kbps *= numerator;
3299         peak_kbps = drm_fixp_from_fraction(kbps, denominator);
3300
3301         return drm_fixp2int_ceil(peak_kbps);
3302 }
3303 EXPORT_SYMBOL(drm_dp_calc_pbn_mode);
3304
3305 static int test_calc_pbn_mode(void)
3306 {
3307         int ret;
3308         ret = drm_dp_calc_pbn_mode(154000, 30);
3309         if (ret != 689) {
3310                 DRM_ERROR("PBN calculation test failed - clock %d, bpp %d, expected PBN %d, actual PBN %d.\n",
3311                                 154000, 30, 689, ret);
3312                 return -EINVAL;
3313         }
3314         ret = drm_dp_calc_pbn_mode(234000, 30);
3315         if (ret != 1047) {
3316                 DRM_ERROR("PBN calculation test failed - clock %d, bpp %d, expected PBN %d, actual PBN %d.\n",
3317                                 234000, 30, 1047, ret);
3318                 return -EINVAL;
3319         }
3320         ret = drm_dp_calc_pbn_mode(297000, 24);
3321         if (ret != 1063) {
3322                 DRM_ERROR("PBN calculation test failed - clock %d, bpp %d, expected PBN %d, actual PBN %d.\n",
3323                                 297000, 24, 1063, ret);
3324                 return -EINVAL;
3325         }
3326         return 0;
3327 }
3328
3329 /* we want to kick the TX after we've ack the up/down IRQs. */
3330 static void drm_dp_mst_kick_tx(struct drm_dp_mst_topology_mgr *mgr)
3331 {
3332         queue_work(system_long_wq, &mgr->tx_work);
3333 }
3334
3335 static void drm_dp_mst_dump_mstb(struct seq_file *m,
3336                                  struct drm_dp_mst_branch *mstb)
3337 {
3338         struct drm_dp_mst_port *port;
3339         int tabs = mstb->lct;
3340         char prefix[10];
3341         int i;
3342
3343         for (i = 0; i < tabs; i++)
3344                 prefix[i] = '\t';
3345         prefix[i] = '\0';
3346
3347         seq_printf(m, "%smst: %p, %d\n", prefix, mstb, mstb->num_ports);
3348         list_for_each_entry(port, &mstb->ports, next) {
3349                 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);
3350                 if (port->mstb)
3351                         drm_dp_mst_dump_mstb(m, port->mstb);
3352         }
3353 }
3354
3355 #define DP_PAYLOAD_TABLE_SIZE           64
3356
3357 static bool dump_dp_payload_table(struct drm_dp_mst_topology_mgr *mgr,
3358                                   char *buf)
3359 {
3360         int i;
3361
3362         for (i = 0; i < DP_PAYLOAD_TABLE_SIZE; i += 16) {
3363                 if (drm_dp_dpcd_read(mgr->aux,
3364                                      DP_PAYLOAD_TABLE_UPDATE_STATUS + i,
3365                                      &buf[i], 16) != 16)
3366                         return false;
3367         }
3368         return true;
3369 }
3370
3371 static void fetch_monitor_name(struct drm_dp_mst_topology_mgr *mgr,
3372                                struct drm_dp_mst_port *port, char *name,
3373                                int namelen)
3374 {
3375         struct edid *mst_edid;
3376
3377         mst_edid = drm_dp_mst_get_edid(port->connector, mgr, port);
3378         drm_edid_get_monitor_name(mst_edid, name, namelen);
3379 }
3380
3381 /**
3382  * drm_dp_mst_dump_topology(): dump topology to seq file.
3383  * @m: seq_file to dump output to
3384  * @mgr: manager to dump current topology for.
3385  *
3386  * helper to dump MST topology to a seq file for debugfs.
3387  */
3388 void drm_dp_mst_dump_topology(struct seq_file *m,
3389                               struct drm_dp_mst_topology_mgr *mgr)
3390 {
3391         int i;
3392         struct drm_dp_mst_port *port;
3393
3394         mutex_lock(&mgr->lock);
3395         if (mgr->mst_primary)
3396                 drm_dp_mst_dump_mstb(m, mgr->mst_primary);
3397
3398         /* dump VCPIs */
3399         mutex_unlock(&mgr->lock);
3400
3401         mutex_lock(&mgr->payload_lock);
3402         seq_printf(m, "vcpi: %lx %lx %d\n", mgr->payload_mask, mgr->vcpi_mask,
3403                 mgr->max_payloads);
3404
3405         for (i = 0; i < mgr->max_payloads; i++) {
3406                 if (mgr->proposed_vcpis[i]) {
3407                         char name[14];
3408
3409                         port = container_of(mgr->proposed_vcpis[i], struct drm_dp_mst_port, vcpi);
3410                         fetch_monitor_name(mgr, port, name, sizeof(name));
3411                         seq_printf(m, "vcpi %d: %d %d %d sink name: %s\n", i,
3412                                    port->port_num, port->vcpi.vcpi,
3413                                    port->vcpi.num_slots,
3414                                    (*name != 0) ? name :  "Unknown");
3415                 } else
3416                         seq_printf(m, "vcpi %d:unused\n", i);
3417         }
3418         for (i = 0; i < mgr->max_payloads; i++) {
3419                 seq_printf(m, "payload %d: %d, %d, %d\n",
3420                            i,
3421                            mgr->payloads[i].payload_state,
3422                            mgr->payloads[i].start_slot,
3423                            mgr->payloads[i].num_slots);
3424
3425
3426         }
3427         mutex_unlock(&mgr->payload_lock);
3428
3429         mutex_lock(&mgr->lock);
3430         if (mgr->mst_primary) {
3431                 u8 buf[DP_PAYLOAD_TABLE_SIZE];
3432                 int ret;
3433
3434                 ret = drm_dp_dpcd_read(mgr->aux, DP_DPCD_REV, buf, DP_RECEIVER_CAP_SIZE);
3435                 seq_printf(m, "dpcd: %*ph\n", DP_RECEIVER_CAP_SIZE, buf);
3436                 ret = drm_dp_dpcd_read(mgr->aux, DP_FAUX_CAP, buf, 2);
3437                 seq_printf(m, "faux/mst: %*ph\n", 2, buf);
3438                 ret = drm_dp_dpcd_read(mgr->aux, DP_MSTM_CTRL, buf, 1);
3439                 seq_printf(m, "mst ctrl: %*ph\n", 1, buf);
3440
3441                 /* dump the standard OUI branch header */
3442                 ret = drm_dp_dpcd_read(mgr->aux, DP_BRANCH_OUI, buf, DP_BRANCH_OUI_HEADER_SIZE);
3443                 seq_printf(m, "branch oui: %*phN devid: ", 3, buf);
3444                 for (i = 0x3; i < 0x8 && buf[i]; i++)
3445                         seq_printf(m, "%c", buf[i]);
3446                 seq_printf(m, " revision: hw: %x.%x sw: %x.%x\n",
3447                            buf[0x9] >> 4, buf[0x9] & 0xf, buf[0xa], buf[0xb]);
3448                 if (dump_dp_payload_table(mgr, buf))
3449                         seq_printf(m, "payload table: %*ph\n", DP_PAYLOAD_TABLE_SIZE, buf);
3450         }
3451
3452         mutex_unlock(&mgr->lock);
3453
3454 }
3455 EXPORT_SYMBOL(drm_dp_mst_dump_topology);
3456
3457 static void drm_dp_tx_work(struct work_struct *work)
3458 {
3459         struct drm_dp_mst_topology_mgr *mgr = container_of(work, struct drm_dp_mst_topology_mgr, tx_work);
3460
3461         mutex_lock(&mgr->qlock);
3462         if (!list_empty(&mgr->tx_msg_downq))
3463                 process_single_down_tx_qlock(mgr);
3464         mutex_unlock(&mgr->qlock);
3465 }
3466
3467 static void drm_dp_destroy_connector_work(struct work_struct *work)
3468 {
3469         struct drm_dp_mst_topology_mgr *mgr = container_of(work, struct drm_dp_mst_topology_mgr, destroy_connector_work);
3470         struct drm_dp_mst_port *port;
3471         bool send_hotplug = false;
3472         /*
3473          * Not a regular list traverse as we have to drop the destroy
3474          * connector lock before destroying the connector, to avoid AB->BA
3475          * ordering between this lock and the config mutex.
3476          */
3477         for (;;) {
3478                 mutex_lock(&mgr->destroy_connector_lock);
3479                 port = list_first_entry_or_null(&mgr->destroy_connector_list, struct drm_dp_mst_port, next);
3480                 if (!port) {
3481                         mutex_unlock(&mgr->destroy_connector_lock);
3482                         break;
3483                 }
3484                 list_del(&port->next);
3485                 mutex_unlock(&mgr->destroy_connector_lock);
3486
3487                 INIT_LIST_HEAD(&port->next);
3488
3489                 mgr->cbs->destroy_connector(mgr, port->connector);
3490
3491                 drm_dp_port_teardown_pdt(port, port->pdt);
3492                 port->pdt = DP_PEER_DEVICE_NONE;
3493
3494                 drm_dp_mst_put_port_malloc(port);
3495                 send_hotplug = true;
3496         }
3497         if (send_hotplug)
3498                 drm_kms_helper_hotplug_event(mgr->dev);
3499 }
3500
3501 static struct drm_private_state *
3502 drm_dp_mst_duplicate_state(struct drm_private_obj *obj)
3503 {
3504         struct drm_dp_mst_topology_state *state;
3505
3506         state = kmemdup(obj->state, sizeof(*state), GFP_KERNEL);
3507         if (!state)
3508                 return NULL;
3509
3510         __drm_atomic_helper_private_obj_duplicate_state(obj, &state->base);
3511
3512         return &state->base;
3513 }
3514
3515 static void drm_dp_mst_destroy_state(struct drm_private_obj *obj,
3516                                      struct drm_private_state *state)
3517 {
3518         struct drm_dp_mst_topology_state *mst_state =
3519                 to_dp_mst_topology_state(state);
3520
3521         kfree(mst_state);
3522 }
3523
3524 static const struct drm_private_state_funcs mst_state_funcs = {
3525         .atomic_duplicate_state = drm_dp_mst_duplicate_state,
3526         .atomic_destroy_state = drm_dp_mst_destroy_state,
3527 };
3528
3529 /**
3530  * drm_atomic_get_mst_topology_state: get MST topology state
3531  *
3532  * @state: global atomic state
3533  * @mgr: MST topology manager, also the private object in this case
3534  *
3535  * This function wraps drm_atomic_get_priv_obj_state() passing in the MST atomic
3536  * state vtable so that the private object state returned is that of a MST
3537  * topology object. Also, drm_atomic_get_private_obj_state() expects the caller
3538  * to care of the locking, so warn if don't hold the connection_mutex.
3539  *
3540  * RETURNS:
3541  *
3542  * The MST topology state or error pointer.
3543  */
3544 struct drm_dp_mst_topology_state *drm_atomic_get_mst_topology_state(struct drm_atomic_state *state,
3545                                                                     struct drm_dp_mst_topology_mgr *mgr)
3546 {
3547         struct drm_device *dev = mgr->dev;
3548
3549         WARN_ON(!drm_modeset_is_locked(&dev->mode_config.connection_mutex));
3550         return to_dp_mst_topology_state(drm_atomic_get_private_obj_state(state, &mgr->base));
3551 }
3552 EXPORT_SYMBOL(drm_atomic_get_mst_topology_state);
3553
3554 /**
3555  * drm_dp_mst_topology_mgr_init - initialise a topology manager
3556  * @mgr: manager struct to initialise
3557  * @dev: device providing this structure - for i2c addition.
3558  * @aux: DP helper aux channel to talk to this device
3559  * @max_dpcd_transaction_bytes: hw specific DPCD transaction limit
3560  * @max_payloads: maximum number of payloads this GPU can source
3561  * @conn_base_id: the connector object ID the MST device is connected to.
3562  *
3563  * Return 0 for success, or negative error code on failure
3564  */
3565 int drm_dp_mst_topology_mgr_init(struct drm_dp_mst_topology_mgr *mgr,
3566                                  struct drm_device *dev, struct drm_dp_aux *aux,
3567                                  int max_dpcd_transaction_bytes,
3568                                  int max_payloads, int conn_base_id)
3569 {
3570         struct drm_dp_mst_topology_state *mst_state;
3571
3572         mutex_init(&mgr->lock);
3573         mutex_init(&mgr->qlock);
3574         mutex_init(&mgr->payload_lock);
3575         mutex_init(&mgr->destroy_connector_lock);
3576         INIT_LIST_HEAD(&mgr->tx_msg_downq);
3577         INIT_LIST_HEAD(&mgr->destroy_connector_list);
3578         INIT_WORK(&mgr->work, drm_dp_mst_link_probe_work);
3579         INIT_WORK(&mgr->tx_work, drm_dp_tx_work);
3580         INIT_WORK(&mgr->destroy_connector_work, drm_dp_destroy_connector_work);
3581         init_waitqueue_head(&mgr->tx_waitq);
3582         mgr->dev = dev;
3583         mgr->aux = aux;
3584         mgr->max_dpcd_transaction_bytes = max_dpcd_transaction_bytes;
3585         mgr->max_payloads = max_payloads;
3586         mgr->conn_base_id = conn_base_id;
3587         if (max_payloads + 1 > sizeof(mgr->payload_mask) * 8 ||
3588             max_payloads + 1 > sizeof(mgr->vcpi_mask) * 8)
3589                 return -EINVAL;
3590         mgr->payloads = kcalloc(max_payloads, sizeof(struct drm_dp_payload), GFP_KERNEL);
3591         if (!mgr->payloads)
3592                 return -ENOMEM;
3593         mgr->proposed_vcpis = kcalloc(max_payloads, sizeof(struct drm_dp_vcpi *), GFP_KERNEL);
3594         if (!mgr->proposed_vcpis)
3595                 return -ENOMEM;
3596         set_bit(0, &mgr->payload_mask);
3597         if (test_calc_pbn_mode() < 0)
3598                 DRM_ERROR("MST PBN self-test failed\n");
3599
3600         mst_state = kzalloc(sizeof(*mst_state), GFP_KERNEL);
3601         if (mst_state == NULL)
3602                 return -ENOMEM;
3603
3604         mst_state->mgr = mgr;
3605
3606         /* max. time slots - one slot for MTP header */
3607         mst_state->avail_slots = 63;
3608
3609         drm_atomic_private_obj_init(dev, &mgr->base,
3610                                     &mst_state->base,
3611                                     &mst_state_funcs);
3612
3613         return 0;
3614 }
3615 EXPORT_SYMBOL(drm_dp_mst_topology_mgr_init);
3616
3617 /**
3618  * drm_dp_mst_topology_mgr_destroy() - destroy topology manager.
3619  * @mgr: manager to destroy
3620  */
3621 void drm_dp_mst_topology_mgr_destroy(struct drm_dp_mst_topology_mgr *mgr)
3622 {
3623         drm_dp_mst_topology_mgr_set_mst(mgr, false);
3624         flush_work(&mgr->work);
3625         flush_work(&mgr->destroy_connector_work);
3626         mutex_lock(&mgr->payload_lock);
3627         kfree(mgr->payloads);
3628         mgr->payloads = NULL;
3629         kfree(mgr->proposed_vcpis);
3630         mgr->proposed_vcpis = NULL;
3631         mutex_unlock(&mgr->payload_lock);
3632         mgr->dev = NULL;
3633         mgr->aux = NULL;
3634         drm_atomic_private_obj_fini(&mgr->base);
3635         mgr->funcs = NULL;
3636 }
3637 EXPORT_SYMBOL(drm_dp_mst_topology_mgr_destroy);
3638
3639 static bool remote_i2c_read_ok(const struct i2c_msg msgs[], int num)
3640 {
3641         int i;
3642
3643         if (num - 1 > DP_REMOTE_I2C_READ_MAX_TRANSACTIONS)
3644                 return false;
3645
3646         for (i = 0; i < num - 1; i++) {
3647                 if (msgs[i].flags & I2C_M_RD ||
3648                     msgs[i].len > 0xff)
3649                         return false;
3650         }
3651
3652         return msgs[num - 1].flags & I2C_M_RD &&
3653                 msgs[num - 1].len <= 0xff;
3654 }
3655
3656 /* I2C device */
3657 static int drm_dp_mst_i2c_xfer(struct i2c_adapter *adapter, struct i2c_msg *msgs,
3658                                int num)
3659 {
3660         struct drm_dp_aux *aux = adapter->algo_data;
3661         struct drm_dp_mst_port *port = container_of(aux, struct drm_dp_mst_port, aux);
3662         struct drm_dp_mst_branch *mstb;
3663         struct drm_dp_mst_topology_mgr *mgr = port->mgr;
3664         unsigned int i;
3665         struct drm_dp_sideband_msg_req_body msg;
3666         struct drm_dp_sideband_msg_tx *txmsg = NULL;
3667         int ret;
3668
3669         mstb = drm_dp_mst_topology_get_mstb_validated(mgr, port->parent);
3670         if (!mstb)
3671                 return -EREMOTEIO;
3672
3673         if (!remote_i2c_read_ok(msgs, num)) {
3674                 DRM_DEBUG_KMS("Unsupported I2C transaction for MST device\n");
3675                 ret = -EIO;
3676                 goto out;
3677         }
3678
3679         memset(&msg, 0, sizeof(msg));
3680         msg.req_type = DP_REMOTE_I2C_READ;
3681         msg.u.i2c_read.num_transactions = num - 1;
3682         msg.u.i2c_read.port_number = port->port_num;
3683         for (i = 0; i < num - 1; i++) {
3684                 msg.u.i2c_read.transactions[i].i2c_dev_id = msgs[i].addr;
3685                 msg.u.i2c_read.transactions[i].num_bytes = msgs[i].len;
3686                 msg.u.i2c_read.transactions[i].bytes = msgs[i].buf;
3687                 msg.u.i2c_read.transactions[i].no_stop_bit = !(msgs[i].flags & I2C_M_STOP);
3688         }
3689         msg.u.i2c_read.read_i2c_device_id = msgs[num - 1].addr;
3690         msg.u.i2c_read.num_bytes_read = msgs[num - 1].len;
3691
3692         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
3693         if (!txmsg) {
3694                 ret = -ENOMEM;
3695                 goto out;
3696         }
3697
3698         txmsg->dst = mstb;
3699         drm_dp_encode_sideband_req(&msg, txmsg);
3700
3701         drm_dp_queue_down_tx(mgr, txmsg);
3702
3703         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
3704         if (ret > 0) {
3705
3706                 if (txmsg->reply.reply_type == 1) { /* got a NAK back */
3707                         ret = -EREMOTEIO;
3708                         goto out;
3709                 }
3710                 if (txmsg->reply.u.remote_i2c_read_ack.num_bytes != msgs[num - 1].len) {
3711                         ret = -EIO;
3712                         goto out;
3713                 }
3714                 memcpy(msgs[num - 1].buf, txmsg->reply.u.remote_i2c_read_ack.bytes, msgs[num - 1].len);
3715                 ret = num;
3716         }
3717 out:
3718         kfree(txmsg);
3719         drm_dp_mst_topology_put_mstb(mstb);
3720         return ret;
3721 }
3722
3723 static u32 drm_dp_mst_i2c_functionality(struct i2c_adapter *adapter)
3724 {
3725         return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL |
3726                I2C_FUNC_SMBUS_READ_BLOCK_DATA |
3727                I2C_FUNC_SMBUS_BLOCK_PROC_CALL |
3728                I2C_FUNC_10BIT_ADDR;
3729 }
3730
3731 static const struct i2c_algorithm drm_dp_mst_i2c_algo = {
3732         .functionality = drm_dp_mst_i2c_functionality,
3733         .master_xfer = drm_dp_mst_i2c_xfer,
3734 };
3735
3736 /**
3737  * drm_dp_mst_register_i2c_bus() - register an I2C adapter for I2C-over-AUX
3738  * @aux: DisplayPort AUX channel
3739  *
3740  * Returns 0 on success or a negative error code on failure.
3741  */
3742 static int drm_dp_mst_register_i2c_bus(struct drm_dp_aux *aux)
3743 {
3744         aux->ddc.algo = &drm_dp_mst_i2c_algo;
3745         aux->ddc.algo_data = aux;
3746         aux->ddc.retries = 3;
3747
3748         aux->ddc.class = I2C_CLASS_DDC;
3749         aux->ddc.owner = THIS_MODULE;
3750         aux->ddc.dev.parent = aux->dev;
3751         aux->ddc.dev.of_node = aux->dev->of_node;
3752
3753         strlcpy(aux->ddc.name, aux->name ? aux->name : dev_name(aux->dev),
3754                 sizeof(aux->ddc.name));
3755
3756         return i2c_add_adapter(&aux->ddc);
3757 }
3758
3759 /**
3760  * drm_dp_mst_unregister_i2c_bus() - unregister an I2C-over-AUX adapter
3761  * @aux: DisplayPort AUX channel
3762  */
3763 static void drm_dp_mst_unregister_i2c_bus(struct drm_dp_aux *aux)
3764 {
3765         i2c_del_adapter(&aux->ddc);
3766 }