drivers/thunderbolt/tunnel.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Thunderbolt driver - Tunneling support
   4  *
   5  * Copyright (c) 2014 Andreas Noever <[email protected]>
   6  * Copyright (C) 2019, Intel Corporation
   7  */
   8
   9 #include <linux/delay.h>
  10 #include <linux/slab.h>
  11 #include <linux/list.h>
  12 #include <linux/ktime.h>
  13 #include <linux/string_helpers.h>
  14
  15 #include "tunnel.h"
  16 #include "tb.h"
  17
  18 /* PCIe adapters use always HopID of 8 for both directions */
  19 #define TB_PCI_HOPID                    8
  20
  21 #define TB_PCI_PATH_DOWN                0
  22 #define TB_PCI_PATH_UP                  1
  23
  24 #define TB_PCI_PRIORITY                 3
  25 #define TB_PCI_WEIGHT                   1
  26
  27 /* USB3 adapters use always HopID of 8 for both directions */
  28 #define TB_USB3_HOPID                   8
  29
  30 #define TB_USB3_PATH_DOWN               0
  31 #define TB_USB3_PATH_UP                 1
  32
  33 #define TB_USB3_PRIORITY                3
  34 #define TB_USB3_WEIGHT                  2
  35
  36 /* DP adapters use HopID 8 for AUX and 9 for Video */
  37 #define TB_DP_AUX_TX_HOPID              8
  38 #define TB_DP_AUX_RX_HOPID              8
  39 #define TB_DP_VIDEO_HOPID               9
  40
  41 #define TB_DP_VIDEO_PATH_OUT            0
  42 #define TB_DP_AUX_PATH_OUT              1
  43 #define TB_DP_AUX_PATH_IN               2
  44
  45 #define TB_DP_VIDEO_PRIORITY            1
  46 #define TB_DP_VIDEO_WEIGHT              1
  47
  48 #define TB_DP_AUX_PRIORITY              2
  49 #define TB_DP_AUX_WEIGHT                1
  50
  51 /* Minimum number of credits needed for PCIe path */
  52 #define TB_MIN_PCIE_CREDITS             6U
  53 /*
  54  * Number of credits we try to allocate for each DMA path if not limited
  55  * by the host router baMaxHI.
  56  */
  57 #define TB_DMA_CREDITS                  14
  58 /* Minimum number of credits for DMA path */
  59 #define TB_MIN_DMA_CREDITS              1
  60
  61 #define TB_DMA_PRIORITY                 5
  62 #define TB_DMA_WEIGHT                   1
  63
  64 /*
  65  * Reserve additional bandwidth for USB 3.x and PCIe bulk traffic
  66  * according to USB4 v2 Connection Manager guide. This ends up reserving
  67  * 1500 Mb/s for PCIe and 3000 Mb/s for USB 3.x taking weights into
  68  * account.
  69  */
  70 #define USB4_V2_PCI_MIN_BANDWIDTH       (1500 * TB_PCI_WEIGHT)
  71 #define USB4_V2_USB3_MIN_BANDWIDTH      (1500 * TB_USB3_WEIGHT)
  72
  73 static unsigned int dma_credits = TB_DMA_CREDITS;
  74 module_param(dma_credits, uint, 0444);
  75 MODULE_PARM_DESC(dma_credits, "specify custom credits for DMA tunnels (default: "
  76                 __MODULE_STRING(TB_DMA_CREDITS) ")");
  77
  78 static bool bw_alloc_mode = true;
  79 module_param(bw_alloc_mode, bool, 0444);
  80 MODULE_PARM_DESC(bw_alloc_mode,
  81                  "enable bandwidth allocation mode if supported (default: true)");
  82
  83 static const char * const tb_tunnel_names[] = { "PCI", "DP", "DMA", "USB3" };
  84
  85 static inline unsigned int tb_usable_credits(const struct tb_port *port)
  86 {
  87         return port->total_credits - port->ctl_credits;
  88 }
  89
  90 /**
  91  * tb_available_credits() - Available credits for PCIe and DMA
  92  * @port: Lane adapter to check
  93  * @max_dp_streams: If non-%NULL stores maximum number of simultaneous DP
  94  *                  streams possible through this lane adapter
  95  */
  96 static unsigned int tb_available_credits(const struct tb_port *port,
  97                                          size_t *max_dp_streams)
  98 {
  99         const struct tb_switch *sw = port->sw;
 100         int credits, usb3, pcie, spare;
 101         size_t ndp;
 102
 103         usb3 = tb_acpi_may_tunnel_usb3() ? sw->max_usb3_credits : 0;
 104         pcie = tb_acpi_may_tunnel_pcie() ? sw->max_pcie_credits : 0;
 105
 106         if (tb_acpi_is_xdomain_allowed()) {
 107                 spare = min_not_zero(sw->max_dma_credits, dma_credits);
 108                 /* Add some credits for potential second DMA tunnel */
 109                 spare += TB_MIN_DMA_CREDITS;
 110         } else {
 111                 spare = 0;
 112         }
 113
 114         credits = tb_usable_credits(port);
 115         if (tb_acpi_may_tunnel_dp()) {
 116                 /*
 117                  * Maximum number of DP streams possible through the
 118                  * lane adapter.
 119                  */
 120                 if (sw->min_dp_aux_credits + sw->min_dp_main_credits)
 121                         ndp = (credits - (usb3 + pcie + spare)) /
 122                               (sw->min_dp_aux_credits + sw->min_dp_main_credits);
 123                 else
 124                         ndp = 0;
 125         } else {
 126                 ndp = 0;
 127         }
 128         credits -= ndp * (sw->min_dp_aux_credits + sw->min_dp_main_credits);
 129         credits -= usb3;
 130
 131         if (max_dp_streams)
 132                 *max_dp_streams = ndp;
 133
 134         return credits > 0 ? credits : 0;
 135 }
 136
 137 static void tb_init_pm_support(struct tb_path_hop *hop)
 138 {
 139         struct tb_port *out_port = hop->out_port;
 140         struct tb_port *in_port = hop->in_port;
 141
 142         if (tb_port_is_null(in_port) && tb_port_is_null(out_port) &&
 143             usb4_switch_version(in_port->sw) >= 2)
 144                 hop->pm_support = true;
 145 }
 146
 147 static struct tb_tunnel *tb_tunnel_alloc(struct tb *tb, size_t npaths,
 148                                          enum tb_tunnel_type type)
 149 {
 150         struct tb_tunnel *tunnel;
 151
 152         tunnel = kzalloc(sizeof(*tunnel), GFP_KERNEL);
 153         if (!tunnel)
 154                 return NULL;
 155
 156         tunnel->paths = kcalloc(npaths, sizeof(tunnel->paths[0]), GFP_KERNEL);
 157         if (!tunnel->paths) {
 158                 tb_tunnel_free(tunnel);
 159                 return NULL;
 160         }
 161
 162         INIT_LIST_HEAD(&tunnel->list);
 163         tunnel->tb = tb;
 164         tunnel->npaths = npaths;
 165         tunnel->type = type;
 166
 167         return tunnel;
 168 }
 169
 170 static int tb_pci_set_ext_encapsulation(struct tb_tunnel *tunnel, bool enable)
 171 {
 172         struct tb_port *port = tb_upstream_port(tunnel->dst_port->sw);
 173         int ret;
 174
 175         /* Only supported of both routers are at least USB4 v2 */
 176         if ((usb4_switch_version(tunnel->src_port->sw) < 2) ||
 177            (usb4_switch_version(tunnel->dst_port->sw) < 2))
 178                 return 0;
 179
 180         if (enable && tb_port_get_link_generation(port) < 4)
 181                 return 0;
 182
 183         ret = usb4_pci_port_set_ext_encapsulation(tunnel->src_port, enable);
 184         if (ret)
 185                 return ret;
 186
 187         /*
 188          * Downstream router could be unplugged so disable of encapsulation
 189          * in upstream router is still possible.
 190          */
 191         ret = usb4_pci_port_set_ext_encapsulation(tunnel->dst_port, enable);
 192         if (ret) {
 193                 if (enable)
 194                         return ret;
 195                 if (ret != -ENODEV)
 196                         return ret;
 197         }
 198
 199         tb_tunnel_dbg(tunnel, "extended encapsulation %s\n",
 200                       str_enabled_disabled(enable));
 201         return 0;
 202 }
 203
 204 static int tb_pci_activate(struct tb_tunnel *tunnel, bool activate)
 205 {
 206         int res;
 207
 208         if (activate) {
 209                 res = tb_pci_set_ext_encapsulation(tunnel, activate);
 210                 if (res)
 211                         return res;
 212         }
 213
 214         if (activate)
 215                 res = tb_pci_port_enable(tunnel->dst_port, activate);
 216         else
 217                 res = tb_pci_port_enable(tunnel->src_port, activate);
 218         if (res)
 219                 return res;
 220
 221
 222         if (activate) {
 223                 res = tb_pci_port_enable(tunnel->src_port, activate);
 224                 if (res)
 225                         return res;
 226         } else {
 227                 /* Downstream router could be unplugged */
 228                 tb_pci_port_enable(tunnel->dst_port, activate);
 229         }
 230
 231         return activate ? 0 : tb_pci_set_ext_encapsulation(tunnel, activate);
 232 }
 233
 234 static int tb_pci_init_credits(struct tb_path_hop *hop)
 235 {
 236         struct tb_port *port = hop->in_port;
 237         struct tb_switch *sw = port->sw;
 238         unsigned int credits;
 239
 240         if (tb_port_use_credit_allocation(port)) {
 241                 unsigned int available;
 242
 243                 available = tb_available_credits(port, NULL);
 244                 credits = min(sw->max_pcie_credits, available);
 245
 246                 if (credits < TB_MIN_PCIE_CREDITS)
 247                         return -ENOSPC;
 248
 249                 credits = max(TB_MIN_PCIE_CREDITS, credits);
 250         } else {
 251                 if (tb_port_is_null(port))
 252                         credits = port->bonded ? 32 : 16;
 253                 else
 254                         credits = 7;
 255         }
 256
 257         hop->initial_credits = credits;
 258         return 0;
 259 }
 260
 261 static int tb_pci_init_path(struct tb_path *path)
 262 {
 263         struct tb_path_hop *hop;
 264
 265         path->egress_fc_enable = TB_PATH_SOURCE | TB_PATH_INTERNAL;
 266         path->egress_shared_buffer = TB_PATH_NONE;
 267         path->ingress_fc_enable = TB_PATH_ALL;
 268         path->ingress_shared_buffer = TB_PATH_NONE;
 269         path->priority = TB_PCI_PRIORITY;
 270         path->weight = TB_PCI_WEIGHT;
 271         path->drop_packages = 0;
 272
 273         tb_path_for_each_hop(path, hop) {
 274                 int ret;
 275
 276                 ret = tb_pci_init_credits(hop);
 277                 if (ret)
 278                         return ret;
 279         }
 280
 281         return 0;
 282 }
 283
 284 /**
 285  * tb_tunnel_discover_pci() - Discover existing PCIe tunnels
 286  * @tb: Pointer to the domain structure
 287  * @down: PCIe downstream adapter
 288  * @alloc_hopid: Allocate HopIDs from visited ports
 289  *
 290  * If @down adapter is active, follows the tunnel to the PCIe upstream
 291  * adapter and back. Returns the discovered tunnel or %NULL if there was
 292  * no tunnel.
 293  */
 294 struct tb_tunnel *tb_tunnel_discover_pci(struct tb *tb, struct tb_port *down,
 295                                          bool alloc_hopid)
 296 {
 297         struct tb_tunnel *tunnel;
 298         struct tb_path *path;
 299
 300         if (!tb_pci_port_is_enabled(down))
 301                 return NULL;
 302
 303         tunnel = tb_tunnel_alloc(tb, 2, TB_TUNNEL_PCI);
 304         if (!tunnel)
 305                 return NULL;
 306
 307         tunnel->activate = tb_pci_activate;
 308         tunnel->src_port = down;
 309
 310         /*
 311          * Discover both paths even if they are not complete. We will
 312          * clean them up by calling tb_tunnel_deactivate() below in that
 313          * case.
 314          */
 315         path = tb_path_discover(down, TB_PCI_HOPID, NULL, -1,
 316                                 &tunnel->dst_port, "PCIe Up", alloc_hopid);
 317         if (!path) {
 318                 /* Just disable the downstream port */
 319                 tb_pci_port_enable(down, false);
 320                 goto err_free;
 321         }
 322         tunnel->paths[TB_PCI_PATH_UP] = path;
 323         if (tb_pci_init_path(tunnel->paths[TB_PCI_PATH_UP]))
 324                 goto err_free;
 325
 326         path = tb_path_discover(tunnel->dst_port, -1, down, TB_PCI_HOPID, NULL,
 327                                 "PCIe Down", alloc_hopid);
 328         if (!path)
 329                 goto err_deactivate;
 330         tunnel->paths[TB_PCI_PATH_DOWN] = path;
 331         if (tb_pci_init_path(tunnel->paths[TB_PCI_PATH_DOWN]))
 332                 goto err_deactivate;
 333
 334         /* Validate that the tunnel is complete */
 335         if (!tb_port_is_pcie_up(tunnel->dst_port)) {
 336                 tb_port_warn(tunnel->dst_port,
 337                              "path does not end on a PCIe adapter, cleaning up\n");
 338                 goto err_deactivate;
 339         }
 340
 341         if (down != tunnel->src_port) {
 342                 tb_tunnel_warn(tunnel, "path is not complete, cleaning up\n");
 343                 goto err_deactivate;
 344         }
 345
 346         if (!tb_pci_port_is_enabled(tunnel->dst_port)) {
 347                 tb_tunnel_warn(tunnel,
 348                                "tunnel is not fully activated, cleaning up\n");
 349                 goto err_deactivate;
 350         }
 351
 352         tb_tunnel_dbg(tunnel, "discovered\n");
 353         return tunnel;
 354
 355 err_deactivate:
 356         tb_tunnel_deactivate(tunnel);
 357 err_free:
 358         tb_tunnel_free(tunnel);
 359
 360         return NULL;
 361 }
 362
 363 /**
 364  * tb_tunnel_alloc_pci() - allocate a pci tunnel
 365  * @tb: Pointer to the domain structure
 366  * @up: PCIe upstream adapter port
 367  * @down: PCIe downstream adapter port
 368  *
 369  * Allocate a PCI tunnel. The ports must be of type TB_TYPE_PCIE_UP and
 370  * TB_TYPE_PCIE_DOWN.
 371  *
 372  * Return: Returns a tb_tunnel on success or NULL on failure.
 373  */
 374 struct tb_tunnel *tb_tunnel_alloc_pci(struct tb *tb, struct tb_port *up,
 375                                       struct tb_port *down)
 376 {
 377         struct tb_tunnel *tunnel;
 378         struct tb_path *path;
 379
 380         tunnel = tb_tunnel_alloc(tb, 2, TB_TUNNEL_PCI);
 381         if (!tunnel)
 382                 return NULL;
 383
 384         tunnel->activate = tb_pci_activate;
 385         tunnel->src_port = down;
 386         tunnel->dst_port = up;
 387
 388         path = tb_path_alloc(tb, down, TB_PCI_HOPID, up, TB_PCI_HOPID, 0,
 389                              "PCIe Down");
 390         if (!path)
 391                 goto err_free;
 392         tunnel->paths[TB_PCI_PATH_DOWN] = path;
 393         if (tb_pci_init_path(path))
 394                 goto err_free;
 395
 396         path = tb_path_alloc(tb, up, TB_PCI_HOPID, down, TB_PCI_HOPID, 0,
 397                              "PCIe Up");
 398         if (!path)
 399                 goto err_free;
 400         tunnel->paths[TB_PCI_PATH_UP] = path;
 401         if (tb_pci_init_path(path))
 402                 goto err_free;
 403
 404         return tunnel;
 405
 406 err_free:
 407         tb_tunnel_free(tunnel);
 408         return NULL;
 409 }
 410
 411 /**
 412  * tb_tunnel_reserved_pci() - Amount of bandwidth to reserve for PCIe
 413  * @port: Lane 0 adapter
 414  * @reserved_up: Upstream bandwidth in Mb/s to reserve
 415  * @reserved_down: Downstream bandwidth in Mb/s to reserve
 416  *
 417  * Can be called to any connected lane 0 adapter to find out how much
 418  * bandwidth needs to be left in reserve for possible PCIe bulk traffic.
 419  * Returns true if there is something to be reserved and writes the
 420  * amount to @reserved_down/@reserved_up. Otherwise returns false and
 421  * does not touch the parameters.
 422  */
 423 bool tb_tunnel_reserved_pci(struct tb_port *port, int *reserved_up,
 424                             int *reserved_down)
 425 {
 426         if (WARN_ON_ONCE(!port->remote))
 427                 return false;
 428
 429         if (!tb_acpi_may_tunnel_pcie())
 430                 return false;
 431
 432         if (tb_port_get_link_generation(port) < 4)
 433                 return false;
 434
 435         /* Must have PCIe adapters */
 436         if (tb_is_upstream_port(port)) {
 437                 if (!tb_switch_find_port(port->sw, TB_TYPE_PCIE_UP))
 438                         return false;
 439                 if (!tb_switch_find_port(port->remote->sw, TB_TYPE_PCIE_DOWN))
 440                         return false;
 441         } else {
 442                 if (!tb_switch_find_port(port->sw, TB_TYPE_PCIE_DOWN))
 443                         return false;
 444                 if (!tb_switch_find_port(port->remote->sw, TB_TYPE_PCIE_UP))
 445                         return false;
 446         }
 447
 448         *reserved_up = USB4_V2_PCI_MIN_BANDWIDTH;
 449         *reserved_down = USB4_V2_PCI_MIN_BANDWIDTH;
 450
 451         tb_port_dbg(port, "reserving %u/%u Mb/s for PCIe\n", *reserved_up,
 452                     *reserved_down);
 453         return true;
 454 }
 455
 456 static bool tb_dp_is_usb4(const struct tb_switch *sw)
 457 {
 458         /* Titan Ridge DP adapters need the same treatment as USB4 */
 459         return tb_switch_is_usb4(sw) || tb_switch_is_titan_ridge(sw);
 460 }
 461
 462 static int tb_dp_cm_handshake(struct tb_port *in, struct tb_port *out,
 463                               int timeout_msec)
 464 {
 465         ktime_t timeout = ktime_add_ms(ktime_get(), timeout_msec);
 466         u32 val;
 467         int ret;
 468
 469         /* Both ends need to support this */
 470         if (!tb_dp_is_usb4(in->sw) || !tb_dp_is_usb4(out->sw))
 471                 return 0;
 472
 473         ret = tb_port_read(out, &val, TB_CFG_PORT,
 474                            out->cap_adap + DP_STATUS_CTRL, 1);
 475         if (ret)
 476                 return ret;
 477
 478         val |= DP_STATUS_CTRL_UF | DP_STATUS_CTRL_CMHS;
 479
 480         ret = tb_port_write(out, &val, TB_CFG_PORT,
 481                             out->cap_adap + DP_STATUS_CTRL, 1);
 482         if (ret)
 483                 return ret;
 484
 485         do {
 486                 ret = tb_port_read(out, &val, TB_CFG_PORT,
 487                                    out->cap_adap + DP_STATUS_CTRL, 1);
 488                 if (ret)
 489                         return ret;
 490                 if (!(val & DP_STATUS_CTRL_CMHS))
 491                         return 0;
 492                 usleep_range(100, 150);
 493         } while (ktime_before(ktime_get(), timeout));
 494
 495         return -ETIMEDOUT;
 496 }
 497
 498 /*
 499  * Returns maximum possible rate from capability supporting only DP 2.0
 500  * and below. Used when DP BW allocation mode is not enabled.
 501  */
 502 static inline u32 tb_dp_cap_get_rate(u32 val)
 503 {
 504         u32 rate = (val & DP_COMMON_CAP_RATE_MASK) >> DP_COMMON_CAP_RATE_SHIFT;
 505
 506         switch (rate) {
 507         case DP_COMMON_CAP_RATE_RBR:
 508                 return 1620;
 509         case DP_COMMON_CAP_RATE_HBR:
 510                 return 2700;
 511         case DP_COMMON_CAP_RATE_HBR2:
 512                 return 5400;
 513         case DP_COMMON_CAP_RATE_HBR3:
 514                 return 8100;
 515         default:
 516                 return 0;
 517         }
 518 }
 519
 520 /*
 521  * Returns maximum possible rate from capability supporting DP 2.1
 522  * UHBR20, 13.5 and 10 rates as well. Use only when DP BW allocation
 523  * mode is enabled.
 524  */
 525 static inline u32 tb_dp_cap_get_rate_ext(u32 val)
 526 {
 527         if (val & DP_COMMON_CAP_UHBR20)
 528                 return 20000;
 529         else if (val & DP_COMMON_CAP_UHBR13_5)
 530                 return 13500;
 531         else if (val & DP_COMMON_CAP_UHBR10)
 532                 return 10000;
 533
 534         return tb_dp_cap_get_rate(val);
 535 }
 536
 537 static inline bool tb_dp_is_uhbr_rate(unsigned int rate)
 538 {
 539         return rate >= 10000;
 540 }
 541
 542 static inline u32 tb_dp_cap_set_rate(u32 val, u32 rate)
 543 {
 544         val &= ~DP_COMMON_CAP_RATE_MASK;
 545         switch (rate) {
 546         default:
 547                 WARN(1, "invalid rate %u passed, defaulting to 1620 MB/s\n", rate);
 548                 fallthrough;
 549         case 1620:
 550                 val |= DP_COMMON_CAP_RATE_RBR << DP_COMMON_CAP_RATE_SHIFT;
 551                 break;
 552         case 2700:
 553                 val |= DP_COMMON_CAP_RATE_HBR << DP_COMMON_CAP_RATE_SHIFT;
 554                 break;
 555         case 5400:
 556                 val |= DP_COMMON_CAP_RATE_HBR2 << DP_COMMON_CAP_RATE_SHIFT;
 557                 break;
 558         case 8100:
 559                 val |= DP_COMMON_CAP_RATE_HBR3 << DP_COMMON_CAP_RATE_SHIFT;
 560                 break;
 561         }
 562         return val;
 563 }
 564
 565 static inline u32 tb_dp_cap_get_lanes(u32 val)
 566 {
 567         u32 lanes = (val & DP_COMMON_CAP_LANES_MASK) >> DP_COMMON_CAP_LANES_SHIFT;
 568
 569         switch (lanes) {
 570         case DP_COMMON_CAP_1_LANE:
 571                 return 1;
 572         case DP_COMMON_CAP_2_LANES:
 573                 return 2;
 574         case DP_COMMON_CAP_4_LANES:
 575                 return 4;
 576         default:
 577                 return 0;
 578         }
 579 }
 580
 581 static inline u32 tb_dp_cap_set_lanes(u32 val, u32 lanes)
 582 {
 583         val &= ~DP_COMMON_CAP_LANES_MASK;
 584         switch (lanes) {
 585         default:
 586                 WARN(1, "invalid number of lanes %u passed, defaulting to 1\n",
 587                      lanes);
 588                 fallthrough;
 589         case 1:
 590                 val |= DP_COMMON_CAP_1_LANE << DP_COMMON_CAP_LANES_SHIFT;
 591                 break;
 592         case 2:
 593                 val |= DP_COMMON_CAP_2_LANES << DP_COMMON_CAP_LANES_SHIFT;
 594                 break;
 595         case 4:
 596                 val |= DP_COMMON_CAP_4_LANES << DP_COMMON_CAP_LANES_SHIFT;
 597                 break;
 598         }
 599         return val;
 600 }
 601
 602 static unsigned int tb_dp_bandwidth(unsigned int rate, unsigned int lanes)
 603 {
 604         /* Tunneling removes the DP 8b/10b 128/132b encoding */
 605         if (tb_dp_is_uhbr_rate(rate))
 606                 return rate * lanes * 128 / 132;
 607         return rate * lanes * 8 / 10;
 608 }
 609
 610 static int tb_dp_reduce_bandwidth(int max_bw, u32 in_rate, u32 in_lanes,
 611                                   u32 out_rate, u32 out_lanes, u32 *new_rate,
 612                                   u32 *new_lanes)
 613 {
 614         static const u32 dp_bw[][2] = {
 615                 /* Mb/s, lanes */
 616                 { 8100, 4 }, /* 25920 Mb/s */
 617                 { 5400, 4 }, /* 17280 Mb/s */
 618                 { 8100, 2 }, /* 12960 Mb/s */
 619                 { 2700, 4 }, /* 8640 Mb/s */
 620                 { 5400, 2 }, /* 8640 Mb/s */
 621                 { 8100, 1 }, /* 6480 Mb/s */
 622                 { 1620, 4 }, /* 5184 Mb/s */
 623                 { 5400, 1 }, /* 4320 Mb/s */
 624                 { 2700, 2 }, /* 4320 Mb/s */
 625                 { 1620, 2 }, /* 2592 Mb/s */
 626                 { 2700, 1 }, /* 2160 Mb/s */
 627                 { 1620, 1 }, /* 1296 Mb/s */
 628         };
 629         unsigned int i;
 630
 631         /*
 632          * Find a combination that can fit into max_bw and does not
 633          * exceed the maximum rate and lanes supported by the DP OUT and
 634          * DP IN adapters.
 635          */
 636         for (i = 0; i < ARRAY_SIZE(dp_bw); i++) {
 637                 if (dp_bw[i][0] > out_rate || dp_bw[i][1] > out_lanes)
 638                         continue;
 639
 640                 if (dp_bw[i][0] > in_rate || dp_bw[i][1] > in_lanes)
 641                         continue;
 642
 643                 if (tb_dp_bandwidth(dp_bw[i][0], dp_bw[i][1]) <= max_bw) {
 644                         *new_rate = dp_bw[i][0];
 645                         *new_lanes = dp_bw[i][1];
 646                         return 0;
 647                 }
 648         }
 649
 650         return -ENOSR;
 651 }
 652
 653 static int tb_dp_xchg_caps(struct tb_tunnel *tunnel)
 654 {
 655         u32 out_dp_cap, out_rate, out_lanes, in_dp_cap, in_rate, in_lanes, bw;
 656         struct tb_port *out = tunnel->dst_port;
 657         struct tb_port *in = tunnel->src_port;
 658         int ret, max_bw;
 659
 660         /*
 661          * Copy DP_LOCAL_CAP register to DP_REMOTE_CAP register for
 662          * newer generation hardware.
 663          */
 664         if (in->sw->generation < 2 || out->sw->generation < 2)
 665                 return 0;
 666
 667         /*
 668          * Perform connection manager handshake between IN and OUT ports
 669          * before capabilities exchange can take place.
 670          */
 671         ret = tb_dp_cm_handshake(in, out, 3000);
 672         if (ret)
 673                 return ret;
 674
 675         /* Read both DP_LOCAL_CAP registers */
 676         ret = tb_port_read(in, &in_dp_cap, TB_CFG_PORT,
 677                            in->cap_adap + DP_LOCAL_CAP, 1);
 678         if (ret)
 679                 return ret;
 680
 681         ret = tb_port_read(out, &out_dp_cap, TB_CFG_PORT,
 682                            out->cap_adap + DP_LOCAL_CAP, 1);
 683         if (ret)
 684                 return ret;
 685
 686         /* Write IN local caps to OUT remote caps */
 687         ret = tb_port_write(out, &in_dp_cap, TB_CFG_PORT,
 688                             out->cap_adap + DP_REMOTE_CAP, 1);
 689         if (ret)
 690                 return ret;
 691
 692         in_rate = tb_dp_cap_get_rate(in_dp_cap);
 693         in_lanes = tb_dp_cap_get_lanes(in_dp_cap);
 694         tb_tunnel_dbg(tunnel,
 695                       "DP IN maximum supported bandwidth %u Mb/s x%u = %u Mb/s\n",
 696                       in_rate, in_lanes, tb_dp_bandwidth(in_rate, in_lanes));
 697
 698         /*
 699          * If the tunnel bandwidth is limited (max_bw is set) then see
 700          * if we need to reduce bandwidth to fit there.
 701          */
 702         out_rate = tb_dp_cap_get_rate(out_dp_cap);
 703         out_lanes = tb_dp_cap_get_lanes(out_dp_cap);
 704         bw = tb_dp_bandwidth(out_rate, out_lanes);
 705         tb_tunnel_dbg(tunnel,
 706                       "DP OUT maximum supported bandwidth %u Mb/s x%u = %u Mb/s\n",
 707                       out_rate, out_lanes, bw);
 708
 709         if (tb_tunnel_direction_downstream(tunnel))
 710                 max_bw = tunnel->max_down;
 711         else
 712                 max_bw = tunnel->max_up;
 713
 714         if (max_bw && bw > max_bw) {
 715                 u32 new_rate, new_lanes, new_bw;
 716
 717                 ret = tb_dp_reduce_bandwidth(max_bw, in_rate, in_lanes,
 718                                              out_rate, out_lanes, &new_rate,
 719                                              &new_lanes);
 720                 if (ret) {
 721                         tb_tunnel_info(tunnel, "not enough bandwidth\n");
 722                         return ret;
 723                 }
 724
 725                 new_bw = tb_dp_bandwidth(new_rate, new_lanes);
 726                 tb_tunnel_dbg(tunnel,
 727                               "bandwidth reduced to %u Mb/s x%u = %u Mb/s\n",
 728                               new_rate, new_lanes, new_bw);
 729
 730                 /*
 731                  * Set new rate and number of lanes before writing it to
 732                  * the IN port remote caps.
 733                  */
 734                 out_dp_cap = tb_dp_cap_set_rate(out_dp_cap, new_rate);
 735                 out_dp_cap = tb_dp_cap_set_lanes(out_dp_cap, new_lanes);
 736         }
 737
 738         /*
 739          * Titan Ridge does not disable AUX timers when it gets
 740          * SET_CONFIG with SET_LTTPR_MODE set. This causes problems with
 741          * DP tunneling.
 742          */
 743         if (tb_route(out->sw) && tb_switch_is_titan_ridge(out->sw)) {
 744                 out_dp_cap |= DP_COMMON_CAP_LTTPR_NS;
 745                 tb_tunnel_dbg(tunnel, "disabling LTTPR\n");
 746         }
 747
 748         return tb_port_write(in, &out_dp_cap, TB_CFG_PORT,
 749                              in->cap_adap + DP_REMOTE_CAP, 1);
 750 }
 751
 752 static int tb_dp_bandwidth_alloc_mode_enable(struct tb_tunnel *tunnel)
 753 {
 754         int ret, estimated_bw, granularity, tmp;
 755         struct tb_port *out = tunnel->dst_port;
 756         struct tb_port *in = tunnel->src_port;
 757         u32 out_dp_cap, out_rate, out_lanes;
 758         u32 in_dp_cap, in_rate, in_lanes;
 759         u32 rate, lanes;
 760
 761         if (!bw_alloc_mode)
 762                 return 0;
 763
 764         ret = usb4_dp_port_set_cm_bandwidth_mode_supported(in, true);
 765         if (ret)
 766                 return ret;
 767
 768         ret = usb4_dp_port_set_group_id(in, in->group->index);
 769         if (ret)
 770                 return ret;
 771
 772         /*
 773          * Get the non-reduced rate and lanes based on the lowest
 774          * capability of both adapters.
 775          */
 776         ret = tb_port_read(in, &in_dp_cap, TB_CFG_PORT,
 777                            in->cap_adap + DP_LOCAL_CAP, 1);
 778         if (ret)
 779                 return ret;
 780
 781         ret = tb_port_read(out, &out_dp_cap, TB_CFG_PORT,
 782                            out->cap_adap + DP_LOCAL_CAP, 1);
 783         if (ret)
 784                 return ret;
 785
 786         in_rate = tb_dp_cap_get_rate(in_dp_cap);
 787         in_lanes = tb_dp_cap_get_lanes(in_dp_cap);
 788         out_rate = tb_dp_cap_get_rate(out_dp_cap);
 789         out_lanes = tb_dp_cap_get_lanes(out_dp_cap);
 790
 791         rate = min(in_rate, out_rate);
 792         lanes = min(in_lanes, out_lanes);
 793         tmp = tb_dp_bandwidth(rate, lanes);
 794
 795         tb_tunnel_dbg(tunnel, "non-reduced bandwidth %u Mb/s x%u = %u Mb/s\n",
 796                       rate, lanes, tmp);
 797
 798         ret = usb4_dp_port_set_nrd(in, rate, lanes);
 799         if (ret)
 800                 return ret;
 801
 802         /*
 803          * Pick up granularity that supports maximum possible bandwidth.
 804          * For that we use the UHBR rates too.
 805          */
 806         in_rate = tb_dp_cap_get_rate_ext(in_dp_cap);
 807         out_rate = tb_dp_cap_get_rate_ext(out_dp_cap);
 808         rate = min(in_rate, out_rate);
 809         tmp = tb_dp_bandwidth(rate, lanes);
 810
 811         tb_tunnel_dbg(tunnel,
 812                       "maximum bandwidth through allocation mode %u Mb/s x%u = %u Mb/s\n",
 813                       rate, lanes, tmp);
 814
 815         for (granularity = 250; tmp / granularity > 255 && granularity <= 1000;
 816              granularity *= 2)
 817                 ;
 818
 819         tb_tunnel_dbg(tunnel, "granularity %d Mb/s\n", granularity);
 820
 821         /*
 822          * Returns -EINVAL if granularity above is outside of the
 823          * accepted ranges.
 824          */
 825         ret = usb4_dp_port_set_granularity(in, granularity);
 826         if (ret)
 827                 return ret;
 828
 829         /*
 830          * Bandwidth estimation is pretty much what we have in
 831          * max_up/down fields. For discovery we just read what the
 832          * estimation was set to.
 833          */
 834         if (tb_tunnel_direction_downstream(tunnel))
 835                 estimated_bw = tunnel->max_down;
 836         else
 837                 estimated_bw = tunnel->max_up;
 838
 839         tb_tunnel_dbg(tunnel, "estimated bandwidth %d Mb/s\n", estimated_bw);
 840
 841         ret = usb4_dp_port_set_estimated_bandwidth(in, estimated_bw);
 842         if (ret)
 843                 return ret;
 844
 845         /* Initial allocation should be 0 according the spec */
 846         ret = usb4_dp_port_allocate_bandwidth(in, 0);
 847         if (ret)
 848                 return ret;
 849
 850         tb_tunnel_dbg(tunnel, "bandwidth allocation mode enabled\n");
 851         return 0;
 852 }
 853
 854 static int tb_dp_init(struct tb_tunnel *tunnel)
 855 {
 856         struct tb_port *in = tunnel->src_port;
 857         struct tb_switch *sw = in->sw;
 858         struct tb *tb = in->sw->tb;
 859         int ret;
 860
 861         ret = tb_dp_xchg_caps(tunnel);
 862         if (ret)
 863                 return ret;
 864
 865         if (!tb_switch_is_usb4(sw))
 866                 return 0;
 867
 868         if (!usb4_dp_port_bandwidth_mode_supported(in))
 869                 return 0;
 870
 871         tb_tunnel_dbg(tunnel, "bandwidth allocation mode supported\n");
 872
 873         ret = usb4_dp_port_set_cm_id(in, tb->index);
 874         if (ret)
 875                 return ret;
 876
 877         return tb_dp_bandwidth_alloc_mode_enable(tunnel);
 878 }
 879
 880 static void tb_dp_deinit(struct tb_tunnel *tunnel)
 881 {
 882         struct tb_port *in = tunnel->src_port;
 883
 884         if (!usb4_dp_port_bandwidth_mode_supported(in))
 885                 return;
 886         if (usb4_dp_port_bandwidth_mode_enabled(in)) {
 887                 usb4_dp_port_set_cm_bandwidth_mode_supported(in, false);
 888                 tb_tunnel_dbg(tunnel, "bandwidth allocation mode disabled\n");
 889         }
 890 }
 891
 892 static int tb_dp_activate(struct tb_tunnel *tunnel, bool active)
 893 {
 894         int ret;
 895
 896         if (active) {
 897                 struct tb_path **paths;
 898                 int last;
 899
 900                 paths = tunnel->paths;
 901                 last = paths[TB_DP_VIDEO_PATH_OUT]->path_length - 1;
 902
 903                 tb_dp_port_set_hops(tunnel->src_port,
 904                         paths[TB_DP_VIDEO_PATH_OUT]->hops[0].in_hop_index,
 905                         paths[TB_DP_AUX_PATH_OUT]->hops[0].in_hop_index,
 906                         paths[TB_DP_AUX_PATH_IN]->hops[last].next_hop_index);
 907
 908                 tb_dp_port_set_hops(tunnel->dst_port,
 909                         paths[TB_DP_VIDEO_PATH_OUT]->hops[last].next_hop_index,
 910                         paths[TB_DP_AUX_PATH_IN]->hops[0].in_hop_index,
 911                         paths[TB_DP_AUX_PATH_OUT]->hops[last].next_hop_index);
 912         } else {
 913                 tb_dp_port_hpd_clear(tunnel->src_port);
 914                 tb_dp_port_set_hops(tunnel->src_port, 0, 0, 0);
 915                 if (tb_port_is_dpout(tunnel->dst_port))
 916                         tb_dp_port_set_hops(tunnel->dst_port, 0, 0, 0);
 917         }
 918
 919         ret = tb_dp_port_enable(tunnel->src_port, active);
 920         if (ret)
 921                 return ret;
 922
 923         if (tb_port_is_dpout(tunnel->dst_port))
 924                 return tb_dp_port_enable(tunnel->dst_port, active);
 925
 926         return 0;
 927 }
 928
 929 /**
 930  * tb_dp_bandwidth_mode_maximum_bandwidth() - Maximum possible bandwidth
 931  * @tunnel: DP tunnel to check
 932  * @max_bw_rounded: Maximum bandwidth in Mb/s rounded up to the next granularity
 933  *
 934  * Returns maximum possible bandwidth for this tunnel in Mb/s.
 935  */
 936 static int tb_dp_bandwidth_mode_maximum_bandwidth(struct tb_tunnel *tunnel,
 937                                                   int *max_bw_rounded)
 938 {
 939         struct tb_port *in = tunnel->src_port;
 940         int ret, rate, lanes, max_bw;
 941         u32 cap;
 942
 943         /*
 944          * DP IN adapter DP_LOCAL_CAP gets updated to the lowest AUX
 945          * read parameter values so this so we can use this to determine
 946          * the maximum possible bandwidth over this link.
 947          *
 948          * See USB4 v2 spec 1.0 10.4.4.5.
 949          */
 950         ret = tb_port_read(in, &cap, TB_CFG_PORT,
 951                            in->cap_adap + DP_LOCAL_CAP, 1);
 952         if (ret)
 953                 return ret;
 954
 955         rate = tb_dp_cap_get_rate_ext(cap);
 956         lanes = tb_dp_cap_get_lanes(cap);
 957
 958         max_bw = tb_dp_bandwidth(rate, lanes);
 959
 960         if (max_bw_rounded) {
 961                 ret = usb4_dp_port_granularity(in);
 962                 if (ret < 0)
 963                         return ret;
 964                 *max_bw_rounded = roundup(max_bw, ret);
 965         }
 966
 967         return max_bw;
 968 }
 969
 970 static int tb_dp_bandwidth_mode_consumed_bandwidth(struct tb_tunnel *tunnel,
 971                                                    int *consumed_up,
 972                                                    int *consumed_down)
 973 {
 974         struct tb_port *in = tunnel->src_port;
 975         int ret, allocated_bw, max_bw_rounded;
 976
 977         if (!usb4_dp_port_bandwidth_mode_enabled(in))
 978                 return -EOPNOTSUPP;
 979
 980         if (!tunnel->bw_mode)
 981                 return -EOPNOTSUPP;
 982
 983         /* Read what was allocated previously if any */
 984         ret = usb4_dp_port_allocated_bandwidth(in);
 985         if (ret < 0)
 986                 return ret;
 987         allocated_bw = ret;
 988
 989         ret = tb_dp_bandwidth_mode_maximum_bandwidth(tunnel, &max_bw_rounded);
 990         if (ret < 0)
 991                 return ret;
 992         if (allocated_bw == max_bw_rounded)
 993                 allocated_bw = ret;
 994
 995         if (tb_tunnel_direction_downstream(tunnel)) {
 996                 *consumed_up = 0;
 997                 *consumed_down = allocated_bw;
 998         } else {
 999                 *consumed_up = allocated_bw;
1000                 *consumed_down = 0;
1001         }
1002
1003         return 0;
1004 }
1005
1006 static int tb_dp_allocated_bandwidth(struct tb_tunnel *tunnel, int *allocated_up,
1007                                      int *allocated_down)
1008 {
1009         struct tb_port *in = tunnel->src_port;
1010
1011         /*
1012          * If we have already set the allocated bandwidth then use that.
1013          * Otherwise we read it from the DPRX.
1014          */
1015         if (usb4_dp_port_bandwidth_mode_enabled(in) && tunnel->bw_mode) {
1016                 int ret, allocated_bw, max_bw_rounded;
1017
1018                 ret = usb4_dp_port_allocated_bandwidth(in);
1019                 if (ret < 0)
1020                         return ret;
1021                 allocated_bw = ret;
1022
1023                 ret = tb_dp_bandwidth_mode_maximum_bandwidth(tunnel,
1024                                                              &max_bw_rounded);
1025                 if (ret < 0)
1026                         return ret;
1027                 if (allocated_bw == max_bw_rounded)
1028                         allocated_bw = ret;
1029
1030                 if (tb_tunnel_direction_downstream(tunnel)) {
1031                         *allocated_up = 0;
1032                         *allocated_down = allocated_bw;
1033                 } else {
1034                         *allocated_up = allocated_bw;
1035                         *allocated_down = 0;
1036                 }
1037                 return 0;
1038         }
1039
1040         return tunnel->consumed_bandwidth(tunnel, allocated_up,
1041                                           allocated_down);
1042 }
1043
1044 static int tb_dp_alloc_bandwidth(struct tb_tunnel *tunnel, int *alloc_up,
1045                                  int *alloc_down)
1046 {
1047         struct tb_port *in = tunnel->src_port;
1048         int max_bw_rounded, ret, tmp;
1049
1050         if (!usb4_dp_port_bandwidth_mode_enabled(in))
1051                 return -EOPNOTSUPP;
1052
1053         ret = tb_dp_bandwidth_mode_maximum_bandwidth(tunnel, &max_bw_rounded);
1054         if (ret < 0)
1055                 return ret;
1056
1057         if (tb_tunnel_direction_downstream(tunnel)) {
1058                 tmp = min(*alloc_down, max_bw_rounded);
1059                 ret = usb4_dp_port_allocate_bandwidth(in, tmp);
1060                 if (ret)
1061                         return ret;
1062                 *alloc_down = tmp;
1063                 *alloc_up = 0;
1064         } else {
1065                 tmp = min(*alloc_up, max_bw_rounded);
1066                 ret = usb4_dp_port_allocate_bandwidth(in, tmp);
1067                 if (ret)
1068                         return ret;
1069                 *alloc_down = 0;
1070                 *alloc_up = tmp;
1071         }
1072
1073         /* Now we can use BW mode registers to figure out the bandwidth */
1074         /* TODO: need to handle discovery too */
1075         tunnel->bw_mode = true;
1076         return 0;
1077 }
1078
1079 static int tb_dp_wait_dprx(struct tb_tunnel *tunnel, int timeout_msec)
1080 {
1081         ktime_t timeout = ktime_add_ms(ktime_get(), timeout_msec);
1082         struct tb_port *in = tunnel->src_port;
1083
1084         /*
1085          * Wait for DPRX done. Normally it should be already set for
1086          * active tunnel.
1087          */
1088         do {
1089                 u32 val;
1090                 int ret;
1091
1092                 ret = tb_port_read(in, &val, TB_CFG_PORT,
1093                                    in->cap_adap + DP_COMMON_CAP, 1);
1094                 if (ret)
1095                         return ret;
1096
1097                 if (val & DP_COMMON_CAP_DPRX_DONE) {
1098                         tb_tunnel_dbg(tunnel, "DPRX read done\n");
1099                         return 0;
1100                 }
1101                 usleep_range(100, 150);
1102         } while (ktime_before(ktime_get(), timeout));
1103
1104         tb_tunnel_dbg(tunnel, "DPRX read timeout\n");
1105         return -ETIMEDOUT;
1106 }
1107
1108 /* Read cap from tunnel DP IN */
1109 static int tb_dp_read_cap(struct tb_tunnel *tunnel, unsigned int cap, u32 *rate,
1110                           u32 *lanes)
1111 {
1112         struct tb_port *in = tunnel->src_port;
1113         u32 val;
1114         int ret;
1115
1116         switch (cap) {
1117         case DP_LOCAL_CAP:
1118         case DP_REMOTE_CAP:
1119         case DP_COMMON_CAP:
1120                 break;
1121
1122         default:
1123                 tb_tunnel_WARN(tunnel, "invalid capability index %#x\n", cap);
1124                 return -EINVAL;
1125         }
1126
1127         /*
1128          * Read from the copied remote cap so that we take into account
1129          * if capabilities were reduced during exchange.
1130          */
1131         ret = tb_port_read(in, &val, TB_CFG_PORT, in->cap_adap + cap, 1);
1132         if (ret)
1133                 return ret;
1134
1135         *rate = tb_dp_cap_get_rate(val);
1136         *lanes = tb_dp_cap_get_lanes(val);
1137         return 0;
1138 }
1139
1140 static int tb_dp_maximum_bandwidth(struct tb_tunnel *tunnel, int *max_up,
1141                                    int *max_down)
1142 {
1143         int ret;
1144
1145         if (!usb4_dp_port_bandwidth_mode_enabled(tunnel->src_port))
1146                 return -EOPNOTSUPP;
1147
1148         ret = tb_dp_bandwidth_mode_maximum_bandwidth(tunnel, NULL);
1149         if (ret < 0)
1150                 return ret;
1151
1152         if (tb_tunnel_direction_downstream(tunnel)) {
1153                 *max_up = 0;
1154                 *max_down = ret;
1155         } else {
1156                 *max_up = ret;
1157                 *max_down = 0;
1158         }
1159
1160         return 0;
1161 }
1162
1163 static int tb_dp_consumed_bandwidth(struct tb_tunnel *tunnel, int *consumed_up,
1164                                     int *consumed_down)
1165 {
1166         const struct tb_switch *sw = tunnel->src_port->sw;
1167         u32 rate = 0, lanes = 0;
1168         int ret;
1169
1170         if (tb_dp_is_usb4(sw)) {
1171                 /*
1172                  * On USB4 routers check if the bandwidth allocation
1173                  * mode is enabled first and then read the bandwidth
1174                  * through those registers.
1175                  */
1176                 ret = tb_dp_bandwidth_mode_consumed_bandwidth(tunnel, consumed_up,
1177                                                               consumed_down);
1178                 if (ret < 0) {
1179                         if (ret != -EOPNOTSUPP)
1180                                 return ret;
1181                 } else if (!ret) {
1182                         return 0;
1183                 }
1184                 /*
1185                  * Then see if the DPRX negotiation is ready and if yes
1186                  * return that bandwidth (it may be smaller than the
1187                  * reduced one). According to VESA spec, the DPRX
1188                  * negotiation shall compete in 5 seconds after tunnel
1189                  * established. We give it 100ms extra just in case.
1190                  */
1191                 ret = tb_dp_wait_dprx(tunnel, 5100);
1192                 if (ret)
1193                         return ret;
1194                 ret = tb_dp_read_cap(tunnel, DP_COMMON_CAP, &rate, &lanes);
1195                 if (ret)
1196                         return ret;
1197         } else if (sw->generation >= 2) {
1198                 ret = tb_dp_read_cap(tunnel, DP_REMOTE_CAP, &rate, &lanes);
1199                 if (ret)
1200                         return ret;
1201         } else {
1202                 /* No bandwidth management for legacy devices  */
1203                 *consumed_up = 0;
1204                 *consumed_down = 0;
1205                 return 0;
1206         }
1207
1208         if (tb_tunnel_direction_downstream(tunnel)) {
1209                 *consumed_up = 0;
1210                 *consumed_down = tb_dp_bandwidth(rate, lanes);
1211         } else {
1212                 *consumed_up = tb_dp_bandwidth(rate, lanes);
1213                 *consumed_down = 0;
1214         }
1215
1216         return 0;
1217 }
1218
1219 static void tb_dp_init_aux_credits(struct tb_path_hop *hop)
1220 {
1221         struct tb_port *port = hop->in_port;
1222         struct tb_switch *sw = port->sw;
1223
1224         if (tb_port_use_credit_allocation(port))
1225                 hop->initial_credits = sw->min_dp_aux_credits;
1226         else
1227                 hop->initial_credits = 1;
1228 }
1229
1230 static void tb_dp_init_aux_path(struct tb_path *path, bool pm_support)
1231 {
1232         struct tb_path_hop *hop;
1233
1234         path->egress_fc_enable = TB_PATH_SOURCE | TB_PATH_INTERNAL;
1235         path->egress_shared_buffer = TB_PATH_NONE;
1236         path->ingress_fc_enable = TB_PATH_ALL;
1237         path->ingress_shared_buffer = TB_PATH_NONE;
1238         path->priority = TB_DP_AUX_PRIORITY;
1239         path->weight = TB_DP_AUX_WEIGHT;
1240
1241         tb_path_for_each_hop(path, hop) {
1242                 tb_dp_init_aux_credits(hop);
1243                 if (pm_support)
1244                         tb_init_pm_support(hop);
1245         }
1246 }
1247
1248 static int tb_dp_init_video_credits(struct tb_path_hop *hop)
1249 {
1250         struct tb_port *port = hop->in_port;
1251         struct tb_switch *sw = port->sw;
1252
1253         if (tb_port_use_credit_allocation(port)) {
1254                 unsigned int nfc_credits;
1255                 size_t max_dp_streams;
1256
1257                 tb_available_credits(port, &max_dp_streams);
1258                 /*
1259                  * Read the number of currently allocated NFC credits
1260                  * from the lane adapter. Since we only use them for DP
1261                  * tunneling we can use that to figure out how many DP
1262                  * tunnels already go through the lane adapter.
1263                  */
1264                 nfc_credits = port->config.nfc_credits &
1265                                 ADP_CS_4_NFC_BUFFERS_MASK;
1266                 if (nfc_credits / sw->min_dp_main_credits > max_dp_streams)
1267                         return -ENOSPC;
1268
1269                 hop->nfc_credits = sw->min_dp_main_credits;
1270         } else {
1271                 hop->nfc_credits = min(port->total_credits - 2, 12U);
1272         }
1273
1274         return 0;
1275 }
1276
1277 static int tb_dp_init_video_path(struct tb_path *path, bool pm_support)
1278 {
1279         struct tb_path_hop *hop;
1280
1281         path->egress_fc_enable = TB_PATH_NONE;
1282         path->egress_shared_buffer = TB_PATH_NONE;
1283         path->ingress_fc_enable = TB_PATH_NONE;
1284         path->ingress_shared_buffer = TB_PATH_NONE;
1285         path->priority = TB_DP_VIDEO_PRIORITY;
1286         path->weight = TB_DP_VIDEO_WEIGHT;
1287
1288         tb_path_for_each_hop(path, hop) {
1289                 int ret;
1290
1291                 ret = tb_dp_init_video_credits(hop);
1292                 if (ret)
1293                         return ret;
1294                 if (pm_support)
1295                         tb_init_pm_support(hop);
1296         }
1297
1298         return 0;
1299 }
1300
1301 static void tb_dp_dump(struct tb_tunnel *tunnel)
1302 {
1303         struct tb_port *in, *out;
1304         u32 dp_cap, rate, lanes;
1305
1306         in = tunnel->src_port;
1307         out = tunnel->dst_port;
1308
1309         if (tb_port_read(in, &dp_cap, TB_CFG_PORT,
1310                          in->cap_adap + DP_LOCAL_CAP, 1))
1311                 return;
1312
1313         rate = tb_dp_cap_get_rate(dp_cap);
1314         lanes = tb_dp_cap_get_lanes(dp_cap);
1315
1316         tb_tunnel_dbg(tunnel,
1317                       "DP IN maximum supported bandwidth %u Mb/s x%u = %u Mb/s\n",
1318                       rate, lanes, tb_dp_bandwidth(rate, lanes));
1319
1320         if (tb_port_read(out, &dp_cap, TB_CFG_PORT,
1321                          out->cap_adap + DP_LOCAL_CAP, 1))
1322                 return;
1323
1324         rate = tb_dp_cap_get_rate(dp_cap);
1325         lanes = tb_dp_cap_get_lanes(dp_cap);
1326
1327         tb_tunnel_dbg(tunnel,
1328                       "DP OUT maximum supported bandwidth %u Mb/s x%u = %u Mb/s\n",
1329                       rate, lanes, tb_dp_bandwidth(rate, lanes));
1330
1331         if (tb_port_read(in, &dp_cap, TB_CFG_PORT,
1332                          in->cap_adap + DP_REMOTE_CAP, 1))
1333                 return;
1334
1335         rate = tb_dp_cap_get_rate(dp_cap);
1336         lanes = tb_dp_cap_get_lanes(dp_cap);
1337
1338         tb_tunnel_dbg(tunnel, "reduced bandwidth %u Mb/s x%u = %u Mb/s\n",
1339                       rate, lanes, tb_dp_bandwidth(rate, lanes));
1340 }
1341
1342 /**
1343  * tb_tunnel_discover_dp() - Discover existing Display Port tunnels
1344  * @tb: Pointer to the domain structure
1345  * @in: DP in adapter
1346  * @alloc_hopid: Allocate HopIDs from visited ports
1347  *
1348  * If @in adapter is active, follows the tunnel to the DP out adapter
1349  * and back. Returns the discovered tunnel or %NULL if there was no
1350  * tunnel.
1351  *
1352  * Return: DP tunnel or %NULL if no tunnel found.
1353  */
1354 struct tb_tunnel *tb_tunnel_discover_dp(struct tb *tb, struct tb_port *in,
1355                                         bool alloc_hopid)
1356 {
1357         struct tb_tunnel *tunnel;
1358         struct tb_port *port;
1359         struct tb_path *path;
1360
1361         if (!tb_dp_port_is_enabled(in))
1362                 return NULL;
1363
1364         tunnel = tb_tunnel_alloc(tb, 3, TB_TUNNEL_DP);
1365         if (!tunnel)
1366                 return NULL;
1367
1368         tunnel->init = tb_dp_init;
1369         tunnel->deinit = tb_dp_deinit;
1370         tunnel->activate = tb_dp_activate;
1371         tunnel->maximum_bandwidth = tb_dp_maximum_bandwidth;
1372         tunnel->allocated_bandwidth = tb_dp_allocated_bandwidth;
1373         tunnel->alloc_bandwidth = tb_dp_alloc_bandwidth;
1374         tunnel->consumed_bandwidth = tb_dp_consumed_bandwidth;
1375         tunnel->src_port = in;
1376
1377         path = tb_path_discover(in, TB_DP_VIDEO_HOPID, NULL, -1,
1378                                 &tunnel->dst_port, "Video", alloc_hopid);
1379         if (!path) {
1380                 /* Just disable the DP IN port */
1381                 tb_dp_port_enable(in, false);
1382                 goto err_free;
1383         }
1384         tunnel->paths[TB_DP_VIDEO_PATH_OUT] = path;
1385         if (tb_dp_init_video_path(tunnel->paths[TB_DP_VIDEO_PATH_OUT], false))
1386                 goto err_free;
1387
1388         path = tb_path_discover(in, TB_DP_AUX_TX_HOPID, NULL, -1, NULL, "AUX TX",
1389                                 alloc_hopid);
1390         if (!path)
1391                 goto err_deactivate;
1392         tunnel->paths[TB_DP_AUX_PATH_OUT] = path;
1393         tb_dp_init_aux_path(tunnel->paths[TB_DP_AUX_PATH_OUT], false);
1394
1395         path = tb_path_discover(tunnel->dst_port, -1, in, TB_DP_AUX_RX_HOPID,
1396                                 &port, "AUX RX", alloc_hopid);
1397         if (!path)
1398                 goto err_deactivate;
1399         tunnel->paths[TB_DP_AUX_PATH_IN] = path;
1400         tb_dp_init_aux_path(tunnel->paths[TB_DP_AUX_PATH_IN], false);
1401
1402         /* Validate that the tunnel is complete */
1403         if (!tb_port_is_dpout(tunnel->dst_port)) {
1404                 tb_port_warn(in, "path does not end on a DP adapter, cleaning up\n");
1405                 goto err_deactivate;
1406         }
1407
1408         if (!tb_dp_port_is_enabled(tunnel->dst_port))
1409                 goto err_deactivate;
1410
1411         if (!tb_dp_port_hpd_is_active(tunnel->dst_port))
1412                 goto err_deactivate;
1413
1414         if (port != tunnel->src_port) {
1415                 tb_tunnel_warn(tunnel, "path is not complete, cleaning up\n");
1416                 goto err_deactivate;
1417         }
1418
1419         tb_dp_dump(tunnel);
1420
1421         tb_tunnel_dbg(tunnel, "discovered\n");
1422         return tunnel;
1423
1424 err_deactivate:
1425         tb_tunnel_deactivate(tunnel);
1426 err_free:
1427         tb_tunnel_free(tunnel);
1428
1429         return NULL;
1430 }
1431
1432 /**
1433  * tb_tunnel_alloc_dp() - allocate a Display Port tunnel
1434  * @tb: Pointer to the domain structure
1435  * @in: DP in adapter port
1436  * @out: DP out adapter port
1437  * @link_nr: Preferred lane adapter when the link is not bonded
1438  * @max_up: Maximum available upstream bandwidth for the DP tunnel.
1439  *          %0 if no available bandwidth.
1440  * @max_down: Maximum available downstream bandwidth for the DP tunnel.
1441  *            %0 if no available bandwidth.
1442  *
1443  * Allocates a tunnel between @in and @out that is capable of tunneling
1444  * Display Port traffic.
1445  *
1446  * Return: Returns a tb_tunnel on success or NULL on failure.
1447  */
1448 struct tb_tunnel *tb_tunnel_alloc_dp(struct tb *tb, struct tb_port *in,
1449                                      struct tb_port *out, int link_nr,
1450                                      int max_up, int max_down)
1451 {
1452         struct tb_tunnel *tunnel;
1453         struct tb_path **paths;
1454         struct tb_path *path;
1455         bool pm_support;
1456
1457         if (WARN_ON(!in->cap_adap || !out->cap_adap))
1458                 return NULL;
1459
1460         tunnel = tb_tunnel_alloc(tb, 3, TB_TUNNEL_DP);
1461         if (!tunnel)
1462                 return NULL;
1463
1464         tunnel->init = tb_dp_init;
1465         tunnel->deinit = tb_dp_deinit;
1466         tunnel->activate = tb_dp_activate;
1467         tunnel->maximum_bandwidth = tb_dp_maximum_bandwidth;
1468         tunnel->allocated_bandwidth = tb_dp_allocated_bandwidth;
1469         tunnel->alloc_bandwidth = tb_dp_alloc_bandwidth;
1470         tunnel->consumed_bandwidth = tb_dp_consumed_bandwidth;
1471         tunnel->src_port = in;
1472         tunnel->dst_port = out;
1473         tunnel->max_up = max_up;
1474         tunnel->max_down = max_down;
1475
1476         paths = tunnel->paths;
1477         pm_support = usb4_switch_version(in->sw) >= 2;
1478
1479         path = tb_path_alloc(tb, in, TB_DP_VIDEO_HOPID, out, TB_DP_VIDEO_HOPID,
1480                              link_nr, "Video");
1481         if (!path)
1482                 goto err_free;
1483         tb_dp_init_video_path(path, pm_support);
1484         paths[TB_DP_VIDEO_PATH_OUT] = path;
1485
1486         path = tb_path_alloc(tb, in, TB_DP_AUX_TX_HOPID, out,
1487                              TB_DP_AUX_TX_HOPID, link_nr, "AUX TX");
1488         if (!path)
1489                 goto err_free;
1490         tb_dp_init_aux_path(path, pm_support);
1491         paths[TB_DP_AUX_PATH_OUT] = path;
1492
1493         path = tb_path_alloc(tb, out, TB_DP_AUX_RX_HOPID, in,
1494                              TB_DP_AUX_RX_HOPID, link_nr, "AUX RX");
1495         if (!path)
1496                 goto err_free;
1497         tb_dp_init_aux_path(path, pm_support);
1498         paths[TB_DP_AUX_PATH_IN] = path;
1499
1500         return tunnel;
1501
1502 err_free:
1503         tb_tunnel_free(tunnel);
1504         return NULL;
1505 }
1506
1507 static unsigned int tb_dma_available_credits(const struct tb_port *port)
1508 {
1509         const struct tb_switch *sw = port->sw;
1510         int credits;
1511
1512         credits = tb_available_credits(port, NULL);
1513         if (tb_acpi_may_tunnel_pcie())
1514                 credits -= sw->max_pcie_credits;
1515         credits -= port->dma_credits;
1516
1517         return credits > 0 ? credits : 0;
1518 }
1519
1520 static int tb_dma_reserve_credits(struct tb_path_hop *hop, unsigned int credits)
1521 {
1522         struct tb_port *port = hop->in_port;
1523
1524         if (tb_port_use_credit_allocation(port)) {
1525                 unsigned int available = tb_dma_available_credits(port);
1526
1527                 /*
1528                  * Need to have at least TB_MIN_DMA_CREDITS, otherwise
1529                  * DMA path cannot be established.
1530                  */
1531                 if (available < TB_MIN_DMA_CREDITS)
1532                         return -ENOSPC;
1533
1534                 while (credits > available)
1535                         credits--;
1536
1537                 tb_port_dbg(port, "reserving %u credits for DMA path\n",
1538                             credits);
1539
1540                 port->dma_credits += credits;
1541         } else {
1542                 if (tb_port_is_null(port))
1543                         credits = port->bonded ? 14 : 6;
1544                 else
1545                         credits = min(port->total_credits, credits);
1546         }
1547
1548         hop->initial_credits = credits;
1549         return 0;
1550 }
1551
1552 /* Path from lane adapter to NHI */
1553 static int tb_dma_init_rx_path(struct tb_path *path, unsigned int credits)
1554 {
1555         struct tb_path_hop *hop;
1556         unsigned int i, tmp;
1557
1558         path->egress_fc_enable = TB_PATH_SOURCE | TB_PATH_INTERNAL;
1559         path->ingress_fc_enable = TB_PATH_ALL;
1560         path->egress_shared_buffer = TB_PATH_NONE;
1561         path->ingress_shared_buffer = TB_PATH_NONE;
1562         path->priority = TB_DMA_PRIORITY;
1563         path->weight = TB_DMA_WEIGHT;
1564         path->clear_fc = true;
1565
1566         /*
1567          * First lane adapter is the one connected to the remote host.
1568          * We don't tunnel other traffic over this link so can use all
1569          * the credits (except the ones reserved for control traffic).
1570          */
1571         hop = &path->hops[0];
1572         tmp = min(tb_usable_credits(hop->in_port), credits);
1573         hop->initial_credits = tmp;
1574         hop->in_port->dma_credits += tmp;
1575
1576         for (i = 1; i < path->path_length; i++) {
1577                 int ret;
1578
1579                 ret = tb_dma_reserve_credits(&path->hops[i], credits);
1580                 if (ret)
1581                         return ret;
1582         }
1583
1584         return 0;
1585 }
1586
1587 /* Path from NHI to lane adapter */
1588 static int tb_dma_init_tx_path(struct tb_path *path, unsigned int credits)
1589 {
1590         struct tb_path_hop *hop;
1591
1592         path->egress_fc_enable = TB_PATH_ALL;
1593         path->ingress_fc_enable = TB_PATH_ALL;
1594         path->egress_shared_buffer = TB_PATH_NONE;
1595         path->ingress_shared_buffer = TB_PATH_NONE;
1596         path->priority = TB_DMA_PRIORITY;
1597         path->weight = TB_DMA_WEIGHT;
1598         path->clear_fc = true;
1599
1600         tb_path_for_each_hop(path, hop) {
1601                 int ret;
1602
1603                 ret = tb_dma_reserve_credits(hop, credits);
1604                 if (ret)
1605                         return ret;
1606         }
1607
1608         return 0;
1609 }
1610
1611 static void tb_dma_release_credits(struct tb_path_hop *hop)
1612 {
1613         struct tb_port *port = hop->in_port;
1614
1615         if (tb_port_use_credit_allocation(port)) {
1616                 port->dma_credits -= hop->initial_credits;
1617
1618                 tb_port_dbg(port, "released %u DMA path credits\n",
1619                             hop->initial_credits);
1620         }
1621 }
1622
1623 static void tb_dma_deinit_path(struct tb_path *path)
1624 {
1625         struct tb_path_hop *hop;
1626
1627         tb_path_for_each_hop(path, hop)
1628                 tb_dma_release_credits(hop);
1629 }
1630
1631 static void tb_dma_deinit(struct tb_tunnel *tunnel)
1632 {
1633         int i;
1634
1635         for (i = 0; i < tunnel->npaths; i++) {
1636                 if (!tunnel->paths[i])
1637                         continue;
1638                 tb_dma_deinit_path(tunnel->paths[i]);
1639         }
1640 }
1641
1642 /**
1643  * tb_tunnel_alloc_dma() - allocate a DMA tunnel
1644  * @tb: Pointer to the domain structure
1645  * @nhi: Host controller port
1646  * @dst: Destination null port which the other domain is connected to
1647  * @transmit_path: HopID used for transmitting packets
1648  * @transmit_ring: NHI ring number used to send packets towards the
1649  *                 other domain. Set to %-1 if TX path is not needed.
1650  * @receive_path: HopID used for receiving packets
1651  * @receive_ring: NHI ring number used to receive packets from the
1652  *                other domain. Set to %-1 if RX path is not needed.
1653  *
1654  * Return: Returns a tb_tunnel on success or NULL on failure.
1655  */
1656 struct tb_tunnel *tb_tunnel_alloc_dma(struct tb *tb, struct tb_port *nhi,
1657                                       struct tb_port *dst, int transmit_path,
1658                                       int transmit_ring, int receive_path,
1659                                       int receive_ring)
1660 {
1661         struct tb_tunnel *tunnel;
1662         size_t npaths = 0, i = 0;
1663         struct tb_path *path;
1664         int credits;
1665
1666         /* Ring 0 is reserved for control channel */
1667         if (WARN_ON(!receive_ring || !transmit_ring))
1668                 return NULL;
1669
1670         if (receive_ring > 0)
1671                 npaths++;
1672         if (transmit_ring > 0)
1673                 npaths++;
1674
1675         if (WARN_ON(!npaths))
1676                 return NULL;
1677
1678         tunnel = tb_tunnel_alloc(tb, npaths, TB_TUNNEL_DMA);
1679         if (!tunnel)
1680                 return NULL;
1681
1682         tunnel->src_port = nhi;
1683         tunnel->dst_port = dst;
1684         tunnel->deinit = tb_dma_deinit;
1685
1686         credits = min_not_zero(dma_credits, nhi->sw->max_dma_credits);
1687
1688         if (receive_ring > 0) {
1689                 path = tb_path_alloc(tb, dst, receive_path, nhi, receive_ring, 0,
1690                                      "DMA RX");
1691                 if (!path)
1692                         goto err_free;
1693                 tunnel->paths[i++] = path;
1694                 if (tb_dma_init_rx_path(path, credits)) {
1695                         tb_tunnel_dbg(tunnel, "not enough buffers for RX path\n");
1696                         goto err_free;
1697                 }
1698         }
1699
1700         if (transmit_ring > 0) {
1701                 path = tb_path_alloc(tb, nhi, transmit_ring, dst, transmit_path, 0,
1702                                      "DMA TX");
1703                 if (!path)
1704                         goto err_free;
1705                 tunnel->paths[i++] = path;
1706                 if (tb_dma_init_tx_path(path, credits)) {
1707                         tb_tunnel_dbg(tunnel, "not enough buffers for TX path\n");
1708                         goto err_free;
1709                 }
1710         }
1711
1712         return tunnel;
1713
1714 err_free:
1715         tb_tunnel_free(tunnel);
1716         return NULL;
1717 }
1718
1719 /**
1720  * tb_tunnel_match_dma() - Match DMA tunnel
1721  * @tunnel: Tunnel to match
1722  * @transmit_path: HopID used for transmitting packets. Pass %-1 to ignore.
1723  * @transmit_ring: NHI ring number used to send packets towards the
1724  *                 other domain. Pass %-1 to ignore.
1725  * @receive_path: HopID used for receiving packets. Pass %-1 to ignore.
1726  * @receive_ring: NHI ring number used to receive packets from the
1727  *                other domain. Pass %-1 to ignore.
1728  *
1729  * This function can be used to match specific DMA tunnel, if there are
1730  * multiple DMA tunnels going through the same XDomain connection.
1731  * Returns true if there is match and false otherwise.
1732  */
1733 bool tb_tunnel_match_dma(const struct tb_tunnel *tunnel, int transmit_path,
1734                          int transmit_ring, int receive_path, int receive_ring)
1735 {
1736         const struct tb_path *tx_path = NULL, *rx_path = NULL;
1737         int i;
1738
1739         if (!receive_ring || !transmit_ring)
1740                 return false;
1741
1742         for (i = 0; i < tunnel->npaths; i++) {
1743                 const struct tb_path *path = tunnel->paths[i];
1744
1745                 if (!path)
1746                         continue;
1747
1748                 if (tb_port_is_nhi(path->hops[0].in_port))
1749                         tx_path = path;
1750                 else if (tb_port_is_nhi(path->hops[path->path_length - 1].out_port))
1751                         rx_path = path;
1752         }
1753
1754         if (transmit_ring > 0 || transmit_path > 0) {
1755                 if (!tx_path)
1756                         return false;
1757                 if (transmit_ring > 0 &&
1758                     (tx_path->hops[0].in_hop_index != transmit_ring))
1759                         return false;
1760                 if (transmit_path > 0 &&
1761                     (tx_path->hops[tx_path->path_length - 1].next_hop_index != transmit_path))
1762                         return false;
1763         }
1764
1765         if (receive_ring > 0 || receive_path > 0) {
1766                 if (!rx_path)
1767                         return false;
1768                 if (receive_path > 0 &&
1769                     (rx_path->hops[0].in_hop_index != receive_path))
1770                         return false;
1771                 if (receive_ring > 0 &&
1772                     (rx_path->hops[rx_path->path_length - 1].next_hop_index != receive_ring))
1773                         return false;
1774         }
1775
1776         return true;
1777 }
1778
1779 static int tb_usb3_max_link_rate(struct tb_port *up, struct tb_port *down)
1780 {
1781         int ret, up_max_rate, down_max_rate;
1782
1783         ret = usb4_usb3_port_max_link_rate(up);
1784         if (ret < 0)
1785                 return ret;
1786         up_max_rate = ret;
1787
1788         ret = usb4_usb3_port_max_link_rate(down);
1789         if (ret < 0)
1790                 return ret;
1791         down_max_rate = ret;
1792
1793         return min(up_max_rate, down_max_rate);
1794 }
1795
1796 static int tb_usb3_init(struct tb_tunnel *tunnel)
1797 {
1798         tb_tunnel_dbg(tunnel, "allocating initial bandwidth %d/%d Mb/s\n",
1799                       tunnel->allocated_up, tunnel->allocated_down);
1800
1801         return usb4_usb3_port_allocate_bandwidth(tunnel->src_port,
1802                                                  &tunnel->allocated_up,
1803                                                  &tunnel->allocated_down);
1804 }
1805
1806 static int tb_usb3_activate(struct tb_tunnel *tunnel, bool activate)
1807 {
1808         int res;
1809
1810         res = tb_usb3_port_enable(tunnel->src_port, activate);
1811         if (res)
1812                 return res;
1813
1814         if (tb_port_is_usb3_up(tunnel->dst_port))
1815                 return tb_usb3_port_enable(tunnel->dst_port, activate);
1816
1817         return 0;
1818 }
1819
1820 static int tb_usb3_consumed_bandwidth(struct tb_tunnel *tunnel,
1821                 int *consumed_up, int *consumed_down)
1822 {
1823         struct tb_port *port = tb_upstream_port(tunnel->dst_port->sw);
1824         int pcie_weight = tb_acpi_may_tunnel_pcie() ? TB_PCI_WEIGHT : 0;
1825
1826         /*
1827          * PCIe tunneling, if enabled, affects the USB3 bandwidth so
1828          * take that it into account here.
1829          */
1830         *consumed_up = tunnel->allocated_up *
1831                 (TB_USB3_WEIGHT + pcie_weight) / TB_USB3_WEIGHT;
1832         *consumed_down = tunnel->allocated_down *
1833                 (TB_USB3_WEIGHT + pcie_weight) / TB_USB3_WEIGHT;
1834
1835         if (tb_port_get_link_generation(port) >= 4) {
1836                 *consumed_up = max(*consumed_up, USB4_V2_USB3_MIN_BANDWIDTH);
1837                 *consumed_down = max(*consumed_down, USB4_V2_USB3_MIN_BANDWIDTH);
1838         }
1839
1840         return 0;
1841 }
1842
1843 static int tb_usb3_release_unused_bandwidth(struct tb_tunnel *tunnel)
1844 {
1845         int ret;
1846
1847         ret = usb4_usb3_port_release_bandwidth(tunnel->src_port,
1848                                                &tunnel->allocated_up,
1849                                                &tunnel->allocated_down);
1850         if (ret)
1851                 return ret;
1852
1853         tb_tunnel_dbg(tunnel, "decreased bandwidth allocation to %d/%d Mb/s\n",
1854                       tunnel->allocated_up, tunnel->allocated_down);
1855         return 0;
1856 }
1857
1858 static void tb_usb3_reclaim_available_bandwidth(struct tb_tunnel *tunnel,
1859                                                 int *available_up,
1860                                                 int *available_down)
1861 {
1862         int ret, max_rate, allocate_up, allocate_down;
1863
1864         ret = tb_usb3_max_link_rate(tunnel->dst_port, tunnel->src_port);
1865         if (ret < 0) {
1866                 tb_tunnel_warn(tunnel, "failed to read maximum link rate\n");
1867                 return;
1868         }
1869
1870         /*
1871          * 90% of the max rate can be allocated for isochronous
1872          * transfers.
1873          */
1874         max_rate = ret * 90 / 100;
1875
1876         /* No need to reclaim if already at maximum */
1877         if (tunnel->allocated_up >= max_rate &&
1878             tunnel->allocated_down >= max_rate)
1879                 return;
1880
1881         /* Don't go lower than what is already allocated */
1882         allocate_up = min(max_rate, *available_up);
1883         if (allocate_up < tunnel->allocated_up)
1884                 allocate_up = tunnel->allocated_up;
1885
1886         allocate_down = min(max_rate, *available_down);
1887         if (allocate_down < tunnel->allocated_down)
1888                 allocate_down = tunnel->allocated_down;
1889
1890         /* If no changes no need to do more */
1891         if (allocate_up == tunnel->allocated_up &&
1892             allocate_down == tunnel->allocated_down)
1893                 return;
1894
1895         ret = usb4_usb3_port_allocate_bandwidth(tunnel->src_port, &allocate_up,
1896                                                 &allocate_down);
1897         if (ret) {
1898                 tb_tunnel_info(tunnel, "failed to allocate bandwidth\n");
1899                 return;
1900         }
1901
1902         tunnel->allocated_up = allocate_up;
1903         *available_up -= tunnel->allocated_up;
1904
1905         tunnel->allocated_down = allocate_down;
1906         *available_down -= tunnel->allocated_down;
1907
1908         tb_tunnel_dbg(tunnel, "increased bandwidth allocation to %d/%d Mb/s\n",
1909                       tunnel->allocated_up, tunnel->allocated_down);
1910 }
1911
1912 static void tb_usb3_init_credits(struct tb_path_hop *hop)
1913 {
1914         struct tb_port *port = hop->in_port;
1915         struct tb_switch *sw = port->sw;
1916         unsigned int credits;
1917
1918         if (tb_port_use_credit_allocation(port)) {
1919                 credits = sw->max_usb3_credits;
1920         } else {
1921                 if (tb_port_is_null(port))
1922                         credits = port->bonded ? 32 : 16;
1923                 else
1924                         credits = 7;
1925         }
1926
1927         hop->initial_credits = credits;
1928 }
1929
1930 static void tb_usb3_init_path(struct tb_path *path)
1931 {
1932         struct tb_path_hop *hop;
1933
1934         path->egress_fc_enable = TB_PATH_SOURCE | TB_PATH_INTERNAL;
1935         path->egress_shared_buffer = TB_PATH_NONE;
1936         path->ingress_fc_enable = TB_PATH_ALL;
1937         path->ingress_shared_buffer = TB_PATH_NONE;
1938         path->priority = TB_USB3_PRIORITY;
1939         path->weight = TB_USB3_WEIGHT;
1940         path->drop_packages = 0;
1941
1942         tb_path_for_each_hop(path, hop)
1943                 tb_usb3_init_credits(hop);
1944 }
1945
1946 /**
1947  * tb_tunnel_discover_usb3() - Discover existing USB3 tunnels
1948  * @tb: Pointer to the domain structure
1949  * @down: USB3 downstream adapter
1950  * @alloc_hopid: Allocate HopIDs from visited ports
1951  *
1952  * If @down adapter is active, follows the tunnel to the USB3 upstream
1953  * adapter and back. Returns the discovered tunnel or %NULL if there was
1954  * no tunnel.
1955  */
1956 struct tb_tunnel *tb_tunnel_discover_usb3(struct tb *tb, struct tb_port *down,
1957                                           bool alloc_hopid)
1958 {
1959         struct tb_tunnel *tunnel;
1960         struct tb_path *path;
1961
1962         if (!tb_usb3_port_is_enabled(down))
1963                 return NULL;
1964
1965         tunnel = tb_tunnel_alloc(tb, 2, TB_TUNNEL_USB3);
1966         if (!tunnel)
1967                 return NULL;
1968
1969         tunnel->activate = tb_usb3_activate;
1970         tunnel->src_port = down;
1971
1972         /*
1973          * Discover both paths even if they are not complete. We will
1974          * clean them up by calling tb_tunnel_deactivate() below in that
1975          * case.
1976          */
1977         path = tb_path_discover(down, TB_USB3_HOPID, NULL, -1,
1978                                 &tunnel->dst_port, "USB3 Down", alloc_hopid);
1979         if (!path) {
1980                 /* Just disable the downstream port */
1981                 tb_usb3_port_enable(down, false);
1982                 goto err_free;
1983         }
1984         tunnel->paths[TB_USB3_PATH_DOWN] = path;
1985         tb_usb3_init_path(tunnel->paths[TB_USB3_PATH_DOWN]);
1986
1987         path = tb_path_discover(tunnel->dst_port, -1, down, TB_USB3_HOPID, NULL,
1988                                 "USB3 Up", alloc_hopid);
1989         if (!path)
1990                 goto err_deactivate;
1991         tunnel->paths[TB_USB3_PATH_UP] = path;
1992         tb_usb3_init_path(tunnel->paths[TB_USB3_PATH_UP]);
1993
1994         /* Validate that the tunnel is complete */
1995         if (!tb_port_is_usb3_up(tunnel->dst_port)) {
1996                 tb_port_warn(tunnel->dst_port,
1997                              "path does not end on an USB3 adapter, cleaning up\n");
1998                 goto err_deactivate;
1999         }
2000
2001         if (down != tunnel->src_port) {
2002                 tb_tunnel_warn(tunnel, "path is not complete, cleaning up\n");
2003                 goto err_deactivate;
2004         }
2005
2006         if (!tb_usb3_port_is_enabled(tunnel->dst_port)) {
2007                 tb_tunnel_warn(tunnel,
2008                                "tunnel is not fully activated, cleaning up\n");
2009                 goto err_deactivate;
2010         }
2011
2012         if (!tb_route(down->sw)) {
2013                 int ret;
2014
2015                 /*
2016                  * Read the initial bandwidth allocation for the first
2017                  * hop tunnel.
2018                  */
2019                 ret = usb4_usb3_port_allocated_bandwidth(down,
2020                         &tunnel->allocated_up, &tunnel->allocated_down);
2021                 if (ret)
2022                         goto err_deactivate;
2023
2024                 tb_tunnel_dbg(tunnel, "currently allocated bandwidth %d/%d Mb/s\n",
2025                               tunnel->allocated_up, tunnel->allocated_down);
2026
2027                 tunnel->init = tb_usb3_init;
2028                 tunnel->consumed_bandwidth = tb_usb3_consumed_bandwidth;
2029                 tunnel->release_unused_bandwidth =
2030                         tb_usb3_release_unused_bandwidth;
2031                 tunnel->reclaim_available_bandwidth =
2032                         tb_usb3_reclaim_available_bandwidth;
2033         }
2034
2035         tb_tunnel_dbg(tunnel, "discovered\n");
2036         return tunnel;
2037
2038 err_deactivate:
2039         tb_tunnel_deactivate(tunnel);
2040 err_free:
2041         tb_tunnel_free(tunnel);
2042
2043         return NULL;
2044 }
2045
2046 /**
2047  * tb_tunnel_alloc_usb3() - allocate a USB3 tunnel
2048  * @tb: Pointer to the domain structure
2049  * @up: USB3 upstream adapter port
2050  * @down: USB3 downstream adapter port
2051  * @max_up: Maximum available upstream bandwidth for the USB3 tunnel.
2052  *          %0 if no available bandwidth.
2053  * @max_down: Maximum available downstream bandwidth for the USB3 tunnel.
2054  *            %0 if no available bandwidth.
2055  *
2056  * Allocate an USB3 tunnel. The ports must be of type @TB_TYPE_USB3_UP and
2057  * @TB_TYPE_USB3_DOWN.
2058  *
2059  * Return: Returns a tb_tunnel on success or %NULL on failure.
2060  */
2061 struct tb_tunnel *tb_tunnel_alloc_usb3(struct tb *tb, struct tb_port *up,
2062                                        struct tb_port *down, int max_up,
2063                                        int max_down)
2064 {
2065         struct tb_tunnel *tunnel;
2066         struct tb_path *path;
2067         int max_rate = 0;
2068
2069         if (!tb_route(down->sw) && (max_up > 0 || max_down > 0)) {
2070                 /*
2071                  * For USB3 isochronous transfers, we allow bandwidth which is
2072                  * not higher than 90% of maximum supported bandwidth by USB3
2073                  * adapters.
2074                  */
2075                 max_rate = tb_usb3_max_link_rate(down, up);
2076                 if (max_rate < 0)
2077                         return NULL;
2078
2079                 max_rate = max_rate * 90 / 100;
2080                 tb_port_dbg(up, "maximum required bandwidth for USB3 tunnel %d Mb/s\n",
2081                             max_rate);
2082         }
2083
2084         tunnel = tb_tunnel_alloc(tb, 2, TB_TUNNEL_USB3);
2085         if (!tunnel)
2086                 return NULL;
2087
2088         tunnel->activate = tb_usb3_activate;
2089         tunnel->src_port = down;
2090         tunnel->dst_port = up;
2091         tunnel->max_up = max_up;
2092         tunnel->max_down = max_down;
2093
2094         path = tb_path_alloc(tb, down, TB_USB3_HOPID, up, TB_USB3_HOPID, 0,
2095                              "USB3 Down");
2096         if (!path) {
2097                 tb_tunnel_free(tunnel);
2098                 return NULL;
2099         }
2100         tb_usb3_init_path(path);
2101         tunnel->paths[TB_USB3_PATH_DOWN] = path;
2102
2103         path = tb_path_alloc(tb, up, TB_USB3_HOPID, down, TB_USB3_HOPID, 0,
2104                              "USB3 Up");
2105         if (!path) {
2106                 tb_tunnel_free(tunnel);
2107                 return NULL;
2108         }
2109         tb_usb3_init_path(path);
2110         tunnel->paths[TB_USB3_PATH_UP] = path;
2111
2112         if (!tb_route(down->sw)) {
2113                 tunnel->allocated_up = min(max_rate, max_up);
2114                 tunnel->allocated_down = min(max_rate, max_down);
2115
2116                 tunnel->init = tb_usb3_init;
2117                 tunnel->consumed_bandwidth = tb_usb3_consumed_bandwidth;
2118                 tunnel->release_unused_bandwidth =
2119                         tb_usb3_release_unused_bandwidth;
2120                 tunnel->reclaim_available_bandwidth =
2121                         tb_usb3_reclaim_available_bandwidth;
2122         }
2123
2124         return tunnel;
2125 }
2126
2127 /**
2128  * tb_tunnel_free() - free a tunnel
2129  * @tunnel: Tunnel to be freed
2130  *
2131  * Frees a tunnel. The tunnel does not need to be deactivated.
2132  */
2133 void tb_tunnel_free(struct tb_tunnel *tunnel)
2134 {
2135         int i;
2136
2137         if (!tunnel)
2138                 return;
2139
2140         if (tunnel->deinit)
2141                 tunnel->deinit(tunnel);
2142
2143         for (i = 0; i < tunnel->npaths; i++) {
2144                 if (tunnel->paths[i])
2145                         tb_path_free(tunnel->paths[i]);
2146         }
2147
2148         kfree(tunnel->paths);
2149         kfree(tunnel);
2150 }
2151
2152 /**
2153  * tb_tunnel_is_invalid - check whether an activated path is still valid
2154  * @tunnel: Tunnel to check
2155  */
2156 bool tb_tunnel_is_invalid(struct tb_tunnel *tunnel)
2157 {
2158         int i;
2159
2160         for (i = 0; i < tunnel->npaths; i++) {
2161                 WARN_ON(!tunnel->paths[i]->activated);
2162                 if (tb_path_is_invalid(tunnel->paths[i]))
2163                         return true;
2164         }
2165
2166         return false;
2167 }
2168
2169 /**
2170  * tb_tunnel_restart() - activate a tunnel after a hardware reset
2171  * @tunnel: Tunnel to restart
2172  *
2173  * Return: 0 on success and negative errno in case if failure
2174  */
2175 int tb_tunnel_restart(struct tb_tunnel *tunnel)
2176 {
2177         int res, i;
2178
2179         tb_tunnel_dbg(tunnel, "activating\n");
2180
2181         /*
2182          * Make sure all paths are properly disabled before enabling
2183          * them again.
2184          */
2185         for (i = 0; i < tunnel->npaths; i++) {
2186                 if (tunnel->paths[i]->activated) {
2187                         tb_path_deactivate(tunnel->paths[i]);
2188                         tunnel->paths[i]->activated = false;
2189                 }
2190         }
2191
2192         if (tunnel->init) {
2193                 res = tunnel->init(tunnel);
2194                 if (res)
2195                         return res;
2196         }
2197
2198         for (i = 0; i < tunnel->npaths; i++) {
2199                 res = tb_path_activate(tunnel->paths[i]);
2200                 if (res)
2201                         goto err;
2202         }
2203
2204         if (tunnel->activate) {
2205                 res = tunnel->activate(tunnel, true);
2206                 if (res)
2207                         goto err;
2208         }
2209
2210         return 0;
2211
2212 err:
2213         tb_tunnel_warn(tunnel, "activation failed\n");
2214         tb_tunnel_deactivate(tunnel);
2215         return res;
2216 }
2217
2218 /**
2219  * tb_tunnel_activate() - activate a tunnel
2220  * @tunnel: Tunnel to activate
2221  *
2222  * Return: Returns 0 on success or an error code on failure.
2223  */
2224 int tb_tunnel_activate(struct tb_tunnel *tunnel)
2225 {
2226         int i;
2227
2228         for (i = 0; i < tunnel->npaths; i++) {
2229                 if (tunnel->paths[i]->activated) {
2230                         tb_tunnel_WARN(tunnel,
2231                                        "trying to activate an already activated tunnel\n");
2232                         return -EINVAL;
2233                 }
2234         }
2235
2236         return tb_tunnel_restart(tunnel);
2237 }
2238
2239 /**
2240  * tb_tunnel_deactivate() - deactivate a tunnel
2241  * @tunnel: Tunnel to deactivate
2242  */
2243 void tb_tunnel_deactivate(struct tb_tunnel *tunnel)
2244 {
2245         int i;
2246
2247         tb_tunnel_dbg(tunnel, "deactivating\n");
2248
2249         if (tunnel->activate)
2250                 tunnel->activate(tunnel, false);
2251
2252         for (i = 0; i < tunnel->npaths; i++) {
2253                 if (tunnel->paths[i] && tunnel->paths[i]->activated)
2254                         tb_path_deactivate(tunnel->paths[i]);
2255         }
2256 }
2257
2258 /**
2259  * tb_tunnel_port_on_path() - Does the tunnel go through port
2260  * @tunnel: Tunnel to check
2261  * @port: Port to check
2262  *
2263  * Returns true if @tunnel goes through @port (direction does not matter),
2264  * false otherwise.
2265  */
2266 bool tb_tunnel_port_on_path(const struct tb_tunnel *tunnel,
2267                             const struct tb_port *port)
2268 {
2269         int i;
2270
2271         for (i = 0; i < tunnel->npaths; i++) {
2272                 if (!tunnel->paths[i])
2273                         continue;
2274
2275                 if (tb_path_port_on_path(tunnel->paths[i], port))
2276                         return true;
2277         }
2278
2279         return false;
2280 }
2281
2282 static bool tb_tunnel_is_active(const struct tb_tunnel *tunnel)
2283 {
2284         int i;
2285
2286         for (i = 0; i < tunnel->npaths; i++) {
2287                 if (!tunnel->paths[i])
2288                         return false;
2289                 if (!tunnel->paths[i]->activated)
2290                         return false;
2291         }
2292
2293         return true;
2294 }
2295
2296 /**
2297  * tb_tunnel_maximum_bandwidth() - Return maximum possible bandwidth
2298  * @tunnel: Tunnel to check
2299  * @max_up: Maximum upstream bandwidth in Mb/s
2300  * @max_down: Maximum downstream bandwidth in Mb/s
2301  *
2302  * Returns maximum possible bandwidth this tunnel can go if not limited
2303  * by other bandwidth clients. If the tunnel does not support this
2304  * returns %-EOPNOTSUPP.
2305  */
2306 int tb_tunnel_maximum_bandwidth(struct tb_tunnel *tunnel, int *max_up,
2307                                 int *max_down)
2308 {
2309         if (!tb_tunnel_is_active(tunnel))
2310                 return -EINVAL;
2311
2312         if (tunnel->maximum_bandwidth)
2313                 return tunnel->maximum_bandwidth(tunnel, max_up, max_down);
2314         return -EOPNOTSUPP;
2315 }
2316
2317 /**
2318  * tb_tunnel_allocated_bandwidth() - Return bandwidth allocated for the tunnel
2319  * @tunnel: Tunnel to check
2320  * @allocated_up: Currently allocated upstream bandwidth in Mb/s is stored here
2321  * @allocated_down: Currently allocated downstream bandwidth in Mb/s is
2322  *                  stored here
2323  *
2324  * Returns the bandwidth allocated for the tunnel. This may be higher
2325  * than what the tunnel actually consumes.
2326  */
2327 int tb_tunnel_allocated_bandwidth(struct tb_tunnel *tunnel, int *allocated_up,
2328                                   int *allocated_down)
2329 {
2330         if (!tb_tunnel_is_active(tunnel))
2331                 return -EINVAL;
2332
2333         if (tunnel->allocated_bandwidth)
2334                 return tunnel->allocated_bandwidth(tunnel, allocated_up,
2335                                                    allocated_down);
2336         return -EOPNOTSUPP;
2337 }
2338
2339 /**
2340  * tb_tunnel_alloc_bandwidth() - Change tunnel bandwidth allocation
2341  * @tunnel: Tunnel whose bandwidth allocation to change
2342  * @alloc_up: New upstream bandwidth in Mb/s
2343  * @alloc_down: New downstream bandwidth in Mb/s
2344  *
2345  * Tries to change tunnel bandwidth allocation. If succeeds returns %0
2346  * and updates @alloc_up and @alloc_down to that was actually allocated
2347  * (it may not be the same as passed originally). Returns negative errno
2348  * in case of failure.
2349  */
2350 int tb_tunnel_alloc_bandwidth(struct tb_tunnel *tunnel, int *alloc_up,
2351                               int *alloc_down)
2352 {
2353         if (!tb_tunnel_is_active(tunnel))
2354                 return -EINVAL;
2355
2356         if (tunnel->alloc_bandwidth)
2357                 return tunnel->alloc_bandwidth(tunnel, alloc_up, alloc_down);
2358
2359         return -EOPNOTSUPP;
2360 }
2361
2362 /**
2363  * tb_tunnel_consumed_bandwidth() - Return bandwidth consumed by the tunnel
2364  * @tunnel: Tunnel to check
2365  * @consumed_up: Consumed bandwidth in Mb/s from @dst_port to @src_port.
2366  *               Can be %NULL.
2367  * @consumed_down: Consumed bandwidth in Mb/s from @src_port to @dst_port.
2368  *                 Can be %NULL.
2369  *
2370  * Stores the amount of isochronous bandwidth @tunnel consumes in
2371  * @consumed_up and @consumed_down. In case of success returns %0,
2372  * negative errno otherwise.
2373  */
2374 int tb_tunnel_consumed_bandwidth(struct tb_tunnel *tunnel, int *consumed_up,
2375                                  int *consumed_down)
2376 {
2377         int up_bw = 0, down_bw = 0;
2378
2379         if (!tb_tunnel_is_active(tunnel))
2380                 goto out;
2381
2382         if (tunnel->consumed_bandwidth) {
2383                 int ret;
2384
2385                 ret = tunnel->consumed_bandwidth(tunnel, &up_bw, &down_bw);
2386                 if (ret)
2387                         return ret;
2388
2389                 tb_tunnel_dbg(tunnel, "consumed bandwidth %d/%d Mb/s\n", up_bw,
2390                               down_bw);
2391         }
2392
2393 out:
2394         if (consumed_up)
2395                 *consumed_up = up_bw;
2396         if (consumed_down)
2397                 *consumed_down = down_bw;
2398
2399         return 0;
2400 }
2401
2402 /**
2403  * tb_tunnel_release_unused_bandwidth() - Release unused bandwidth
2404  * @tunnel: Tunnel whose unused bandwidth to release
2405  *
2406  * If tunnel supports dynamic bandwidth management (USB3 tunnels at the
2407  * moment) this function makes it to release all the unused bandwidth.
2408  *
2409  * Returns %0 in case of success and negative errno otherwise.
2410  */
2411 int tb_tunnel_release_unused_bandwidth(struct tb_tunnel *tunnel)
2412 {
2413         if (!tb_tunnel_is_active(tunnel))
2414                 return 0;
2415
2416         if (tunnel->release_unused_bandwidth) {
2417                 int ret;
2418
2419                 ret = tunnel->release_unused_bandwidth(tunnel);
2420                 if (ret)
2421                         return ret;
2422         }
2423
2424         return 0;
2425 }
2426
2427 /**
2428  * tb_tunnel_reclaim_available_bandwidth() - Reclaim available bandwidth
2429  * @tunnel: Tunnel reclaiming available bandwidth
2430  * @available_up: Available upstream bandwidth (in Mb/s)
2431  * @available_down: Available downstream bandwidth (in Mb/s)
2432  *
2433  * Reclaims bandwidth from @available_up and @available_down and updates
2434  * the variables accordingly (e.g decreases both according to what was
2435  * reclaimed by the tunnel). If nothing was reclaimed the values are
2436  * kept as is.
2437  */
2438 void tb_tunnel_reclaim_available_bandwidth(struct tb_tunnel *tunnel,
2439                                            int *available_up,
2440                                            int *available_down)
2441 {
2442         if (!tb_tunnel_is_active(tunnel))
2443                 return;
2444
2445         if (tunnel->reclaim_available_bandwidth)
2446                 tunnel->reclaim_available_bandwidth(tunnel, available_up,
2447                                                     available_down);
2448 }
2449
2450 const char *tb_tunnel_type_name(const struct tb_tunnel *tunnel)
2451 {
2452         return tb_tunnel_names[tunnel->type];
2453 }