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 (val & ROUTER_CS_26_ONS)
 186                 return -EOPNOTSUPP;
 187
 188         *status = (val & ROUTER_CS_26_STATUS_MASK) >> ROUTER_CS_26_STATUS_SHIFT;
 189         return 0;
 190 }
 191
 192 /**
 193  * usb4_switch_setup() - Additional setup for USB4 device
 194  * @sw: USB4 router to setup
 195  *
 196  * USB4 routers need additional settings in order to enable all the
 197  * tunneling. This function enables USB and PCIe tunneling if it can be
 198  * enabled (e.g the parent switch also supports them). If USB tunneling
 199  * is not available for some reason (like that there is Thunderbolt 3
 200  * switch upstream) then the internal xHCI controller is enabled
 201  * instead.
 202  */
 203 int usb4_switch_setup(struct tb_switch *sw)
 204 {
 205         struct tb_switch *parent;
 206         bool tbt3, xhci;
 207         u32 val = 0;
 208         int ret;
 209
 210         if (!tb_route(sw))
 211                 return 0;
 212
 213         ret = tb_sw_read(sw, &val, TB_CFG_SWITCH, ROUTER_CS_6, 1);
 214         if (ret)
 215                 return ret;
 216
 217         xhci = val & ROUTER_CS_6_HCI;
 218         tbt3 = !(val & ROUTER_CS_6_TNS);
 219
 220         tb_sw_dbg(sw, "TBT3 support: %s, xHCI: %s\n",
 221                   tbt3 ? "yes" : "no", xhci ? "yes" : "no");
 222
 223         ret = tb_sw_read(sw, &val, TB_CFG_SWITCH, ROUTER_CS_5, 1);
 224         if (ret)
 225                 return ret;
 226
 227         parent = tb_switch_parent(sw);
 228
 229         if (tb_switch_find_port(parent, TB_TYPE_USB3_DOWN)) {
 230                 val |= ROUTER_CS_5_UTO;
 231                 xhci = false;
 232         }
 233
 234         /* Only enable PCIe tunneling if the parent router supports it */
 235         if (tb_switch_find_port(parent, TB_TYPE_PCIE_DOWN)) {
 236                 val |= ROUTER_CS_5_PTO;
 237                 /*
 238                  * xHCI can be enabled if PCIe tunneling is supported
 239                  * and the parent does not have any USB3 dowstream
 240                  * adapters (so we cannot do USB 3.x tunneling).
 241                  */
 242                 if (xhci)
 243                         val |= ROUTER_CS_5_HCO;
 244         }
 245
 246         /* TBT3 supported by the CM */
 247         val |= ROUTER_CS_5_C3S;
 248         /* Tunneling configuration is ready now */
 249         val |= ROUTER_CS_5_CV;
 250
 251         ret = tb_sw_write(sw, &val, TB_CFG_SWITCH, ROUTER_CS_5, 1);
 252         if (ret)
 253                 return ret;
 254
 255         return usb4_switch_wait_for_bit(sw, ROUTER_CS_6, ROUTER_CS_6_CR,
 256                                         ROUTER_CS_6_CR, 50);
 257 }
 258
 259 /**
 260  * usb4_switch_read_uid() - Read UID from USB4 router
 261  * @sw: USB4 router
 262  *
 263  * Reads 64-bit UID from USB4 router config space.
 264  */
 265 int usb4_switch_read_uid(struct tb_switch *sw, u64 *uid)
 266 {
 267         return tb_sw_read(sw, uid, TB_CFG_SWITCH, ROUTER_CS_7, 2);
 268 }
 269
 270 static int usb4_switch_drom_read_block(struct tb_switch *sw,
 271                                        unsigned int dwaddress, void *buf,
 272                                        size_t dwords)
 273 {
 274         u8 status = 0;
 275         u32 metadata;
 276         int ret;
 277
 278         metadata = (dwords << USB4_DROM_SIZE_SHIFT) & USB4_DROM_SIZE_MASK;
 279         metadata |= (dwaddress << USB4_DROM_ADDRESS_SHIFT) &
 280                 USB4_DROM_ADDRESS_MASK;
 281
 282         ret = usb4_switch_op_write_metadata(sw, metadata);
 283         if (ret)
 284                 return ret;
 285
 286         ret = usb4_switch_op(sw, USB4_SWITCH_OP_DROM_READ, &status);
 287         if (ret)
 288                 return ret;
 289
 290         if (status)
 291                 return -EIO;
 292
 293         return usb4_switch_op_read_data(sw, buf, dwords);
 294 }
 295
 296 /**
 297  * usb4_switch_drom_read() - Read arbitrary bytes from USB4 router DROM
 298  * @sw: USB4 router
 299  *
 300  * Uses USB4 router operations to read router DROM. For devices this
 301  * should always work but for hosts it may return %-EOPNOTSUPP in which
 302  * case the host router does not have DROM.
 303  */
 304 int usb4_switch_drom_read(struct tb_switch *sw, unsigned int address, void *buf,
 305                           size_t size)
 306 {
 307         return usb4_switch_do_read_data(sw, address, buf, size,
 308                                         usb4_switch_drom_read_block);
 309 }
 310
 311 static int usb4_set_port_configured(struct tb_port *port, bool configured)
 312 {
 313         int ret;
 314         u32 val;
 315
 316         ret = tb_port_read(port, &val, TB_CFG_PORT,
 317                            port->cap_usb4 + PORT_CS_19, 1);
 318         if (ret)
 319                 return ret;
 320
 321         if (configured)
 322                 val |= PORT_CS_19_PC;
 323         else
 324                 val &= ~PORT_CS_19_PC;
 325
 326         return tb_port_write(port, &val, TB_CFG_PORT,
 327                              port->cap_usb4 + PORT_CS_19, 1);
 328 }
 329
 330 /**
 331  * usb4_switch_configure_link() - Set upstream USB4 link configured
 332  * @sw: USB4 router
 333  *
 334  * Sets the upstream USB4 link to be configured for power management
 335  * purposes.
 336  */
 337 int usb4_switch_configure_link(struct tb_switch *sw)
 338 {
 339         struct tb_port *up;
 340
 341         if (!tb_route(sw))
 342                 return 0;
 343
 344         up = tb_upstream_port(sw);
 345         return usb4_set_port_configured(up, true);
 346 }
 347
 348 /**
 349  * usb4_switch_unconfigure_link() - Un-set upstream USB4 link configuration
 350  * @sw: USB4 router
 351  *
 352  * Reverse of usb4_switch_configure_link().
 353  */
 354 void usb4_switch_unconfigure_link(struct tb_switch *sw)
 355 {
 356         struct tb_port *up;
 357
 358         if (sw->is_unplugged || !tb_route(sw))
 359                 return;
 360
 361         up = tb_upstream_port(sw);
 362         usb4_set_port_configured(up, false);
 363 }
 364
 365 /**
 366  * usb4_switch_lane_bonding_possible() - Are conditions met for lane bonding
 367  * @sw: USB4 router
 368  *
 369  * Checks whether conditions are met so that lane bonding can be
 370  * established with the upstream router. Call only for device routers.
 371  */
 372 bool usb4_switch_lane_bonding_possible(struct tb_switch *sw)
 373 {
 374         struct tb_port *up;
 375         int ret;
 376         u32 val;
 377
 378         up = tb_upstream_port(sw);
 379         ret = tb_port_read(up, &val, TB_CFG_PORT, up->cap_usb4 + PORT_CS_18, 1);
 380         if (ret)
 381                 return false;
 382
 383         return !!(val & PORT_CS_18_BE);
 384 }
 385
 386 /**
 387  * usb4_switch_set_sleep() - Prepare the router to enter sleep
 388  * @sw: USB4 router
 389  *
 390  * Enables wakes and sets sleep bit for the router. Returns when the
 391  * router sleep ready bit has been asserted.
 392  */
 393 int usb4_switch_set_sleep(struct tb_switch *sw)
 394 {
 395         int ret;
 396         u32 val;
 397
 398         /* Set sleep bit and wait for sleep ready to be asserted */
 399         ret = tb_sw_read(sw, &val, TB_CFG_SWITCH, ROUTER_CS_5, 1);
 400         if (ret)
 401                 return ret;
 402
 403         val |= ROUTER_CS_5_SLP;
 404
 405         ret = tb_sw_write(sw, &val, TB_CFG_SWITCH, ROUTER_CS_5, 1);
 406         if (ret)
 407                 return ret;
 408
 409         return usb4_switch_wait_for_bit(sw, ROUTER_CS_6, ROUTER_CS_6_SLPR,
 410                                         ROUTER_CS_6_SLPR, 500);
 411 }
 412
 413 /**
 414  * usb4_switch_nvm_sector_size() - Return router NVM sector size
 415  * @sw: USB4 router
 416  *
 417  * If the router supports NVM operations this function returns the NVM
 418  * sector size in bytes. If NVM operations are not supported returns
 419  * %-EOPNOTSUPP.
 420  */
 421 int usb4_switch_nvm_sector_size(struct tb_switch *sw)
 422 {
 423         u32 metadata;
 424         u8 status;
 425         int ret;
 426
 427         ret = usb4_switch_op(sw, USB4_SWITCH_OP_NVM_SECTOR_SIZE, &status);
 428         if (ret)
 429                 return ret;
 430
 431         if (status)
 432                 return status == 0x2 ? -EOPNOTSUPP : -EIO;
 433
 434         ret = usb4_switch_op_read_metadata(sw, &metadata);
 435         if (ret)
 436                 return ret;
 437
 438         return metadata & USB4_NVM_SECTOR_SIZE_MASK;
 439 }
 440
 441 static int usb4_switch_nvm_read_block(struct tb_switch *sw,
 442         unsigned int dwaddress, void *buf, size_t dwords)
 443 {
 444         u8 status = 0;
 445         u32 metadata;
 446         int ret;
 447
 448         metadata = (dwords << USB4_NVM_READ_LENGTH_SHIFT) &
 449                    USB4_NVM_READ_LENGTH_MASK;
 450         metadata |= (dwaddress << USB4_NVM_READ_OFFSET_SHIFT) &
 451                    USB4_NVM_READ_OFFSET_MASK;
 452
 453         ret = usb4_switch_op_write_metadata(sw, metadata);
 454         if (ret)
 455                 return ret;
 456
 457         ret = usb4_switch_op(sw, USB4_SWITCH_OP_NVM_READ, &status);
 458         if (ret)
 459                 return ret;
 460
 461         if (status)
 462                 return -EIO;
 463
 464         return usb4_switch_op_read_data(sw, buf, dwords);
 465 }
 466
 467 /**
 468  * usb4_switch_nvm_read() - Read arbitrary bytes from router NVM
 469  * @sw: USB4 router
 470  * @address: Starting address in bytes
 471  * @buf: Read data is placed here
 472  * @size: How many bytes to read
 473  *
 474  * Reads NVM contents of the router. If NVM is not supported returns
 475  * %-EOPNOTSUPP.
 476  */
 477 int usb4_switch_nvm_read(struct tb_switch *sw, unsigned int address, void *buf,
 478                          size_t size)
 479 {
 480         return usb4_switch_do_read_data(sw, address, buf, size,
 481                                         usb4_switch_nvm_read_block);
 482 }
 483
 484 static int usb4_switch_nvm_set_offset(struct tb_switch *sw,
 485                                       unsigned int address)
 486 {
 487         u32 metadata, dwaddress;
 488         u8 status = 0;
 489         int ret;
 490
 491         dwaddress = address / 4;
 492         metadata = (dwaddress << USB4_NVM_SET_OFFSET_SHIFT) &
 493                    USB4_NVM_SET_OFFSET_MASK;
 494
 495         ret = usb4_switch_op_write_metadata(sw, metadata);
 496         if (ret)
 497                 return ret;
 498
 499         ret = usb4_switch_op(sw, USB4_SWITCH_OP_NVM_SET_OFFSET, &status);
 500         if (ret)
 501                 return ret;
 502
 503         return status ? -EIO : 0;
 504 }
 505
 506 static int usb4_switch_nvm_write_next_block(struct tb_switch *sw,
 507                                             const void *buf, size_t dwords)
 508 {
 509         u8 status;
 510         int ret;
 511
 512         ret = usb4_switch_op_write_data(sw, buf, dwords);
 513         if (ret)
 514                 return ret;
 515
 516         ret = usb4_switch_op(sw, USB4_SWITCH_OP_NVM_WRITE, &status);
 517         if (ret)
 518                 return ret;
 519
 520         return status ? -EIO : 0;
 521 }
 522
 523 /**
 524  * usb4_switch_nvm_write() - Write to the router NVM
 525  * @sw: USB4 router
 526  * @address: Start address where to write in bytes
 527  * @buf: Pointer to the data to write
 528  * @size: Size of @buf in bytes
 529  *
 530  * Writes @buf to the router NVM using USB4 router operations. If NVM
 531  * write is not supported returns %-EOPNOTSUPP.
 532  */
 533 int usb4_switch_nvm_write(struct tb_switch *sw, unsigned int address,
 534                           const void *buf, size_t size)
 535 {
 536         int ret;
 537
 538         ret = usb4_switch_nvm_set_offset(sw, address);
 539         if (ret)
 540                 return ret;
 541
 542         return usb4_switch_do_write_data(sw, address, buf, size,
 543                                          usb4_switch_nvm_write_next_block);
 544 }
 545
 546 /**
 547  * usb4_switch_nvm_authenticate() - Authenticate new NVM
 548  * @sw: USB4 router
 549  *
 550  * After the new NVM has been written via usb4_switch_nvm_write(), this
 551  * function triggers NVM authentication process. If the authentication
 552  * is successful the router is power cycled and the new NVM starts
 553  * running. In case of failure returns negative errno.
 554  */
 555 int usb4_switch_nvm_authenticate(struct tb_switch *sw)
 556 {
 557         u8 status = 0;
 558         int ret;
 559
 560         ret = usb4_switch_op(sw, USB4_SWITCH_OP_NVM_AUTH, &status);
 561         if (ret)
 562                 return ret;
 563
 564         switch (status) {
 565         case 0x0:
 566                 tb_sw_dbg(sw, "NVM authentication successful\n");
 567                 return 0;
 568         case 0x1:
 569                 return -EINVAL;
 570         case 0x2:
 571                 return -EAGAIN;
 572         case 0x3:
 573                 return -EOPNOTSUPP;
 574         default:
 575                 return -EIO;
 576         }
 577 }
 578
 579 /**
 580  * usb4_switch_query_dp_resource() - Query availability of DP IN resource
 581  * @sw: USB4 router
 582  * @in: DP IN adapter
 583  *
 584  * For DP tunneling this function can be used to query availability of
 585  * DP IN resource. Returns true if the resource is available for DP
 586  * tunneling, false otherwise.
 587  */
 588 bool usb4_switch_query_dp_resource(struct tb_switch *sw, struct tb_port *in)
 589 {
 590         u8 status;
 591         int ret;
 592
 593         ret = usb4_switch_op_write_metadata(sw, in->port);
 594         if (ret)
 595                 return false;
 596
 597         ret = usb4_switch_op(sw, USB4_SWITCH_OP_QUERY_DP_RESOURCE, &status);
 598         /*
 599          * If DP resource allocation is not supported assume it is
 600          * always available.
 601          */
 602         if (ret == -EOPNOTSUPP)
 603                 return true;
 604         else if (ret)
 605                 return false;
 606
 607         return !status;
 608 }
 609
 610 /**
 611  * usb4_switch_alloc_dp_resource() - Allocate DP IN resource
 612  * @sw: USB4 router
 613  * @in: DP IN adapter
 614  *
 615  * Allocates DP IN resource for DP tunneling using USB4 router
 616  * operations. If the resource was allocated returns %0. Otherwise
 617  * returns negative errno, in particular %-EBUSY if the resource is
 618  * already allocated.
 619  */
 620 int usb4_switch_alloc_dp_resource(struct tb_switch *sw, struct tb_port *in)
 621 {
 622         u8 status;
 623         int ret;
 624
 625         ret = usb4_switch_op_write_metadata(sw, in->port);
 626         if (ret)
 627                 return ret;
 628
 629         ret = usb4_switch_op(sw, USB4_SWITCH_OP_ALLOC_DP_RESOURCE, &status);
 630         if (ret == -EOPNOTSUPP)
 631                 return 0;
 632         else if (ret)
 633                 return ret;
 634
 635         return status ? -EBUSY : 0;
 636 }
 637
 638 /**
 639  * usb4_switch_dealloc_dp_resource() - Releases allocated DP IN resource
 640  * @sw: USB4 router
 641  * @in: DP IN adapter
 642  *
 643  * Releases the previously allocated DP IN resource.
 644  */
 645 int usb4_switch_dealloc_dp_resource(struct tb_switch *sw, struct tb_port *in)
 646 {
 647         u8 status;
 648         int ret;
 649
 650         ret = usb4_switch_op_write_metadata(sw, in->port);
 651         if (ret)
 652                 return ret;
 653
 654         ret = usb4_switch_op(sw, USB4_SWITCH_OP_DEALLOC_DP_RESOURCE, &status);
 655         if (ret == -EOPNOTSUPP)
 656                 return 0;
 657         else if (ret)
 658                 return ret;
 659
 660         return status ? -EIO : 0;
 661 }
 662
 663 static int usb4_port_idx(const struct tb_switch *sw, const struct tb_port *port)
 664 {
 665         struct tb_port *p;
 666         int usb4_idx = 0;
 667
 668         /* Assume port is primary */
 669         tb_switch_for_each_port(sw, p) {
 670                 if (!tb_port_is_null(p))
 671                         continue;
 672                 if (tb_is_upstream_port(p))
 673                         continue;
 674                 if (!p->link_nr) {
 675                         if (p == port)
 676                                 break;
 677                         usb4_idx++;
 678                 }
 679         }
 680
 681         return usb4_idx;
 682 }
 683
 684 /**
 685  * usb4_switch_map_pcie_down() - Map USB4 port to a PCIe downstream adapter
 686  * @sw: USB4 router
 687  * @port: USB4 port
 688  *
 689  * USB4 routers have direct mapping between USB4 ports and PCIe
 690  * downstream adapters where the PCIe topology is extended. This
 691  * function returns the corresponding downstream PCIe adapter or %NULL
 692  * if no such mapping was possible.
 693  */
 694 struct tb_port *usb4_switch_map_pcie_down(struct tb_switch *sw,
 695                                           const struct tb_port *port)
 696 {
 697         int usb4_idx = usb4_port_idx(sw, port);
 698         struct tb_port *p;
 699         int pcie_idx = 0;
 700
 701         /* Find PCIe down port matching usb4_port */
 702         tb_switch_for_each_port(sw, p) {
 703                 if (!tb_port_is_pcie_down(p))
 704                         continue;
 705
 706                 if (pcie_idx == usb4_idx && !tb_pci_port_is_enabled(p))
 707                         return p;
 708
 709                 pcie_idx++;
 710         }
 711
 712         return NULL;
 713 }
 714
 715 /**
 716  * usb4_switch_map_usb3_down() - Map USB4 port to a USB3 downstream adapter
 717  * @sw: USB4 router
 718  * @port: USB4 port
 719  *
 720  * USB4 routers have direct mapping between USB4 ports and USB 3.x
 721  * downstream adapters where the USB 3.x topology is extended. This
 722  * function returns the corresponding downstream USB 3.x adapter or
 723  * %NULL if no such mapping was possible.
 724  */
 725 struct tb_port *usb4_switch_map_usb3_down(struct tb_switch *sw,
 726                                           const struct tb_port *port)
 727 {
 728         int usb4_idx = usb4_port_idx(sw, port);
 729         struct tb_port *p;
 730         int usb_idx = 0;
 731
 732         /* Find USB3 down port matching usb4_port */
 733         tb_switch_for_each_port(sw, p) {
 734                 if (!tb_port_is_usb3_down(p))
 735                         continue;
 736
 737                 if (usb_idx == usb4_idx && !tb_usb3_port_is_enabled(p))
 738                         return p;
 739
 740                 usb_idx++;
 741         }
 742
 743         return NULL;
 744 }
 745
 746 /**
 747  * usb4_port_unlock() - Unlock USB4 downstream port
 748  * @port: USB4 port to unlock
 749  *
 750  * Unlocks USB4 downstream port so that the connection manager can
 751  * access the router below this port.
 752  */
 753 int usb4_port_unlock(struct tb_port *port)
 754 {
 755         int ret;
 756         u32 val;
 757
 758         ret = tb_port_read(port, &val, TB_CFG_PORT, ADP_CS_4, 1);
 759         if (ret)
 760                 return ret;
 761
 762         val &= ~ADP_CS_4_LCK;
 763         return tb_port_write(port, &val, TB_CFG_PORT, ADP_CS_4, 1);
 764 }