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/delay.h>
  24 #include <linux/errno.h>
  25 #include <linux/i2c.h>
  26 #include <linux/init.h>
  27 #include <linux/kernel.h>
  28 #include <linux/sched.h>
  29 #include <linux/seq_file.h>
  30
  31 #if IS_ENABLED(CONFIG_DRM_DEBUG_DP_MST_TOPOLOGY_REFS)
  32 #include <linux/stacktrace.h>
  33 #include <linux/sort.h>
  34 #include <linux/timekeeping.h>
  35 #include <linux/math64.h>
  36 #endif
  37
  38 #include <drm/drm_atomic.h>
  39 #include <drm/drm_atomic_helper.h>
  40 #include <drm/drm_dp_mst_helper.h>
  41 #include <drm/drm_drv.h>
  42 #include <drm/drm_print.h>
  43 #include <drm/drm_probe_helper.h>
  44
  45 #include "drm_crtc_helper_internal.h"
  46 #include "drm_dp_mst_topology_internal.h"
  47
  48 /**
  49  * DOC: dp mst helper
  50  *
  51  * These functions contain parts of the DisplayPort 1.2a MultiStream Transport
  52  * protocol. The helpers contain a topology manager and bandwidth manager.
  53  * The helpers encapsulate the sending and received of sideband msgs.
  54  */
  55 struct drm_dp_pending_up_req {
  56         struct drm_dp_sideband_msg_hdr hdr;
  57         struct drm_dp_sideband_msg_req_body msg;
  58         struct list_head next;
  59 };
  60
  61 static bool dump_dp_payload_table(struct drm_dp_mst_topology_mgr *mgr,
  62                                   char *buf);
  63
  64 static void drm_dp_mst_topology_put_port(struct drm_dp_mst_port *port);
  65
  66 static int drm_dp_dpcd_write_payload(struct drm_dp_mst_topology_mgr *mgr,
  67                                      int id,
  68                                      struct drm_dp_payload *payload);
  69
  70 static int drm_dp_send_dpcd_read(struct drm_dp_mst_topology_mgr *mgr,
  71                                  struct drm_dp_mst_port *port,
  72                                  int offset, int size, u8 *bytes);
  73 static int drm_dp_send_dpcd_write(struct drm_dp_mst_topology_mgr *mgr,
  74                                   struct drm_dp_mst_port *port,
  75                                   int offset, int size, u8 *bytes);
  76
  77 static int drm_dp_send_link_address(struct drm_dp_mst_topology_mgr *mgr,
  78                                     struct drm_dp_mst_branch *mstb);
  79
  80 static void
  81 drm_dp_send_clear_payload_id_table(struct drm_dp_mst_topology_mgr *mgr,
  82                                    struct drm_dp_mst_branch *mstb);
  83
  84 static int drm_dp_send_enum_path_resources(struct drm_dp_mst_topology_mgr *mgr,
  85                                            struct drm_dp_mst_branch *mstb,
  86                                            struct drm_dp_mst_port *port);
  87 static bool drm_dp_validate_guid(struct drm_dp_mst_topology_mgr *mgr,
  88                                  u8 *guid);
  89
  90 static int drm_dp_mst_register_i2c_bus(struct drm_dp_aux *aux);
  91 static void drm_dp_mst_unregister_i2c_bus(struct drm_dp_aux *aux);
  92 static void drm_dp_mst_kick_tx(struct drm_dp_mst_topology_mgr *mgr);
  93
  94 #define DBG_PREFIX "[dp_mst]"
  95
  96 #define DP_STR(x) [DP_ ## x] = #x
  97
  98 static const char *drm_dp_mst_req_type_str(u8 req_type)
  99 {
 100         static const char * const req_type_str[] = {
 101                 DP_STR(GET_MSG_TRANSACTION_VERSION),
 102                 DP_STR(LINK_ADDRESS),
 103                 DP_STR(CONNECTION_STATUS_NOTIFY),
 104                 DP_STR(ENUM_PATH_RESOURCES),
 105                 DP_STR(ALLOCATE_PAYLOAD),
 106                 DP_STR(QUERY_PAYLOAD),
 107                 DP_STR(RESOURCE_STATUS_NOTIFY),
 108                 DP_STR(CLEAR_PAYLOAD_ID_TABLE),
 109                 DP_STR(REMOTE_DPCD_READ),
 110                 DP_STR(REMOTE_DPCD_WRITE),
 111                 DP_STR(REMOTE_I2C_READ),
 112                 DP_STR(REMOTE_I2C_WRITE),
 113                 DP_STR(POWER_UP_PHY),
 114                 DP_STR(POWER_DOWN_PHY),
 115                 DP_STR(SINK_EVENT_NOTIFY),
 116                 DP_STR(QUERY_STREAM_ENC_STATUS),
 117         };
 118
 119         if (req_type >= ARRAY_SIZE(req_type_str) ||
 120             !req_type_str[req_type])
 121                 return "unknown";
 122
 123         return req_type_str[req_type];
 124 }
 125
 126 #undef DP_STR
 127 #define DP_STR(x) [DP_NAK_ ## x] = #x
 128
 129 static const char *drm_dp_mst_nak_reason_str(u8 nak_reason)
 130 {
 131         static const char * const nak_reason_str[] = {
 132                 DP_STR(WRITE_FAILURE),
 133                 DP_STR(INVALID_READ),
 134                 DP_STR(CRC_FAILURE),
 135                 DP_STR(BAD_PARAM),
 136                 DP_STR(DEFER),
 137                 DP_STR(LINK_FAILURE),
 138                 DP_STR(NO_RESOURCES),
 139                 DP_STR(DPCD_FAIL),
 140                 DP_STR(I2C_NAK),
 141                 DP_STR(ALLOCATE_FAIL),
 142         };
 143
 144         if (nak_reason >= ARRAY_SIZE(nak_reason_str) ||
 145             !nak_reason_str[nak_reason])
 146                 return "unknown";
 147
 148         return nak_reason_str[nak_reason];
 149 }
 150
 151 #undef DP_STR
 152 #define DP_STR(x) [DRM_DP_SIDEBAND_TX_ ## x] = #x
 153
 154 static const char *drm_dp_mst_sideband_tx_state_str(int state)
 155 {
 156         static const char * const sideband_reason_str[] = {
 157                 DP_STR(QUEUED),
 158                 DP_STR(START_SEND),
 159                 DP_STR(SENT),
 160                 DP_STR(RX),
 161                 DP_STR(TIMEOUT),
 162         };
 163
 164         if (state >= ARRAY_SIZE(sideband_reason_str) ||
 165             !sideband_reason_str[state])
 166                 return "unknown";
 167
 168         return sideband_reason_str[state];
 169 }
 170
 171 static int
 172 drm_dp_mst_rad_to_str(const u8 rad[8], u8 lct, char *out, size_t len)
 173 {
 174         int i;
 175         u8 unpacked_rad[16];
 176
 177         for (i = 0; i < lct; i++) {
 178                 if (i % 2)
 179                         unpacked_rad[i] = rad[i / 2] >> 4;
 180                 else
 181                         unpacked_rad[i] = rad[i / 2] & BIT_MASK(4);
 182         }
 183
 184         /* TODO: Eventually add something to printk so we can format the rad
 185          * like this: 1.2.3
 186          */
 187         return snprintf(out, len, "%*phC", lct, unpacked_rad);
 188 }
 189
 190 /* sideband msg handling */
 191 static u8 drm_dp_msg_header_crc4(const uint8_t *data, size_t num_nibbles)
 192 {
 193         u8 bitmask = 0x80;
 194         u8 bitshift = 7;
 195         u8 array_index = 0;
 196         int number_of_bits = num_nibbles * 4;
 197         u8 remainder = 0;
 198
 199         while (number_of_bits != 0) {
 200                 number_of_bits--;
 201                 remainder <<= 1;
 202                 remainder |= (data[array_index] & bitmask) >> bitshift;
 203                 bitmask >>= 1;
 204                 bitshift--;
 205                 if (bitmask == 0) {
 206                         bitmask = 0x80;
 207                         bitshift = 7;
 208                         array_index++;
 209                 }
 210                 if ((remainder & 0x10) == 0x10)
 211                         remainder ^= 0x13;
 212         }
 213
 214         number_of_bits = 4;
 215         while (number_of_bits != 0) {
 216                 number_of_bits--;
 217                 remainder <<= 1;
 218                 if ((remainder & 0x10) != 0)
 219                         remainder ^= 0x13;
 220         }
 221
 222         return remainder;
 223 }
 224
 225 static u8 drm_dp_msg_data_crc4(const uint8_t *data, u8 number_of_bytes)
 226 {
 227         u8 bitmask = 0x80;
 228         u8 bitshift = 7;
 229         u8 array_index = 0;
 230         int number_of_bits = number_of_bytes * 8;
 231         u16 remainder = 0;
 232
 233         while (number_of_bits != 0) {
 234                 number_of_bits--;
 235                 remainder <<= 1;
 236                 remainder |= (data[array_index] & bitmask) >> bitshift;
 237                 bitmask >>= 1;
 238                 bitshift--;
 239                 if (bitmask == 0) {
 240                         bitmask = 0x80;
 241                         bitshift = 7;
 242                         array_index++;
 243                 }
 244                 if ((remainder & 0x100) == 0x100)
 245                         remainder ^= 0xd5;
 246         }
 247
 248         number_of_bits = 8;
 249         while (number_of_bits != 0) {
 250                 number_of_bits--;
 251                 remainder <<= 1;
 252                 if ((remainder & 0x100) != 0)
 253                         remainder ^= 0xd5;
 254         }
 255
 256         return remainder & 0xff;
 257 }
 258 static inline u8 drm_dp_calc_sb_hdr_size(struct drm_dp_sideband_msg_hdr *hdr)
 259 {
 260         u8 size = 3;
 261         size += (hdr->lct / 2);
 262         return size;
 263 }
 264
 265 static void drm_dp_encode_sideband_msg_hdr(struct drm_dp_sideband_msg_hdr *hdr,
 266                                            u8 *buf, int *len)
 267 {
 268         int idx = 0;
 269         int i;
 270         u8 crc4;
 271         buf[idx++] = ((hdr->lct & 0xf) << 4) | (hdr->lcr & 0xf);
 272         for (i = 0; i < (hdr->lct / 2); i++)
 273                 buf[idx++] = hdr->rad[i];
 274         buf[idx++] = (hdr->broadcast << 7) | (hdr->path_msg << 6) |
 275                 (hdr->msg_len & 0x3f);
 276         buf[idx++] = (hdr->somt << 7) | (hdr->eomt << 6) | (hdr->seqno << 4);
 277
 278         crc4 = drm_dp_msg_header_crc4(buf, (idx * 2) - 1);
 279         buf[idx - 1] |= (crc4 & 0xf);
 280
 281         *len = idx;
 282 }
 283
 284 static bool drm_dp_decode_sideband_msg_hdr(struct drm_dp_sideband_msg_hdr *hdr,
 285                                            u8 *buf, int buflen, u8 *hdrlen)
 286 {
 287         u8 crc4;
 288         u8 len;
 289         int i;
 290         u8 idx;
 291         if (buf[0] == 0)
 292                 return false;
 293         len = 3;
 294         len += ((buf[0] & 0xf0) >> 4) / 2;
 295         if (len > buflen)
 296                 return false;
 297         crc4 = drm_dp_msg_header_crc4(buf, (len * 2) - 1);
 298
 299         if ((crc4 & 0xf) != (buf[len - 1] & 0xf)) {
 300                 DRM_DEBUG_KMS("crc4 mismatch 0x%x 0x%x\n", crc4, buf[len - 1]);
 301                 return false;
 302         }
 303
 304         hdr->lct = (buf[0] & 0xf0) >> 4;
 305         hdr->lcr = (buf[0] & 0xf);
 306         idx = 1;
 307         for (i = 0; i < (hdr->lct / 2); i++)
 308                 hdr->rad[i] = buf[idx++];
 309         hdr->broadcast = (buf[idx] >> 7) & 0x1;
 310         hdr->path_msg = (buf[idx] >> 6) & 0x1;
 311         hdr->msg_len = buf[idx] & 0x3f;
 312         idx++;
 313         hdr->somt = (buf[idx] >> 7) & 0x1;
 314         hdr->eomt = (buf[idx] >> 6) & 0x1;
 315         hdr->seqno = (buf[idx] >> 4) & 0x1;
 316         idx++;
 317         *hdrlen = idx;
 318         return true;
 319 }
 320
 321 void
 322 drm_dp_encode_sideband_req(const struct drm_dp_sideband_msg_req_body *req,
 323                            struct drm_dp_sideband_msg_tx *raw)
 324 {
 325         int idx = 0;
 326         int i;
 327         u8 *buf = raw->msg;
 328         buf[idx++] = req->req_type & 0x7f;
 329
 330         switch (req->req_type) {
 331         case DP_ENUM_PATH_RESOURCES:
 332         case DP_POWER_DOWN_PHY:
 333         case DP_POWER_UP_PHY:
 334                 buf[idx] = (req->u.port_num.port_number & 0xf) << 4;
 335                 idx++;
 336                 break;
 337         case DP_ALLOCATE_PAYLOAD:
 338                 buf[idx] = (req->u.allocate_payload.port_number & 0xf) << 4 |
 339                         (req->u.allocate_payload.number_sdp_streams & 0xf);
 340                 idx++;
 341                 buf[idx] = (req->u.allocate_payload.vcpi & 0x7f);
 342                 idx++;
 343                 buf[idx] = (req->u.allocate_payload.pbn >> 8);
 344                 idx++;
 345                 buf[idx] = (req->u.allocate_payload.pbn & 0xff);
 346                 idx++;
 347                 for (i = 0; i < req->u.allocate_payload.number_sdp_streams / 2; i++) {
 348                         buf[idx] = ((req->u.allocate_payload.sdp_stream_sink[i * 2] & 0xf) << 4) |
 349                                 (req->u.allocate_payload.sdp_stream_sink[i * 2 + 1] & 0xf);
 350                         idx++;
 351                 }
 352                 if (req->u.allocate_payload.number_sdp_streams & 1) {
 353                         i = req->u.allocate_payload.number_sdp_streams - 1;
 354                         buf[idx] = (req->u.allocate_payload.sdp_stream_sink[i] & 0xf) << 4;
 355                         idx++;
 356                 }
 357                 break;
 358         case DP_QUERY_PAYLOAD:
 359                 buf[idx] = (req->u.query_payload.port_number & 0xf) << 4;
 360                 idx++;
 361                 buf[idx] = (req->u.query_payload.vcpi & 0x7f);
 362                 idx++;
 363                 break;
 364         case DP_REMOTE_DPCD_READ:
 365                 buf[idx] = (req->u.dpcd_read.port_number & 0xf) << 4;
 366                 buf[idx] |= ((req->u.dpcd_read.dpcd_address & 0xf0000) >> 16) & 0xf;
 367                 idx++;
 368                 buf[idx] = (req->u.dpcd_read.dpcd_address & 0xff00) >> 8;
 369                 idx++;
 370                 buf[idx] = (req->u.dpcd_read.dpcd_address & 0xff);
 371                 idx++;
 372                 buf[idx] = (req->u.dpcd_read.num_bytes);
 373                 idx++;
 374                 break;
 375
 376         case DP_REMOTE_DPCD_WRITE:
 377                 buf[idx] = (req->u.dpcd_write.port_number & 0xf) << 4;
 378                 buf[idx] |= ((req->u.dpcd_write.dpcd_address & 0xf0000) >> 16) & 0xf;
 379                 idx++;
 380                 buf[idx] = (req->u.dpcd_write.dpcd_address & 0xff00) >> 8;
 381                 idx++;
 382                 buf[idx] = (req->u.dpcd_write.dpcd_address & 0xff);
 383                 idx++;
 384                 buf[idx] = (req->u.dpcd_write.num_bytes);
 385                 idx++;
 386                 memcpy(&buf[idx], req->u.dpcd_write.bytes, req->u.dpcd_write.num_bytes);
 387                 idx += req->u.dpcd_write.num_bytes;
 388                 break;
 389         case DP_REMOTE_I2C_READ:
 390                 buf[idx] = (req->u.i2c_read.port_number & 0xf) << 4;
 391                 buf[idx] |= (req->u.i2c_read.num_transactions & 0x3);
 392                 idx++;
 393                 for (i = 0; i < (req->u.i2c_read.num_transactions & 0x3); i++) {
 394                         buf[idx] = req->u.i2c_read.transactions[i].i2c_dev_id & 0x7f;
 395                         idx++;
 396                         buf[idx] = req->u.i2c_read.transactions[i].num_bytes;
 397                         idx++;
 398                         memcpy(&buf[idx], req->u.i2c_read.transactions[i].bytes, req->u.i2c_read.transactions[i].num_bytes);
 399                         idx += req->u.i2c_read.transactions[i].num_bytes;
 400
 401                         buf[idx] = (req->u.i2c_read.transactions[i].no_stop_bit & 0x1) << 5;
 402                         buf[idx] |= (req->u.i2c_read.transactions[i].i2c_transaction_delay & 0xf);
 403                         idx++;
 404                 }
 405                 buf[idx] = (req->u.i2c_read.read_i2c_device_id) & 0x7f;
 406                 idx++;
 407                 buf[idx] = (req->u.i2c_read.num_bytes_read);
 408                 idx++;
 409                 break;
 410
 411         case DP_REMOTE_I2C_WRITE:
 412                 buf[idx] = (req->u.i2c_write.port_number & 0xf) << 4;
 413                 idx++;
 414                 buf[idx] = (req->u.i2c_write.write_i2c_device_id) & 0x7f;
 415                 idx++;
 416                 buf[idx] = (req->u.i2c_write.num_bytes);
 417                 idx++;
 418                 memcpy(&buf[idx], req->u.i2c_write.bytes, req->u.i2c_write.num_bytes);
 419                 idx += req->u.i2c_write.num_bytes;
 420                 break;
 421         }
 422         raw->cur_len = idx;
 423 }
 424 EXPORT_SYMBOL_FOR_TESTS_ONLY(drm_dp_encode_sideband_req);
 425
 426 /* Decode a sideband request we've encoded, mainly used for debugging */
 427 int
 428 drm_dp_decode_sideband_req(const struct drm_dp_sideband_msg_tx *raw,
 429                            struct drm_dp_sideband_msg_req_body *req)
 430 {
 431         const u8 *buf = raw->msg;
 432         int i, idx = 0;
 433
 434         req->req_type = buf[idx++] & 0x7f;
 435         switch (req->req_type) {
 436         case DP_ENUM_PATH_RESOURCES:
 437         case DP_POWER_DOWN_PHY:
 438         case DP_POWER_UP_PHY:
 439                 req->u.port_num.port_number = (buf[idx] >> 4) & 0xf;
 440                 break;
 441         case DP_ALLOCATE_PAYLOAD:
 442                 {
 443                         struct drm_dp_allocate_payload *a =
 444                                 &req->u.allocate_payload;
 445
 446                         a->number_sdp_streams = buf[idx] & 0xf;
 447                         a->port_number = (buf[idx] >> 4) & 0xf;
 448
 449                         WARN_ON(buf[++idx] & 0x80);
 450                         a->vcpi = buf[idx] & 0x7f;
 451
 452                         a->pbn = buf[++idx] << 8;
 453                         a->pbn |= buf[++idx];
 454
 455                         idx++;
 456                         for (i = 0; i < a->number_sdp_streams; i++) {
 457                                 a->sdp_stream_sink[i] =
 458                                         (buf[idx + (i / 2)] >> ((i % 2) ? 0 : 4)) & 0xf;
 459                         }
 460                 }
 461                 break;
 462         case DP_QUERY_PAYLOAD:
 463                 req->u.query_payload.port_number = (buf[idx] >> 4) & 0xf;
 464                 WARN_ON(buf[++idx] & 0x80);
 465                 req->u.query_payload.vcpi = buf[idx] & 0x7f;
 466                 break;
 467         case DP_REMOTE_DPCD_READ:
 468                 {
 469                         struct drm_dp_remote_dpcd_read *r = &req->u.dpcd_read;
 470
 471                         r->port_number = (buf[idx] >> 4) & 0xf;
 472
 473                         r->dpcd_address = (buf[idx] << 16) & 0xf0000;
 474                         r->dpcd_address |= (buf[++idx] << 8) & 0xff00;
 475                         r->dpcd_address |= buf[++idx] & 0xff;
 476
 477                         r->num_bytes = buf[++idx];
 478                 }
 479                 break;
 480         case DP_REMOTE_DPCD_WRITE:
 481                 {
 482                         struct drm_dp_remote_dpcd_write *w =
 483                                 &req->u.dpcd_write;
 484
 485                         w->port_number = (buf[idx] >> 4) & 0xf;
 486
 487                         w->dpcd_address = (buf[idx] << 16) & 0xf0000;
 488                         w->dpcd_address |= (buf[++idx] << 8) & 0xff00;
 489                         w->dpcd_address |= buf[++idx] & 0xff;
 490
 491                         w->num_bytes = buf[++idx];
 492
 493                         w->bytes = kmemdup(&buf[++idx], w->num_bytes,
 494                                            GFP_KERNEL);
 495                         if (!w->bytes)
 496                                 return -ENOMEM;
 497                 }
 498                 break;
 499         case DP_REMOTE_I2C_READ:
 500                 {
 501                         struct drm_dp_remote_i2c_read *r = &req->u.i2c_read;
 502                         struct drm_dp_remote_i2c_read_tx *tx;
 503                         bool failed = false;
 504
 505                         r->num_transactions = buf[idx] & 0x3;
 506                         r->port_number = (buf[idx] >> 4) & 0xf;
 507                         for (i = 0; i < r->num_transactions; i++) {
 508                                 tx = &r->transactions[i];
 509
 510                                 tx->i2c_dev_id = buf[++idx] & 0x7f;
 511                                 tx->num_bytes = buf[++idx];
 512                                 tx->bytes = kmemdup(&buf[++idx],
 513                                                     tx->num_bytes,
 514                                                     GFP_KERNEL);
 515                                 if (!tx->bytes) {
 516                                         failed = true;
 517                                         break;
 518                                 }
 519                                 idx += tx->num_bytes;
 520                                 tx->no_stop_bit = (buf[idx] >> 5) & 0x1;
 521                                 tx->i2c_transaction_delay = buf[idx] & 0xf;
 522                         }
 523
 524                         if (failed) {
 525                                 for (i = 0; i < r->num_transactions; i++) {
 526                                         tx = &r->transactions[i];
 527                                         kfree(tx->bytes);
 528                                 }
 529                                 return -ENOMEM;
 530                         }
 531
 532                         r->read_i2c_device_id = buf[++idx] & 0x7f;
 533                         r->num_bytes_read = buf[++idx];
 534                 }
 535                 break;
 536         case DP_REMOTE_I2C_WRITE:
 537                 {
 538                         struct drm_dp_remote_i2c_write *w = &req->u.i2c_write;
 539
 540                         w->port_number = (buf[idx] >> 4) & 0xf;
 541                         w->write_i2c_device_id = buf[++idx] & 0x7f;
 542                         w->num_bytes = buf[++idx];
 543                         w->bytes = kmemdup(&buf[++idx], w->num_bytes,
 544                                            GFP_KERNEL);
 545                         if (!w->bytes)
 546                                 return -ENOMEM;
 547                 }
 548                 break;
 549         }
 550
 551         return 0;
 552 }
 553 EXPORT_SYMBOL_FOR_TESTS_ONLY(drm_dp_decode_sideband_req);
 554
 555 void
 556 drm_dp_dump_sideband_msg_req_body(const struct drm_dp_sideband_msg_req_body *req,
 557                                   int indent, struct drm_printer *printer)
 558 {
 559         int i;
 560
 561 #define P(f, ...) drm_printf_indent(printer, indent, f, ##__VA_ARGS__)
 562         if (req->req_type == DP_LINK_ADDRESS) {
 563                 /* No contents to print */
 564                 P("type=%s\n", drm_dp_mst_req_type_str(req->req_type));
 565                 return;
 566         }
 567
 568         P("type=%s contents:\n", drm_dp_mst_req_type_str(req->req_type));
 569         indent++;
 570
 571         switch (req->req_type) {
 572         case DP_ENUM_PATH_RESOURCES:
 573         case DP_POWER_DOWN_PHY:
 574         case DP_POWER_UP_PHY:
 575                 P("port=%d\n", req->u.port_num.port_number);
 576                 break;
 577         case DP_ALLOCATE_PAYLOAD:
 578                 P("port=%d vcpi=%d pbn=%d sdp_streams=%d %*ph\n",
 579                   req->u.allocate_payload.port_number,
 580                   req->u.allocate_payload.vcpi, req->u.allocate_payload.pbn,
 581                   req->u.allocate_payload.number_sdp_streams,
 582                   req->u.allocate_payload.number_sdp_streams,
 583                   req->u.allocate_payload.sdp_stream_sink);
 584                 break;
 585         case DP_QUERY_PAYLOAD:
 586                 P("port=%d vcpi=%d\n",
 587                   req->u.query_payload.port_number,
 588                   req->u.query_payload.vcpi);
 589                 break;
 590         case DP_REMOTE_DPCD_READ:
 591                 P("port=%d dpcd_addr=%05x len=%d\n",
 592                   req->u.dpcd_read.port_number, req->u.dpcd_read.dpcd_address,
 593                   req->u.dpcd_read.num_bytes);
 594                 break;
 595         case DP_REMOTE_DPCD_WRITE:
 596                 P("port=%d addr=%05x len=%d: %*ph\n",
 597                   req->u.dpcd_write.port_number,
 598                   req->u.dpcd_write.dpcd_address,
 599                   req->u.dpcd_write.num_bytes, req->u.dpcd_write.num_bytes,
 600                   req->u.dpcd_write.bytes);
 601                 break;
 602         case DP_REMOTE_I2C_READ:
 603                 P("port=%d num_tx=%d id=%d size=%d:\n",
 604                   req->u.i2c_read.port_number,
 605                   req->u.i2c_read.num_transactions,
 606                   req->u.i2c_read.read_i2c_device_id,
 607                   req->u.i2c_read.num_bytes_read);
 608
 609                 indent++;
 610                 for (i = 0; i < req->u.i2c_read.num_transactions; i++) {
 611                         const struct drm_dp_remote_i2c_read_tx *rtx =
 612                                 &req->u.i2c_read.transactions[i];
 613
 614                         P("%d: id=%03d size=%03d no_stop_bit=%d tx_delay=%03d: %*ph\n",
 615                           i, rtx->i2c_dev_id, rtx->num_bytes,
 616                           rtx->no_stop_bit, rtx->i2c_transaction_delay,
 617                           rtx->num_bytes, rtx->bytes);
 618                 }
 619                 break;
 620         case DP_REMOTE_I2C_WRITE:
 621                 P("port=%d id=%d size=%d: %*ph\n",
 622                   req->u.i2c_write.port_number,
 623                   req->u.i2c_write.write_i2c_device_id,
 624                   req->u.i2c_write.num_bytes, req->u.i2c_write.num_bytes,
 625                   req->u.i2c_write.bytes);
 626                 break;
 627         default:
 628                 P("???\n");
 629                 break;
 630         }
 631 #undef P
 632 }
 633 EXPORT_SYMBOL_FOR_TESTS_ONLY(drm_dp_dump_sideband_msg_req_body);
 634
 635 static inline void
 636 drm_dp_mst_dump_sideband_msg_tx(struct drm_printer *p,
 637                                 const struct drm_dp_sideband_msg_tx *txmsg)
 638 {
 639         struct drm_dp_sideband_msg_req_body req;
 640         char buf[64];
 641         int ret;
 642         int i;
 643
 644         drm_dp_mst_rad_to_str(txmsg->dst->rad, txmsg->dst->lct, buf,
 645                               sizeof(buf));
 646         drm_printf(p, "txmsg cur_offset=%x cur_len=%x seqno=%x state=%s path_msg=%d dst=%s\n",
 647                    txmsg->cur_offset, txmsg->cur_len, txmsg->seqno,
 648                    drm_dp_mst_sideband_tx_state_str(txmsg->state),
 649                    txmsg->path_msg, buf);
 650
 651         ret = drm_dp_decode_sideband_req(txmsg, &req);
 652         if (ret) {
 653                 drm_printf(p, "<failed to decode sideband req: %d>\n", ret);
 654                 return;
 655         }
 656         drm_dp_dump_sideband_msg_req_body(&req, 1, p);
 657
 658         switch (req.req_type) {
 659         case DP_REMOTE_DPCD_WRITE:
 660                 kfree(req.u.dpcd_write.bytes);
 661                 break;
 662         case DP_REMOTE_I2C_READ:
 663                 for (i = 0; i < req.u.i2c_read.num_transactions; i++)
 664                         kfree(req.u.i2c_read.transactions[i].bytes);
 665                 break;
 666         case DP_REMOTE_I2C_WRITE:
 667                 kfree(req.u.i2c_write.bytes);
 668                 break;
 669         }
 670 }
 671
 672 static void drm_dp_crc_sideband_chunk_req(u8 *msg, u8 len)
 673 {
 674         u8 crc4;
 675         crc4 = drm_dp_msg_data_crc4(msg, len);
 676         msg[len] = crc4;
 677 }
 678
 679 static void drm_dp_encode_sideband_reply(struct drm_dp_sideband_msg_reply_body *rep,
 680                                          struct drm_dp_sideband_msg_tx *raw)
 681 {
 682         int idx = 0;
 683         u8 *buf = raw->msg;
 684
 685         buf[idx++] = (rep->reply_type & 0x1) << 7 | (rep->req_type & 0x7f);
 686
 687         raw->cur_len = idx;
 688 }
 689
 690 /* this adds a chunk of msg to the builder to get the final msg */
 691 static bool drm_dp_sideband_msg_build(struct drm_dp_sideband_msg_rx *msg,
 692                                       u8 *replybuf, u8 replybuflen, bool hdr)
 693 {
 694         int ret;
 695         u8 crc4;
 696
 697         if (hdr) {
 698                 u8 hdrlen;
 699                 struct drm_dp_sideband_msg_hdr recv_hdr;
 700                 ret = drm_dp_decode_sideband_msg_hdr(&recv_hdr, replybuf, replybuflen, &hdrlen);
 701                 if (ret == false) {
 702                         print_hex_dump(KERN_DEBUG, "failed hdr", DUMP_PREFIX_NONE, 16, 1, replybuf, replybuflen, false);
 703                         return false;
 704                 }
 705
 706                 /*
 707                  * ignore out-of-order messages or messages that are part of a
 708                  * failed transaction
 709                  */
 710                 if (!recv_hdr.somt && !msg->have_somt)
 711                         return false;
 712
 713                 /* get length contained in this portion */
 714                 msg->curchunk_len = recv_hdr.msg_len;
 715                 msg->curchunk_hdrlen = hdrlen;
 716
 717                 /* we have already gotten an somt - don't bother parsing */
 718                 if (recv_hdr.somt && msg->have_somt)
 719                         return false;
 720
 721                 if (recv_hdr.somt) {
 722                         memcpy(&msg->initial_hdr, &recv_hdr, sizeof(struct drm_dp_sideband_msg_hdr));
 723                         msg->have_somt = true;
 724                 }
 725                 if (recv_hdr.eomt)
 726                         msg->have_eomt = true;
 727
 728                 /* copy the bytes for the remainder of this header chunk */
 729                 msg->curchunk_idx = min(msg->curchunk_len, (u8)(replybuflen - hdrlen));
 730                 memcpy(&msg->chunk[0], replybuf + hdrlen, msg->curchunk_idx);
 731         } else {
 732                 memcpy(&msg->chunk[msg->curchunk_idx], replybuf, replybuflen);
 733                 msg->curchunk_idx += replybuflen;
 734         }
 735
 736         if (msg->curchunk_idx >= msg->curchunk_len) {
 737                 /* do CRC */
 738                 crc4 = drm_dp_msg_data_crc4(msg->chunk, msg->curchunk_len - 1);
 739                 /* copy chunk into bigger msg */
 740                 memcpy(&msg->msg[msg->curlen], msg->chunk, msg->curchunk_len - 1);
 741                 msg->curlen += msg->curchunk_len - 1;
 742         }
 743         return true;
 744 }
 745
 746 static bool drm_dp_sideband_parse_link_address(struct drm_dp_sideband_msg_rx *raw,
 747                                                struct drm_dp_sideband_msg_reply_body *repmsg)
 748 {
 749         int idx = 1;
 750         int i;
 751         memcpy(repmsg->u.link_addr.guid, &raw->msg[idx], 16);
 752         idx += 16;
 753         repmsg->u.link_addr.nports = raw->msg[idx] & 0xf;
 754         idx++;
 755         if (idx > raw->curlen)
 756                 goto fail_len;
 757         for (i = 0; i < repmsg->u.link_addr.nports; i++) {
 758                 if (raw->msg[idx] & 0x80)
 759                         repmsg->u.link_addr.ports[i].input_port = 1;
 760
 761                 repmsg->u.link_addr.ports[i].peer_device_type = (raw->msg[idx] >> 4) & 0x7;
 762                 repmsg->u.link_addr.ports[i].port_number = (raw->msg[idx] & 0xf);
 763
 764                 idx++;
 765                 if (idx > raw->curlen)
 766                         goto fail_len;
 767                 repmsg->u.link_addr.ports[i].mcs = (raw->msg[idx] >> 7) & 0x1;
 768                 repmsg->u.link_addr.ports[i].ddps = (raw->msg[idx] >> 6) & 0x1;
 769                 if (repmsg->u.link_addr.ports[i].input_port == 0)
 770                         repmsg->u.link_addr.ports[i].legacy_device_plug_status = (raw->msg[idx] >> 5) & 0x1;
 771                 idx++;
 772                 if (idx > raw->curlen)
 773                         goto fail_len;
 774                 if (repmsg->u.link_addr.ports[i].input_port == 0) {
 775                         repmsg->u.link_addr.ports[i].dpcd_revision = (raw->msg[idx]);
 776                         idx++;
 777                         if (idx > raw->curlen)
 778                                 goto fail_len;
 779                         memcpy(repmsg->u.link_addr.ports[i].peer_guid, &raw->msg[idx], 16);
 780                         idx += 16;
 781                         if (idx > raw->curlen)
 782                                 goto fail_len;
 783                         repmsg->u.link_addr.ports[i].num_sdp_streams = (raw->msg[idx] >> 4) & 0xf;
 784                         repmsg->u.link_addr.ports[i].num_sdp_stream_sinks = (raw->msg[idx] & 0xf);
 785                         idx++;
 786
 787                 }
 788                 if (idx > raw->curlen)
 789                         goto fail_len;
 790         }
 791
 792         return true;
 793 fail_len:
 794         DRM_DEBUG_KMS("link address reply parse length fail %d %d\n", idx, raw->curlen);
 795         return false;
 796 }
 797
 798 static bool drm_dp_sideband_parse_remote_dpcd_read(struct drm_dp_sideband_msg_rx *raw,
 799                                                    struct drm_dp_sideband_msg_reply_body *repmsg)
 800 {
 801         int idx = 1;
 802         repmsg->u.remote_dpcd_read_ack.port_number = raw->msg[idx] & 0xf;
 803         idx++;
 804         if (idx > raw->curlen)
 805                 goto fail_len;
 806         repmsg->u.remote_dpcd_read_ack.num_bytes = raw->msg[idx];
 807         idx++;
 808         if (idx > raw->curlen)
 809                 goto fail_len;
 810
 811         memcpy(repmsg->u.remote_dpcd_read_ack.bytes, &raw->msg[idx], repmsg->u.remote_dpcd_read_ack.num_bytes);
 812         return true;
 813 fail_len:
 814         DRM_DEBUG_KMS("link address reply parse length fail %d %d\n", idx, raw->curlen);
 815         return false;
 816 }
 817
 818 static bool drm_dp_sideband_parse_remote_dpcd_write(struct drm_dp_sideband_msg_rx *raw,
 819                                                       struct drm_dp_sideband_msg_reply_body *repmsg)
 820 {
 821         int idx = 1;
 822         repmsg->u.remote_dpcd_write_ack.port_number = raw->msg[idx] & 0xf;
 823         idx++;
 824         if (idx > raw->curlen)
 825                 goto fail_len;
 826         return true;
 827 fail_len:
 828         DRM_DEBUG_KMS("parse length fail %d %d\n", idx, raw->curlen);
 829         return false;
 830 }
 831
 832 static bool drm_dp_sideband_parse_remote_i2c_read_ack(struct drm_dp_sideband_msg_rx *raw,
 833                                                       struct drm_dp_sideband_msg_reply_body *repmsg)
 834 {
 835         int idx = 1;
 836
 837         repmsg->u.remote_i2c_read_ack.port_number = (raw->msg[idx] & 0xf);
 838         idx++;
 839         if (idx > raw->curlen)
 840                 goto fail_len;
 841         repmsg->u.remote_i2c_read_ack.num_bytes = raw->msg[idx];
 842         idx++;
 843         /* TODO check */
 844         memcpy(repmsg->u.remote_i2c_read_ack.bytes, &raw->msg[idx], repmsg->u.remote_i2c_read_ack.num_bytes);
 845         return true;
 846 fail_len:
 847         DRM_DEBUG_KMS("remote i2c reply parse length fail %d %d\n", idx, raw->curlen);
 848         return false;
 849 }
 850
 851 static bool drm_dp_sideband_parse_enum_path_resources_ack(struct drm_dp_sideband_msg_rx *raw,
 852                                                           struct drm_dp_sideband_msg_reply_body *repmsg)
 853 {
 854         int idx = 1;
 855         repmsg->u.path_resources.port_number = (raw->msg[idx] >> 4) & 0xf;
 856         idx++;
 857         if (idx > raw->curlen)
 858                 goto fail_len;
 859         repmsg->u.path_resources.full_payload_bw_number = (raw->msg[idx] << 8) | (raw->msg[idx+1]);
 860         idx += 2;
 861         if (idx > raw->curlen)
 862                 goto fail_len;
 863         repmsg->u.path_resources.avail_payload_bw_number = (raw->msg[idx] << 8) | (raw->msg[idx+1]);
 864         idx += 2;
 865         if (idx > raw->curlen)
 866                 goto fail_len;
 867         return true;
 868 fail_len:
 869         DRM_DEBUG_KMS("enum resource parse length fail %d %d\n", idx, raw->curlen);
 870         return false;
 871 }
 872
 873 static bool drm_dp_sideband_parse_allocate_payload_ack(struct drm_dp_sideband_msg_rx *raw,
 874                                                           struct drm_dp_sideband_msg_reply_body *repmsg)
 875 {
 876         int idx = 1;
 877         repmsg->u.allocate_payload.port_number = (raw->msg[idx] >> 4) & 0xf;
 878         idx++;
 879         if (idx > raw->curlen)
 880                 goto fail_len;
 881         repmsg->u.allocate_payload.vcpi = raw->msg[idx];
 882         idx++;
 883         if (idx > raw->curlen)
 884                 goto fail_len;
 885         repmsg->u.allocate_payload.allocated_pbn = (raw->msg[idx] << 8) | (raw->msg[idx+1]);
 886         idx += 2;
 887         if (idx > raw->curlen)
 888                 goto fail_len;
 889         return true;
 890 fail_len:
 891         DRM_DEBUG_KMS("allocate payload parse length fail %d %d\n", idx, raw->curlen);
 892         return false;
 893 }
 894
 895 static bool drm_dp_sideband_parse_query_payload_ack(struct drm_dp_sideband_msg_rx *raw,
 896                                                     struct drm_dp_sideband_msg_reply_body *repmsg)
 897 {
 898         int idx = 1;
 899         repmsg->u.query_payload.port_number = (raw->msg[idx] >> 4) & 0xf;
 900         idx++;
 901         if (idx > raw->curlen)
 902                 goto fail_len;
 903         repmsg->u.query_payload.allocated_pbn = (raw->msg[idx] << 8) | (raw->msg[idx + 1]);
 904         idx += 2;
 905         if (idx > raw->curlen)
 906                 goto fail_len;
 907         return true;
 908 fail_len:
 909         DRM_DEBUG_KMS("query payload parse length fail %d %d\n", idx, raw->curlen);
 910         return false;
 911 }
 912
 913 static bool drm_dp_sideband_parse_power_updown_phy_ack(struct drm_dp_sideband_msg_rx *raw,
 914                                                        struct drm_dp_sideband_msg_reply_body *repmsg)
 915 {
 916         int idx = 1;
 917
 918         repmsg->u.port_number.port_number = (raw->msg[idx] >> 4) & 0xf;
 919         idx++;
 920         if (idx > raw->curlen) {
 921                 DRM_DEBUG_KMS("power up/down phy parse length fail %d %d\n",
 922                               idx, raw->curlen);
 923                 return false;
 924         }
 925         return true;
 926 }
 927
 928 static bool drm_dp_sideband_parse_reply(struct drm_dp_sideband_msg_rx *raw,
 929                                         struct drm_dp_sideband_msg_reply_body *msg)
 930 {
 931         memset(msg, 0, sizeof(*msg));
 932         msg->reply_type = (raw->msg[0] & 0x80) >> 7;
 933         msg->req_type = (raw->msg[0] & 0x7f);
 934
 935         if (msg->reply_type == DP_SIDEBAND_REPLY_NAK) {
 936                 memcpy(msg->u.nak.guid, &raw->msg[1], 16);
 937                 msg->u.nak.reason = raw->msg[17];
 938                 msg->u.nak.nak_data = raw->msg[18];
 939                 return false;
 940         }
 941
 942         switch (msg->req_type) {
 943         case DP_LINK_ADDRESS:
 944                 return drm_dp_sideband_parse_link_address(raw, msg);
 945         case DP_QUERY_PAYLOAD:
 946                 return drm_dp_sideband_parse_query_payload_ack(raw, msg);
 947         case DP_REMOTE_DPCD_READ:
 948                 return drm_dp_sideband_parse_remote_dpcd_read(raw, msg);
 949         case DP_REMOTE_DPCD_WRITE:
 950                 return drm_dp_sideband_parse_remote_dpcd_write(raw, msg);
 951         case DP_REMOTE_I2C_READ:
 952                 return drm_dp_sideband_parse_remote_i2c_read_ack(raw, msg);
 953         case DP_ENUM_PATH_RESOURCES:
 954                 return drm_dp_sideband_parse_enum_path_resources_ack(raw, msg);
 955         case DP_ALLOCATE_PAYLOAD:
 956                 return drm_dp_sideband_parse_allocate_payload_ack(raw, msg);
 957         case DP_POWER_DOWN_PHY:
 958         case DP_POWER_UP_PHY:
 959                 return drm_dp_sideband_parse_power_updown_phy_ack(raw, msg);
 960         case DP_CLEAR_PAYLOAD_ID_TABLE:
 961                 return true; /* since there's nothing to parse */
 962         default:
 963                 DRM_ERROR("Got unknown reply 0x%02x (%s)\n", msg->req_type,
 964                           drm_dp_mst_req_type_str(msg->req_type));
 965                 return false;
 966         }
 967 }
 968
 969 static bool drm_dp_sideband_parse_connection_status_notify(struct drm_dp_sideband_msg_rx *raw,
 970                                                            struct drm_dp_sideband_msg_req_body *msg)
 971 {
 972         int idx = 1;
 973
 974         msg->u.conn_stat.port_number = (raw->msg[idx] & 0xf0) >> 4;
 975         idx++;
 976         if (idx > raw->curlen)
 977                 goto fail_len;
 978
 979         memcpy(msg->u.conn_stat.guid, &raw->msg[idx], 16);
 980         idx += 16;
 981         if (idx > raw->curlen)
 982                 goto fail_len;
 983
 984         msg->u.conn_stat.legacy_device_plug_status = (raw->msg[idx] >> 6) & 0x1;
 985         msg->u.conn_stat.displayport_device_plug_status = (raw->msg[idx] >> 5) & 0x1;
 986         msg->u.conn_stat.message_capability_status = (raw->msg[idx] >> 4) & 0x1;
 987         msg->u.conn_stat.input_port = (raw->msg[idx] >> 3) & 0x1;
 988         msg->u.conn_stat.peer_device_type = (raw->msg[idx] & 0x7);
 989         idx++;
 990         return true;
 991 fail_len:
 992         DRM_DEBUG_KMS("connection status reply parse length fail %d %d\n", idx, raw->curlen);
 993         return false;
 994 }
 995
 996 static bool drm_dp_sideband_parse_resource_status_notify(struct drm_dp_sideband_msg_rx *raw,
 997                                                            struct drm_dp_sideband_msg_req_body *msg)
 998 {
 999         int idx = 1;
1000
1001         msg->u.resource_stat.port_number = (raw->msg[idx] & 0xf0) >> 4;
1002         idx++;
1003         if (idx > raw->curlen)
1004                 goto fail_len;
1005
1006         memcpy(msg->u.resource_stat.guid, &raw->msg[idx], 16);
1007         idx += 16;
1008         if (idx > raw->curlen)
1009                 goto fail_len;
1010
1011         msg->u.resource_stat.available_pbn = (raw->msg[idx] << 8) | (raw->msg[idx + 1]);
1012         idx++;
1013         return true;
1014 fail_len:
1015         DRM_DEBUG_KMS("resource status reply parse length fail %d %d\n", idx, raw->curlen);
1016         return false;
1017 }
1018
1019 static bool drm_dp_sideband_parse_req(struct drm_dp_sideband_msg_rx *raw,
1020                                       struct drm_dp_sideband_msg_req_body *msg)
1021 {
1022         memset(msg, 0, sizeof(*msg));
1023         msg->req_type = (raw->msg[0] & 0x7f);
1024
1025         switch (msg->req_type) {
1026         case DP_CONNECTION_STATUS_NOTIFY:
1027                 return drm_dp_sideband_parse_connection_status_notify(raw, msg);
1028         case DP_RESOURCE_STATUS_NOTIFY:
1029                 return drm_dp_sideband_parse_resource_status_notify(raw, msg);
1030         default:
1031                 DRM_ERROR("Got unknown request 0x%02x (%s)\n", msg->req_type,
1032                           drm_dp_mst_req_type_str(msg->req_type));
1033                 return false;
1034         }
1035 }
1036
1037 static int build_dpcd_write(struct drm_dp_sideband_msg_tx *msg, u8 port_num, u32 offset, u8 num_bytes, u8 *bytes)
1038 {
1039         struct drm_dp_sideband_msg_req_body req;
1040
1041         req.req_type = DP_REMOTE_DPCD_WRITE;
1042         req.u.dpcd_write.port_number = port_num;
1043         req.u.dpcd_write.dpcd_address = offset;
1044         req.u.dpcd_write.num_bytes = num_bytes;
1045         req.u.dpcd_write.bytes = bytes;
1046         drm_dp_encode_sideband_req(&req, msg);
1047
1048         return 0;
1049 }
1050
1051 static int build_link_address(struct drm_dp_sideband_msg_tx *msg)
1052 {
1053         struct drm_dp_sideband_msg_req_body req;
1054
1055         req.req_type = DP_LINK_ADDRESS;
1056         drm_dp_encode_sideband_req(&req, msg);
1057         return 0;
1058 }
1059
1060 static int build_clear_payload_id_table(struct drm_dp_sideband_msg_tx *msg)
1061 {
1062         struct drm_dp_sideband_msg_req_body req;
1063
1064         req.req_type = DP_CLEAR_PAYLOAD_ID_TABLE;
1065         drm_dp_encode_sideband_req(&req, msg);
1066         return 0;
1067 }
1068
1069 static int build_enum_path_resources(struct drm_dp_sideband_msg_tx *msg, int port_num)
1070 {
1071         struct drm_dp_sideband_msg_req_body req;
1072
1073         req.req_type = DP_ENUM_PATH_RESOURCES;
1074         req.u.port_num.port_number = port_num;
1075         drm_dp_encode_sideband_req(&req, msg);
1076         msg->path_msg = true;
1077         return 0;
1078 }
1079
1080 static int build_allocate_payload(struct drm_dp_sideband_msg_tx *msg, int port_num,
1081                                   u8 vcpi, uint16_t pbn,
1082                                   u8 number_sdp_streams,
1083                                   u8 *sdp_stream_sink)
1084 {
1085         struct drm_dp_sideband_msg_req_body req;
1086         memset(&req, 0, sizeof(req));
1087         req.req_type = DP_ALLOCATE_PAYLOAD;
1088         req.u.allocate_payload.port_number = port_num;
1089         req.u.allocate_payload.vcpi = vcpi;
1090         req.u.allocate_payload.pbn = pbn;
1091         req.u.allocate_payload.number_sdp_streams = number_sdp_streams;
1092         memcpy(req.u.allocate_payload.sdp_stream_sink, sdp_stream_sink,
1093                    number_sdp_streams);
1094         drm_dp_encode_sideband_req(&req, msg);
1095         msg->path_msg = true;
1096         return 0;
1097 }
1098
1099 static int build_power_updown_phy(struct drm_dp_sideband_msg_tx *msg,
1100                                   int port_num, bool power_up)
1101 {
1102         struct drm_dp_sideband_msg_req_body req;
1103
1104         if (power_up)
1105                 req.req_type = DP_POWER_UP_PHY;
1106         else
1107                 req.req_type = DP_POWER_DOWN_PHY;
1108
1109         req.u.port_num.port_number = port_num;
1110         drm_dp_encode_sideband_req(&req, msg);
1111         msg->path_msg = true;
1112         return 0;
1113 }
1114
1115 static int drm_dp_mst_assign_payload_id(struct drm_dp_mst_topology_mgr *mgr,
1116                                         struct drm_dp_vcpi *vcpi)
1117 {
1118         int ret, vcpi_ret;
1119
1120         mutex_lock(&mgr->payload_lock);
1121         ret = find_first_zero_bit(&mgr->payload_mask, mgr->max_payloads + 1);
1122         if (ret > mgr->max_payloads) {
1123                 ret = -EINVAL;
1124                 DRM_DEBUG_KMS("out of payload ids %d\n", ret);
1125                 goto out_unlock;
1126         }
1127
1128         vcpi_ret = find_first_zero_bit(&mgr->vcpi_mask, mgr->max_payloads + 1);
1129         if (vcpi_ret > mgr->max_payloads) {
1130                 ret = -EINVAL;
1131                 DRM_DEBUG_KMS("out of vcpi ids %d\n", ret);
1132                 goto out_unlock;
1133         }
1134
1135         set_bit(ret, &mgr->payload_mask);
1136         set_bit(vcpi_ret, &mgr->vcpi_mask);
1137         vcpi->vcpi = vcpi_ret + 1;
1138         mgr->proposed_vcpis[ret - 1] = vcpi;
1139 out_unlock:
1140         mutex_unlock(&mgr->payload_lock);
1141         return ret;
1142 }
1143
1144 static void drm_dp_mst_put_payload_id(struct drm_dp_mst_topology_mgr *mgr,
1145                                       int vcpi)
1146 {
1147         int i;
1148         if (vcpi == 0)
1149                 return;
1150
1151         mutex_lock(&mgr->payload_lock);
1152         DRM_DEBUG_KMS("putting payload %d\n", vcpi);
1153         clear_bit(vcpi - 1, &mgr->vcpi_mask);
1154
1155         for (i = 0; i < mgr->max_payloads; i++) {
1156                 if (mgr->proposed_vcpis[i] &&
1157                     mgr->proposed_vcpis[i]->vcpi == vcpi) {
1158                         mgr->proposed_vcpis[i] = NULL;
1159                         clear_bit(i + 1, &mgr->payload_mask);
1160                 }
1161         }
1162         mutex_unlock(&mgr->payload_lock);
1163 }
1164
1165 static bool check_txmsg_state(struct drm_dp_mst_topology_mgr *mgr,
1166                               struct drm_dp_sideband_msg_tx *txmsg)
1167 {
1168         unsigned int state;
1169
1170         /*
1171          * All updates to txmsg->state are protected by mgr->qlock, and the two
1172          * cases we check here are terminal states. For those the barriers
1173          * provided by the wake_up/wait_event pair are enough.
1174          */
1175         state = READ_ONCE(txmsg->state);
1176         return (state == DRM_DP_SIDEBAND_TX_RX ||
1177                 state == DRM_DP_SIDEBAND_TX_TIMEOUT);
1178 }
1179
1180 static int drm_dp_mst_wait_tx_reply(struct drm_dp_mst_branch *mstb,
1181                                     struct drm_dp_sideband_msg_tx *txmsg)
1182 {
1183         struct drm_dp_mst_topology_mgr *mgr = mstb->mgr;
1184         int ret;
1185
1186         ret = wait_event_timeout(mgr->tx_waitq,
1187                                  check_txmsg_state(mgr, txmsg),
1188                                  (4 * HZ));
1189         mutex_lock(&mstb->mgr->qlock);
1190         if (ret > 0) {
1191                 if (txmsg->state == DRM_DP_SIDEBAND_TX_TIMEOUT) {
1192                         ret = -EIO;
1193                         goto out;
1194                 }
1195         } else {
1196                 DRM_DEBUG_KMS("timedout msg send %p %d %d\n", txmsg, txmsg->state, txmsg->seqno);
1197
1198                 /* dump some state */
1199                 ret = -EIO;
1200
1201                 /* remove from q */
1202                 if (txmsg->state == DRM_DP_SIDEBAND_TX_QUEUED ||
1203                     txmsg->state == DRM_DP_SIDEBAND_TX_START_SEND) {
1204                         list_del(&txmsg->next);
1205                 }
1206
1207                 if (txmsg->state == DRM_DP_SIDEBAND_TX_START_SEND ||
1208                     txmsg->state == DRM_DP_SIDEBAND_TX_SENT) {
1209                         mstb->tx_slots[txmsg->seqno] = NULL;
1210                 }
1211         }
1212 out:
1213         if (unlikely(ret == -EIO) && drm_debug_enabled(DRM_UT_DP)) {
1214                 struct drm_printer p = drm_debug_printer(DBG_PREFIX);
1215
1216                 drm_dp_mst_dump_sideband_msg_tx(&p, txmsg);
1217         }
1218         mutex_unlock(&mgr->qlock);
1219
1220         return ret;
1221 }
1222
1223 static struct drm_dp_mst_branch *drm_dp_add_mst_branch_device(u8 lct, u8 *rad)
1224 {
1225         struct drm_dp_mst_branch *mstb;
1226
1227         mstb = kzalloc(sizeof(*mstb), GFP_KERNEL);
1228         if (!mstb)
1229                 return NULL;
1230
1231         mstb->lct = lct;
1232         if (lct > 1)
1233                 memcpy(mstb->rad, rad, lct / 2);
1234         INIT_LIST_HEAD(&mstb->ports);
1235         kref_init(&mstb->topology_kref);
1236         kref_init(&mstb->malloc_kref);
1237         return mstb;
1238 }
1239
1240 static void drm_dp_free_mst_branch_device(struct kref *kref)
1241 {
1242         struct drm_dp_mst_branch *mstb =
1243                 container_of(kref, struct drm_dp_mst_branch, malloc_kref);
1244
1245         if (mstb->port_parent)
1246                 drm_dp_mst_put_port_malloc(mstb->port_parent);
1247
1248         kfree(mstb);
1249 }
1250
1251 /**
1252  * DOC: Branch device and port refcounting
1253  *
1254  * Topology refcount overview
1255  * ~~~~~~~~~~~~~~~~~~~~~~~~~~
1256  *
1257  * The refcounting schemes for &struct drm_dp_mst_branch and &struct
1258  * drm_dp_mst_port are somewhat unusual. Both ports and branch devices have
1259  * two different kinds of refcounts: topology refcounts, and malloc refcounts.
1260  *
1261  * Topology refcounts are not exposed to drivers, and are handled internally
1262  * by the DP MST helpers. The helpers use them in order to prevent the
1263  * in-memory topology state from being changed in the middle of critical
1264  * operations like changing the internal state of payload allocations. This
1265  * means each branch and port will be considered to be connected to the rest
1266  * of the topology until its topology refcount reaches zero. Additionally,
1267  * for ports this means that their associated &struct drm_connector will stay
1268  * registered with userspace until the port's refcount reaches 0.
1269  *
1270  * Malloc refcount overview
1271  * ~~~~~~~~~~~~~~~~~~~~~~~~
1272  *
1273  * Malloc references are used to keep a &struct drm_dp_mst_port or &struct
1274  * drm_dp_mst_branch allocated even after all of its topology references have
1275  * been dropped, so that the driver or MST helpers can safely access each
1276  * branch's last known state before it was disconnected from the topology.
1277  * When the malloc refcount of a port or branch reaches 0, the memory
1278  * allocation containing the &struct drm_dp_mst_branch or &struct
1279  * drm_dp_mst_port respectively will be freed.
1280  *
1281  * For &struct drm_dp_mst_branch, malloc refcounts are not currently exposed
1282  * to drivers. As of writing this documentation, there are no drivers that
1283  * have a usecase for accessing &struct drm_dp_mst_branch outside of the MST
1284  * helpers. Exposing this API to drivers in a race-free manner would take more
1285  * tweaking of the refcounting scheme, however patches are welcome provided
1286  * there is a legitimate driver usecase for this.
1287  *
1288  * Refcount relationships in a topology
1289  * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
1290  *
1291  * Let's take a look at why the relationship between topology and malloc
1292  * refcounts is designed the way it is.
1293  *
1294  * .. kernel-figure:: dp-mst/topology-figure-1.dot
1295  *
1296  *    An example of topology and malloc refs in a DP MST topology with two
1297  *    active payloads. Topology refcount increments are indicated by solid
1298  *    lines, and malloc refcount increments are indicated by dashed lines.
1299  *    Each starts from the branch which incremented the refcount, and ends at
1300  *    the branch to which the refcount belongs to, i.e. the arrow points the
1301  *    same way as the C pointers used to reference a structure.
1302  *
1303  * As you can see in the above figure, every branch increments the topology
1304  * refcount of its children, and increments the malloc refcount of its
1305  * parent. Additionally, every payload increments the malloc refcount of its
1306  * assigned port by 1.
1307  *
1308  * So, what would happen if MSTB #3 from the above figure was unplugged from
1309  * the system, but the driver hadn't yet removed payload #2 from port #3? The
1310  * topology would start to look like the figure below.
1311  *
1312  * .. kernel-figure:: dp-mst/topology-figure-2.dot
1313  *
1314  *    Ports and branch devices which have been released from memory are
1315  *    colored grey, and references which have been removed are colored red.
1316  *
1317  * Whenever a port or branch device's topology refcount reaches zero, it will
1318  * decrement the topology refcounts of all its children, the malloc refcount
1319  * of its parent, and finally its own malloc refcount. For MSTB #4 and port
1320  * #4, this means they both have been disconnected from the topology and freed
1321  * from memory. But, because payload #2 is still holding a reference to port
1322  * #3, port #3 is removed from the topology but its &struct drm_dp_mst_port
1323  * is still accessible from memory. This also means port #3 has not yet
1324  * decremented the malloc refcount of MSTB #3, so its &struct
1325  * drm_dp_mst_branch will also stay allocated in memory until port #3's
1326  * malloc refcount reaches 0.
1327  *
1328  * This relationship is necessary because in order to release payload #2, we
1329  * need to be able to figure out the last relative of port #3 that's still
1330  * connected to the topology. In this case, we would travel up the topology as
1331  * shown below.
1332  *
1333  * .. kernel-figure:: dp-mst/topology-figure-3.dot
1334  *
1335  * And finally, remove payload #2 by communicating with port #2 through
1336  * sideband transactions.
1337  */
1338
1339 /**
1340  * drm_dp_mst_get_mstb_malloc() - Increment the malloc refcount of a branch
1341  * device
1342  * @mstb: The &struct drm_dp_mst_branch to increment the malloc refcount of
1343  *
1344  * Increments &drm_dp_mst_branch.malloc_kref. When
1345  * &drm_dp_mst_branch.malloc_kref reaches 0, the memory allocation for @mstb
1346  * will be released and @mstb may no longer be used.
1347  *
1348  * See also: drm_dp_mst_put_mstb_malloc()
1349  */
1350 static void
1351 drm_dp_mst_get_mstb_malloc(struct drm_dp_mst_branch *mstb)
1352 {
1353         kref_get(&mstb->malloc_kref);
1354         DRM_DEBUG("mstb %p (%d)\n", mstb, kref_read(&mstb->malloc_kref));
1355 }
1356
1357 /**
1358  * drm_dp_mst_put_mstb_malloc() - Decrement the malloc refcount of a branch
1359  * device
1360  * @mstb: The &struct drm_dp_mst_branch to decrement the malloc refcount of
1361  *
1362  * Decrements &drm_dp_mst_branch.malloc_kref. When
1363  * &drm_dp_mst_branch.malloc_kref reaches 0, the memory allocation for @mstb
1364  * will be released and @mstb may no longer be used.
1365  *
1366  * See also: drm_dp_mst_get_mstb_malloc()
1367  */
1368 static void
1369 drm_dp_mst_put_mstb_malloc(struct drm_dp_mst_branch *mstb)
1370 {
1371         DRM_DEBUG("mstb %p (%d)\n", mstb, kref_read(&mstb->malloc_kref) - 1);
1372         kref_put(&mstb->malloc_kref, drm_dp_free_mst_branch_device);
1373 }
1374
1375 static void drm_dp_free_mst_port(struct kref *kref)
1376 {
1377         struct drm_dp_mst_port *port =
1378                 container_of(kref, struct drm_dp_mst_port, malloc_kref);
1379
1380         drm_dp_mst_put_mstb_malloc(port->parent);
1381         kfree(port);
1382 }
1383
1384 /**
1385  * drm_dp_mst_get_port_malloc() - Increment the malloc refcount of an MST port
1386  * @port: The &struct drm_dp_mst_port to increment the malloc refcount of
1387  *
1388  * Increments &drm_dp_mst_port.malloc_kref. When &drm_dp_mst_port.malloc_kref
1389  * reaches 0, the memory allocation for @port will be released and @port may
1390  * no longer be used.
1391  *
1392  * Because @port could potentially be freed at any time by the DP MST helpers
1393  * if &drm_dp_mst_port.malloc_kref reaches 0, including during a call to this
1394  * function, drivers that which to make use of &struct drm_dp_mst_port should
1395  * ensure that they grab at least one main malloc reference to their MST ports
1396  * in &drm_dp_mst_topology_cbs.add_connector. This callback is called before
1397  * there is any chance for &drm_dp_mst_port.malloc_kref to reach 0.
1398  *
1399  * See also: drm_dp_mst_put_port_malloc()
1400  */
1401 void
1402 drm_dp_mst_get_port_malloc(struct drm_dp_mst_port *port)
1403 {
1404         kref_get(&port->malloc_kref);
1405         DRM_DEBUG("port %p (%d)\n", port, kref_read(&port->malloc_kref));
1406 }
1407 EXPORT_SYMBOL(drm_dp_mst_get_port_malloc);
1408
1409 /**
1410  * drm_dp_mst_put_port_malloc() - Decrement the malloc refcount of an MST port
1411  * @port: The &struct drm_dp_mst_port to decrement the malloc refcount of
1412  *
1413  * Decrements &drm_dp_mst_port.malloc_kref. When &drm_dp_mst_port.malloc_kref
1414  * reaches 0, the memory allocation for @port will be released and @port may
1415  * no longer be used.
1416  *
1417  * See also: drm_dp_mst_get_port_malloc()
1418  */
1419 void
1420 drm_dp_mst_put_port_malloc(struct drm_dp_mst_port *port)
1421 {
1422         DRM_DEBUG("port %p (%d)\n", port, kref_read(&port->malloc_kref) - 1);
1423         kref_put(&port->malloc_kref, drm_dp_free_mst_port);
1424 }
1425 EXPORT_SYMBOL(drm_dp_mst_put_port_malloc);
1426
1427 #if IS_ENABLED(CONFIG_DRM_DEBUG_DP_MST_TOPOLOGY_REFS)
1428
1429 #define STACK_DEPTH 8
1430
1431 static noinline void
1432 __topology_ref_save(struct drm_dp_mst_topology_mgr *mgr,
1433                     struct drm_dp_mst_topology_ref_history *history,
1434                     enum drm_dp_mst_topology_ref_type type)
1435 {
1436         struct drm_dp_mst_topology_ref_entry *entry = NULL;
1437         depot_stack_handle_t backtrace;
1438         ulong stack_entries[STACK_DEPTH];
1439         uint n;
1440         int i;
1441
1442         n = stack_trace_save(stack_entries, ARRAY_SIZE(stack_entries), 1);
1443         backtrace = stack_depot_save(stack_entries, n, GFP_KERNEL);
1444         if (!backtrace)
1445                 return;
1446
1447         /* Try to find an existing entry for this backtrace */
1448         for (i = 0; i < history->len; i++) {
1449                 if (history->entries[i].backtrace == backtrace) {
1450                         entry = &history->entries[i];
1451                         break;
1452                 }
1453         }
1454
1455         /* Otherwise add one */
1456         if (!entry) {
1457                 struct drm_dp_mst_topology_ref_entry *new;
1458                 int new_len = history->len + 1;
1459
1460                 new = krealloc(history->entries, sizeof(*new) * new_len,
1461                                GFP_KERNEL);
1462                 if (!new)
1463                         return;
1464
1465                 entry = &new[history->len];
1466                 history->len = new_len;
1467                 history->entries = new;
1468
1469                 entry->backtrace = backtrace;
1470                 entry->type = type;
1471                 entry->count = 0;
1472         }
1473         entry->count++;
1474         entry->ts_nsec = ktime_get_ns();
1475 }
1476
1477 static int
1478 topology_ref_history_cmp(const void *a, const void *b)
1479 {
1480         const struct drm_dp_mst_topology_ref_entry *entry_a = a, *entry_b = b;
1481
1482         if (entry_a->ts_nsec > entry_b->ts_nsec)
1483                 return 1;
1484         else if (entry_a->ts_nsec < entry_b->ts_nsec)
1485                 return -1;
1486         else
1487                 return 0;
1488 }
1489
1490 static inline const char *
1491 topology_ref_type_to_str(enum drm_dp_mst_topology_ref_type type)
1492 {
1493         if (type == DRM_DP_MST_TOPOLOGY_REF_GET)
1494                 return "get";
1495         else
1496                 return "put";
1497 }
1498
1499 static void
1500 __dump_topology_ref_history(struct drm_dp_mst_topology_ref_history *history,
1501                             void *ptr, const char *type_str)
1502 {
1503         struct drm_printer p = drm_debug_printer(DBG_PREFIX);
1504         char *buf = kzalloc(PAGE_SIZE, GFP_KERNEL);
1505         int i;
1506
1507         if (!buf)
1508                 return;
1509
1510         if (!history->len)
1511                 goto out;
1512
1513         /* First, sort the list so that it goes from oldest to newest
1514          * reference entry
1515          */
1516         sort(history->entries, history->len, sizeof(*history->entries),
1517              topology_ref_history_cmp, NULL);
1518
1519         drm_printf(&p, "%s (%p) topology count reached 0, dumping history:\n",
1520                    type_str, ptr);
1521
1522         for (i = 0; i < history->len; i++) {
1523                 const struct drm_dp_mst_topology_ref_entry *entry =
1524                         &history->entries[i];
1525                 ulong *entries;
1526                 uint nr_entries;
1527                 u64 ts_nsec = entry->ts_nsec;
1528                 u32 rem_nsec = do_div(ts_nsec, 1000000000);
1529
1530                 nr_entries = stack_depot_fetch(entry->backtrace, &entries);
1531                 stack_trace_snprint(buf, PAGE_SIZE, entries, nr_entries, 4);
1532
1533                 drm_printf(&p, "  %d %ss (last at %5llu.%06u):\n%s",
1534                            entry->count,
1535                            topology_ref_type_to_str(entry->type),
1536                            ts_nsec, rem_nsec / 1000, buf);
1537         }
1538
1539         /* Now free the history, since this is the only time we expose it */
1540         kfree(history->entries);
1541 out:
1542         kfree(buf);
1543 }
1544
1545 static __always_inline void
1546 drm_dp_mst_dump_mstb_topology_history(struct drm_dp_mst_branch *mstb)
1547 {
1548         __dump_topology_ref_history(&mstb->topology_ref_history, mstb,
1549                                     "MSTB");
1550 }
1551
1552 static __always_inline void
1553 drm_dp_mst_dump_port_topology_history(struct drm_dp_mst_port *port)
1554 {
1555         __dump_topology_ref_history(&port->topology_ref_history, port,
1556                                     "Port");
1557 }
1558
1559 static __always_inline void
1560 save_mstb_topology_ref(struct drm_dp_mst_branch *mstb,
1561                        enum drm_dp_mst_topology_ref_type type)
1562 {
1563         __topology_ref_save(mstb->mgr, &mstb->topology_ref_history, type);
1564 }
1565
1566 static __always_inline void
1567 save_port_topology_ref(struct drm_dp_mst_port *port,
1568                        enum drm_dp_mst_topology_ref_type type)
1569 {
1570         __topology_ref_save(port->mgr, &port->topology_ref_history, type);
1571 }
1572
1573 static inline void
1574 topology_ref_history_lock(struct drm_dp_mst_topology_mgr *mgr)
1575 {
1576         mutex_lock(&mgr->topology_ref_history_lock);
1577 }
1578
1579 static inline void
1580 topology_ref_history_unlock(struct drm_dp_mst_topology_mgr *mgr)
1581 {
1582         mutex_unlock(&mgr->topology_ref_history_lock);
1583 }
1584 #else
1585 static inline void
1586 topology_ref_history_lock(struct drm_dp_mst_topology_mgr *mgr) {}
1587 static inline void
1588 topology_ref_history_unlock(struct drm_dp_mst_topology_mgr *mgr) {}
1589 static inline void
1590 drm_dp_mst_dump_mstb_topology_history(struct drm_dp_mst_branch *mstb) {}
1591 static inline void
1592 drm_dp_mst_dump_port_topology_history(struct drm_dp_mst_port *port) {}
1593 #define save_mstb_topology_ref(mstb, type)
1594 #define save_port_topology_ref(port, type)
1595 #endif
1596
1597 static void drm_dp_destroy_mst_branch_device(struct kref *kref)
1598 {
1599         struct drm_dp_mst_branch *mstb =
1600                 container_of(kref, struct drm_dp_mst_branch, topology_kref);
1601         struct drm_dp_mst_topology_mgr *mgr = mstb->mgr;
1602
1603         drm_dp_mst_dump_mstb_topology_history(mstb);
1604
1605         INIT_LIST_HEAD(&mstb->destroy_next);
1606
1607         /*
1608          * This can get called under mgr->mutex, so we need to perform the
1609          * actual destruction of the mstb in another worker
1610          */
1611         mutex_lock(&mgr->delayed_destroy_lock);
1612         list_add(&mstb->destroy_next, &mgr->destroy_branch_device_list);
1613         mutex_unlock(&mgr->delayed_destroy_lock);
1614         schedule_work(&mgr->delayed_destroy_work);
1615 }
1616
1617 /**
1618  * drm_dp_mst_topology_try_get_mstb() - Increment the topology refcount of a
1619  * branch device unless it's zero
1620  * @mstb: &struct drm_dp_mst_branch to increment the topology refcount of
1621  *
1622  * Attempts to grab a topology reference to @mstb, if it hasn't yet been
1623  * removed from the topology (e.g. &drm_dp_mst_branch.topology_kref has
1624  * reached 0). Holding a topology reference implies that a malloc reference
1625  * will be held to @mstb as long as the user holds the topology reference.
1626  *
1627  * Care should be taken to ensure that the user has at least one malloc
1628  * reference to @mstb. If you already have a topology reference to @mstb, you
1629  * should use drm_dp_mst_topology_get_mstb() instead.
1630  *
1631  * See also:
1632  * drm_dp_mst_topology_get_mstb()
1633  * drm_dp_mst_topology_put_mstb()
1634  *
1635  * Returns:
1636  * * 1: A topology reference was grabbed successfully
1637  * * 0: @port is no longer in the topology, no reference was grabbed
1638  */
1639 static int __must_check
1640 drm_dp_mst_topology_try_get_mstb(struct drm_dp_mst_branch *mstb)
1641 {
1642         int ret;
1643
1644         topology_ref_history_lock(mstb->mgr);
1645         ret = kref_get_unless_zero(&mstb->topology_kref);
1646         if (ret) {
1647                 DRM_DEBUG("mstb %p (%d)\n",
1648                           mstb, kref_read(&mstb->topology_kref));
1649                 save_mstb_topology_ref(mstb, DRM_DP_MST_TOPOLOGY_REF_GET);
1650         }
1651
1652         topology_ref_history_unlock(mstb->mgr);
1653
1654         return ret;
1655 }
1656
1657 /**
1658  * drm_dp_mst_topology_get_mstb() - Increment the topology refcount of a
1659  * branch device
1660  * @mstb: The &struct drm_dp_mst_branch to increment the topology refcount of
1661  *
1662  * Increments &drm_dp_mst_branch.topology_refcount without checking whether or
1663  * not it's already reached 0. This is only valid to use in scenarios where
1664  * you are already guaranteed to have at least one active topology reference
1665  * to @mstb. Otherwise, drm_dp_mst_topology_try_get_mstb() must be used.
1666  *
1667  * See also:
1668  * drm_dp_mst_topology_try_get_mstb()
1669  * drm_dp_mst_topology_put_mstb()
1670  */
1671 static void drm_dp_mst_topology_get_mstb(struct drm_dp_mst_branch *mstb)
1672 {
1673         topology_ref_history_lock(mstb->mgr);
1674
1675         save_mstb_topology_ref(mstb, DRM_DP_MST_TOPOLOGY_REF_GET);
1676         WARN_ON(kref_read(&mstb->topology_kref) == 0);
1677         kref_get(&mstb->topology_kref);
1678         DRM_DEBUG("mstb %p (%d)\n", mstb, kref_read(&mstb->topology_kref));
1679
1680         topology_ref_history_unlock(mstb->mgr);
1681 }
1682
1683 /**
1684  * drm_dp_mst_topology_put_mstb() - release a topology reference to a branch
1685  * device
1686  * @mstb: The &struct drm_dp_mst_branch to release the topology reference from
1687  *
1688  * Releases a topology reference from @mstb by decrementing
1689  * &drm_dp_mst_branch.topology_kref.
1690  *
1691  * See also:
1692  * drm_dp_mst_topology_try_get_mstb()
1693  * drm_dp_mst_topology_get_mstb()
1694  */
1695 static void
1696 drm_dp_mst_topology_put_mstb(struct drm_dp_mst_branch *mstb)
1697 {
1698         topology_ref_history_lock(mstb->mgr);
1699
1700         DRM_DEBUG("mstb %p (%d)\n",
1701                   mstb, kref_read(&mstb->topology_kref) - 1);
1702         save_mstb_topology_ref(mstb, DRM_DP_MST_TOPOLOGY_REF_PUT);
1703
1704         topology_ref_history_unlock(mstb->mgr);
1705         kref_put(&mstb->topology_kref, drm_dp_destroy_mst_branch_device);
1706 }
1707
1708 static void drm_dp_destroy_port(struct kref *kref)
1709 {
1710         struct drm_dp_mst_port *port =
1711                 container_of(kref, struct drm_dp_mst_port, topology_kref);
1712         struct drm_dp_mst_topology_mgr *mgr = port->mgr;
1713
1714         drm_dp_mst_dump_port_topology_history(port);
1715
1716         /* There's nothing that needs locking to destroy an input port yet */
1717         if (port->input) {
1718                 drm_dp_mst_put_port_malloc(port);
1719                 return;
1720         }
1721
1722         kfree(port->cached_edid);
1723
1724         /*
1725          * we can't destroy the connector here, as we might be holding the
1726          * mode_config.mutex from an EDID retrieval
1727          */
1728         mutex_lock(&mgr->delayed_destroy_lock);
1729         list_add(&port->next, &mgr->destroy_port_list);
1730         mutex_unlock(&mgr->delayed_destroy_lock);
1731         schedule_work(&mgr->delayed_destroy_work);
1732 }
1733
1734 /**
1735  * drm_dp_mst_topology_try_get_port() - Increment the topology refcount of a
1736  * port unless it's zero
1737  * @port: &struct drm_dp_mst_port to increment the topology refcount of
1738  *
1739  * Attempts to grab a topology reference to @port, if it hasn't yet been
1740  * removed from the topology (e.g. &drm_dp_mst_port.topology_kref has reached
1741  * 0). Holding a topology reference implies that a malloc reference will be
1742  * held to @port as long as the user holds the topology reference.
1743  *
1744  * Care should be taken to ensure that the user has at least one malloc
1745  * reference to @port. If you already have a topology reference to @port, you
1746  * should use drm_dp_mst_topology_get_port() instead.
1747  *
1748  * See also:
1749  * drm_dp_mst_topology_get_port()
1750  * drm_dp_mst_topology_put_port()
1751  *
1752  * Returns:
1753  * * 1: A topology reference was grabbed successfully
1754  * * 0: @port is no longer in the topology, no reference was grabbed
1755  */
1756 static int __must_check
1757 drm_dp_mst_topology_try_get_port(struct drm_dp_mst_port *port)
1758 {
1759         int ret;
1760
1761         topology_ref_history_lock(port->mgr);
1762         ret = kref_get_unless_zero(&port->topology_kref);
1763         if (ret) {
1764                 DRM_DEBUG("port %p (%d)\n",
1765                           port, kref_read(&port->topology_kref));
1766                 save_port_topology_ref(port, DRM_DP_MST_TOPOLOGY_REF_GET);
1767         }
1768
1769         topology_ref_history_unlock(port->mgr);
1770         return ret;
1771 }
1772
1773 /**
1774  * drm_dp_mst_topology_get_port() - Increment the topology refcount of a port
1775  * @port: The &struct drm_dp_mst_port to increment the topology refcount of
1776  *
1777  * Increments &drm_dp_mst_port.topology_refcount without checking whether or
1778  * not it's already reached 0. This is only valid to use in scenarios where
1779  * you are already guaranteed to have at least one active topology reference
1780  * to @port. Otherwise, drm_dp_mst_topology_try_get_port() must be used.
1781  *
1782  * See also:
1783  * drm_dp_mst_topology_try_get_port()
1784  * drm_dp_mst_topology_put_port()
1785  */
1786 static void drm_dp_mst_topology_get_port(struct drm_dp_mst_port *port)
1787 {
1788         topology_ref_history_lock(port->mgr);
1789
1790         WARN_ON(kref_read(&port->topology_kref) == 0);
1791         kref_get(&port->topology_kref);
1792         DRM_DEBUG("port %p (%d)\n", port, kref_read(&port->topology_kref));
1793         save_port_topology_ref(port, DRM_DP_MST_TOPOLOGY_REF_GET);
1794
1795         topology_ref_history_unlock(port->mgr);
1796 }
1797
1798 /**
1799  * drm_dp_mst_topology_put_port() - release a topology reference to a port
1800  * @port: The &struct drm_dp_mst_port to release the topology reference from
1801  *
1802  * Releases a topology reference from @port by decrementing
1803  * &drm_dp_mst_port.topology_kref.
1804  *
1805  * See also:
1806  * drm_dp_mst_topology_try_get_port()
1807  * drm_dp_mst_topology_get_port()
1808  */
1809 static void drm_dp_mst_topology_put_port(struct drm_dp_mst_port *port)
1810 {
1811         topology_ref_history_lock(port->mgr);
1812
1813         DRM_DEBUG("port %p (%d)\n",
1814                   port, kref_read(&port->topology_kref) - 1);
1815         save_port_topology_ref(port, DRM_DP_MST_TOPOLOGY_REF_PUT);
1816
1817         topology_ref_history_unlock(port->mgr);
1818         kref_put(&port->topology_kref, drm_dp_destroy_port);
1819 }
1820
1821 static struct drm_dp_mst_branch *
1822 drm_dp_mst_topology_get_mstb_validated_locked(struct drm_dp_mst_branch *mstb,
1823                                               struct drm_dp_mst_branch *to_find)
1824 {
1825         struct drm_dp_mst_port *port;
1826         struct drm_dp_mst_branch *rmstb;
1827
1828         if (to_find == mstb)
1829                 return mstb;
1830
1831         list_for_each_entry(port, &mstb->ports, next) {
1832                 if (port->mstb) {
1833                         rmstb = drm_dp_mst_topology_get_mstb_validated_locked(
1834                             port->mstb, to_find);
1835                         if (rmstb)
1836                                 return rmstb;
1837                 }
1838         }
1839         return NULL;
1840 }
1841
1842 static struct drm_dp_mst_branch *
1843 drm_dp_mst_topology_get_mstb_validated(struct drm_dp_mst_topology_mgr *mgr,
1844                                        struct drm_dp_mst_branch *mstb)
1845 {
1846         struct drm_dp_mst_branch *rmstb = NULL;
1847
1848         mutex_lock(&mgr->lock);
1849         if (mgr->mst_primary) {
1850                 rmstb = drm_dp_mst_topology_get_mstb_validated_locked(
1851                     mgr->mst_primary, mstb);
1852
1853                 if (rmstb && !drm_dp_mst_topology_try_get_mstb(rmstb))
1854                         rmstb = NULL;
1855         }
1856         mutex_unlock(&mgr->lock);
1857         return rmstb;
1858 }
1859
1860 static struct drm_dp_mst_port *
1861 drm_dp_mst_topology_get_port_validated_locked(struct drm_dp_mst_branch *mstb,
1862                                               struct drm_dp_mst_port *to_find)
1863 {
1864         struct drm_dp_mst_port *port, *mport;
1865
1866         list_for_each_entry(port, &mstb->ports, next) {
1867                 if (port == to_find)
1868                         return port;
1869
1870                 if (port->mstb) {
1871                         mport = drm_dp_mst_topology_get_port_validated_locked(
1872                             port->mstb, to_find);
1873                         if (mport)
1874                                 return mport;
1875                 }
1876         }
1877         return NULL;
1878 }
1879
1880 static struct drm_dp_mst_port *
1881 drm_dp_mst_topology_get_port_validated(struct drm_dp_mst_topology_mgr *mgr,
1882                                        struct drm_dp_mst_port *port)
1883 {
1884         struct drm_dp_mst_port *rport = NULL;
1885
1886         mutex_lock(&mgr->lock);
1887         if (mgr->mst_primary) {
1888                 rport = drm_dp_mst_topology_get_port_validated_locked(
1889                     mgr->mst_primary, port);
1890
1891                 if (rport && !drm_dp_mst_topology_try_get_port(rport))
1892                         rport = NULL;
1893         }
1894         mutex_unlock(&mgr->lock);
1895         return rport;
1896 }
1897
1898 static struct drm_dp_mst_port *drm_dp_get_port(struct drm_dp_mst_branch *mstb, u8 port_num)
1899 {
1900         struct drm_dp_mst_port *port;
1901         int ret;
1902
1903         list_for_each_entry(port, &mstb->ports, next) {
1904                 if (port->port_num == port_num) {
1905                         ret = drm_dp_mst_topology_try_get_port(port);
1906                         return ret ? port : NULL;
1907                 }
1908         }
1909
1910         return NULL;
1911 }
1912
1913 /*
1914  * calculate a new RAD for this MST branch device
1915  * if parent has an LCT of 2 then it has 1 nibble of RAD,
1916  * if parent has an LCT of 3 then it has 2 nibbles of RAD,
1917  */
1918 static u8 drm_dp_calculate_rad(struct drm_dp_mst_port *port,
1919                                  u8 *rad)
1920 {
1921         int parent_lct = port->parent->lct;
1922         int shift = 4;
1923         int idx = (parent_lct - 1) / 2;
1924         if (parent_lct > 1) {
1925                 memcpy(rad, port->parent->rad, idx + 1);
1926                 shift = (parent_lct % 2) ? 4 : 0;
1927         } else
1928                 rad[0] = 0;
1929
1930         rad[idx] |= port->port_num << shift;
1931         return parent_lct + 1;
1932 }
1933
1934 static int drm_dp_port_set_pdt(struct drm_dp_mst_port *port, u8 new_pdt)
1935 {
1936         struct drm_dp_mst_topology_mgr *mgr = port->mgr;
1937         struct drm_dp_mst_branch *mstb;
1938         u8 rad[8], lct;
1939         int ret = 0;
1940
1941         if (port->pdt == new_pdt)
1942                 return 0;
1943
1944         /* Teardown the old pdt, if there is one */
1945         switch (port->pdt) {
1946         case DP_PEER_DEVICE_DP_LEGACY_CONV:
1947         case DP_PEER_DEVICE_SST_SINK:
1948                 /*
1949                  * If the new PDT would also have an i2c bus, don't bother
1950                  * with reregistering it
1951                  */
1952                 if (new_pdt == DP_PEER_DEVICE_DP_LEGACY_CONV ||
1953                     new_pdt == DP_PEER_DEVICE_SST_SINK) {
1954                         port->pdt = new_pdt;
1955                         return 0;
1956                 }
1957
1958                 /* remove i2c over sideband */
1959                 drm_dp_mst_unregister_i2c_bus(&port->aux);
1960                 break;
1961         case DP_PEER_DEVICE_MST_BRANCHING:
1962                 mutex_lock(&mgr->lock);
1963                 drm_dp_mst_topology_put_mstb(port->mstb);
1964                 port->mstb = NULL;
1965                 mutex_unlock(&mgr->lock);
1966                 break;
1967         }
1968
1969         port->pdt = new_pdt;
1970         switch (port->pdt) {
1971         case DP_PEER_DEVICE_DP_LEGACY_CONV:
1972         case DP_PEER_DEVICE_SST_SINK:
1973                 /* add i2c over sideband */
1974                 ret = drm_dp_mst_register_i2c_bus(&port->aux);
1975                 break;
1976
1977         case DP_PEER_DEVICE_MST_BRANCHING:
1978                 lct = drm_dp_calculate_rad(port, rad);
1979                 mstb = drm_dp_add_mst_branch_device(lct, rad);
1980                 if (!mstb) {
1981                         ret = -ENOMEM;
1982                         DRM_ERROR("Failed to create MSTB for port %p", port);
1983                         goto out;
1984                 }
1985
1986                 mutex_lock(&mgr->lock);
1987                 port->mstb = mstb;
1988                 mstb->mgr = port->mgr;
1989                 mstb->port_parent = port;
1990
1991                 /*
1992                  * Make sure this port's memory allocation stays
1993                  * around until its child MSTB releases it
1994                  */
1995                 drm_dp_mst_get_port_malloc(port);
1996                 mutex_unlock(&mgr->lock);
1997
1998                 /* And make sure we send a link address for this */
1999                 ret = 1;
2000                 break;
2001         }
2002
2003 out:
2004         if (ret < 0)
2005                 port->pdt = DP_PEER_DEVICE_NONE;
2006         return ret;
2007 }
2008
2009 /**
2010  * drm_dp_mst_dpcd_read() - read a series of bytes from the DPCD via sideband
2011  * @aux: Fake sideband AUX CH
2012  * @offset: address of the (first) register to read
2013  * @buffer: buffer to store the register values
2014  * @size: number of bytes in @buffer
2015  *
2016  * Performs the same functionality for remote devices via
2017  * sideband messaging as drm_dp_dpcd_read() does for local
2018  * devices via actual AUX CH.
2019  *
2020  * Return: Number of bytes read, or negative error code on failure.
2021  */
2022 ssize_t drm_dp_mst_dpcd_read(struct drm_dp_aux *aux,
2023                              unsigned int offset, void *buffer, size_t size)
2024 {
2025         struct drm_dp_mst_port *port = container_of(aux, struct drm_dp_mst_port,
2026                                                     aux);
2027
2028         return drm_dp_send_dpcd_read(port->mgr, port,
2029                                      offset, size, buffer);
2030 }
2031
2032 /**
2033  * drm_dp_mst_dpcd_write() - write a series of bytes to the DPCD via sideband
2034  * @aux: Fake sideband AUX CH
2035  * @offset: address of the (first) register to write
2036  * @buffer: buffer containing the values to write
2037  * @size: number of bytes in @buffer
2038  *
2039  * Performs the same functionality for remote devices via
2040  * sideband messaging as drm_dp_dpcd_write() does for local
2041  * devices via actual AUX CH.
2042  *
2043  * Return: 0 on success, negative error code on failure.
2044  */
2045 ssize_t drm_dp_mst_dpcd_write(struct drm_dp_aux *aux,
2046                               unsigned int offset, void *buffer, size_t size)
2047 {
2048         struct drm_dp_mst_port *port = container_of(aux, struct drm_dp_mst_port,
2049                                                     aux);
2050
2051         return drm_dp_send_dpcd_write(port->mgr, port,
2052                                       offset, size, buffer);
2053 }
2054
2055 static void drm_dp_check_mstb_guid(struct drm_dp_mst_branch *mstb, u8 *guid)
2056 {
2057         int ret;
2058
2059         memcpy(mstb->guid, guid, 16);
2060
2061         if (!drm_dp_validate_guid(mstb->mgr, mstb->guid)) {
2062                 if (mstb->port_parent) {
2063                         ret = drm_dp_send_dpcd_write(
2064                                         mstb->mgr,
2065                                         mstb->port_parent,
2066                                         DP_GUID,
2067                                         16,
2068                                         mstb->guid);
2069                 } else {
2070
2071                         ret = drm_dp_dpcd_write(
2072                                         mstb->mgr->aux,
2073                                         DP_GUID,
2074                                         mstb->guid,
2075                                         16);
2076                 }
2077         }
2078 }
2079
2080 static void build_mst_prop_path(const struct drm_dp_mst_branch *mstb,
2081                                 int pnum,
2082                                 char *proppath,
2083                                 size_t proppath_size)
2084 {
2085         int i;
2086         char temp[8];
2087         snprintf(proppath, proppath_size, "mst:%d", mstb->mgr->conn_base_id);
2088         for (i = 0; i < (mstb->lct - 1); i++) {
2089                 int shift = (i % 2) ? 0 : 4;
2090                 int port_num = (mstb->rad[i / 2] >> shift) & 0xf;
2091                 snprintf(temp, sizeof(temp), "-%d", port_num);
2092                 strlcat(proppath, temp, proppath_size);
2093         }
2094         snprintf(temp, sizeof(temp), "-%d", pnum);
2095         strlcat(proppath, temp, proppath_size);
2096 }
2097
2098 /**
2099  * drm_dp_mst_connector_late_register() - Late MST connector registration
2100  * @connector: The MST connector
2101  * @port: The MST port for this connector
2102  *
2103  * Helper to register the remote aux device for this MST port. Drivers should
2104  * call this from their mst connector's late_register hook to enable MST aux
2105  * devices.
2106  *
2107  * Return: 0 on success, negative error code on failure.
2108  */
2109 int drm_dp_mst_connector_late_register(struct drm_connector *connector,
2110                                        struct drm_dp_mst_port *port)
2111 {
2112         DRM_DEBUG_KMS("registering %s remote bus for %s\n",
2113                       port->aux.name, connector->kdev->kobj.name);
2114
2115         port->aux.dev = connector->kdev;
2116         return drm_dp_aux_register_devnode(&port->aux);
2117 }
2118 EXPORT_SYMBOL(drm_dp_mst_connector_late_register);
2119
2120 /**
2121  * drm_dp_mst_connector_early_unregister() - Early MST connector unregistration
2122  * @connector: The MST connector
2123  * @port: The MST port for this connector
2124  *
2125  * Helper to unregister the remote aux device for this MST port, registered by
2126  * drm_dp_mst_connector_late_register(). Drivers should call this from their mst
2127  * connector's early_unregister hook.
2128  */
2129 void drm_dp_mst_connector_early_unregister(struct drm_connector *connector,
2130                                            struct drm_dp_mst_port *port)
2131 {
2132         DRM_DEBUG_KMS("unregistering %s remote bus for %s\n",
2133                       port->aux.name, connector->kdev->kobj.name);
2134         drm_dp_aux_unregister_devnode(&port->aux);
2135 }
2136 EXPORT_SYMBOL(drm_dp_mst_connector_early_unregister);
2137
2138 static void
2139 drm_dp_mst_port_add_connector(struct drm_dp_mst_branch *mstb,
2140                               struct drm_dp_mst_port *port)
2141 {
2142         struct drm_dp_mst_topology_mgr *mgr = port->mgr;
2143         char proppath[255];
2144         int ret;
2145
2146         build_mst_prop_path(mstb, port->port_num, proppath, sizeof(proppath));
2147         port->connector = mgr->cbs->add_connector(mgr, port, proppath);
2148         if (!port->connector) {
2149                 ret = -ENOMEM;
2150                 goto error;
2151         }
2152
2153         if ((port->pdt == DP_PEER_DEVICE_DP_LEGACY_CONV ||
2154              port->pdt == DP_PEER_DEVICE_SST_SINK) &&
2155             port->port_num >= DP_MST_LOGICAL_PORT_0) {
2156                 port->cached_edid = drm_get_edid(port->connector,
2157                                                  &port->aux.ddc);
2158                 drm_connector_set_tile_property(port->connector);
2159         }
2160
2161         mgr->cbs->register_connector(port->connector);
2162         return;
2163
2164 error:
2165         DRM_ERROR("Failed to create connector for port %p: %d\n", port, ret);
2166 }
2167
2168 /*
2169  * Drop a topology reference, and unlink the port from the in-memory topology
2170  * layout
2171  */
2172 static void
2173 drm_dp_mst_topology_unlink_port(struct drm_dp_mst_topology_mgr *mgr,
2174                                 struct drm_dp_mst_port *port)
2175 {
2176         mutex_lock(&mgr->lock);
2177         list_del(&port->next);
2178         mutex_unlock(&mgr->lock);
2179         drm_dp_mst_topology_put_port(port);
2180 }
2181
2182 static struct drm_dp_mst_port *
2183 drm_dp_mst_add_port(struct drm_device *dev,
2184                     struct drm_dp_mst_topology_mgr *mgr,
2185                     struct drm_dp_mst_branch *mstb, u8 port_number)
2186 {
2187         struct drm_dp_mst_port *port = kzalloc(sizeof(*port), GFP_KERNEL);
2188
2189         if (!port)
2190                 return NULL;
2191
2192         kref_init(&port->topology_kref);
2193         kref_init(&port->malloc_kref);
2194         port->parent = mstb;
2195         port->port_num = port_number;
2196         port->mgr = mgr;
2197         port->aux.name = "DPMST";
2198         port->aux.dev = dev->dev;
2199         port->aux.is_remote = true;
2200
2201         /*
2202          * Make sure the memory allocation for our parent branch stays
2203          * around until our own memory allocation is released
2204          */
2205         drm_dp_mst_get_mstb_malloc(mstb);
2206
2207         return port;
2208 }
2209
2210 static int
2211 drm_dp_mst_handle_link_address_port(struct drm_dp_mst_branch *mstb,
2212                                     struct drm_device *dev,
2213                                     struct drm_dp_link_addr_reply_port *port_msg)
2214 {
2215         struct drm_dp_mst_topology_mgr *mgr = mstb->mgr;
2216         struct drm_dp_mst_port *port;
2217         int old_ddps = 0, ret;
2218         u8 new_pdt = DP_PEER_DEVICE_NONE;
2219         bool created = false, send_link_addr = false, changed = false;
2220
2221         port = drm_dp_get_port(mstb, port_msg->port_number);
2222         if (!port) {
2223                 port = drm_dp_mst_add_port(dev, mgr, mstb,
2224                                            port_msg->port_number);
2225                 if (!port)
2226                         return -ENOMEM;
2227                 created = true;
2228                 changed = true;
2229         } else if (!port->input && port_msg->input_port && port->connector) {
2230                 /* Since port->connector can't be changed here, we create a
2231                  * new port if input_port changes from 0 to 1
2232                  */
2233                 drm_dp_mst_topology_unlink_port(mgr, port);
2234                 drm_dp_mst_topology_put_port(port);
2235                 port = drm_dp_mst_add_port(dev, mgr, mstb,
2236                                            port_msg->port_number);
2237                 if (!port)
2238                         return -ENOMEM;
2239                 changed = true;
2240                 created = true;
2241         } else if (port->input && !port_msg->input_port) {
2242                 changed = true;
2243         } else if (port->connector) {
2244                 /* We're updating a port that's exposed to userspace, so do it
2245                  * under lock
2246                  */
2247                 drm_modeset_lock(&mgr->base.lock, NULL);
2248
2249                 old_ddps = port->ddps;
2250                 changed = port->ddps != port_msg->ddps ||
2251                         (port->ddps &&
2252                          (port->ldps != port_msg->legacy_device_plug_status ||
2253                           port->dpcd_rev != port_msg->dpcd_revision ||
2254                           port->mcs != port_msg->mcs ||
2255                           port->pdt != port_msg->peer_device_type ||
2256                           port->num_sdp_stream_sinks !=
2257                           port_msg->num_sdp_stream_sinks));
2258         }
2259
2260         port->input = port_msg->input_port;
2261         if (!port->input)
2262                 new_pdt = port_msg->peer_device_type;
2263         port->mcs = port_msg->mcs;
2264         port->ddps = port_msg->ddps;
2265         port->ldps = port_msg->legacy_device_plug_status;
2266         port->dpcd_rev = port_msg->dpcd_revision;
2267         port->num_sdp_streams = port_msg->num_sdp_streams;
2268         port->num_sdp_stream_sinks = port_msg->num_sdp_stream_sinks;
2269
2270         /* manage mstb port lists with mgr lock - take a reference
2271            for this list */
2272         if (created) {
2273                 mutex_lock(&mgr->lock);
2274                 drm_dp_mst_topology_get_port(port);
2275                 list_add(&port->next, &mstb->ports);
2276                 mutex_unlock(&mgr->lock);
2277         }
2278
2279         if (old_ddps != port->ddps) {
2280                 if (port->ddps) {
2281                         if (!port->input) {
2282                                 drm_dp_send_enum_path_resources(mgr, mstb,
2283                                                                 port);
2284                         }
2285                 } else {
2286                         port->available_pbn = 0;
2287                 }
2288         }
2289
2290         ret = drm_dp_port_set_pdt(port, new_pdt);
2291         if (ret == 1) {
2292                 send_link_addr = true;
2293         } else if (ret < 0) {
2294                 DRM_ERROR("Failed to change PDT on port %p: %d\n",
2295                           port, ret);
2296                 goto fail;
2297         }
2298
2299         /*
2300          * If this port wasn't just created, then we're reprobing because
2301          * we're coming out of suspend. In this case, always resend the link
2302          * address if there's an MSTB on this port
2303          */
2304         if (!created && port->pdt == DP_PEER_DEVICE_MST_BRANCHING)
2305                 send_link_addr = true;
2306
2307         if (port->connector)
2308                 drm_modeset_unlock(&mgr->base.lock);
2309         else if (!port->input)
2310                 drm_dp_mst_port_add_connector(mstb, port);
2311
2312         if (send_link_addr && port->mstb) {
2313                 ret = drm_dp_send_link_address(mgr, port->mstb);
2314                 if (ret == 1) /* MSTB below us changed */
2315                         changed = true;
2316                 else if (ret < 0)
2317                         goto fail_put;
2318         }
2319
2320         /* put reference to this port */
2321         drm_dp_mst_topology_put_port(port);
2322         return changed;
2323
2324 fail:
2325         drm_dp_mst_topology_unlink_port(mgr, port);
2326         if (port->connector)
2327                 drm_modeset_unlock(&mgr->base.lock);
2328 fail_put:
2329         drm_dp_mst_topology_put_port(port);
2330         return ret;
2331 }
2332
2333 static void
2334 drm_dp_mst_handle_conn_stat(struct drm_dp_mst_branch *mstb,
2335                             struct drm_dp_connection_status_notify *conn_stat)
2336 {
2337         struct drm_dp_mst_topology_mgr *mgr = mstb->mgr;
2338         struct drm_dp_mst_port *port;
2339         int old_ddps, ret;
2340         u8 new_pdt;
2341         bool dowork = false, create_connector = false;
2342
2343         port = drm_dp_get_port(mstb, conn_stat->port_number);
2344         if (!port)
2345                 return;
2346
2347         if (port->connector) {
2348                 if (!port->input && conn_stat->input_port) {
2349                         /*
2350                          * We can't remove a connector from an already exposed
2351                          * port, so just throw the port out and make sure we
2352                          * reprobe the link address of it's parent MSTB
2353                          */
2354                         drm_dp_mst_topology_unlink_port(mgr, port);
2355                         mstb->link_address_sent = false;
2356                         dowork = true;
2357                         goto out;
2358                 }
2359
2360                 /* Locking is only needed if the port's exposed to userspace */
2361                 drm_modeset_lock(&mgr->base.lock, NULL);
2362         } else if (port->input && !conn_stat->input_port) {
2363                 create_connector = true;
2364                 /* Reprobe link address so we get num_sdp_streams */
2365                 mstb->link_address_sent = false;
2366                 dowork = true;
2367         }
2368
2369         old_ddps = port->ddps;
2370         port->input = conn_stat->input_port;
2371         port->mcs = conn_stat->message_capability_status;
2372         port->ldps = conn_stat->legacy_device_plug_status;
2373         port->ddps = conn_stat->displayport_device_plug_status;
2374
2375         if (old_ddps != port->ddps) {
2376                 if (port->ddps) {
2377                         dowork = true;
2378                 } else {
2379                         port->available_pbn = 0;
2380                 }
2381         }
2382
2383         new_pdt = port->input ? DP_PEER_DEVICE_NONE : conn_stat->peer_device_type;
2384
2385         ret = drm_dp_port_set_pdt(port, new_pdt);
2386         if (ret == 1) {
2387                 dowork = true;
2388         } else if (ret < 0) {
2389                 DRM_ERROR("Failed to change PDT for port %p: %d\n",
2390                           port, ret);
2391                 dowork = false;
2392         }
2393
2394         if (port->connector)
2395                 drm_modeset_unlock(&mgr->base.lock);
2396         else if (create_connector)
2397                 drm_dp_mst_port_add_connector(mstb, port);
2398
2399 out:
2400         drm_dp_mst_topology_put_port(port);
2401         if (dowork)
2402                 queue_work(system_long_wq, &mstb->mgr->work);
2403 }
2404
2405 static struct drm_dp_mst_branch *drm_dp_get_mst_branch_device(struct drm_dp_mst_topology_mgr *mgr,
2406                                                                u8 lct, u8 *rad)
2407 {
2408         struct drm_dp_mst_branch *mstb;
2409         struct drm_dp_mst_port *port;
2410         int i, ret;
2411         /* find the port by iterating down */
2412
2413         mutex_lock(&mgr->lock);
2414         mstb = mgr->mst_primary;
2415
2416         if (!mstb)
2417                 goto out;
2418
2419         for (i = 0; i < lct - 1; i++) {
2420                 int shift = (i % 2) ? 0 : 4;
2421                 int port_num = (rad[i / 2] >> shift) & 0xf;
2422
2423                 list_for_each_entry(port, &mstb->ports, next) {
2424                         if (port->port_num == port_num) {
2425                                 mstb = port->mstb;
2426                                 if (!mstb) {
2427                                         DRM_ERROR("failed to lookup MSTB with lct %d, rad %02x\n", lct, rad[0]);
2428                                         goto out;
2429                                 }
2430
2431                                 break;
2432                         }
2433                 }
2434         }
2435         ret = drm_dp_mst_topology_try_get_mstb(mstb);
2436         if (!ret)
2437                 mstb = NULL;
2438 out:
2439         mutex_unlock(&mgr->lock);
2440         return mstb;
2441 }
2442
2443 static struct drm_dp_mst_branch *get_mst_branch_device_by_guid_helper(
2444         struct drm_dp_mst_branch *mstb,
2445         const uint8_t *guid)
2446 {
2447         struct drm_dp_mst_branch *found_mstb;
2448         struct drm_dp_mst_port *port;
2449
2450         if (memcmp(mstb->guid, guid, 16) == 0)
2451                 return mstb;
2452
2453
2454         list_for_each_entry(port, &mstb->ports, next) {
2455                 if (!port->mstb)
2456                         continue;
2457
2458                 found_mstb = get_mst_branch_device_by_guid_helper(port->mstb, guid);
2459
2460                 if (found_mstb)
2461                         return found_mstb;
2462         }
2463
2464         return NULL;
2465 }
2466
2467 static struct drm_dp_mst_branch *
2468 drm_dp_get_mst_branch_device_by_guid(struct drm_dp_mst_topology_mgr *mgr,
2469                                      const uint8_t *guid)
2470 {
2471         struct drm_dp_mst_branch *mstb;
2472         int ret;
2473
2474         /* find the port by iterating down */
2475         mutex_lock(&mgr->lock);
2476
2477         mstb = get_mst_branch_device_by_guid_helper(mgr->mst_primary, guid);
2478         if (mstb) {
2479                 ret = drm_dp_mst_topology_try_get_mstb(mstb);
2480                 if (!ret)
2481                         mstb = NULL;
2482         }
2483
2484         mutex_unlock(&mgr->lock);
2485         return mstb;
2486 }
2487
2488 static int drm_dp_check_and_send_link_address(struct drm_dp_mst_topology_mgr *mgr,
2489                                                struct drm_dp_mst_branch *mstb)
2490 {
2491         struct drm_dp_mst_port *port;
2492         int ret;
2493         bool changed = false;
2494
2495         if (!mstb->link_address_sent) {
2496                 ret = drm_dp_send_link_address(mgr, mstb);
2497                 if (ret == 1)
2498                         changed = true;
2499                 else if (ret < 0)
2500                         return ret;
2501         }
2502
2503         list_for_each_entry(port, &mstb->ports, next) {
2504                 struct drm_dp_mst_branch *mstb_child = NULL;
2505
2506                 if (port->input || !port->ddps)
2507                         continue;
2508
2509                 if (!port->available_pbn) {
2510                         drm_modeset_lock(&mgr->base.lock, NULL);
2511                         drm_dp_send_enum_path_resources(mgr, mstb, port);
2512                         drm_modeset_unlock(&mgr->base.lock);
2513                         changed = true;
2514                 }
2515
2516                 if (port->mstb)
2517                         mstb_child = drm_dp_mst_topology_get_mstb_validated(
2518                             mgr, port->mstb);
2519
2520                 if (mstb_child) {
2521                         ret = drm_dp_check_and_send_link_address(mgr,
2522                                                                  mstb_child);
2523                         drm_dp_mst_topology_put_mstb(mstb_child);
2524                         if (ret == 1)
2525                                 changed = true;
2526                         else if (ret < 0)
2527                                 return ret;
2528                 }
2529         }
2530
2531         return changed;
2532 }
2533
2534 static void drm_dp_mst_link_probe_work(struct work_struct *work)
2535 {
2536         struct drm_dp_mst_topology_mgr *mgr =
2537                 container_of(work, struct drm_dp_mst_topology_mgr, work);
2538         struct drm_device *dev = mgr->dev;
2539         struct drm_dp_mst_branch *mstb;
2540         int ret;
2541         bool clear_payload_id_table;
2542
2543         mutex_lock(&mgr->probe_lock);
2544
2545         mutex_lock(&mgr->lock);
2546         clear_payload_id_table = !mgr->payload_id_table_cleared;
2547         mgr->payload_id_table_cleared = true;
2548
2549         mstb = mgr->mst_primary;
2550         if (mstb) {
2551                 ret = drm_dp_mst_topology_try_get_mstb(mstb);
2552                 if (!ret)
2553                         mstb = NULL;
2554         }
2555         mutex_unlock(&mgr->lock);
2556         if (!mstb) {
2557                 mutex_unlock(&mgr->probe_lock);
2558                 return;
2559         }
2560
2561         /*
2562          * Certain branch devices seem to incorrectly report an available_pbn
2563          * of 0 on downstream sinks, even after clearing the
2564          * DP_PAYLOAD_ALLOCATE_* registers in
2565          * drm_dp_mst_topology_mgr_set_mst(). Namely, the CableMatters USB-C
2566          * 2x DP hub. Sending a CLEAR_PAYLOAD_ID_TABLE message seems to make
2567          * things work again.
2568          */
2569         if (clear_payload_id_table) {
2570                 DRM_DEBUG_KMS("Clearing payload ID table\n");
2571                 drm_dp_send_clear_payload_id_table(mgr, mstb);
2572         }
2573
2574         ret = drm_dp_check_and_send_link_address(mgr, mstb);
2575         drm_dp_mst_topology_put_mstb(mstb);
2576
2577         mutex_unlock(&mgr->probe_lock);
2578         if (ret)
2579                 drm_kms_helper_hotplug_event(dev);
2580 }
2581
2582 static bool drm_dp_validate_guid(struct drm_dp_mst_topology_mgr *mgr,
2583                                  u8 *guid)
2584 {
2585         u64 salt;
2586
2587         if (memchr_inv(guid, 0, 16))
2588                 return true;
2589
2590         salt = get_jiffies_64();
2591
2592         memcpy(&guid[0], &salt, sizeof(u64));
2593         memcpy(&guid[8], &salt, sizeof(u64));
2594
2595         return false;
2596 }
2597
2598 static int build_dpcd_read(struct drm_dp_sideband_msg_tx *msg, u8 port_num, u32 offset, u8 num_bytes)
2599 {
2600         struct drm_dp_sideband_msg_req_body req;
2601
2602         req.req_type = DP_REMOTE_DPCD_READ;
2603         req.u.dpcd_read.port_number = port_num;
2604         req.u.dpcd_read.dpcd_address = offset;
2605         req.u.dpcd_read.num_bytes = num_bytes;
2606         drm_dp_encode_sideband_req(&req, msg);
2607
2608         return 0;
2609 }
2610
2611 static int drm_dp_send_sideband_msg(struct drm_dp_mst_topology_mgr *mgr,
2612                                     bool up, u8 *msg, int len)
2613 {
2614         int ret;
2615         int regbase = up ? DP_SIDEBAND_MSG_UP_REP_BASE : DP_SIDEBAND_MSG_DOWN_REQ_BASE;
2616         int tosend, total, offset;
2617         int retries = 0;
2618
2619 retry:
2620         total = len;
2621         offset = 0;
2622         do {
2623                 tosend = min3(mgr->max_dpcd_transaction_bytes, 16, total);
2624
2625                 ret = drm_dp_dpcd_write(mgr->aux, regbase + offset,
2626                                         &msg[offset],
2627                                         tosend);
2628                 if (ret != tosend) {
2629                         if (ret == -EIO && retries < 5) {
2630                                 retries++;
2631                                 goto retry;
2632                         }
2633                         DRM_DEBUG_KMS("failed to dpcd write %d %d\n", tosend, ret);
2634
2635                         return -EIO;
2636                 }
2637                 offset += tosend;
2638                 total -= tosend;
2639         } while (total > 0);
2640         return 0;
2641 }
2642
2643 static int set_hdr_from_dst_qlock(struct drm_dp_sideband_msg_hdr *hdr,
2644                                   struct drm_dp_sideband_msg_tx *txmsg)
2645 {
2646         struct drm_dp_mst_branch *mstb = txmsg->dst;
2647         u8 req_type;
2648
2649         /* both msg slots are full */
2650         if (txmsg->seqno == -1) {
2651                 if (mstb->tx_slots[0] && mstb->tx_slots[1]) {
2652                         DRM_DEBUG_KMS("%s: failed to find slot\n", __func__);
2653                         return -EAGAIN;
2654                 }
2655                 if (mstb->tx_slots[0] == NULL && mstb->tx_slots[1] == NULL) {
2656                         txmsg->seqno = mstb->last_seqno;
2657                         mstb->last_seqno ^= 1;
2658                 } else if (mstb->tx_slots[0] == NULL)
2659                         txmsg->seqno = 0;
2660                 else
2661                         txmsg->seqno = 1;
2662                 mstb->tx_slots[txmsg->seqno] = txmsg;
2663         }
2664
2665         req_type = txmsg->msg[0] & 0x7f;
2666         if (req_type == DP_CONNECTION_STATUS_NOTIFY ||
2667                 req_type == DP_RESOURCE_STATUS_NOTIFY)
2668                 hdr->broadcast = 1;
2669         else
2670                 hdr->broadcast = 0;
2671         hdr->path_msg = txmsg->path_msg;
2672         hdr->lct = mstb->lct;
2673         hdr->lcr = mstb->lct - 1;
2674         if (mstb->lct > 1)
2675                 memcpy(hdr->rad, mstb->rad, mstb->lct / 2);
2676         hdr->seqno = txmsg->seqno;
2677         return 0;
2678 }
2679 /*
2680  * process a single block of the next message in the sideband queue
2681  */
2682 static int process_single_tx_qlock(struct drm_dp_mst_topology_mgr *mgr,
2683                                    struct drm_dp_sideband_msg_tx *txmsg,
2684                                    bool up)
2685 {
2686         u8 chunk[48];
2687         struct drm_dp_sideband_msg_hdr hdr;
2688         int len, space, idx, tosend;
2689         int ret;
2690
2691         memset(&hdr, 0, sizeof(struct drm_dp_sideband_msg_hdr));
2692
2693         if (txmsg->state == DRM_DP_SIDEBAND_TX_QUEUED) {
2694                 txmsg->seqno = -1;
2695                 txmsg->state = DRM_DP_SIDEBAND_TX_START_SEND;
2696         }
2697
2698         /* make hdr from dst mst - for replies use seqno
2699            otherwise assign one */
2700         ret = set_hdr_from_dst_qlock(&hdr, txmsg);
2701         if (ret < 0)
2702                 return ret;
2703
2704         /* amount left to send in this message */
2705         len = txmsg->cur_len - txmsg->cur_offset;
2706
2707         /* 48 - sideband msg size - 1 byte for data CRC, x header bytes */
2708         space = 48 - 1 - drm_dp_calc_sb_hdr_size(&hdr);
2709
2710         tosend = min(len, space);
2711         if (len == txmsg->cur_len)
2712                 hdr.somt = 1;
2713         if (space >= len)
2714                 hdr.eomt = 1;
2715
2716
2717         hdr.msg_len = tosend + 1;
2718         drm_dp_encode_sideband_msg_hdr(&hdr, chunk, &idx);
2719         memcpy(&chunk[idx], &txmsg->msg[txmsg->cur_offset], tosend);
2720         /* add crc at end */
2721         drm_dp_crc_sideband_chunk_req(&chunk[idx], tosend);
2722         idx += tosend + 1;
2723
2724         ret = drm_dp_send_sideband_msg(mgr, up, chunk, idx);
2725         if (unlikely(ret) && drm_debug_enabled(DRM_UT_DP)) {
2726                 struct drm_printer p = drm_debug_printer(DBG_PREFIX);
2727
2728                 drm_printf(&p, "sideband msg failed to send\n");
2729                 drm_dp_mst_dump_sideband_msg_tx(&p, txmsg);
2730                 return ret;
2731         }
2732
2733         txmsg->cur_offset += tosend;
2734         if (txmsg->cur_offset == txmsg->cur_len) {
2735                 txmsg->state = DRM_DP_SIDEBAND_TX_SENT;
2736                 return 1;
2737         }
2738         return 0;
2739 }
2740
2741 static void process_single_down_tx_qlock(struct drm_dp_mst_topology_mgr *mgr)
2742 {
2743         struct drm_dp_sideband_msg_tx *txmsg;
2744         int ret;
2745
2746         WARN_ON(!mutex_is_locked(&mgr->qlock));
2747
2748         /* construct a chunk from the first msg in the tx_msg queue */
2749         if (list_empty(&mgr->tx_msg_downq))
2750                 return;
2751
2752         txmsg = list_first_entry(&mgr->tx_msg_downq, struct drm_dp_sideband_msg_tx, next);
2753         ret = process_single_tx_qlock(mgr, txmsg, false);
2754         if (ret == 1) {
2755                 /* txmsg is sent it should be in the slots now */
2756                 list_del(&txmsg->next);
2757         } else if (ret) {
2758                 DRM_DEBUG_KMS("failed to send msg in q %d\n", ret);
2759                 list_del(&txmsg->next);
2760                 if (txmsg->seqno != -1)
2761                         txmsg->dst->tx_slots[txmsg->seqno] = NULL;
2762                 txmsg->state = DRM_DP_SIDEBAND_TX_TIMEOUT;
2763                 wake_up_all(&mgr->tx_waitq);
2764         }
2765 }
2766
2767 /* called holding qlock */
2768 static void process_single_up_tx_qlock(struct drm_dp_mst_topology_mgr *mgr,
2769                                        struct drm_dp_sideband_msg_tx *txmsg)
2770 {
2771         int ret;
2772
2773         /* construct a chunk from the first msg in the tx_msg queue */
2774         ret = process_single_tx_qlock(mgr, txmsg, true);
2775
2776         if (ret != 1)
2777                 DRM_DEBUG_KMS("failed to send msg in q %d\n", ret);
2778
2779         if (txmsg->seqno != -1) {
2780                 WARN_ON((unsigned int)txmsg->seqno >
2781                         ARRAY_SIZE(txmsg->dst->tx_slots));
2782                 txmsg->dst->tx_slots[txmsg->seqno] = NULL;
2783         }
2784 }
2785
2786 static void drm_dp_queue_down_tx(struct drm_dp_mst_topology_mgr *mgr,
2787                                  struct drm_dp_sideband_msg_tx *txmsg)
2788 {
2789         mutex_lock(&mgr->qlock);
2790         list_add_tail(&txmsg->next, &mgr->tx_msg_downq);
2791
2792         if (drm_debug_enabled(DRM_UT_DP)) {
2793                 struct drm_printer p = drm_debug_printer(DBG_PREFIX);
2794
2795                 drm_dp_mst_dump_sideband_msg_tx(&p, txmsg);
2796         }
2797
2798         if (list_is_singular(&mgr->tx_msg_downq))
2799                 process_single_down_tx_qlock(mgr);
2800         mutex_unlock(&mgr->qlock);
2801 }
2802
2803 static void
2804 drm_dp_dump_link_address(struct drm_dp_link_address_ack_reply *reply)
2805 {
2806         struct drm_dp_link_addr_reply_port *port_reply;
2807         int i;
2808
2809         for (i = 0; i < reply->nports; i++) {
2810                 port_reply = &reply->ports[i];
2811                 DRM_DEBUG_KMS("port %d: input %d, pdt: %d, pn: %d, dpcd_rev: %02x, mcs: %d, ddps: %d, ldps %d, sdp %d/%d\n",
2812                               i,
2813                               port_reply->input_port,
2814                               port_reply->peer_device_type,
2815                               port_reply->port_number,
2816                               port_reply->dpcd_revision,
2817                               port_reply->mcs,
2818                               port_reply->ddps,
2819                               port_reply->legacy_device_plug_status,
2820                               port_reply->num_sdp_streams,
2821                               port_reply->num_sdp_stream_sinks);
2822         }
2823 }
2824
2825 static int drm_dp_send_link_address(struct drm_dp_mst_topology_mgr *mgr,
2826                                      struct drm_dp_mst_branch *mstb)
2827 {
2828         struct drm_dp_sideband_msg_tx *txmsg;
2829         struct drm_dp_link_address_ack_reply *reply;
2830         struct drm_dp_mst_port *port, *tmp;
2831         int i, len, ret, port_mask = 0;
2832         bool changed = false;
2833
2834         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
2835         if (!txmsg)
2836                 return -ENOMEM;
2837
2838         txmsg->dst = mstb;
2839         len = build_link_address(txmsg);
2840
2841         mstb->link_address_sent = true;
2842         drm_dp_queue_down_tx(mgr, txmsg);
2843
2844         /* FIXME: Actually do some real error handling here */
2845         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
2846         if (ret <= 0) {
2847                 DRM_ERROR("Sending link address failed with %d\n", ret);
2848                 goto out;
2849         }
2850         if (txmsg->reply.reply_type == DP_SIDEBAND_REPLY_NAK) {
2851                 DRM_ERROR("link address NAK received\n");
2852                 ret = -EIO;
2853                 goto out;
2854         }
2855
2856         reply = &txmsg->reply.u.link_addr;
2857         DRM_DEBUG_KMS("link address reply: %d\n", reply->nports);
2858         drm_dp_dump_link_address(reply);
2859
2860         drm_dp_check_mstb_guid(mstb, reply->guid);
2861
2862         for (i = 0; i < reply->nports; i++) {
2863                 port_mask |= BIT(reply->ports[i].port_number);
2864                 ret = drm_dp_mst_handle_link_address_port(mstb, mgr->dev,
2865                                                           &reply->ports[i]);
2866                 if (ret == 1)
2867                         changed = true;
2868                 else if (ret < 0)
2869                         goto out;
2870         }
2871
2872         /* Prune any ports that are currently a part of mstb in our in-memory
2873          * topology, but were not seen in this link address. Usually this
2874          * means that they were removed while the topology was out of sync,
2875          * e.g. during suspend/resume
2876          */
2877         mutex_lock(&mgr->lock);
2878         list_for_each_entry_safe(port, tmp, &mstb->ports, next) {
2879                 if (port_mask & BIT(port->port_num))
2880                         continue;
2881
2882                 DRM_DEBUG_KMS("port %d was not in link address, removing\n",
2883                               port->port_num);
2884                 list_del(&port->next);
2885                 drm_dp_mst_topology_put_port(port);
2886                 changed = true;
2887         }
2888         mutex_unlock(&mgr->lock);
2889
2890 out:
2891         if (ret <= 0)
2892                 mstb->link_address_sent = false;
2893         kfree(txmsg);
2894         return ret < 0 ? ret : changed;
2895 }
2896
2897 void drm_dp_send_clear_payload_id_table(struct drm_dp_mst_topology_mgr *mgr,
2898                                         struct drm_dp_mst_branch *mstb)
2899 {
2900         struct drm_dp_sideband_msg_tx *txmsg;
2901         int len, ret;
2902
2903         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
2904         if (!txmsg)
2905                 return;
2906
2907         txmsg->dst = mstb;
2908         len = build_clear_payload_id_table(txmsg);
2909
2910         drm_dp_queue_down_tx(mgr, txmsg);
2911
2912         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
2913         if (ret > 0 && txmsg->reply.reply_type == DP_SIDEBAND_REPLY_NAK)
2914                 DRM_DEBUG_KMS("clear payload table id nak received\n");
2915
2916         kfree(txmsg);
2917 }
2918
2919 static int
2920 drm_dp_send_enum_path_resources(struct drm_dp_mst_topology_mgr *mgr,
2921                                 struct drm_dp_mst_branch *mstb,
2922                                 struct drm_dp_mst_port *port)
2923 {
2924         struct drm_dp_enum_path_resources_ack_reply *path_res;
2925         struct drm_dp_sideband_msg_tx *txmsg;
2926         int len;
2927         int ret;
2928
2929         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
2930         if (!txmsg)
2931                 return -ENOMEM;
2932
2933         txmsg->dst = mstb;
2934         len = build_enum_path_resources(txmsg, port->port_num);
2935
2936         drm_dp_queue_down_tx(mgr, txmsg);
2937
2938         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
2939         if (ret > 0) {
2940                 path_res = &txmsg->reply.u.path_resources;
2941
2942                 if (txmsg->reply.reply_type == DP_SIDEBAND_REPLY_NAK) {
2943                         DRM_DEBUG_KMS("enum path resources nak received\n");
2944                 } else {
2945                         if (port->port_num != path_res->port_number)
2946                                 DRM_ERROR("got incorrect port in response\n");
2947
2948                         DRM_DEBUG_KMS("enum path resources %d: %d %d\n",
2949                                       path_res->port_number,
2950                                       path_res->full_payload_bw_number,
2951                                       path_res->avail_payload_bw_number);
2952                         port->available_pbn =
2953                                 path_res->avail_payload_bw_number;
2954                 }
2955         }
2956
2957         kfree(txmsg);
2958         return 0;
2959 }
2960
2961 static struct drm_dp_mst_port *drm_dp_get_last_connected_port_to_mstb(struct drm_dp_mst_branch *mstb)
2962 {
2963         if (!mstb->port_parent)
2964                 return NULL;
2965
2966         if (mstb->port_parent->mstb != mstb)
2967                 return mstb->port_parent;
2968
2969         return drm_dp_get_last_connected_port_to_mstb(mstb->port_parent->parent);
2970 }
2971
2972 /*
2973  * Searches upwards in the topology starting from mstb to try to find the
2974  * closest available parent of mstb that's still connected to the rest of the
2975  * topology. This can be used in order to perform operations like releasing
2976  * payloads, where the branch device which owned the payload may no longer be
2977  * around and thus would require that the payload on the last living relative
2978  * be freed instead.
2979  */
2980 static struct drm_dp_mst_branch *
2981 drm_dp_get_last_connected_port_and_mstb(struct drm_dp_mst_topology_mgr *mgr,
2982                                         struct drm_dp_mst_branch *mstb,
2983                                         int *port_num)
2984 {
2985         struct drm_dp_mst_branch *rmstb = NULL;
2986         struct drm_dp_mst_port *found_port;
2987
2988         mutex_lock(&mgr->lock);
2989         if (!mgr->mst_primary)
2990                 goto out;
2991
2992         do {
2993                 found_port = drm_dp_get_last_connected_port_to_mstb(mstb);
2994                 if (!found_port)
2995                         break;
2996
2997                 if (drm_dp_mst_topology_try_get_mstb(found_port->parent)) {
2998                         rmstb = found_port->parent;
2999                         *port_num = found_port->port_num;
3000                 } else {
3001                         /* Search again, starting from this parent */
3002                         mstb = found_port->parent;
3003                 }
3004         } while (!rmstb);
3005 out:
3006         mutex_unlock(&mgr->lock);
3007         return rmstb;
3008 }
3009
3010 static int drm_dp_payload_send_msg(struct drm_dp_mst_topology_mgr *mgr,
3011                                    struct drm_dp_mst_port *port,
3012                                    int id,
3013                                    int pbn)
3014 {
3015         struct drm_dp_sideband_msg_tx *txmsg;
3016         struct drm_dp_mst_branch *mstb;
3017         int len, ret, port_num;
3018         u8 sinks[DRM_DP_MAX_SDP_STREAMS];
3019         int i;
3020
3021         port_num = port->port_num;
3022         mstb = drm_dp_mst_topology_get_mstb_validated(mgr, port->parent);
3023         if (!mstb) {
3024                 mstb = drm_dp_get_last_connected_port_and_mstb(mgr,
3025                                                                port->parent,
3026                                                                &port_num);
3027
3028                 if (!mstb)
3029                         return -EINVAL;
3030         }
3031
3032         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
3033         if (!txmsg) {
3034                 ret = -ENOMEM;
3035                 goto fail_put;
3036         }
3037
3038         for (i = 0; i < port->num_sdp_streams; i++)
3039                 sinks[i] = i;
3040
3041         txmsg->dst = mstb;
3042         len = build_allocate_payload(txmsg, port_num,
3043                                      id,
3044                                      pbn, port->num_sdp_streams, sinks);
3045
3046         drm_dp_queue_down_tx(mgr, txmsg);
3047
3048         /*
3049          * FIXME: there is a small chance that between getting the last
3050          * connected mstb and sending the payload message, the last connected
3051          * mstb could also be removed from the topology. In the future, this
3052          * needs to be fixed by restarting the
3053          * drm_dp_get_last_connected_port_and_mstb() search in the event of a
3054          * timeout if the topology is still connected to the system.
3055          */
3056         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
3057         if (ret > 0) {
3058                 if (txmsg->reply.reply_type == DP_SIDEBAND_REPLY_NAK)
3059                         ret = -EINVAL;
3060                 else
3061                         ret = 0;
3062         }
3063         kfree(txmsg);
3064 fail_put:
3065         drm_dp_mst_topology_put_mstb(mstb);
3066         return ret;
3067 }
3068
3069 int drm_dp_send_power_updown_phy(struct drm_dp_mst_topology_mgr *mgr,
3070                                  struct drm_dp_mst_port *port, bool power_up)
3071 {
3072         struct drm_dp_sideband_msg_tx *txmsg;
3073         int len, ret;
3074
3075         port = drm_dp_mst_topology_get_port_validated(mgr, port);
3076         if (!port)
3077                 return -EINVAL;
3078
3079         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
3080         if (!txmsg) {
3081                 drm_dp_mst_topology_put_port(port);
3082                 return -ENOMEM;
3083         }
3084
3085         txmsg->dst = port->parent;
3086         len = build_power_updown_phy(txmsg, port->port_num, power_up);
3087         drm_dp_queue_down_tx(mgr, txmsg);
3088
3089         ret = drm_dp_mst_wait_tx_reply(port->parent, txmsg);
3090         if (ret > 0) {
3091                 if (txmsg->reply.reply_type == DP_SIDEBAND_REPLY_NAK)
3092                         ret = -EINVAL;
3093                 else
3094                         ret = 0;
3095         }
3096         kfree(txmsg);
3097         drm_dp_mst_topology_put_port(port);
3098
3099         return ret;
3100 }
3101 EXPORT_SYMBOL(drm_dp_send_power_updown_phy);
3102
3103 static int drm_dp_create_payload_step1(struct drm_dp_mst_topology_mgr *mgr,
3104                                        int id,
3105                                        struct drm_dp_payload *payload)
3106 {
3107         int ret;
3108
3109         ret = drm_dp_dpcd_write_payload(mgr, id, payload);
3110         if (ret < 0) {
3111                 payload->payload_state = 0;
3112                 return ret;
3113         }
3114         payload->payload_state = DP_PAYLOAD_LOCAL;
3115         return 0;
3116 }
3117
3118 static int drm_dp_create_payload_step2(struct drm_dp_mst_topology_mgr *mgr,
3119                                        struct drm_dp_mst_port *port,
3120                                        int id,
3121                                        struct drm_dp_payload *payload)
3122 {
3123         int ret;
3124         ret = drm_dp_payload_send_msg(mgr, port, id, port->vcpi.pbn);
3125         if (ret < 0)
3126                 return ret;
3127         payload->payload_state = DP_PAYLOAD_REMOTE;
3128         return ret;
3129 }
3130
3131 static int drm_dp_destroy_payload_step1(struct drm_dp_mst_topology_mgr *mgr,
3132                                         struct drm_dp_mst_port *port,
3133                                         int id,
3134                                         struct drm_dp_payload *payload)
3135 {
3136         DRM_DEBUG_KMS("\n");
3137         /* it's okay for these to fail */
3138         if (port) {
3139                 drm_dp_payload_send_msg(mgr, port, id, 0);
3140         }
3141
3142         drm_dp_dpcd_write_payload(mgr, id, payload);
3143         payload->payload_state = DP_PAYLOAD_DELETE_LOCAL;
3144         return 0;
3145 }
3146
3147 static int drm_dp_destroy_payload_step2(struct drm_dp_mst_topology_mgr *mgr,
3148                                         int id,
3149                                         struct drm_dp_payload *payload)
3150 {
3151         payload->payload_state = 0;
3152         return 0;
3153 }
3154
3155 /**
3156  * drm_dp_update_payload_part1() - Execute payload update part 1
3157  * @mgr: manager to use.
3158  *
3159  * This iterates over all proposed virtual channels, and tries to
3160  * allocate space in the link for them. For 0->slots transitions,
3161  * this step just writes the VCPI to the MST device. For slots->0
3162  * transitions, this writes the updated VCPIs and removes the
3163  * remote VC payloads.
3164  *
3165  * after calling this the driver should generate ACT and payload
3166  * packets.
3167  */
3168 int drm_dp_update_payload_part1(struct drm_dp_mst_topology_mgr *mgr)
3169 {
3170         struct drm_dp_payload req_payload;
3171         struct drm_dp_mst_port *port;
3172         int i, j;
3173         int cur_slots = 1;
3174
3175         mutex_lock(&mgr->payload_lock);
3176         for (i = 0; i < mgr->max_payloads; i++) {
3177                 struct drm_dp_vcpi *vcpi = mgr->proposed_vcpis[i];
3178                 struct drm_dp_payload *payload = &mgr->payloads[i];
3179                 bool put_port = false;
3180
3181                 /* solve the current payloads - compare to the hw ones
3182                    - update the hw view */
3183                 req_payload.start_slot = cur_slots;
3184                 if (vcpi) {
3185                         port = container_of(vcpi, struct drm_dp_mst_port,
3186                                             vcpi);
3187
3188                         /* Validated ports don't matter if we're releasing
3189                          * VCPI
3190                          */
3191                         if (vcpi->num_slots) {
3192                                 port = drm_dp_mst_topology_get_port_validated(
3193                                     mgr, port);
3194                                 if (!port) {
3195                                         mutex_unlock(&mgr->payload_lock);
3196                                         return -EINVAL;
3197                                 }
3198                                 put_port = true;
3199                         }
3200
3201                         req_payload.num_slots = vcpi->num_slots;
3202                         req_payload.vcpi = vcpi->vcpi;
3203                 } else {
3204                         port = NULL;
3205                         req_payload.num_slots = 0;
3206                 }
3207
3208                 payload->start_slot = req_payload.start_slot;
3209                 /* work out what is required to happen with this payload */
3210                 if (payload->num_slots != req_payload.num_slots) {
3211
3212                         /* need to push an update for this payload */
3213                         if (req_payload.num_slots) {
3214                                 drm_dp_create_payload_step1(mgr, vcpi->vcpi,
3215                                                             &req_payload);
3216                                 payload->num_slots = req_payload.num_slots;
3217                                 payload->vcpi = req_payload.vcpi;
3218
3219                         } else if (payload->num_slots) {
3220                                 payload->num_slots = 0;
3221                                 drm_dp_destroy_payload_step1(mgr, port,
3222                                                              payload->vcpi,
3223                                                              payload);
3224                                 req_payload.payload_state =
3225                                         payload->payload_state;
3226                                 payload->start_slot = 0;
3227                         }
3228                         payload->payload_state = req_payload.payload_state;
3229                 }
3230                 cur_slots += req_payload.num_slots;
3231
3232                 if (put_port)
3233                         drm_dp_mst_topology_put_port(port);
3234         }
3235
3236         for (i = 0; i < mgr->max_payloads; /* do nothing */) {
3237                 if (mgr->payloads[i].payload_state != DP_PAYLOAD_DELETE_LOCAL) {
3238                         i++;
3239                         continue;
3240                 }
3241
3242                 DRM_DEBUG_KMS("removing payload %d\n", i);
3243                 for (j = i; j < mgr->max_payloads - 1; j++) {
3244                         mgr->payloads[j] = mgr->payloads[j + 1];
3245                         mgr->proposed_vcpis[j] = mgr->proposed_vcpis[j + 1];
3246
3247                         if (mgr->proposed_vcpis[j] &&
3248                             mgr->proposed_vcpis[j]->num_slots) {
3249                                 set_bit(j + 1, &mgr->payload_mask);
3250                         } else {
3251                                 clear_bit(j + 1, &mgr->payload_mask);
3252                         }
3253                 }
3254
3255                 memset(&mgr->payloads[mgr->max_payloads - 1], 0,
3256                        sizeof(struct drm_dp_payload));
3257                 mgr->proposed_vcpis[mgr->max_payloads - 1] = NULL;
3258                 clear_bit(mgr->max_payloads, &mgr->payload_mask);
3259         }
3260         mutex_unlock(&mgr->payload_lock);
3261
3262         return 0;
3263 }
3264 EXPORT_SYMBOL(drm_dp_update_payload_part1);
3265
3266 /**
3267  * drm_dp_update_payload_part2() - Execute payload update part 2
3268  * @mgr: manager to use.
3269  *
3270  * This iterates over all proposed virtual channels, and tries to
3271  * allocate space in the link for them. For 0->slots transitions,
3272  * this step writes the remote VC payload commands. For slots->0
3273  * this just resets some internal state.
3274  */
3275 int drm_dp_update_payload_part2(struct drm_dp_mst_topology_mgr *mgr)
3276 {
3277         struct drm_dp_mst_port *port;
3278         int i;
3279         int ret = 0;
3280         mutex_lock(&mgr->payload_lock);
3281         for (i = 0; i < mgr->max_payloads; i++) {
3282
3283                 if (!mgr->proposed_vcpis[i])
3284                         continue;
3285
3286                 port = container_of(mgr->proposed_vcpis[i], struct drm_dp_mst_port, vcpi);
3287
3288                 DRM_DEBUG_KMS("payload %d %d\n", i, mgr->payloads[i].payload_state);
3289                 if (mgr->payloads[i].payload_state == DP_PAYLOAD_LOCAL) {
3290                         ret = drm_dp_create_payload_step2(mgr, port, mgr->proposed_vcpis[i]->vcpi, &mgr->payloads[i]);
3291                 } else if (mgr->payloads[i].payload_state == DP_PAYLOAD_DELETE_LOCAL) {
3292                         ret = drm_dp_destroy_payload_step2(mgr, mgr->proposed_vcpis[i]->vcpi, &mgr->payloads[i]);
3293                 }
3294                 if (ret) {
3295                         mutex_unlock(&mgr->payload_lock);
3296                         return ret;
3297                 }
3298         }
3299         mutex_unlock(&mgr->payload_lock);
3300         return 0;
3301 }
3302 EXPORT_SYMBOL(drm_dp_update_payload_part2);
3303
3304 static int drm_dp_send_dpcd_read(struct drm_dp_mst_topology_mgr *mgr,
3305                                  struct drm_dp_mst_port *port,
3306                                  int offset, int size, u8 *bytes)
3307 {
3308         int len;
3309         int ret = 0;
3310         struct drm_dp_sideband_msg_tx *txmsg;
3311         struct drm_dp_mst_branch *mstb;
3312
3313         mstb = drm_dp_mst_topology_get_mstb_validated(mgr, port->parent);
3314         if (!mstb)
3315                 return -EINVAL;
3316
3317         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
3318         if (!txmsg) {
3319                 ret = -ENOMEM;
3320                 goto fail_put;
3321         }
3322
3323         len = build_dpcd_read(txmsg, port->port_num, offset, size);
3324         txmsg->dst = port->parent;
3325
3326         drm_dp_queue_down_tx(mgr, txmsg);
3327
3328         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
3329         if (ret < 0)
3330                 goto fail_free;
3331
3332         /* DPCD read should never be NACKed */
3333         if (txmsg->reply.reply_type == 1) {
3334                 DRM_ERROR("mstb %p port %d: DPCD read on addr 0x%x for %d bytes NAKed\n",
3335                           mstb, port->port_num, offset, size);
3336                 ret = -EIO;
3337                 goto fail_free;
3338         }
3339
3340         if (txmsg->reply.u.remote_dpcd_read_ack.num_bytes != size) {
3341                 ret = -EPROTO;
3342                 goto fail_free;
3343         }
3344
3345         ret = min_t(size_t, txmsg->reply.u.remote_dpcd_read_ack.num_bytes,
3346                     size);
3347         memcpy(bytes, txmsg->reply.u.remote_dpcd_read_ack.bytes, ret);
3348
3349 fail_free:
3350         kfree(txmsg);
3351 fail_put:
3352         drm_dp_mst_topology_put_mstb(mstb);
3353
3354         return ret;
3355 }
3356
3357 static int drm_dp_send_dpcd_write(struct drm_dp_mst_topology_mgr *mgr,
3358                                   struct drm_dp_mst_port *port,
3359                                   int offset, int size, u8 *bytes)
3360 {
3361         int len;
3362         int ret;
3363         struct drm_dp_sideband_msg_tx *txmsg;
3364         struct drm_dp_mst_branch *mstb;
3365
3366         mstb = drm_dp_mst_topology_get_mstb_validated(mgr, port->parent);
3367         if (!mstb)
3368                 return -EINVAL;
3369
3370         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
3371         if (!txmsg) {
3372                 ret = -ENOMEM;
3373                 goto fail_put;
3374         }
3375
3376         len = build_dpcd_write(txmsg, port->port_num, offset, size, bytes);
3377         txmsg->dst = mstb;
3378
3379         drm_dp_queue_down_tx(mgr, txmsg);
3380
3381         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
3382         if (ret > 0) {
3383                 if (txmsg->reply.reply_type == DP_SIDEBAND_REPLY_NAK)
3384                         ret = -EIO;
3385                 else
3386                         ret = 0;
3387         }
3388         kfree(txmsg);
3389 fail_put:
3390         drm_dp_mst_topology_put_mstb(mstb);
3391         return ret;
3392 }
3393
3394 static int drm_dp_encode_up_ack_reply(struct drm_dp_sideband_msg_tx *msg, u8 req_type)
3395 {
3396         struct drm_dp_sideband_msg_reply_body reply;
3397
3398         reply.reply_type = DP_SIDEBAND_REPLY_ACK;
3399         reply.req_type = req_type;
3400         drm_dp_encode_sideband_reply(&reply, msg);
3401         return 0;
3402 }
3403
3404 static int drm_dp_send_up_ack_reply(struct drm_dp_mst_topology_mgr *mgr,
3405                                     struct drm_dp_mst_branch *mstb,
3406                                     int req_type, int seqno, bool broadcast)
3407 {
3408         struct drm_dp_sideband_msg_tx *txmsg;
3409
3410         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
3411         if (!txmsg)
3412                 return -ENOMEM;
3413
3414         txmsg->dst = mstb;
3415         txmsg->seqno = seqno;
3416         drm_dp_encode_up_ack_reply(txmsg, req_type);
3417
3418         mutex_lock(&mgr->qlock);
3419
3420         process_single_up_tx_qlock(mgr, txmsg);
3421
3422         mutex_unlock(&mgr->qlock);
3423
3424         kfree(txmsg);
3425         return 0;
3426 }
3427
3428 static int drm_dp_get_vc_payload_bw(u8 dp_link_bw, u8  dp_link_count)
3429 {
3430         if (dp_link_bw == 0 || dp_link_count == 0)
3431                 DRM_DEBUG_KMS("invalid link bandwidth in DPCD: %x (link count: %d)\n",
3432                               dp_link_bw, dp_link_count);
3433
3434         return dp_link_bw * dp_link_count / 2;
3435 }
3436
3437 /**
3438  * drm_dp_mst_topology_mgr_set_mst() - Set the MST state for a topology manager
3439  * @mgr: manager to set state for
3440  * @mst_state: true to enable MST on this connector - false to disable.
3441  *
3442  * This is called by the driver when it detects an MST capable device plugged
3443  * into a DP MST capable port, or when a DP MST capable device is unplugged.
3444  */
3445 int drm_dp_mst_topology_mgr_set_mst(struct drm_dp_mst_topology_mgr *mgr, bool mst_state)
3446 {
3447         int ret = 0;
3448         int i = 0;
3449         struct drm_dp_mst_branch *mstb = NULL;
3450
3451         mutex_lock(&mgr->lock);
3452         if (mst_state == mgr->mst_state)
3453                 goto out_unlock;
3454
3455         mgr->mst_state = mst_state;
3456         /* set the device into MST mode */
3457         if (mst_state) {
3458                 WARN_ON(mgr->mst_primary);
3459
3460                 /* get dpcd info */
3461                 ret = drm_dp_dpcd_read(mgr->aux, DP_DPCD_REV, mgr->dpcd, DP_RECEIVER_CAP_SIZE);
3462                 if (ret != DP_RECEIVER_CAP_SIZE) {
3463                         DRM_DEBUG_KMS("failed to read DPCD\n");
3464                         goto out_unlock;
3465                 }
3466
3467                 mgr->pbn_div = drm_dp_get_vc_payload_bw(mgr->dpcd[1],
3468                                                         mgr->dpcd[2] & DP_MAX_LANE_COUNT_MASK);
3469                 if (mgr->pbn_div == 0) {
3470                         ret = -EINVAL;
3471                         goto out_unlock;
3472                 }
3473
3474                 /* add initial branch device at LCT 1 */
3475                 mstb = drm_dp_add_mst_branch_device(1, NULL);
3476                 if (mstb == NULL) {
3477                         ret = -ENOMEM;
3478                         goto out_unlock;
3479                 }
3480                 mstb->mgr = mgr;
3481
3482                 /* give this the main reference */
3483                 mgr->mst_primary = mstb;
3484                 drm_dp_mst_topology_get_mstb(mgr->mst_primary);
3485
3486                 ret = drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL,
3487                                                          DP_MST_EN | DP_UP_REQ_EN | DP_UPSTREAM_IS_SRC);
3488                 if (ret < 0) {
3489                         goto out_unlock;
3490                 }
3491
3492                 {
3493                         struct drm_dp_payload reset_pay;
3494                         reset_pay.start_slot = 0;
3495                         reset_pay.num_slots = 0x3f;
3496                         drm_dp_dpcd_write_payload(mgr, 0, &reset_pay);
3497                 }
3498
3499                 queue_work(system_long_wq, &mgr->work);
3500
3501                 ret = 0;
3502         } else {
3503                 /* disable MST on the device */
3504                 mstb = mgr->mst_primary;
3505                 mgr->mst_primary = NULL;
3506                 /* this can fail if the device is gone */
3507                 drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL, 0);
3508                 ret = 0;
3509                 mutex_lock(&mgr->payload_lock);
3510                 memset(mgr->payloads, 0, mgr->max_payloads * sizeof(struct drm_dp_payload));
3511                 mgr->payload_mask = 0;
3512                 set_bit(0, &mgr->payload_mask);
3513                 for (i = 0; i < mgr->max_payloads; i++) {
3514                         struct drm_dp_vcpi *vcpi = mgr->proposed_vcpis[i];
3515
3516                         if (vcpi) {
3517                                 vcpi->vcpi = 0;
3518                                 vcpi->num_slots = 0;
3519                         }
3520                         mgr->proposed_vcpis[i] = NULL;
3521                 }
3522                 mgr->vcpi_mask = 0;
3523                 mutex_unlock(&mgr->payload_lock);
3524
3525                 mgr->payload_id_table_cleared = false;
3526         }
3527
3528 out_unlock:
3529         mutex_unlock(&mgr->lock);
3530         if (mstb)
3531                 drm_dp_mst_topology_put_mstb(mstb);
3532         return ret;
3533
3534 }
3535 EXPORT_SYMBOL(drm_dp_mst_topology_mgr_set_mst);
3536
3537 static void
3538 drm_dp_mst_topology_mgr_invalidate_mstb(struct drm_dp_mst_branch *mstb)
3539 {
3540         struct drm_dp_mst_port *port;
3541
3542         /* The link address will need to be re-sent on resume */
3543         mstb->link_address_sent = false;
3544
3545         list_for_each_entry(port, &mstb->ports, next) {
3546                 /* The PBN for each port will also need to be re-probed */
3547                 port->available_pbn = 0;
3548
3549                 if (port->mstb)
3550                         drm_dp_mst_topology_mgr_invalidate_mstb(port->mstb);
3551         }
3552 }
3553
3554 /**
3555  * drm_dp_mst_topology_mgr_suspend() - suspend the MST manager
3556  * @mgr: manager to suspend
3557  *
3558  * This function tells the MST device that we can't handle UP messages
3559  * anymore. This should stop it from sending any since we are suspended.
3560  */
3561 void drm_dp_mst_topology_mgr_suspend(struct drm_dp_mst_topology_mgr *mgr)
3562 {
3563         mutex_lock(&mgr->lock);
3564         drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL,
3565                            DP_MST_EN | DP_UPSTREAM_IS_SRC);
3566         mutex_unlock(&mgr->lock);
3567         flush_work(&mgr->up_req_work);
3568         flush_work(&mgr->work);
3569         flush_work(&mgr->delayed_destroy_work);
3570
3571         mutex_lock(&mgr->lock);
3572         if (mgr->mst_state && mgr->mst_primary)
3573                 drm_dp_mst_topology_mgr_invalidate_mstb(mgr->mst_primary);
3574         mutex_unlock(&mgr->lock);
3575 }
3576 EXPORT_SYMBOL(drm_dp_mst_topology_mgr_suspend);
3577
3578 /**
3579  * drm_dp_mst_topology_mgr_resume() - resume the MST manager
3580  * @mgr: manager to resume
3581  * @sync: whether or not to perform topology reprobing synchronously
3582  *
3583  * This will fetch DPCD and see if the device is still there,
3584  * if it is, it will rewrite the MSTM control bits, and return.
3585  *
3586  * If the device fails this returns -1, and the driver should do
3587  * a full MST reprobe, in case we were undocked.
3588  *
3589  * During system resume (where it is assumed that the driver will be calling
3590  * drm_atomic_helper_resume()) this function should be called beforehand with
3591  * @sync set to true. In contexts like runtime resume where the driver is not
3592  * expected to be calling drm_atomic_helper_resume(), this function should be
3593  * called with @sync set to false in order to avoid deadlocking.
3594  *
3595  * Returns: -1 if the MST topology was removed while we were suspended, 0
3596  * otherwise.
3597  */
3598 int drm_dp_mst_topology_mgr_resume(struct drm_dp_mst_topology_mgr *mgr,
3599                                    bool sync)
3600 {
3601         int ret;
3602         u8 guid[16];
3603
3604         mutex_lock(&mgr->lock);
3605         if (!mgr->mst_primary)
3606                 goto out_fail;
3607
3608         ret = drm_dp_dpcd_read(mgr->aux, DP_DPCD_REV, mgr->dpcd,
3609                                DP_RECEIVER_CAP_SIZE);
3610         if (ret != DP_RECEIVER_CAP_SIZE) {
3611                 DRM_DEBUG_KMS("dpcd read failed - undocked during suspend?\n");
3612                 goto out_fail;
3613         }
3614
3615         ret = drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL,
3616                                  DP_MST_EN |
3617                                  DP_UP_REQ_EN |
3618                                  DP_UPSTREAM_IS_SRC);
3619         if (ret < 0) {
3620                 DRM_DEBUG_KMS("mst write failed - undocked during suspend?\n");
3621                 goto out_fail;
3622         }
3623
3624         /* Some hubs forget their guids after they resume */
3625         ret = drm_dp_dpcd_read(mgr->aux, DP_GUID, guid, 16);
3626         if (ret != 16) {
3627                 DRM_DEBUG_KMS("dpcd read failed - undocked during suspend?\n");
3628                 goto out_fail;
3629         }
3630         drm_dp_check_mstb_guid(mgr->mst_primary, guid);
3631
3632         /*
3633          * For the final step of resuming the topology, we need to bring the
3634          * state of our in-memory topology back into sync with reality. So,
3635          * restart the probing process as if we're probing a new hub
3636          */
3637         queue_work(system_long_wq, &mgr->work);
3638         mutex_unlock(&mgr->lock);
3639
3640         if (sync) {
3641                 DRM_DEBUG_KMS("Waiting for link probe work to finish re-syncing topology...\n");
3642                 flush_work(&mgr->work);
3643         }
3644
3645         return 0;
3646
3647 out_fail:
3648         mutex_unlock(&mgr->lock);
3649         return -1;
3650 }
3651 EXPORT_SYMBOL(drm_dp_mst_topology_mgr_resume);
3652
3653 static bool drm_dp_get_one_sb_msg(struct drm_dp_mst_topology_mgr *mgr, bool up)
3654 {
3655         int len;
3656         u8 replyblock[32];
3657         int replylen, origlen, curreply;
3658         int ret;
3659         struct drm_dp_sideband_msg_rx *msg;
3660         int basereg = up ? DP_SIDEBAND_MSG_UP_REQ_BASE : DP_SIDEBAND_MSG_DOWN_REP_BASE;
3661         msg = up ? &mgr->up_req_recv : &mgr->down_rep_recv;
3662
3663         len = min(mgr->max_dpcd_transaction_bytes, 16);
3664         ret = drm_dp_dpcd_read(mgr->aux, basereg,
3665                                replyblock, len);
3666         if (ret != len) {
3667                 DRM_DEBUG_KMS("failed to read DPCD down rep %d %d\n", len, ret);
3668                 return false;
3669         }
3670         ret = drm_dp_sideband_msg_build(msg, replyblock, len, true);
3671         if (!ret) {
3672                 DRM_DEBUG_KMS("sideband msg build failed %d\n", replyblock[0]);
3673                 return false;
3674         }
3675         replylen = msg->curchunk_len + msg->curchunk_hdrlen;
3676
3677         origlen = replylen;
3678         replylen -= len;
3679         curreply = len;
3680         while (replylen > 0) {
3681                 len = min3(replylen, mgr->max_dpcd_transaction_bytes, 16);
3682                 ret = drm_dp_dpcd_read(mgr->aux, basereg + curreply,
3683                                     replyblock, len);
3684                 if (ret != len) {
3685                         DRM_DEBUG_KMS("failed to read a chunk (len %d, ret %d)\n",
3686                                       len, ret);
3687                         return false;
3688                 }
3689
3690                 ret = drm_dp_sideband_msg_build(msg, replyblock, len, false);
3691                 if (!ret) {
3692                         DRM_DEBUG_KMS("failed to build sideband msg\n");
3693                         return false;
3694                 }
3695
3696                 curreply += len;
3697                 replylen -= len;
3698         }
3699         return true;
3700 }
3701
3702 static int drm_dp_mst_handle_down_rep(struct drm_dp_mst_topology_mgr *mgr)
3703 {
3704         struct drm_dp_sideband_msg_tx *txmsg;
3705         struct drm_dp_mst_branch *mstb;
3706         struct drm_dp_sideband_msg_hdr *hdr = &mgr->down_rep_recv.initial_hdr;
3707         int slot = -1;
3708
3709         if (!drm_dp_get_one_sb_msg(mgr, false))
3710                 goto clear_down_rep_recv;
3711
3712         if (!mgr->down_rep_recv.have_eomt)
3713                 return 0;
3714
3715         mstb = drm_dp_get_mst_branch_device(mgr, hdr->lct, hdr->rad);
3716         if (!mstb) {
3717                 DRM_DEBUG_KMS("Got MST reply from unknown device %d\n",
3718                               hdr->lct);
3719                 goto clear_down_rep_recv;
3720         }
3721
3722         /* find the message */
3723         slot = hdr->seqno;
3724         mutex_lock(&mgr->qlock);
3725         txmsg = mstb->tx_slots[slot];
3726         /* remove from slots */
3727         mutex_unlock(&mgr->qlock);
3728
3729         if (!txmsg) {
3730                 DRM_DEBUG_KMS("Got MST reply with no msg %p %d %d %02x %02x\n",
3731                               mstb, hdr->seqno, hdr->lct, hdr->rad[0],
3732                               mgr->down_rep_recv.msg[0]);
3733                 goto no_msg;
3734         }
3735
3736         drm_dp_sideband_parse_reply(&mgr->down_rep_recv, &txmsg->reply);
3737
3738         if (txmsg->reply.reply_type == DP_SIDEBAND_REPLY_NAK)
3739                 DRM_DEBUG_KMS("Got NAK reply: req 0x%02x (%s), reason 0x%02x (%s), nak data 0x%02x\n",
3740                               txmsg->reply.req_type,
3741                               drm_dp_mst_req_type_str(txmsg->reply.req_type),
3742                               txmsg->reply.u.nak.reason,
3743                               drm_dp_mst_nak_reason_str(txmsg->reply.u.nak.reason),
3744                               txmsg->reply.u.nak.nak_data);
3745
3746         memset(&mgr->down_rep_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
3747         drm_dp_mst_topology_put_mstb(mstb);
3748
3749         mutex_lock(&mgr->qlock);
3750         txmsg->state = DRM_DP_SIDEBAND_TX_RX;
3751         mstb->tx_slots[slot] = NULL;
3752         mutex_unlock(&mgr->qlock);
3753
3754         wake_up_all(&mgr->tx_waitq);
3755
3756         return 0;
3757
3758 no_msg:
3759         drm_dp_mst_topology_put_mstb(mstb);
3760 clear_down_rep_recv:
3761         memset(&mgr->down_rep_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
3762
3763         return 0;
3764 }
3765
3766 static inline bool
3767 drm_dp_mst_process_up_req(struct drm_dp_mst_topology_mgr *mgr,
3768                           struct drm_dp_pending_up_req *up_req)
3769 {
3770         struct drm_dp_mst_branch *mstb = NULL;
3771         struct drm_dp_sideband_msg_req_body *msg = &up_req->msg;
3772         struct drm_dp_sideband_msg_hdr *hdr = &up_req->hdr;
3773         bool hotplug = false;
3774
3775         if (hdr->broadcast) {
3776                 const u8 *guid = NULL;
3777
3778                 if (msg->req_type == DP_CONNECTION_STATUS_NOTIFY)
3779                         guid = msg->u.conn_stat.guid;
3780                 else if (msg->req_type == DP_RESOURCE_STATUS_NOTIFY)
3781                         guid = msg->u.resource_stat.guid;
3782
3783                 mstb = drm_dp_get_mst_branch_device_by_guid(mgr, guid);
3784         } else {
3785                 mstb = drm_dp_get_mst_branch_device(mgr, hdr->lct, hdr->rad);
3786         }
3787
3788         if (!mstb) {
3789                 DRM_DEBUG_KMS("Got MST reply from unknown device %d\n",
3790                               hdr->lct);
3791                 return false;
3792         }
3793
3794         /* TODO: Add missing handler for DP_RESOURCE_STATUS_NOTIFY events */
3795         if (msg->req_type == DP_CONNECTION_STATUS_NOTIFY) {
3796                 drm_dp_mst_handle_conn_stat(mstb, &msg->u.conn_stat);
3797                 hotplug = true;
3798         }
3799
3800         drm_dp_mst_topology_put_mstb(mstb);
3801         return hotplug;
3802 }
3803
3804 static void drm_dp_mst_up_req_work(struct work_struct *work)
3805 {
3806         struct drm_dp_mst_topology_mgr *mgr =
3807                 container_of(work, struct drm_dp_mst_topology_mgr,
3808                              up_req_work);
3809         struct drm_dp_pending_up_req *up_req;
3810         bool send_hotplug = false;
3811
3812         mutex_lock(&mgr->probe_lock);
3813         while (true) {
3814                 mutex_lock(&mgr->up_req_lock);
3815                 up_req = list_first_entry_or_null(&mgr->up_req_list,
3816                                                   struct drm_dp_pending_up_req,
3817                                                   next);
3818                 if (up_req)
3819                         list_del(&up_req->next);
3820                 mutex_unlock(&mgr->up_req_lock);
3821
3822                 if (!up_req)
3823                         break;
3824
3825                 send_hotplug |= drm_dp_mst_process_up_req(mgr, up_req);
3826                 kfree(up_req);
3827         }
3828         mutex_unlock(&mgr->probe_lock);
3829
3830         if (send_hotplug)
3831                 drm_kms_helper_hotplug_event(mgr->dev);
3832 }
3833
3834 static int drm_dp_mst_handle_up_req(struct drm_dp_mst_topology_mgr *mgr)
3835 {
3836         struct drm_dp_sideband_msg_hdr *hdr = &mgr->up_req_recv.initial_hdr;
3837         struct drm_dp_pending_up_req *up_req;
3838         bool seqno;
3839
3840         if (!drm_dp_get_one_sb_msg(mgr, true))
3841                 goto out;
3842
3843         if (!mgr->up_req_recv.have_eomt)
3844                 return 0;
3845
3846         up_req = kzalloc(sizeof(*up_req), GFP_KERNEL);
3847         if (!up_req) {
3848                 DRM_ERROR("Not enough memory to process MST up req\n");
3849                 return -ENOMEM;
3850         }
3851         INIT_LIST_HEAD(&up_req->next);
3852
3853         seqno = hdr->seqno;
3854         drm_dp_sideband_parse_req(&mgr->up_req_recv, &up_req->msg);
3855
3856         if (up_req->msg.req_type != DP_CONNECTION_STATUS_NOTIFY &&
3857             up_req->msg.req_type != DP_RESOURCE_STATUS_NOTIFY) {
3858                 DRM_DEBUG_KMS("Received unknown up req type, ignoring: %x\n",
3859                               up_req->msg.req_type);
3860                 kfree(up_req);
3861                 goto out;
3862         }
3863
3864         drm_dp_send_up_ack_reply(mgr, mgr->mst_primary, up_req->msg.req_type,
3865                                  seqno, false);
3866
3867         if (up_req->msg.req_type == DP_CONNECTION_STATUS_NOTIFY) {
3868                 const struct drm_dp_connection_status_notify *conn_stat =
3869                         &up_req->msg.u.conn_stat;
3870
3871                 DRM_DEBUG_KMS("Got CSN: pn: %d ldps:%d ddps: %d mcs: %d ip: %d pdt: %d\n",
3872                               conn_stat->port_number,
3873                               conn_stat->legacy_device_plug_status,
3874                               conn_stat->displayport_device_plug_status,
3875                               conn_stat->message_capability_status,
3876                               conn_stat->input_port,
3877                               conn_stat->peer_device_type);
3878         } else if (up_req->msg.req_type == DP_RESOURCE_STATUS_NOTIFY) {
3879                 const struct drm_dp_resource_status_notify *res_stat =
3880                         &up_req->msg.u.resource_stat;
3881
3882                 DRM_DEBUG_KMS("Got RSN: pn: %d avail_pbn %d\n",
3883                               res_stat->port_number,
3884                               res_stat->available_pbn);
3885         }
3886
3887         up_req->hdr = *hdr;
3888         mutex_lock(&mgr->up_req_lock);
3889         list_add_tail(&up_req->next, &mgr->up_req_list);
3890         mutex_unlock(&mgr->up_req_lock);
3891         queue_work(system_long_wq, &mgr->up_req_work);
3892
3893 out:
3894         memset(&mgr->up_req_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
3895         return 0;
3896 }
3897
3898 /**
3899  * drm_dp_mst_hpd_irq() - MST hotplug IRQ notify
3900  * @mgr: manager to notify irq for.
3901  * @esi: 4 bytes from SINK_COUNT_ESI
3902  * @handled: whether the hpd interrupt was consumed or not
3903  *
3904  * This should be called from the driver when it detects a short IRQ,
3905  * along with the value of the DEVICE_SERVICE_IRQ_VECTOR_ESI0. The
3906  * topology manager will process the sideband messages received as a result
3907  * of this.
3908  */
3909 int drm_dp_mst_hpd_irq(struct drm_dp_mst_topology_mgr *mgr, u8 *esi, bool *handled)
3910 {
3911         int ret = 0;
3912         int sc;
3913         *handled = false;
3914         sc = esi[0] & 0x3f;
3915
3916         if (sc != mgr->sink_count) {
3917                 mgr->sink_count = sc;
3918                 *handled = true;
3919         }
3920
3921         if (esi[1] & DP_DOWN_REP_MSG_RDY) {
3922                 ret = drm_dp_mst_handle_down_rep(mgr);
3923                 *handled = true;
3924         }
3925
3926         if (esi[1] & DP_UP_REQ_MSG_RDY) {
3927                 ret |= drm_dp_mst_handle_up_req(mgr);
3928                 *handled = true;
3929         }
3930
3931         drm_dp_mst_kick_tx(mgr);
3932         return ret;
3933 }
3934 EXPORT_SYMBOL(drm_dp_mst_hpd_irq);
3935
3936 /**
3937  * drm_dp_mst_detect_port() - get connection status for an MST port
3938  * @connector: DRM connector for this port
3939  * @ctx: The acquisition context to use for grabbing locks
3940  * @mgr: manager for this port
3941  * @port: pointer to a port
3942  *
3943  * This returns the current connection state for a port.
3944  */
3945 int
3946 drm_dp_mst_detect_port(struct drm_connector *connector,
3947                        struct drm_modeset_acquire_ctx *ctx,
3948                        struct drm_dp_mst_topology_mgr *mgr,
3949                        struct drm_dp_mst_port *port)
3950 {
3951         int ret;
3952
3953         /* we need to search for the port in the mgr in case it's gone */
3954         port = drm_dp_mst_topology_get_port_validated(mgr, port);
3955         if (!port)
3956                 return connector_status_disconnected;
3957
3958         ret = drm_modeset_lock(&mgr->base.lock, ctx);
3959         if (ret)
3960                 goto out;
3961
3962         ret = connector_status_disconnected;
3963
3964         if (!port->ddps)
3965                 goto out;
3966
3967         switch (port->pdt) {
3968         case DP_PEER_DEVICE_NONE:
3969         case DP_PEER_DEVICE_MST_BRANCHING:
3970                 break;
3971
3972         case DP_PEER_DEVICE_SST_SINK:
3973                 ret = connector_status_connected;
3974                 /* for logical ports - cache the EDID */
3975                 if (port->port_num >= 8 && !port->cached_edid) {
3976                         port->cached_edid = drm_get_edid(connector, &port->aux.ddc);
3977                 }
3978                 break;
3979         case DP_PEER_DEVICE_DP_LEGACY_CONV:
3980                 if (port->ldps)
3981                         ret = connector_status_connected;
3982                 break;
3983         }
3984 out:
3985         drm_dp_mst_topology_put_port(port);
3986         return ret;
3987 }
3988 EXPORT_SYMBOL(drm_dp_mst_detect_port);
3989
3990 /**
3991  * drm_dp_mst_port_has_audio() - Check whether port has audio capability or not
3992  * @mgr: manager for this port
3993  * @port: unverified pointer to a port.
3994  *
3995  * This returns whether the port supports audio or not.
3996  */
3997 bool drm_dp_mst_port_has_audio(struct drm_dp_mst_topology_mgr *mgr,
3998                                         struct drm_dp_mst_port *port)
3999 {
4000         bool ret = false;
4001
4002         port = drm_dp_mst_topology_get_port_validated(mgr, port);
4003         if (!port)
4004                 return ret;
4005         ret = port->has_audio;
4006         drm_dp_mst_topology_put_port(port);
4007         return ret;
4008 }
4009 EXPORT_SYMBOL(drm_dp_mst_port_has_audio);
4010
4011 /**
4012  * drm_dp_mst_get_edid() - get EDID for an MST port
4013  * @connector: toplevel connector to get EDID for
4014  * @mgr: manager for this port
4015  * @port: unverified pointer to a port.
4016  *
4017  * This returns an EDID for the port connected to a connector,
4018  * It validates the pointer still exists so the caller doesn't require a
4019  * reference.
4020  */
4021 struct edid *drm_dp_mst_get_edid(struct drm_connector *connector, struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port)
4022 {
4023         struct edid *edid = NULL;
4024
4025         /* we need to search for the port in the mgr in case it's gone */
4026         port = drm_dp_mst_topology_get_port_validated(mgr, port);
4027         if (!port)
4028                 return NULL;
4029
4030         if (port->cached_edid)
4031                 edid = drm_edid_duplicate(port->cached_edid);
4032         else {
4033                 edid = drm_get_edid(connector, &port->aux.ddc);
4034         }
4035         port->has_audio = drm_detect_monitor_audio(edid);
4036         drm_dp_mst_topology_put_port(port);
4037         return edid;
4038 }
4039 EXPORT_SYMBOL(drm_dp_mst_get_edid);
4040
4041 /**
4042  * drm_dp_find_vcpi_slots() - Find VCPI slots for this PBN value
4043  * @mgr: manager to use
4044  * @pbn: payload bandwidth to convert into slots.
4045  *
4046  * Calculate the number of VCPI slots that will be required for the given PBN
4047  * value. This function is deprecated, and should not be used in atomic
4048  * drivers.
4049  *
4050  * RETURNS:
4051  * The total slots required for this port, or error.
4052  */
4053 int drm_dp_find_vcpi_slots(struct drm_dp_mst_topology_mgr *mgr,
4054                            int pbn)
4055 {
4056         int num_slots;
4057
4058         num_slots = DIV_ROUND_UP(pbn, mgr->pbn_div);
4059
4060         /* max. time slots - one slot for MTP header */
4061         if (num_slots > 63)
4062                 return -ENOSPC;
4063         return num_slots;
4064 }
4065 EXPORT_SYMBOL(drm_dp_find_vcpi_slots);
4066
4067 static int drm_dp_init_vcpi(struct drm_dp_mst_topology_mgr *mgr,
4068                             struct drm_dp_vcpi *vcpi, int pbn, int slots)
4069 {
4070         int ret;
4071
4072         /* max. time slots - one slot for MTP header */
4073         if (slots > 63)
4074                 return -ENOSPC;
4075
4076         vcpi->pbn = pbn;
4077         vcpi->aligned_pbn = slots * mgr->pbn_div;
4078         vcpi->num_slots = slots;
4079
4080         ret = drm_dp_mst_assign_payload_id(mgr, vcpi);
4081         if (ret < 0)
4082                 return ret;
4083         return 0;
4084 }
4085
4086 /**
4087  * drm_dp_atomic_find_vcpi_slots() - Find and add VCPI slots to the state
4088  * @state: global atomic state
4089  * @mgr: MST topology manager for the port
4090  * @port: port to find vcpi slots for
4091  * @pbn: bandwidth required for the mode in PBN
4092  *
4093  * Allocates VCPI slots to @port, replacing any previous VCPI allocations it
4094  * may have had. Any atomic drivers which support MST must call this function
4095  * in their &drm_encoder_helper_funcs.atomic_check() callback to change the
4096  * current VCPI allocation for the new state, but only when
4097  * &drm_crtc_state.mode_changed or &drm_crtc_state.connectors_changed is set
4098  * to ensure compatibility with userspace applications that still use the
4099  * legacy modesetting UAPI.
4100  *
4101  * Allocations set by this function are not checked against the bandwidth
4102  * restraints of @mgr until the driver calls drm_dp_mst_atomic_check().
4103  *
4104  * Additionally, it is OK to call this function multiple times on the same
4105  * @port as needed. It is not OK however, to call this function and
4106  * drm_dp_atomic_release_vcpi_slots() in the same atomic check phase.
4107  *
4108  * See also:
4109  * drm_dp_atomic_release_vcpi_slots()
4110  * drm_dp_mst_atomic_check()
4111  *
4112  * Returns:
4113  * Total slots in the atomic state assigned for this port, or a negative error
4114  * code if the port no longer exists
4115  */
4116 int drm_dp_atomic_find_vcpi_slots(struct drm_atomic_state *state,
4117                                   struct drm_dp_mst_topology_mgr *mgr,
4118                                   struct drm_dp_mst_port *port, int pbn)
4119 {
4120         struct drm_dp_mst_topology_state *topology_state;
4121         struct drm_dp_vcpi_allocation *pos, *vcpi = NULL;
4122         int prev_slots, req_slots;
4123
4124         topology_state = drm_atomic_get_mst_topology_state(state, mgr);
4125         if (IS_ERR(topology_state))
4126                 return PTR_ERR(topology_state);
4127
4128         /* Find the current allocation for this port, if any */
4129         list_for_each_entry(pos, &topology_state->vcpis, next) {
4130                 if (pos->port == port) {
4131                         vcpi = pos;
4132                         prev_slots = vcpi->vcpi;
4133
4134                         /*
4135                          * This should never happen, unless the driver tries
4136                          * releasing and allocating the same VCPI allocation,
4137                          * which is an error
4138                          */
4139                         if (WARN_ON(!prev_slots)) {
4140                                 DRM_ERROR("cannot allocate and release VCPI on [MST PORT:%p] in the same state\n",
4141                                           port);
4142                                 return -EINVAL;
4143                         }
4144
4145                         break;
4146                 }
4147         }
4148         if (!vcpi)
4149                 prev_slots = 0;
4150
4151         req_slots = DIV_ROUND_UP(pbn, mgr->pbn_div);
4152
4153         DRM_DEBUG_ATOMIC("[CONNECTOR:%d:%s] [MST PORT:%p] VCPI %d -> %d\n",
4154                          port->connector->base.id, port->connector->name,
4155                          port, prev_slots, req_slots);
4156
4157         /* Add the new allocation to the state */
4158         if (!vcpi) {
4159                 vcpi = kzalloc(sizeof(*vcpi), GFP_KERNEL);
4160                 if (!vcpi)
4161                         return -ENOMEM;
4162
4163                 drm_dp_mst_get_port_malloc(port);
4164                 vcpi->port = port;
4165                 list_add(&vcpi->next, &topology_state->vcpis);
4166         }
4167         vcpi->vcpi = req_slots;
4168
4169         return req_slots;
4170 }
4171 EXPORT_SYMBOL(drm_dp_atomic_find_vcpi_slots);
4172
4173 /**
4174  * drm_dp_atomic_release_vcpi_slots() - Release allocated vcpi slots
4175  * @state: global atomic state
4176  * @mgr: MST topology manager for the port
4177  * @port: The port to release the VCPI slots from
4178  *
4179  * Releases any VCPI slots that have been allocated to a port in the atomic
4180  * state. Any atomic drivers which support MST must call this function in
4181  * their &drm_connector_helper_funcs.atomic_check() callback when the
4182  * connector will no longer have VCPI allocated (e.g. because its CRTC was
4183  * removed) when it had VCPI allocated in the previous atomic state.
4184  *
4185  * It is OK to call this even if @port has been removed from the system.
4186  * Additionally, it is OK to call this function multiple times on the same
4187  * @port as needed. It is not OK however, to call this function and
4188  * drm_dp_atomic_find_vcpi_slots() on the same @port in a single atomic check
4189  * phase.
4190  *
4191  * See also:
4192  * drm_dp_atomic_find_vcpi_slots()
4193  * drm_dp_mst_atomic_check()
4194  *
4195  * Returns:
4196  * 0 if all slots for this port were added back to
4197  * &drm_dp_mst_topology_state.avail_slots or negative error code
4198  */
4199 int drm_dp_atomic_release_vcpi_slots(struct drm_atomic_state *state,
4200                                      struct drm_dp_mst_topology_mgr *mgr,
4201                                      struct drm_dp_mst_port *port)
4202 {
4203         struct drm_dp_mst_topology_state *topology_state;
4204         struct drm_dp_vcpi_allocation *pos;
4205         bool found = false;
4206
4207         topology_state = drm_atomic_get_mst_topology_state(state, mgr);
4208         if (IS_ERR(topology_state))
4209                 return PTR_ERR(topology_state);
4210
4211         list_for_each_entry(pos, &topology_state->vcpis, next) {
4212                 if (pos->port == port) {
4213                         found = true;
4214                         break;
4215                 }
4216         }
4217         if (WARN_ON(!found)) {
4218                 DRM_ERROR("no VCPI for [MST PORT:%p] found in mst state %p\n",
4219                           port, &topology_state->base);
4220                 return -EINVAL;
4221         }
4222
4223         DRM_DEBUG_ATOMIC("[MST PORT:%p] VCPI %d -> 0\n", port, pos->vcpi);
4224         if (pos->vcpi) {
4225                 drm_dp_mst_put_port_malloc(port);
4226                 pos->vcpi = 0;
4227         }
4228
4229         return 0;
4230 }
4231 EXPORT_SYMBOL(drm_dp_atomic_release_vcpi_slots);
4232
4233 /**
4234  * drm_dp_mst_allocate_vcpi() - Allocate a virtual channel
4235  * @mgr: manager for this port
4236  * @port: port to allocate a virtual channel for.
4237  * @pbn: payload bandwidth number to request
4238  * @slots: returned number of slots for this PBN.
4239  */
4240 bool drm_dp_mst_allocate_vcpi(struct drm_dp_mst_topology_mgr *mgr,
4241                               struct drm_dp_mst_port *port, int pbn, int slots)
4242 {
4243         int ret;
4244
4245         port = drm_dp_mst_topology_get_port_validated(mgr, port);
4246         if (!port)
4247                 return false;
4248
4249         if (slots < 0)
4250                 return false;
4251
4252         if (port->vcpi.vcpi > 0) {
4253                 DRM_DEBUG_KMS("payload: vcpi %d already allocated for pbn %d - requested pbn %d\n",
4254                               port->vcpi.vcpi, port->vcpi.pbn, pbn);
4255                 if (pbn == port->vcpi.pbn) {
4256                         drm_dp_mst_topology_put_port(port);
4257                         return true;
4258                 }
4259         }
4260
4261         ret = drm_dp_init_vcpi(mgr, &port->vcpi, pbn, slots);
4262         if (ret) {
4263                 DRM_DEBUG_KMS("failed to init vcpi slots=%d max=63 ret=%d\n",
4264                               DIV_ROUND_UP(pbn, mgr->pbn_div), ret);
4265                 goto out;
4266         }
4267         DRM_DEBUG_KMS("initing vcpi for pbn=%d slots=%d\n",
4268                       pbn, port->vcpi.num_slots);
4269
4270         /* Keep port allocated until its payload has been removed */
4271         drm_dp_mst_get_port_malloc(port);
4272         drm_dp_mst_topology_put_port(port);
4273         return true;
4274 out:
4275         return false;
4276 }
4277 EXPORT_SYMBOL(drm_dp_mst_allocate_vcpi);
4278
4279 int drm_dp_mst_get_vcpi_slots(struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port)
4280 {
4281         int slots = 0;
4282         port = drm_dp_mst_topology_get_port_validated(mgr, port);
4283         if (!port)
4284                 return slots;
4285
4286         slots = port->vcpi.num_slots;
4287         drm_dp_mst_topology_put_port(port);
4288         return slots;
4289 }
4290 EXPORT_SYMBOL(drm_dp_mst_get_vcpi_slots);
4291
4292 /**
4293  * drm_dp_mst_reset_vcpi_slots() - Reset number of slots to 0 for VCPI
4294  * @mgr: manager for this port
4295  * @port: unverified pointer to a port.
4296  *
4297  * This just resets the number of slots for the ports VCPI for later programming.
4298  */
4299 void drm_dp_mst_reset_vcpi_slots(struct drm_dp_mst_topology_mgr *mgr, struct drm_dp_mst_port *port)
4300 {
4301         /*
4302          * A port with VCPI will remain allocated until its VCPI is
4303          * released, no verified ref needed
4304          */
4305
4306         port->vcpi.num_slots = 0;
4307 }
4308 EXPORT_SYMBOL(drm_dp_mst_reset_vcpi_slots);
4309
4310 /**
4311  * drm_dp_mst_deallocate_vcpi() - deallocate a VCPI
4312  * @mgr: manager for this port
4313  * @port: port to deallocate vcpi for
4314  *
4315  * This can be called unconditionally, regardless of whether
4316  * drm_dp_mst_allocate_vcpi() succeeded or not.
4317  */
4318 void drm_dp_mst_deallocate_vcpi(struct drm_dp_mst_topology_mgr *mgr,
4319                                 struct drm_dp_mst_port *port)
4320 {
4321         if (!port->vcpi.vcpi)
4322                 return;
4323
4324         drm_dp_mst_put_payload_id(mgr, port->vcpi.vcpi);
4325         port->vcpi.num_slots = 0;
4326         port->vcpi.pbn = 0;
4327         port->vcpi.aligned_pbn = 0;
4328         port->vcpi.vcpi = 0;
4329         drm_dp_mst_put_port_malloc(port);
4330 }
4331 EXPORT_SYMBOL(drm_dp_mst_deallocate_vcpi);
4332
4333 static int drm_dp_dpcd_write_payload(struct drm_dp_mst_topology_mgr *mgr,
4334                                      int id, struct drm_dp_payload *payload)
4335 {
4336         u8 payload_alloc[3], status;
4337         int ret;
4338         int retries = 0;
4339
4340         drm_dp_dpcd_writeb(mgr->aux, DP_PAYLOAD_TABLE_UPDATE_STATUS,
4341                            DP_PAYLOAD_TABLE_UPDATED);
4342
4343         payload_alloc[0] = id;
4344         payload_alloc[1] = payload->start_slot;
4345         payload_alloc[2] = payload->num_slots;
4346
4347         ret = drm_dp_dpcd_write(mgr->aux, DP_PAYLOAD_ALLOCATE_SET, payload_alloc, 3);
4348         if (ret != 3) {
4349                 DRM_DEBUG_KMS("failed to write payload allocation %d\n", ret);
4350                 goto fail;
4351         }
4352
4353 retry:
4354         ret = drm_dp_dpcd_readb(mgr->aux, DP_PAYLOAD_TABLE_UPDATE_STATUS, &status);
4355         if (ret < 0) {
4356                 DRM_DEBUG_KMS("failed to read payload table status %d\n", ret);
4357                 goto fail;
4358         }
4359
4360         if (!(status & DP_PAYLOAD_TABLE_UPDATED)) {
4361                 retries++;
4362                 if (retries < 20) {
4363                         usleep_range(10000, 20000);
4364                         goto retry;
4365                 }
4366                 DRM_DEBUG_KMS("status not set after read payload table status %d\n", status);
4367                 ret = -EINVAL;
4368                 goto fail;
4369         }
4370         ret = 0;
4371 fail:
4372         return ret;
4373 }
4374
4375
4376 /**
4377  * drm_dp_check_act_status() - Check ACT handled status.
4378  * @mgr: manager to use
4379  *
4380  * Check the payload status bits in the DPCD for ACT handled completion.
4381  */
4382 int drm_dp_check_act_status(struct drm_dp_mst_topology_mgr *mgr)
4383 {
4384         u8 status;
4385         int ret;
4386         int count = 0;
4387
4388         do {
4389                 ret = drm_dp_dpcd_readb(mgr->aux, DP_PAYLOAD_TABLE_UPDATE_STATUS, &status);
4390
4391                 if (ret < 0) {
4392                         DRM_DEBUG_KMS("failed to read payload table status %d\n", ret);
4393                         goto fail;
4394                 }
4395
4396                 if (status & DP_PAYLOAD_ACT_HANDLED)
4397                         break;
4398                 count++;
4399                 udelay(100);
4400
4401         } while (count < 30);
4402
4403         if (!(status & DP_PAYLOAD_ACT_HANDLED)) {
4404                 DRM_DEBUG_KMS("failed to get ACT bit %d after %d retries\n", status, count);
4405                 ret = -EINVAL;
4406                 goto fail;
4407         }
4408         return 0;
4409 fail:
4410         return ret;
4411 }
4412 EXPORT_SYMBOL(drm_dp_check_act_status);
4413
4414 /**
4415  * drm_dp_calc_pbn_mode() - Calculate the PBN for a mode.
4416  * @clock: dot clock for the mode
4417  * @bpp: bpp for the mode.
4418  * @dsc: DSC mode. If true, bpp has units of 1/16 of a bit per pixel
4419  *
4420  * This uses the formula in the spec to calculate the PBN value for a mode.
4421  */
4422 int drm_dp_calc_pbn_mode(int clock, int bpp, bool dsc)
4423 {
4424         /*
4425          * margin 5300ppm + 300ppm ~ 0.6% as per spec, factor is 1.006
4426          * The unit of 54/64Mbytes/sec is an arbitrary unit chosen based on
4427          * common multiplier to render an integer PBN for all link rate/lane
4428          * counts combinations
4429          * calculate
4430          * peak_kbps *= (1006/1000)
4431          * peak_kbps *= (64/54)
4432          * peak_kbps *= 8    convert to bytes
4433          *
4434          * If the bpp is in units of 1/16, further divide by 16. Put this
4435          * factor in the numerator rather than the denominator to avoid
4436          * integer overflow
4437          */
4438
4439         if (dsc)
4440                 return DIV_ROUND_UP_ULL(mul_u32_u32(clock * (bpp / 16), 64 * 1006),
4441                                         8 * 54 * 1000 * 1000);
4442
4443         return DIV_ROUND_UP_ULL(mul_u32_u32(clock * bpp, 64 * 1006),
4444                                 8 * 54 * 1000 * 1000);
4445 }
4446 EXPORT_SYMBOL(drm_dp_calc_pbn_mode);
4447
4448 /* we want to kick the TX after we've ack the up/down IRQs. */
4449 static void drm_dp_mst_kick_tx(struct drm_dp_mst_topology_mgr *mgr)
4450 {
4451         queue_work(system_long_wq, &mgr->tx_work);
4452 }
4453
4454 static void drm_dp_mst_dump_mstb(struct seq_file *m,
4455                                  struct drm_dp_mst_branch *mstb)
4456 {
4457         struct drm_dp_mst_port *port;
4458         int tabs = mstb->lct;
4459         char prefix[10];
4460         int i;
4461
4462         for (i = 0; i < tabs; i++)
4463                 prefix[i] = '\t';
4464         prefix[i] = '\0';
4465
4466         seq_printf(m, "%smst: %p, %d\n", prefix, mstb, mstb->num_ports);
4467         list_for_each_entry(port, &mstb->ports, next) {
4468                 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);
4469                 if (port->mstb)
4470                         drm_dp_mst_dump_mstb(m, port->mstb);
4471         }
4472 }
4473
4474 #define DP_PAYLOAD_TABLE_SIZE           64
4475
4476 static bool dump_dp_payload_table(struct drm_dp_mst_topology_mgr *mgr,
4477                                   char *buf)
4478 {
4479         int i;
4480
4481         for (i = 0; i < DP_PAYLOAD_TABLE_SIZE; i += 16) {
4482                 if (drm_dp_dpcd_read(mgr->aux,
4483                                      DP_PAYLOAD_TABLE_UPDATE_STATUS + i,
4484                                      &buf[i], 16) != 16)
4485                         return false;
4486         }
4487         return true;
4488 }
4489
4490 static void fetch_monitor_name(struct drm_dp_mst_topology_mgr *mgr,
4491                                struct drm_dp_mst_port *port, char *name,
4492                                int namelen)
4493 {
4494         struct edid *mst_edid;
4495
4496         mst_edid = drm_dp_mst_get_edid(port->connector, mgr, port);
4497         drm_edid_get_monitor_name(mst_edid, name, namelen);
4498 }
4499
4500 /**
4501  * drm_dp_mst_dump_topology(): dump topology to seq file.
4502  * @m: seq_file to dump output to
4503  * @mgr: manager to dump current topology for.
4504  *
4505  * helper to dump MST topology to a seq file for debugfs.
4506  */
4507 void drm_dp_mst_dump_topology(struct seq_file *m,
4508                               struct drm_dp_mst_topology_mgr *mgr)
4509 {
4510         int i;
4511         struct drm_dp_mst_port *port;
4512
4513         mutex_lock(&mgr->lock);
4514         if (mgr->mst_primary)
4515                 drm_dp_mst_dump_mstb(m, mgr->mst_primary);
4516
4517         /* dump VCPIs */
4518         mutex_unlock(&mgr->lock);
4519
4520         mutex_lock(&mgr->payload_lock);
4521         seq_printf(m, "vcpi: %lx %lx %d\n", mgr->payload_mask, mgr->vcpi_mask,
4522                 mgr->max_payloads);
4523
4524         for (i = 0; i < mgr->max_payloads; i++) {
4525                 if (mgr->proposed_vcpis[i]) {
4526                         char name[14];
4527
4528                         port = container_of(mgr->proposed_vcpis[i], struct drm_dp_mst_port, vcpi);
4529                         fetch_monitor_name(mgr, port, name, sizeof(name));
4530                         seq_printf(m, "vcpi %d: %d %d %d sink name: %s\n", i,
4531                                    port->port_num, port->vcpi.vcpi,
4532                                    port->vcpi.num_slots,
4533                                    (*name != 0) ? name :  "Unknown");
4534                 } else
4535                         seq_printf(m, "vcpi %d:unused\n", i);
4536         }
4537         for (i = 0; i < mgr->max_payloads; i++) {
4538                 seq_printf(m, "payload %d: %d, %d, %d\n",
4539                            i,
4540                            mgr->payloads[i].payload_state,
4541                            mgr->payloads[i].start_slot,
4542                            mgr->payloads[i].num_slots);
4543
4544
4545         }
4546         mutex_unlock(&mgr->payload_lock);
4547
4548         mutex_lock(&mgr->lock);
4549         if (mgr->mst_primary) {
4550                 u8 buf[DP_PAYLOAD_TABLE_SIZE];
4551                 int ret;
4552
4553                 ret = drm_dp_dpcd_read(mgr->aux, DP_DPCD_REV, buf, DP_RECEIVER_CAP_SIZE);
4554                 seq_printf(m, "dpcd: %*ph\n", DP_RECEIVER_CAP_SIZE, buf);
4555                 ret = drm_dp_dpcd_read(mgr->aux, DP_FAUX_CAP, buf, 2);
4556                 seq_printf(m, "faux/mst: %*ph\n", 2, buf);
4557                 ret = drm_dp_dpcd_read(mgr->aux, DP_MSTM_CTRL, buf, 1);
4558                 seq_printf(m, "mst ctrl: %*ph\n", 1, buf);
4559
4560                 /* dump the standard OUI branch header */
4561                 ret = drm_dp_dpcd_read(mgr->aux, DP_BRANCH_OUI, buf, DP_BRANCH_OUI_HEADER_SIZE);
4562                 seq_printf(m, "branch oui: %*phN devid: ", 3, buf);
4563                 for (i = 0x3; i < 0x8 && buf[i]; i++)
4564                         seq_printf(m, "%c", buf[i]);
4565                 seq_printf(m, " revision: hw: %x.%x sw: %x.%x\n",
4566                            buf[0x9] >> 4, buf[0x9] & 0xf, buf[0xa], buf[0xb]);
4567                 if (dump_dp_payload_table(mgr, buf))
4568                         seq_printf(m, "payload table: %*ph\n", DP_PAYLOAD_TABLE_SIZE, buf);
4569         }
4570
4571         mutex_unlock(&mgr->lock);
4572
4573 }
4574 EXPORT_SYMBOL(drm_dp_mst_dump_topology);
4575
4576 static void drm_dp_tx_work(struct work_struct *work)
4577 {
4578         struct drm_dp_mst_topology_mgr *mgr = container_of(work, struct drm_dp_mst_topology_mgr, tx_work);
4579
4580         mutex_lock(&mgr->qlock);
4581         if (!list_empty(&mgr->tx_msg_downq))
4582                 process_single_down_tx_qlock(mgr);
4583         mutex_unlock(&mgr->qlock);
4584 }
4585
4586 static inline void
4587 drm_dp_delayed_destroy_port(struct drm_dp_mst_port *port)
4588 {
4589         if (port->connector)
4590                 port->mgr->cbs->destroy_connector(port->mgr, port->connector);
4591
4592         drm_dp_port_set_pdt(port, DP_PEER_DEVICE_NONE);
4593         drm_dp_mst_put_port_malloc(port);
4594 }
4595
4596 static inline void
4597 drm_dp_delayed_destroy_mstb(struct drm_dp_mst_branch *mstb)
4598 {
4599         struct drm_dp_mst_topology_mgr *mgr = mstb->mgr;
4600         struct drm_dp_mst_port *port, *tmp;
4601         bool wake_tx = false;
4602
4603         mutex_lock(&mgr->lock);
4604         list_for_each_entry_safe(port, tmp, &mstb->ports, next) {
4605                 list_del(&port->next);
4606                 drm_dp_mst_topology_put_port(port);
4607         }
4608         mutex_unlock(&mgr->lock);
4609
4610         /* drop any tx slots msg */
4611         mutex_lock(&mstb->mgr->qlock);
4612         if (mstb->tx_slots[0]) {
4613                 mstb->tx_slots[0]->state = DRM_DP_SIDEBAND_TX_TIMEOUT;
4614                 mstb->tx_slots[0] = NULL;
4615                 wake_tx = true;
4616         }
4617         if (mstb->tx_slots[1]) {
4618                 mstb->tx_slots[1]->state = DRM_DP_SIDEBAND_TX_TIMEOUT;
4619                 mstb->tx_slots[1] = NULL;
4620                 wake_tx = true;
4621         }
4622         mutex_unlock(&mstb->mgr->qlock);
4623
4624         if (wake_tx)
4625                 wake_up_all(&mstb->mgr->tx_waitq);
4626
4627         drm_dp_mst_put_mstb_malloc(mstb);
4628 }
4629
4630 static void drm_dp_delayed_destroy_work(struct work_struct *work)
4631 {
4632         struct drm_dp_mst_topology_mgr *mgr =
4633                 container_of(work, struct drm_dp_mst_topology_mgr,
4634                              delayed_destroy_work);
4635         bool send_hotplug = false, go_again;
4636
4637         /*
4638          * Not a regular list traverse as we have to drop the destroy
4639          * connector lock before destroying the mstb/port, to avoid AB->BA
4640          * ordering between this lock and the config mutex.
4641          */
4642         do {
4643                 go_again = false;
4644
4645                 for (;;) {
4646                         struct drm_dp_mst_branch *mstb;
4647
4648                         mutex_lock(&mgr->delayed_destroy_lock);
4649                         mstb = list_first_entry_or_null(&mgr->destroy_branch_device_list,
4650                                                         struct drm_dp_mst_branch,
4651                                                         destroy_next);
4652                         if (mstb)
4653                                 list_del(&mstb->destroy_next);
4654                         mutex_unlock(&mgr->delayed_destroy_lock);
4655
4656                         if (!mstb)
4657                                 break;
4658
4659                         drm_dp_delayed_destroy_mstb(mstb);
4660                         go_again = true;
4661                 }
4662
4663                 for (;;) {
4664                         struct drm_dp_mst_port *port;
4665
4666                         mutex_lock(&mgr->delayed_destroy_lock);
4667                         port = list_first_entry_or_null(&mgr->destroy_port_list,
4668                                                         struct drm_dp_mst_port,
4669                                                         next);
4670                         if (port)
4671                                 list_del(&port->next);
4672                         mutex_unlock(&mgr->delayed_destroy_lock);
4673
4674                         if (!port)
4675                                 break;
4676
4677                         drm_dp_delayed_destroy_port(port);
4678                         send_hotplug = true;
4679                         go_again = true;
4680                 }
4681         } while (go_again);
4682
4683         if (send_hotplug)
4684                 drm_kms_helper_hotplug_event(mgr->dev);
4685 }
4686
4687 static struct drm_private_state *
4688 drm_dp_mst_duplicate_state(struct drm_private_obj *obj)
4689 {
4690         struct drm_dp_mst_topology_state *state, *old_state =
4691                 to_dp_mst_topology_state(obj->state);
4692         struct drm_dp_vcpi_allocation *pos, *vcpi;
4693
4694         state = kmemdup(old_state, sizeof(*state), GFP_KERNEL);
4695         if (!state)
4696                 return NULL;
4697
4698         __drm_atomic_helper_private_obj_duplicate_state(obj, &state->base);
4699
4700         INIT_LIST_HEAD(&state->vcpis);
4701
4702         list_for_each_entry(pos, &old_state->vcpis, next) {
4703                 /* Prune leftover freed VCPI allocations */
4704                 if (!pos->vcpi)
4705                         continue;
4706
4707                 vcpi = kmemdup(pos, sizeof(*vcpi), GFP_KERNEL);
4708                 if (!vcpi)
4709                         goto fail;
4710
4711                 drm_dp_mst_get_port_malloc(vcpi->port);
4712                 list_add(&vcpi->next, &state->vcpis);
4713         }
4714
4715         return &state->base;
4716
4717 fail:
4718         list_for_each_entry_safe(pos, vcpi, &state->vcpis, next) {
4719                 drm_dp_mst_put_port_malloc(pos->port);
4720                 kfree(pos);
4721         }
4722         kfree(state);
4723
4724         return NULL;
4725 }
4726
4727 static void drm_dp_mst_destroy_state(struct drm_private_obj *obj,
4728                                      struct drm_private_state *state)
4729 {
4730         struct drm_dp_mst_topology_state *mst_state =
4731                 to_dp_mst_topology_state(state);
4732         struct drm_dp_vcpi_allocation *pos, *tmp;
4733
4734         list_for_each_entry_safe(pos, tmp, &mst_state->vcpis, next) {
4735                 /* We only keep references to ports with non-zero VCPIs */
4736                 if (pos->vcpi)
4737                         drm_dp_mst_put_port_malloc(pos->port);
4738                 kfree(pos);
4739         }
4740
4741         kfree(mst_state);
4742 }
4743
4744 static inline int
4745 drm_dp_mst_atomic_check_topology_state(struct drm_dp_mst_topology_mgr *mgr,
4746                                        struct drm_dp_mst_topology_state *mst_state)
4747 {
4748         struct drm_dp_vcpi_allocation *vcpi;
4749         int avail_slots = 63, payload_count = 0;
4750
4751         list_for_each_entry(vcpi, &mst_state->vcpis, next) {
4752                 /* Releasing VCPI is always OK-even if the port is gone */
4753                 if (!vcpi->vcpi) {
4754                         DRM_DEBUG_ATOMIC("[MST PORT:%p] releases all VCPI slots\n",
4755                                          vcpi->port);
4756                         continue;
4757                 }
4758
4759                 DRM_DEBUG_ATOMIC("[MST PORT:%p] requires %d vcpi slots\n",
4760                                  vcpi->port, vcpi->vcpi);
4761
4762                 avail_slots -= vcpi->vcpi;
4763                 if (avail_slots < 0) {
4764                         DRM_DEBUG_ATOMIC("[MST PORT:%p] not enough VCPI slots in mst state %p (avail=%d)\n",
4765                                          vcpi->port, mst_state,
4766                                          avail_slots + vcpi->vcpi);
4767                         return -ENOSPC;
4768                 }
4769
4770                 if (++payload_count > mgr->max_payloads) {
4771                         DRM_DEBUG_ATOMIC("[MST MGR:%p] state %p has too many payloads (max=%d)\n",
4772                                          mgr, mst_state, mgr->max_payloads);
4773                         return -EINVAL;
4774                 }
4775         }
4776         DRM_DEBUG_ATOMIC("[MST MGR:%p] mst state %p VCPI avail=%d used=%d\n",
4777                          mgr, mst_state, avail_slots,
4778                          63 - avail_slots);
4779
4780         return 0;
4781 }
4782
4783 /**
4784  * drm_dp_mst_atomic_check - Check that the new state of an MST topology in an
4785  * atomic update is valid
4786  * @state: Pointer to the new &struct drm_dp_mst_topology_state
4787  *
4788  * Checks the given topology state for an atomic update to ensure that it's
4789  * valid. This includes checking whether there's enough bandwidth to support
4790  * the new VCPI allocations in the atomic update.
4791  *
4792  * Any atomic drivers supporting DP MST must make sure to call this after
4793  * checking the rest of their state in their
4794  * &drm_mode_config_funcs.atomic_check() callback.
4795  *
4796  * See also:
4797  * drm_dp_atomic_find_vcpi_slots()
4798  * drm_dp_atomic_release_vcpi_slots()
4799  *
4800  * Returns:
4801  *
4802  * 0 if the new state is valid, negative error code otherwise.
4803  */
4804 int drm_dp_mst_atomic_check(struct drm_atomic_state *state)
4805 {
4806         struct drm_dp_mst_topology_mgr *mgr;
4807         struct drm_dp_mst_topology_state *mst_state;
4808         int i, ret = 0;
4809
4810         for_each_new_mst_mgr_in_state(state, mgr, mst_state, i) {
4811                 ret = drm_dp_mst_atomic_check_topology_state(mgr, mst_state);
4812                 if (ret)
4813                         break;
4814         }
4815
4816         return ret;
4817 }
4818 EXPORT_SYMBOL(drm_dp_mst_atomic_check);
4819
4820 const struct drm_private_state_funcs drm_dp_mst_topology_state_funcs = {
4821         .atomic_duplicate_state = drm_dp_mst_duplicate_state,
4822         .atomic_destroy_state = drm_dp_mst_destroy_state,
4823 };
4824 EXPORT_SYMBOL(drm_dp_mst_topology_state_funcs);
4825
4826 /**
4827  * drm_atomic_get_mst_topology_state: get MST topology state
4828  *
4829  * @state: global atomic state
4830  * @mgr: MST topology manager, also the private object in this case
4831  *
4832  * This function wraps drm_atomic_get_priv_obj_state() passing in the MST atomic
4833  * state vtable so that the private object state returned is that of a MST
4834  * topology object. Also, drm_atomic_get_private_obj_state() expects the caller
4835  * to care of the locking, so warn if don't hold the connection_mutex.
4836  *
4837  * RETURNS:
4838  *
4839  * The MST topology state or error pointer.
4840  */
4841 struct drm_dp_mst_topology_state *drm_atomic_get_mst_topology_state(struct drm_atomic_state *state,
4842                                                                     struct drm_dp_mst_topology_mgr *mgr)
4843 {
4844         return to_dp_mst_topology_state(drm_atomic_get_private_obj_state(state, &mgr->base));
4845 }
4846 EXPORT_SYMBOL(drm_atomic_get_mst_topology_state);
4847
4848 /**
4849  * drm_dp_mst_topology_mgr_init - initialise a topology manager
4850  * @mgr: manager struct to initialise
4851  * @dev: device providing this structure - for i2c addition.
4852  * @aux: DP helper aux channel to talk to this device
4853  * @max_dpcd_transaction_bytes: hw specific DPCD transaction limit
4854  * @max_payloads: maximum number of payloads this GPU can source
4855  * @conn_base_id: the connector object ID the MST device is connected to.
4856  *
4857  * Return 0 for success, or negative error code on failure
4858  */
4859 int drm_dp_mst_topology_mgr_init(struct drm_dp_mst_topology_mgr *mgr,
4860                                  struct drm_device *dev, struct drm_dp_aux *aux,
4861                                  int max_dpcd_transaction_bytes,
4862                                  int max_payloads, int conn_base_id)
4863 {
4864         struct drm_dp_mst_topology_state *mst_state;
4865
4866         mutex_init(&mgr->lock);
4867         mutex_init(&mgr->qlock);
4868         mutex_init(&mgr->payload_lock);
4869         mutex_init(&mgr->delayed_destroy_lock);
4870         mutex_init(&mgr->up_req_lock);
4871         mutex_init(&mgr->probe_lock);
4872 #if IS_ENABLED(CONFIG_DRM_DEBUG_DP_MST_TOPOLOGY_REFS)
4873         mutex_init(&mgr->topology_ref_history_lock);
4874 #endif
4875         INIT_LIST_HEAD(&mgr->tx_msg_downq);
4876         INIT_LIST_HEAD(&mgr->destroy_port_list);
4877         INIT_LIST_HEAD(&mgr->destroy_branch_device_list);
4878         INIT_LIST_HEAD(&mgr->up_req_list);
4879         INIT_WORK(&mgr->work, drm_dp_mst_link_probe_work);
4880         INIT_WORK(&mgr->tx_work, drm_dp_tx_work);
4881         INIT_WORK(&mgr->delayed_destroy_work, drm_dp_delayed_destroy_work);
4882         INIT_WORK(&mgr->up_req_work, drm_dp_mst_up_req_work);
4883         init_waitqueue_head(&mgr->tx_waitq);
4884         mgr->dev = dev;
4885         mgr->aux = aux;
4886         mgr->max_dpcd_transaction_bytes = max_dpcd_transaction_bytes;
4887         mgr->max_payloads = max_payloads;
4888         mgr->conn_base_id = conn_base_id;
4889         if (max_payloads + 1 > sizeof(mgr->payload_mask) * 8 ||
4890             max_payloads + 1 > sizeof(mgr->vcpi_mask) * 8)
4891                 return -EINVAL;
4892         mgr->payloads = kcalloc(max_payloads, sizeof(struct drm_dp_payload), GFP_KERNEL);
4893         if (!mgr->payloads)
4894                 return -ENOMEM;
4895         mgr->proposed_vcpis = kcalloc(max_payloads, sizeof(struct drm_dp_vcpi *), GFP_KERNEL);
4896         if (!mgr->proposed_vcpis)
4897                 return -ENOMEM;
4898         set_bit(0, &mgr->payload_mask);
4899
4900         mst_state = kzalloc(sizeof(*mst_state), GFP_KERNEL);
4901         if (mst_state == NULL)
4902                 return -ENOMEM;
4903
4904         mst_state->mgr = mgr;
4905         INIT_LIST_HEAD(&mst_state->vcpis);
4906
4907         drm_atomic_private_obj_init(dev, &mgr->base,
4908                                     &mst_state->base,
4909                                     &drm_dp_mst_topology_state_funcs);
4910
4911         return 0;
4912 }
4913 EXPORT_SYMBOL(drm_dp_mst_topology_mgr_init);
4914
4915 /**
4916  * drm_dp_mst_topology_mgr_destroy() - destroy topology manager.
4917  * @mgr: manager to destroy
4918  */
4919 void drm_dp_mst_topology_mgr_destroy(struct drm_dp_mst_topology_mgr *mgr)
4920 {
4921         drm_dp_mst_topology_mgr_set_mst(mgr, false);
4922         flush_work(&mgr->work);
4923         cancel_work_sync(&mgr->delayed_destroy_work);
4924         mutex_lock(&mgr->payload_lock);
4925         kfree(mgr->payloads);
4926         mgr->payloads = NULL;
4927         kfree(mgr->proposed_vcpis);
4928         mgr->proposed_vcpis = NULL;
4929         mutex_unlock(&mgr->payload_lock);
4930         mgr->dev = NULL;
4931         mgr->aux = NULL;
4932         drm_atomic_private_obj_fini(&mgr->base);
4933         mgr->funcs = NULL;
4934
4935         mutex_destroy(&mgr->delayed_destroy_lock);
4936         mutex_destroy(&mgr->payload_lock);
4937         mutex_destroy(&mgr->qlock);
4938         mutex_destroy(&mgr->lock);
4939         mutex_destroy(&mgr->up_req_lock);
4940         mutex_destroy(&mgr->probe_lock);
4941 #if IS_ENABLED(CONFIG_DRM_DEBUG_DP_MST_TOPOLOGY_REFS)
4942         mutex_destroy(&mgr->topology_ref_history_lock);
4943 #endif
4944 }
4945 EXPORT_SYMBOL(drm_dp_mst_topology_mgr_destroy);
4946
4947 static bool remote_i2c_read_ok(const struct i2c_msg msgs[], int num)
4948 {
4949         int i;
4950
4951         if (num - 1 > DP_REMOTE_I2C_READ_MAX_TRANSACTIONS)
4952                 return false;
4953
4954         for (i = 0; i < num - 1; i++) {
4955                 if (msgs[i].flags & I2C_M_RD ||
4956                     msgs[i].len > 0xff)
4957                         return false;
4958         }
4959
4960         return msgs[num - 1].flags & I2C_M_RD &&
4961                 msgs[num - 1].len <= 0xff;
4962 }
4963
4964 /* I2C device */
4965 static int drm_dp_mst_i2c_xfer(struct i2c_adapter *adapter, struct i2c_msg *msgs,
4966                                int num)
4967 {
4968         struct drm_dp_aux *aux = adapter->algo_data;
4969         struct drm_dp_mst_port *port = container_of(aux, struct drm_dp_mst_port, aux);
4970         struct drm_dp_mst_branch *mstb;
4971         struct drm_dp_mst_topology_mgr *mgr = port->mgr;
4972         unsigned int i;
4973         struct drm_dp_sideband_msg_req_body msg;
4974         struct drm_dp_sideband_msg_tx *txmsg = NULL;
4975         int ret;
4976
4977         mstb = drm_dp_mst_topology_get_mstb_validated(mgr, port->parent);
4978         if (!mstb)
4979                 return -EREMOTEIO;
4980
4981         if (!remote_i2c_read_ok(msgs, num)) {
4982                 DRM_DEBUG_KMS("Unsupported I2C transaction for MST device\n");
4983                 ret = -EIO;
4984                 goto out;
4985         }
4986
4987         memset(&msg, 0, sizeof(msg));
4988         msg.req_type = DP_REMOTE_I2C_READ;
4989         msg.u.i2c_read.num_transactions = num - 1;
4990         msg.u.i2c_read.port_number = port->port_num;
4991         for (i = 0; i < num - 1; i++) {
4992                 msg.u.i2c_read.transactions[i].i2c_dev_id = msgs[i].addr;
4993                 msg.u.i2c_read.transactions[i].num_bytes = msgs[i].len;
4994                 msg.u.i2c_read.transactions[i].bytes = msgs[i].buf;
4995                 msg.u.i2c_read.transactions[i].no_stop_bit = !(msgs[i].flags & I2C_M_STOP);
4996         }
4997         msg.u.i2c_read.read_i2c_device_id = msgs[num - 1].addr;
4998         msg.u.i2c_read.num_bytes_read = msgs[num - 1].len;
4999
5000         txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
5001         if (!txmsg) {
5002                 ret = -ENOMEM;
5003                 goto out;
5004         }
5005
5006         txmsg->dst = mstb;
5007         drm_dp_encode_sideband_req(&msg, txmsg);
5008
5009         drm_dp_queue_down_tx(mgr, txmsg);
5010
5011         ret = drm_dp_mst_wait_tx_reply(mstb, txmsg);
5012         if (ret > 0) {
5013
5014                 if (txmsg->reply.reply_type == DP_SIDEBAND_REPLY_NAK) {
5015                         ret = -EREMOTEIO;
5016                         goto out;
5017                 }
5018                 if (txmsg->reply.u.remote_i2c_read_ack.num_bytes != msgs[num - 1].len) {
5019                         ret = -EIO;
5020                         goto out;
5021                 }
5022                 memcpy(msgs[num - 1].buf, txmsg->reply.u.remote_i2c_read_ack.bytes, msgs[num - 1].len);
5023                 ret = num;
5024         }
5025 out:
5026         kfree(txmsg);
5027         drm_dp_mst_topology_put_mstb(mstb);
5028         return ret;
5029 }
5030
5031 static u32 drm_dp_mst_i2c_functionality(struct i2c_adapter *adapter)
5032 {
5033         return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL |
5034                I2C_FUNC_SMBUS_READ_BLOCK_DATA |
5035                I2C_FUNC_SMBUS_BLOCK_PROC_CALL |
5036                I2C_FUNC_10BIT_ADDR;
5037 }
5038
5039 static const struct i2c_algorithm drm_dp_mst_i2c_algo = {
5040         .functionality = drm_dp_mst_i2c_functionality,
5041         .master_xfer = drm_dp_mst_i2c_xfer,
5042 };
5043
5044 /**
5045  * drm_dp_mst_register_i2c_bus() - register an I2C adapter for I2C-over-AUX
5046  * @aux: DisplayPort AUX channel
5047  *
5048  * Returns 0 on success or a negative error code on failure.
5049  */
5050 static int drm_dp_mst_register_i2c_bus(struct drm_dp_aux *aux)
5051 {
5052         aux->ddc.algo = &drm_dp_mst_i2c_algo;
5053         aux->ddc.algo_data = aux;
5054         aux->ddc.retries = 3;
5055
5056         aux->ddc.class = I2C_CLASS_DDC;
5057         aux->ddc.owner = THIS_MODULE;
5058         aux->ddc.dev.parent = aux->dev;
5059         aux->ddc.dev.of_node = aux->dev->of_node;
5060
5061         strlcpy(aux->ddc.name, aux->name ? aux->name : dev_name(aux->dev),
5062                 sizeof(aux->ddc.name));
5063
5064         return i2c_add_adapter(&aux->ddc);
5065 }
5066
5067 /**
5068  * drm_dp_mst_unregister_i2c_bus() - unregister an I2C-over-AUX adapter
5069  * @aux: DisplayPort AUX channel
5070  */
5071 static void drm_dp_mst_unregister_i2c_bus(struct drm_dp_aux *aux)
5072 {
5073         i2c_del_adapter(&aux->ddc);
5074 }