drivers/thunderbolt/usb4.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * USB4 specific functionality
   4  *
   5  * Copyright (C) 2019, Intel Corporation
   6  * Authors: Mika Westerberg <mika.westerberg@linux.intel.com>
   7  *          Rajmohan Mani <rajmohan.mani@intel.com>
   8  */
   9
  10 #include <linux/delay.h>
  11 #include <linux/ktime.h>
  12
  13 #include "tb.h"
  14
  15 #define USB4_DATA_DWORDS                16
  16 #define USB4_DATA_RETRIES               3
  17
  18 enum usb4_switch_op {
  19         USB4_SWITCH_OP_QUERY_DP_RESOURCE = 0x10,
  20         USB4_SWITCH_OP_ALLOC_DP_RESOURCE = 0x11,
  21         USB4_SWITCH_OP_DEALLOC_DP_RESOURCE = 0x12,
  22         USB4_SWITCH_OP_NVM_WRITE = 0x20,
  23         USB4_SWITCH_OP_NVM_AUTH = 0x21,
  24         USB4_SWITCH_OP_NVM_READ = 0x22,
  25         USB4_SWITCH_OP_NVM_SET_OFFSET = 0x23,
  26         USB4_SWITCH_OP_DROM_READ = 0x24,
  27         USB4_SWITCH_OP_NVM_SECTOR_SIZE = 0x25,
  28 };
  29
  30 #define USB4_NVM_READ_OFFSET_MASK       GENMASK(23, 2)
  31 #define USB4_NVM_READ_OFFSET_SHIFT      2
  32 #define USB4_NVM_READ_LENGTH_MASK       GENMASK(27, 24)
  33 #define USB4_NVM_READ_LENGTH_SHIFT      24
  34
  35 #define USB4_NVM_SET_OFFSET_MASK        USB4_NVM_READ_OFFSET_MASK
  36 #define USB4_NVM_SET_OFFSET_SHIFT       USB4_NVM_READ_OFFSET_SHIFT
  37
  38 #define USB4_DROM_ADDRESS_MASK          GENMASK(14, 2)
  39 #define USB4_DROM_ADDRESS_SHIFT         2
  40 #define USB4_DROM_SIZE_MASK             GENMASK(19, 15)
  41 #define USB4_DROM_SIZE_SHIFT            15
  42
  43 #define USB4_NVM_SECTOR_SIZE_MASK       GENMASK(23, 0)
  44
  45 typedef int (*read_block_fn)(struct tb_switch *, unsigned int, void *, size_t);
  46 typedef int (*write_block_fn)(struct tb_switch *, const void *, size_t);
  47
  48 static int usb4_switch_wait_for_bit(struct tb_switch *sw, u32 offset, u32 bit,
  49                                     u32 value, int timeout_msec)
  50 {
  51         ktime_t timeout = ktime_add_ms(ktime_get(), timeout_msec);
  52
  53         do {
  54                 u32 val;
  55                 int ret;
  56
  57                 ret = tb_sw_read(sw, &val, TB_CFG_SWITCH, offset, 1);
  58                 if (ret)
  59                         return ret;
  60
  61                 if ((val & bit) == value)
  62                         return 0;
  63
  64                 usleep_range(50, 100);
  65         } while (ktime_before(ktime_get(), timeout));
  66
  67         return -ETIMEDOUT;
  68 }
  69
  70 static int usb4_switch_op_read_data(struct tb_switch *sw, void *data,
  71                                     size_t dwords)
  72 {
  73         if (dwords > USB4_DATA_DWORDS)
  74                 return -EINVAL;
  75
  76         return tb_sw_read(sw, data, TB_CFG_SWITCH, ROUTER_CS_9, dwords);
  77 }
  78
  79 static int usb4_switch_op_write_data(struct tb_switch *sw, const void *data,
  80                                      size_t dwords)
  81 {
  82         if (dwords > USB4_DATA_DWORDS)
  83                 return -EINVAL;
  84
  85         return tb_sw_write(sw, data, TB_CFG_SWITCH, ROUTER_CS_9, dwords);
  86 }
  87
  88 static int usb4_switch_op_read_metadata(struct tb_switch *sw, u32 *metadata)
  89 {
  90         return tb_sw_read(sw, metadata, TB_CFG_SWITCH, ROUTER_CS_25, 1);
  91 }
  92
  93 static int usb4_switch_op_write_metadata(struct tb_switch *sw, u32 metadata)
  94 {
  95         return tb_sw_write(sw, &metadata, TB_CFG_SWITCH, ROUTER_CS_25, 1);
  96 }
  97
  98 static int usb4_switch_do_read_data(struct tb_switch *sw, u16 address,
  99         void *buf, size_t size, read_block_fn read_block)
 100 {
 101         unsigned int retries = USB4_DATA_RETRIES;
 102         unsigned int offset;
 103
 104         offset = address & 3;
 105         address = address & ~3;
 106
 107         do {
 108                 size_t nbytes = min_t(size_t, size, USB4_DATA_DWORDS * 4);
 109                 unsigned int dwaddress, dwords;
 110                 u8 data[USB4_DATA_DWORDS * 4];
 111                 int ret;
 112
 113                 dwaddress = address / 4;
 114                 dwords = ALIGN(nbytes, 4) / 4;
 115
 116                 ret = read_block(sw, dwaddress, data, dwords);
 117                 if (ret) {
 118                         if (ret == -ETIMEDOUT) {
 119                                 if (retries--)
 120                                         continue;
 121                                 ret = -EIO;
 122                         }
 123                         return ret;
 124                 }
 125
 126                 memcpy(buf, data + offset, nbytes);
 127
 128                 size -= nbytes;
 129                 address += nbytes;
 130                 buf += nbytes;
 131         } while (size > 0);
 132
 133         return 0;
 134 }
 135
 136 static int usb4_switch_do_write_data(struct tb_switch *sw, u16 address,
 137         const void *buf, size_t size, write_block_fn write_next_block)
 138 {
 139         unsigned int retries = USB4_DATA_RETRIES;
 140         unsigned int offset;
 141
 142         offset = address & 3;
 143         address = address & ~3;
 144
 145         do {
 146                 u32 nbytes = min_t(u32, size, USB4_DATA_DWORDS * 4);
 147                 u8 data[USB4_DATA_DWORDS * 4];
 148                 int ret;
 149
 150                 memcpy(data + offset, buf, nbytes);
 151
 152                 ret = write_next_block(sw, data, nbytes / 4);
 153                 if (ret) {
 154                         if (ret == -ETIMEDOUT) {
 155                                 if (retries--)
 156                                         continue;
 157                                 ret = -EIO;
 158                         }
 159                         return ret;
 160                 }
 161
 162                 size -= nbytes;
 163                 address += nbytes;
 164                 buf += nbytes;
 165         } while (size > 0);
 166
 167         return 0;
 168 }
 169
 170 static int usb4_switch_op(struct tb_switch *sw, u16 opcode, u8 *status)
 171 {
 172         u32 val;
 173         int ret;
 174
 175         val = opcode | ROUTER_CS_26_OV;
 176         ret = tb_sw_write(sw, &val, TB_CFG_SWITCH, ROUTER_CS_26, 1);
 177         if (ret)
 178                 return ret;
 179
 180         ret = usb4_switch_wait_for_bit(sw, ROUTER_CS_26, ROUTER_CS_26_OV, 0, 500);
 181         if (ret)
 182                 return ret;
 183
 184         ret = tb_sw_read(sw, &val, TB_CFG_SWITCH, ROUTER_CS_26, 1);
 185         if (ret)
 186                 return ret;
 187
 188         if (val & ROUTER_CS_26_ONS)
 189                 return -EOPNOTSUPP;
 190
 191         *status = (val & ROUTER_CS_26_STATUS_MASK) >> ROUTER_CS_26_STATUS_SHIFT;
 192         return 0;
 193 }
 194
 195 /**
 196  * usb4_switch_setup() - Additional setup for USB4 device
 197  * @sw: USB4 router to setup
 198  *
 199  * USB4 routers need additional settings in order to enable all the
 200  * tunneling. This function enables USB and PCIe tunneling if it can be
 201  * enabled (e.g the parent switch also supports them). If USB tunneling
 202  * is not available for some reason (like that there is Thunderbolt 3
 203  * switch upstream) then the internal xHCI controller is enabled
 204  * instead.
 205  */
 206 int usb4_switch_setup(struct tb_switch *sw)
 207 {
 208         struct tb_switch *parent;
 209         bool tbt3, xhci;
 210         u32 val = 0;
 211         int ret;
 212
 213         if (!tb_route(sw))
 214                 return 0;
 215
 216         ret = tb_sw_read(sw, &val, TB_CFG_SWITCH, ROUTER_CS_6, 1);
 217         if (ret)
 218                 return ret;
 219
 220         xhci = val & ROUTER_CS_6_HCI;
 221         tbt3 = !(val & ROUTER_CS_6_TNS);
 222
 223         tb_sw_dbg(sw, "TBT3 support: %s, xHCI: %s\n",
 224                   tbt3 ? "yes" : "no", xhci ? "yes" : "no");
 225
 226         ret = tb_sw_read(sw, &val, TB_CFG_SWITCH, ROUTER_CS_5, 1);
 227         if (ret)
 228                 return ret;
 229
 230         parent = tb_switch_parent(sw);
 231
 232         if (tb_switch_find_port(parent, TB_TYPE_USB3_DOWN)) {
 233                 val |= ROUTER_CS_5_UTO;
 234                 xhci = false;
 235         }
 236
 237         /* Only enable PCIe tunneling if the parent router supports it */
 238         if (tb_switch_find_port(parent, TB_TYPE_PCIE_DOWN)) {
 239                 val |= ROUTER_CS_5_PTO;
 240                 /*
 241                  * xHCI can be enabled if PCIe tunneling is supported
 242                  * and the parent does not have any USB3 dowstream
 243                  * adapters (so we cannot do USB 3.x tunneling).
 244                  */
 245                 if (xhci)
 246                         val |= ROUTER_CS_5_HCO;
 247         }
 248
 249         /* TBT3 supported by the CM */
 250         val |= ROUTER_CS_5_C3S;
 251         /* Tunneling configuration is ready now */
 252         val |= ROUTER_CS_5_CV;
 253
 254         ret = tb_sw_write(sw, &val, TB_CFG_SWITCH, ROUTER_CS_5, 1);
 255         if (ret)
 256                 return ret;
 257
 258         return usb4_switch_wait_for_bit(sw, ROUTER_CS_6, ROUTER_CS_6_CR,
 259                                         ROUTER_CS_6_CR, 50);
 260 }
 261
 262 /**
 263  * usb4_switch_read_uid() - Read UID from USB4 router
 264  * @sw: USB4 router
 265  * @uid: UID is stored here
 266  *
 267  * Reads 64-bit UID from USB4 router config space.
 268  */
 269 int usb4_switch_read_uid(struct tb_switch *sw, u64 *uid)
 270 {
 271         return tb_sw_read(sw, uid, TB_CFG_SWITCH, ROUTER_CS_7, 2);
 272 }
 273
 274 static int usb4_switch_drom_read_block(struct tb_switch *sw,
 275                                        unsigned int dwaddress, void *buf,
 276                                        size_t dwords)
 277 {
 278         u8 status = 0;
 279         u32 metadata;
 280         int ret;
 281
 282         metadata = (dwords << USB4_DROM_SIZE_SHIFT) & USB4_DROM_SIZE_MASK;
 283         metadata |= (dwaddress << USB4_DROM_ADDRESS_SHIFT) &
 284                 USB4_DROM_ADDRESS_MASK;
 285
 286         ret = usb4_switch_op_write_metadata(sw, metadata);
 287         if (ret)
 288                 return ret;
 289
 290         ret = usb4_switch_op(sw, USB4_SWITCH_OP_DROM_READ, &status);
 291         if (ret)
 292                 return ret;
 293
 294         if (status)
 295                 return -EIO;
 296
 297         return usb4_switch_op_read_data(sw, buf, dwords);
 298 }
 299
 300 /**
 301  * usb4_switch_drom_read() - Read arbitrary bytes from USB4 router DROM
 302  * @sw: USB4 router
 303  * @address: Byte address inside DROM to start reading
 304  * @buf: Buffer where the DROM content is stored
 305  * @size: Number of bytes to read from DROM
 306  *
 307  * Uses USB4 router operations to read router DROM. For devices this
 308  * should always work but for hosts it may return %-EOPNOTSUPP in which
 309  * case the host router does not have DROM.
 310  */
 311 int usb4_switch_drom_read(struct tb_switch *sw, unsigned int address, void *buf,
 312                           size_t size)
 313 {
 314         return usb4_switch_do_read_data(sw, address, buf, size,
 315                                         usb4_switch_drom_read_block);
 316 }
 317
 318 static int usb4_set_port_configured(struct tb_port *port, bool configured)
 319 {
 320         int ret;
 321         u32 val;
 322
 323         ret = tb_port_read(port, &val, TB_CFG_PORT,
 324                            port->cap_usb4 + PORT_CS_19, 1);
 325         if (ret)
 326                 return ret;
 327
 328         if (configured)
 329                 val |= PORT_CS_19_PC;
 330         else
 331                 val &= ~PORT_CS_19_PC;
 332
 333         return tb_port_write(port, &val, TB_CFG_PORT,
 334                              port->cap_usb4 + PORT_CS_19, 1);
 335 }
 336
 337 /**
 338  * usb4_switch_configure_link() - Set upstream USB4 link configured
 339  * @sw: USB4 router
 340  *
 341  * Sets the upstream USB4 link to be configured for power management
 342  * purposes.
 343  */
 344 int usb4_switch_configure_link(struct tb_switch *sw)
 345 {
 346         struct tb_port *up;
 347
 348         if (!tb_route(sw))
 349                 return 0;
 350
 351         up = tb_upstream_port(sw);
 352         return usb4_set_port_configured(up, true);
 353 }
 354
 355 /**
 356  * usb4_switch_unconfigure_link() - Un-set upstream USB4 link configuration
 357  * @sw: USB4 router
 358  *
 359  * Reverse of usb4_switch_configure_link().
 360  */
 361 void usb4_switch_unconfigure_link(struct tb_switch *sw)
 362 {
 363         struct tb_port *up;
 364
 365         if (sw->is_unplugged || !tb_route(sw))
 366                 return;
 367
 368         up = tb_upstream_port(sw);
 369         usb4_set_port_configured(up, false);
 370 }
 371
 372 /**
 373  * usb4_switch_lane_bonding_possible() - Are conditions met for lane bonding
 374  * @sw: USB4 router
 375  *
 376  * Checks whether conditions are met so that lane bonding can be
 377  * established with the upstream router. Call only for device routers.
 378  */
 379 bool usb4_switch_lane_bonding_possible(struct tb_switch *sw)
 380 {
 381         struct tb_port *up;
 382         int ret;
 383         u32 val;
 384
 385         up = tb_upstream_port(sw);
 386         ret = tb_port_read(up, &val, TB_CFG_PORT, up->cap_usb4 + PORT_CS_18, 1);
 387         if (ret)
 388                 return false;
 389
 390         return !!(val & PORT_CS_18_BE);
 391 }
 392
 393 /**
 394  * usb4_switch_set_sleep() - Prepare the router to enter sleep
 395  * @sw: USB4 router
 396  *
 397  * Enables wakes and sets sleep bit for the router. Returns when the
 398  * router sleep ready bit has been asserted.
 399  */
 400 int usb4_switch_set_sleep(struct tb_switch *sw)
 401 {
 402         int ret;
 403         u32 val;
 404
 405         /* Set sleep bit and wait for sleep ready to be asserted */
 406         ret = tb_sw_read(sw, &val, TB_CFG_SWITCH, ROUTER_CS_5, 1);
 407         if (ret)
 408                 return ret;
 409
 410         val |= ROUTER_CS_5_SLP;
 411
 412         ret = tb_sw_write(sw, &val, TB_CFG_SWITCH, ROUTER_CS_5, 1);
 413         if (ret)
 414                 return ret;
 415
 416         return usb4_switch_wait_for_bit(sw, ROUTER_CS_6, ROUTER_CS_6_SLPR,
 417                                         ROUTER_CS_6_SLPR, 500);
 418 }
 419
 420 /**
 421  * usb4_switch_nvm_sector_size() - Return router NVM sector size
 422  * @sw: USB4 router
 423  *
 424  * If the router supports NVM operations this function returns the NVM
 425  * sector size in bytes. If NVM operations are not supported returns
 426  * %-EOPNOTSUPP.
 427  */
 428 int usb4_switch_nvm_sector_size(struct tb_switch *sw)
 429 {
 430         u32 metadata;
 431         u8 status;
 432         int ret;
 433
 434         ret = usb4_switch_op(sw, USB4_SWITCH_OP_NVM_SECTOR_SIZE, &status);
 435         if (ret)
 436                 return ret;
 437
 438         if (status)
 439                 return status == 0x2 ? -EOPNOTSUPP : -EIO;
 440
 441         ret = usb4_switch_op_read_metadata(sw, &metadata);
 442         if (ret)
 443                 return ret;
 444
 445         return metadata & USB4_NVM_SECTOR_SIZE_MASK;
 446 }
 447
 448 static int usb4_switch_nvm_read_block(struct tb_switch *sw,
 449         unsigned int dwaddress, void *buf, size_t dwords)
 450 {
 451         u8 status = 0;
 452         u32 metadata;
 453         int ret;
 454
 455         metadata = (dwords << USB4_NVM_READ_LENGTH_SHIFT) &
 456                    USB4_NVM_READ_LENGTH_MASK;
 457         metadata |= (dwaddress << USB4_NVM_READ_OFFSET_SHIFT) &
 458                    USB4_NVM_READ_OFFSET_MASK;
 459
 460         ret = usb4_switch_op_write_metadata(sw, metadata);
 461         if (ret)
 462                 return ret;
 463
 464         ret = usb4_switch_op(sw, USB4_SWITCH_OP_NVM_READ, &status);
 465         if (ret)
 466                 return ret;
 467
 468         if (status)
 469                 return -EIO;
 470
 471         return usb4_switch_op_read_data(sw, buf, dwords);
 472 }
 473
 474 /**
 475  * usb4_switch_nvm_read() - Read arbitrary bytes from router NVM
 476  * @sw: USB4 router
 477  * @address: Starting address in bytes
 478  * @buf: Read data is placed here
 479  * @size: How many bytes to read
 480  *
 481  * Reads NVM contents of the router. If NVM is not supported returns
 482  * %-EOPNOTSUPP.
 483  */
 484 int usb4_switch_nvm_read(struct tb_switch *sw, unsigned int address, void *buf,
 485                          size_t size)
 486 {
 487         return usb4_switch_do_read_data(sw, address, buf, size,
 488                                         usb4_switch_nvm_read_block);
 489 }
 490
 491 static int usb4_switch_nvm_set_offset(struct tb_switch *sw,
 492                                       unsigned int address)
 493 {
 494         u32 metadata, dwaddress;
 495         u8 status = 0;
 496         int ret;
 497
 498         dwaddress = address / 4;
 499         metadata = (dwaddress << USB4_NVM_SET_OFFSET_SHIFT) &
 500                    USB4_NVM_SET_OFFSET_MASK;
 501
 502         ret = usb4_switch_op_write_metadata(sw, metadata);
 503         if (ret)
 504                 return ret;
 505
 506         ret = usb4_switch_op(sw, USB4_SWITCH_OP_NVM_SET_OFFSET, &status);
 507         if (ret)
 508                 return ret;
 509
 510         return status ? -EIO : 0;
 511 }
 512
 513 static int usb4_switch_nvm_write_next_block(struct tb_switch *sw,
 514                                             const void *buf, size_t dwords)
 515 {
 516         u8 status;
 517         int ret;
 518
 519         ret = usb4_switch_op_write_data(sw, buf, dwords);
 520         if (ret)
 521                 return ret;
 522
 523         ret = usb4_switch_op(sw, USB4_SWITCH_OP_NVM_WRITE, &status);
 524         if (ret)
 525                 return ret;
 526
 527         return status ? -EIO : 0;
 528 }
 529
 530 /**
 531  * usb4_switch_nvm_write() - Write to the router NVM
 532  * @sw: USB4 router
 533  * @address: Start address where to write in bytes
 534  * @buf: Pointer to the data to write
 535  * @size: Size of @buf in bytes
 536  *
 537  * Writes @buf to the router NVM using USB4 router operations. If NVM
 538  * write is not supported returns %-EOPNOTSUPP.
 539  */
 540 int usb4_switch_nvm_write(struct tb_switch *sw, unsigned int address,
 541                           const void *buf, size_t size)
 542 {
 543         int ret;
 544
 545         ret = usb4_switch_nvm_set_offset(sw, address);
 546         if (ret)
 547                 return ret;
 548
 549         return usb4_switch_do_write_data(sw, address, buf, size,
 550                                          usb4_switch_nvm_write_next_block);
 551 }
 552
 553 /**
 554  * usb4_switch_nvm_authenticate() - Authenticate new NVM
 555  * @sw: USB4 router
 556  *
 557  * After the new NVM has been written via usb4_switch_nvm_write(), this
 558  * function triggers NVM authentication process. If the authentication
 559  * is successful the router is power cycled and the new NVM starts
 560  * running. In case of failure returns negative errno.
 561  */
 562 int usb4_switch_nvm_authenticate(struct tb_switch *sw)
 563 {
 564         u8 status = 0;
 565         int ret;
 566
 567         ret = usb4_switch_op(sw, USB4_SWITCH_OP_NVM_AUTH, &status);
 568         if (ret)
 569                 return ret;
 570
 571         switch (status) {
 572         case 0x0:
 573                 tb_sw_dbg(sw, "NVM authentication successful\n");
 574                 return 0;
 575         case 0x1:
 576                 return -EINVAL;
 577         case 0x2:
 578                 return -EAGAIN;
 579         case 0x3:
 580                 return -EOPNOTSUPP;
 581         default:
 582                 return -EIO;
 583         }
 584 }
 585
 586 /**
 587  * usb4_switch_query_dp_resource() - Query availability of DP IN resource
 588  * @sw: USB4 router
 589  * @in: DP IN adapter
 590  *
 591  * For DP tunneling this function can be used to query availability of
 592  * DP IN resource. Returns true if the resource is available for DP
 593  * tunneling, false otherwise.
 594  */
 595 bool usb4_switch_query_dp_resource(struct tb_switch *sw, struct tb_port *in)
 596 {
 597         u8 status;
 598         int ret;
 599
 600         ret = usb4_switch_op_write_metadata(sw, in->port);
 601         if (ret)
 602                 return false;
 603
 604         ret = usb4_switch_op(sw, USB4_SWITCH_OP_QUERY_DP_RESOURCE, &status);
 605         /*
 606          * If DP resource allocation is not supported assume it is
 607          * always available.
 608          */
 609         if (ret == -EOPNOTSUPP)
 610                 return true;
 611         else if (ret)
 612                 return false;
 613
 614         return !status;
 615 }
 616
 617 /**
 618  * usb4_switch_alloc_dp_resource() - Allocate DP IN resource
 619  * @sw: USB4 router
 620  * @in: DP IN adapter
 621  *
 622  * Allocates DP IN resource for DP tunneling using USB4 router
 623  * operations. If the resource was allocated returns %0. Otherwise
 624  * returns negative errno, in particular %-EBUSY if the resource is
 625  * already allocated.
 626  */
 627 int usb4_switch_alloc_dp_resource(struct tb_switch *sw, struct tb_port *in)
 628 {
 629         u8 status;
 630         int ret;
 631
 632         ret = usb4_switch_op_write_metadata(sw, in->port);
 633         if (ret)
 634                 return ret;
 635
 636         ret = usb4_switch_op(sw, USB4_SWITCH_OP_ALLOC_DP_RESOURCE, &status);
 637         if (ret == -EOPNOTSUPP)
 638                 return 0;
 639         else if (ret)
 640                 return ret;
 641
 642         return status ? -EBUSY : 0;
 643 }
 644
 645 /**
 646  * usb4_switch_dealloc_dp_resource() - Releases allocated DP IN resource
 647  * @sw: USB4 router
 648  * @in: DP IN adapter
 649  *
 650  * Releases the previously allocated DP IN resource.
 651  */
 652 int usb4_switch_dealloc_dp_resource(struct tb_switch *sw, struct tb_port *in)
 653 {
 654         u8 status;
 655         int ret;
 656
 657         ret = usb4_switch_op_write_metadata(sw, in->port);
 658         if (ret)
 659                 return ret;
 660
 661         ret = usb4_switch_op(sw, USB4_SWITCH_OP_DEALLOC_DP_RESOURCE, &status);
 662         if (ret == -EOPNOTSUPP)
 663                 return 0;
 664         else if (ret)
 665                 return ret;
 666
 667         return status ? -EIO : 0;
 668 }
 669
 670 static int usb4_port_idx(const struct tb_switch *sw, const struct tb_port *port)
 671 {
 672         struct tb_port *p;
 673         int usb4_idx = 0;
 674
 675         /* Assume port is primary */
 676         tb_switch_for_each_port(sw, p) {
 677                 if (!tb_port_is_null(p))
 678                         continue;
 679                 if (tb_is_upstream_port(p))
 680                         continue;
 681                 if (!p->link_nr) {
 682                         if (p == port)
 683                                 break;
 684                         usb4_idx++;
 685                 }
 686         }
 687
 688         return usb4_idx;
 689 }
 690
 691 /**
 692  * usb4_switch_map_pcie_down() - Map USB4 port to a PCIe downstream adapter
 693  * @sw: USB4 router
 694  * @port: USB4 port
 695  *
 696  * USB4 routers have direct mapping between USB4 ports and PCIe
 697  * downstream adapters where the PCIe topology is extended. This
 698  * function returns the corresponding downstream PCIe adapter or %NULL
 699  * if no such mapping was possible.
 700  */
 701 struct tb_port *usb4_switch_map_pcie_down(struct tb_switch *sw,
 702                                           const struct tb_port *port)
 703 {
 704         int usb4_idx = usb4_port_idx(sw, port);
 705         struct tb_port *p;
 706         int pcie_idx = 0;
 707
 708         /* Find PCIe down port matching usb4_port */
 709         tb_switch_for_each_port(sw, p) {
 710                 if (!tb_port_is_pcie_down(p))
 711                         continue;
 712
 713                 if (pcie_idx == usb4_idx && !tb_pci_port_is_enabled(p))
 714                         return p;
 715
 716                 pcie_idx++;
 717         }
 718
 719         return NULL;
 720 }
 721
 722 /**
 723  * usb4_switch_map_usb3_down() - Map USB4 port to a USB3 downstream adapter
 724  * @sw: USB4 router
 725  * @port: USB4 port
 726  *
 727  * USB4 routers have direct mapping between USB4 ports and USB 3.x
 728  * downstream adapters where the USB 3.x topology is extended. This
 729  * function returns the corresponding downstream USB 3.x adapter or
 730  * %NULL if no such mapping was possible.
 731  */
 732 struct tb_port *usb4_switch_map_usb3_down(struct tb_switch *sw,
 733                                           const struct tb_port *port)
 734 {
 735         int usb4_idx = usb4_port_idx(sw, port);
 736         struct tb_port *p;
 737         int usb_idx = 0;
 738
 739         /* Find USB3 down port matching usb4_port */
 740         tb_switch_for_each_port(sw, p) {
 741                 if (!tb_port_is_usb3_down(p))
 742                         continue;
 743
 744                 if (usb_idx == usb4_idx && !tb_usb3_port_is_enabled(p))
 745                         return p;
 746
 747                 usb_idx++;
 748         }
 749
 750         return NULL;
 751 }
 752
 753 /**
 754  * usb4_port_unlock() - Unlock USB4 downstream port
 755  * @port: USB4 port to unlock
 756  *
 757  * Unlocks USB4 downstream port so that the connection manager can
 758  * access the router below this port.
 759  */
 760 int usb4_port_unlock(struct tb_port *port)
 761 {
 762         int ret;
 763         u32 val;
 764
 765         ret = tb_port_read(port, &val, TB_CFG_PORT, ADP_CS_4, 1);
 766         if (ret)
 767                 return ret;
 768
 769         val &= ~ADP_CS_4_LCK;
 770         return tb_port_write(port, &val, TB_CFG_PORT, ADP_CS_4, 1);
 771 }