|
| 1 | +// SPDX-License-Identifier: GPL-2.0+ |
| 2 | +/* |
| 3 | + * PCIe bandwidth controller |
| 4 | + * |
| 5 | + * Author: Alexandru Gagniuc <mr.nuke.me@gmail.com> |
| 6 | + * |
| 7 | + * Copyright (C) 2019 Dell Inc |
| 8 | + * Copyright (C) 2023-2024 Intel Corporation |
| 9 | + * |
| 10 | + * This service port driver hooks into the Bandwidth Notification interrupt |
| 11 | + * watching for changes or links becoming degraded in operation. It updates |
| 12 | + * the cached Current Link Speed that is exposed to user space through sysfs. |
| 13 | + */ |
| 14 | + |
| 15 | +#define dev_fmt(fmt) "bwctrl: " fmt |
| 16 | + |
| 17 | +#include <linux/atomic.h> |
| 18 | +#include <linux/cleanup.h> |
| 19 | +#include <linux/errno.h> |
| 20 | +#include <linux/interrupt.h> |
| 21 | +#include <linux/pci.h> |
| 22 | +#include <linux/rwsem.h> |
| 23 | +#include <linux/slab.h> |
| 24 | +#include <linux/types.h> |
| 25 | + |
| 26 | +#include "../pci.h" |
| 27 | +#include "portdrv.h" |
| 28 | + |
| 29 | +/** |
| 30 | + * struct pcie_bwctrl_data - PCIe bandwidth controller |
| 31 | + * @lbms_count: Count for LBMS (since last reset) |
| 32 | + */ |
| 33 | +struct pcie_bwctrl_data { |
| 34 | + atomic_t lbms_count; |
| 35 | +}; |
| 36 | + |
| 37 | +/* Prevents port removal during LBMS count accessors */ |
| 38 | +static DECLARE_RWSEM(pcie_bwctrl_lbms_rwsem); |
| 39 | + |
| 40 | +static void pcie_bwnotif_enable(struct pcie_device *srv) |
| 41 | +{ |
| 42 | + struct pcie_bwctrl_data *data = srv->port->link_bwctrl; |
| 43 | + struct pci_dev *port = srv->port; |
| 44 | + u16 link_status; |
| 45 | + int ret; |
| 46 | + |
| 47 | + /* Count LBMS seen so far as one */ |
| 48 | + ret = pcie_capability_read_word(port, PCI_EXP_LNKSTA, &link_status); |
| 49 | + if (ret == PCIBIOS_SUCCESSFUL && link_status & PCI_EXP_LNKSTA_LBMS) |
| 50 | + atomic_inc(&data->lbms_count); |
| 51 | + |
| 52 | + pcie_capability_set_word(port, PCI_EXP_LNKCTL, |
| 53 | + PCI_EXP_LNKCTL_LBMIE | PCI_EXP_LNKCTL_LABIE); |
| 54 | + pcie_capability_write_word(port, PCI_EXP_LNKSTA, |
| 55 | + PCI_EXP_LNKSTA_LBMS | PCI_EXP_LNKSTA_LABS); |
| 56 | + |
| 57 | + /* |
| 58 | + * Update after enabling notifications & clearing status bits ensures |
| 59 | + * link speed is up to date. |
| 60 | + */ |
| 61 | + pcie_update_link_speed(port->subordinate); |
| 62 | +} |
| 63 | + |
| 64 | +static void pcie_bwnotif_disable(struct pci_dev *port) |
| 65 | +{ |
| 66 | + pcie_capability_clear_word(port, PCI_EXP_LNKCTL, |
| 67 | + PCI_EXP_LNKCTL_LBMIE | PCI_EXP_LNKCTL_LABIE); |
| 68 | +} |
| 69 | + |
| 70 | +static irqreturn_t pcie_bwnotif_irq(int irq, void *context) |
| 71 | +{ |
| 72 | + struct pcie_device *srv = context; |
| 73 | + struct pcie_bwctrl_data *data = srv->port->link_bwctrl; |
| 74 | + struct pci_dev *port = srv->port; |
| 75 | + u16 link_status, events; |
| 76 | + int ret; |
| 77 | + |
| 78 | + ret = pcie_capability_read_word(port, PCI_EXP_LNKSTA, &link_status); |
| 79 | + if (ret != PCIBIOS_SUCCESSFUL) |
| 80 | + return IRQ_NONE; |
| 81 | + |
| 82 | + events = link_status & (PCI_EXP_LNKSTA_LBMS | PCI_EXP_LNKSTA_LABS); |
| 83 | + if (!events) |
| 84 | + return IRQ_NONE; |
| 85 | + |
| 86 | + if (events & PCI_EXP_LNKSTA_LBMS) |
| 87 | + atomic_inc(&data->lbms_count); |
| 88 | + |
| 89 | + pcie_capability_write_word(port, PCI_EXP_LNKSTA, events); |
| 90 | + |
| 91 | + /* |
| 92 | + * Interrupts will not be triggered from any further Link Speed |
| 93 | + * change until LBMS is cleared by the write. Therefore, re-read the |
| 94 | + * speed (inside pcie_update_link_speed()) after LBMS has been |
| 95 | + * cleared to avoid missing link speed changes. |
| 96 | + */ |
| 97 | + pcie_update_link_speed(port->subordinate); |
| 98 | + |
| 99 | + return IRQ_HANDLED; |
| 100 | +} |
| 101 | + |
| 102 | +void pcie_reset_lbms_count(struct pci_dev *port) |
| 103 | +{ |
| 104 | + struct pcie_bwctrl_data *data; |
| 105 | + |
| 106 | + guard(rwsem_read)(&pcie_bwctrl_lbms_rwsem); |
| 107 | + data = port->link_bwctrl; |
| 108 | + if (data) |
| 109 | + atomic_set(&data->lbms_count, 0); |
| 110 | + else |
| 111 | + pcie_capability_write_word(port, PCI_EXP_LNKSTA, |
| 112 | + PCI_EXP_LNKSTA_LBMS); |
| 113 | +} |
| 114 | + |
| 115 | +int pcie_lbms_count(struct pci_dev *port, unsigned long *val) |
| 116 | +{ |
| 117 | + struct pcie_bwctrl_data *data; |
| 118 | + |
| 119 | + guard(rwsem_read)(&pcie_bwctrl_lbms_rwsem); |
| 120 | + data = port->link_bwctrl; |
| 121 | + if (!data) |
| 122 | + return -ENOTTY; |
| 123 | + |
| 124 | + *val = atomic_read(&data->lbms_count); |
| 125 | + |
| 126 | + return 0; |
| 127 | +} |
| 128 | + |
| 129 | +static int pcie_bwnotif_probe(struct pcie_device *srv) |
| 130 | +{ |
| 131 | + struct pci_dev *port = srv->port; |
| 132 | + int ret; |
| 133 | + |
| 134 | + struct pcie_bwctrl_data *data = devm_kzalloc(&srv->device, |
| 135 | + sizeof(*data), GFP_KERNEL); |
| 136 | + if (!data) |
| 137 | + return -ENOMEM; |
| 138 | + |
| 139 | + ret = devm_request_irq(&srv->device, srv->irq, pcie_bwnotif_irq, |
| 140 | + IRQF_SHARED, "PCIe bwctrl", srv); |
| 141 | + if (ret) |
| 142 | + return ret; |
| 143 | + |
| 144 | + scoped_guard(rwsem_write, &pcie_bwctrl_lbms_rwsem) { |
| 145 | + port->link_bwctrl = no_free_ptr(data); |
| 146 | + pcie_bwnotif_enable(srv); |
| 147 | + } |
| 148 | + |
| 149 | + pci_dbg(port, "enabled with IRQ %d\n", srv->irq); |
| 150 | + |
| 151 | + return 0; |
| 152 | +} |
| 153 | + |
| 154 | +static void pcie_bwnotif_remove(struct pcie_device *srv) |
| 155 | +{ |
| 156 | + pcie_bwnotif_disable(srv->port); |
| 157 | + scoped_guard(rwsem_write, &pcie_bwctrl_lbms_rwsem) |
| 158 | + srv->port->link_bwctrl = NULL; |
| 159 | +} |
| 160 | + |
| 161 | +static int pcie_bwnotif_suspend(struct pcie_device *srv) |
| 162 | +{ |
| 163 | + pcie_bwnotif_disable(srv->port); |
| 164 | + return 0; |
| 165 | +} |
| 166 | + |
| 167 | +static int pcie_bwnotif_resume(struct pcie_device *srv) |
| 168 | +{ |
| 169 | + pcie_bwnotif_enable(srv); |
| 170 | + return 0; |
| 171 | +} |
| 172 | + |
| 173 | +static struct pcie_port_service_driver pcie_bwctrl_driver = { |
| 174 | + .name = "pcie_bwctrl", |
| 175 | + .port_type = PCIE_ANY_PORT, |
| 176 | + .service = PCIE_PORT_SERVICE_BWCTRL, |
| 177 | + .probe = pcie_bwnotif_probe, |
| 178 | + .suspend = pcie_bwnotif_suspend, |
| 179 | + .resume = pcie_bwnotif_resume, |
| 180 | + .remove = pcie_bwnotif_remove, |
| 181 | +}; |
| 182 | + |
| 183 | +int __init pcie_bwctrl_init(void) |
| 184 | +{ |
| 185 | + return pcie_port_service_register(&pcie_bwctrl_driver); |
| 186 | +} |
0 commit comments