1 files changed, 202 insertions, 0 deletions

diff --git a/drivers/pci/access.c b/drivers/pci/access.c
index ba91a7e17519..3af0478c057b 100644
--- a/drivers/pci/access.c
+++ b/drivers/pci/access.c
@@ -469,3 +469,205 @@ void pci_cfg_access_unlock(struct pci_dev *dev)
         raw_spin_unlock_irqrestore(&pci_lock, flags);
 }
 EXPORT_SYMBOL_GPL(pci_cfg_access_unlock);
+
+static inline int pcie_cap_version(const struct pci_dev *dev)
+{
+        return dev->pcie_flags_reg & PCI_EXP_FLAGS_VERS;
+}
+
+static inline bool pcie_cap_has_devctl(const struct pci_dev *dev)
+{
+        return true;
+}
+
+static inline bool pcie_cap_has_lnkctl(const struct pci_dev *dev)
+{
+        int type = pci_pcie_type(dev);
+
+        return pcie_cap_version(dev) > 1 ||
+               type == PCI_EXP_TYPE_ROOT_PORT ||
+               type == PCI_EXP_TYPE_ENDPOINT ||
+               type == PCI_EXP_TYPE_LEG_END;
+}
+
+static inline bool pcie_cap_has_sltctl(const struct pci_dev *dev)
+{
+        int type = pci_pcie_type(dev);
+
+        return pcie_cap_version(dev) > 1 ||
+               type == PCI_EXP_TYPE_ROOT_PORT ||
+               (type == PCI_EXP_TYPE_DOWNSTREAM &&
+                dev->pcie_flags_reg & PCI_EXP_FLAGS_SLOT);
+}
+
+static inline bool pcie_cap_has_rtctl(const struct pci_dev *dev)
+{
+        int type = pci_pcie_type(dev);
+
+        return pcie_cap_version(dev) > 1 ||
+               type == PCI_EXP_TYPE_ROOT_PORT ||
+               type == PCI_EXP_TYPE_RC_EC;
+}
+
+static bool pcie_capability_reg_implemented(struct pci_dev *dev, int pos)
+{
+        if (!pci_is_pcie(dev))
+                return false;
+
+        switch (pos) {
+        case PCI_EXP_FLAGS_TYPE:
+                return true;
+        case PCI_EXP_DEVCAP:
+        case PCI_EXP_DEVCTL:
+        case PCI_EXP_DEVSTA:
+                return pcie_cap_has_devctl(dev);
+        case PCI_EXP_LNKCAP:
+        case PCI_EXP_LNKCTL:
+        case PCI_EXP_LNKSTA:
+                return pcie_cap_has_lnkctl(dev);
+        case PCI_EXP_SLTCAP:
+        case PCI_EXP_SLTCTL:
+        case PCI_EXP_SLTSTA:
+                return pcie_cap_has_sltctl(dev);
+        case PCI_EXP_RTCTL:
+        case PCI_EXP_RTCAP:
+        case PCI_EXP_RTSTA:
+                return pcie_cap_has_rtctl(dev);
+        case PCI_EXP_DEVCAP2:
+        case PCI_EXP_DEVCTL2:
+        case PCI_EXP_LNKCAP2:
+        case PCI_EXP_LNKCTL2:
+        case PCI_EXP_LNKSTA2:
+                return pcie_cap_version(dev) > 1;
+        default:
+                return false;
+        }
+}
+
+/*
+ * Note that these accessor functions are only for the "PCI Express
+ * Capability" (see PCIe spec r3.0, sec 7.8).  They do not apply to the
+ * other "PCI Express Extended Capabilities" (AER, VC, ACS, MFVC, etc.)
+ */
+int pcie_capability_read_word(struct pci_dev *dev, int pos, u16 *val)
+{
+        int ret;
+
+        *val = 0;
+        if (pos & 1)
+                return -EINVAL;
+
+        if (pcie_capability_reg_implemented(dev, pos)) {
+                ret = pci_read_config_word(dev, pci_pcie_cap(dev) + pos, val);
+                /*
+                 * Reset *val to 0 if pci_read_config_word() fails, it may
+                 * have been written as 0xFFFF if hardware error happens
+                 * during pci_read_config_word().
+                 */
+                if (ret)
+                        *val = 0;
+                return ret;
+        }
+
+        /*
+         * For Functions that do not implement the Slot Capabilities,
+         * Slot Status, and Slot Control registers, these spaces must
+         * be hardwired to 0b, with the exception of the Presence Detect
+         * State bit in the Slot Status register of Downstream Ports,
+         * which must be hardwired to 1b.  (PCIe Base Spec 3.0, sec 7.8)
+         */
+        if (pci_is_pcie(dev) && pos == PCI_EXP_SLTSTA &&
+                 pci_pcie_type(dev) == PCI_EXP_TYPE_DOWNSTREAM) {
+                *val = PCI_EXP_SLTSTA_PDS;
+        }
+
+        return 0;
+}
+EXPORT_SYMBOL(pcie_capability_read_word);
+
+int pcie_capability_read_dword(struct pci_dev *dev, int pos, u32 *val)
+{
+        int ret;
+
+        *val = 0;
+        if (pos & 3)
+                return -EINVAL;
+
+        if (pcie_capability_reg_implemented(dev, pos)) {
+                ret = pci_read_config_dword(dev, pci_pcie_cap(dev) + pos, val);
+                /*
+                 * Reset *val to 0 if pci_read_config_dword() fails, it may
+                 * have been written as 0xFFFFFFFF if hardware error happens
+                 * during pci_read_config_dword().
+                 */
+                if (ret)
+                        *val = 0;
+                return ret;
+        }
+
+        if (pci_is_pcie(dev) && pos == PCI_EXP_SLTCTL &&
+                 pci_pcie_type(dev) == PCI_EXP_TYPE_DOWNSTREAM) {
+                *val = PCI_EXP_SLTSTA_PDS;
+        }
+
+        return 0;
+}
+EXPORT_SYMBOL(pcie_capability_read_dword);
+
+int pcie_capability_write_word(struct pci_dev *dev, int pos, u16 val)
+{
+        if (pos & 1)
+                return -EINVAL;
+
+        if (!pcie_capability_reg_implemented(dev, pos))
+                return 0;
+
+        return pci_write_config_word(dev, pci_pcie_cap(dev) + pos, val);
+}
+EXPORT_SYMBOL(pcie_capability_write_word);
+
+int pcie_capability_write_dword(struct pci_dev *dev, int pos, u32 val)
+{
+        if (pos & 3)
+                return -EINVAL;
+
+        if (!pcie_capability_reg_implemented(dev, pos))
+                return 0;
+
+        return pci_write_config_dword(dev, pci_pcie_cap(dev) + pos, val);
+}
+EXPORT_SYMBOL(pcie_capability_write_dword);
+
+int pcie_capability_clear_and_set_word(struct pci_dev *dev, int pos,
+                                       u16 clear, u16 set)
+{
+        int ret;
+        u16 val;
+
+        ret = pcie_capability_read_word(dev, pos, &val);
+        if (!ret) {
+                val &= ~clear;
+                val |= set;
+                ret = pcie_capability_write_word(dev, pos, val);
+        }
+
+        return ret;
+}
+EXPORT_SYMBOL(pcie_capability_clear_and_set_word);
+
+int pcie_capability_clear_and_set_dword(struct pci_dev *dev, int pos,
+                                        u32 clear, u32 set)
+{
+        int ret;
+        u32 val;
+
+        ret = pcie_capability_read_dword(dev, pos, &val);
+        if (!ret) {
+                val &= ~clear;
+                val |= set;
+                ret = pcie_capability_write_dword(dev, pos, val);
+        }
+
+        return ret;
+}
+EXPORT_SYMBOL(pcie_capability_clear_and_set_dword);