drivers/gpu/drm/drm_dp_mst_topology.c

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