[PATCH 04/15] PCI: Add pci_find_vsec_capability() to find a specific VSEC

[Gustavo Pimentel]

Add pci_find_vsec_capability() that crawls through the device config
space searching in all Vendor-Specific Extended Capabilities for a
particular capability ID.

Vendor-Specific Extended Capability (VSEC) is a PCIe capability (acts
like a wrapper) specified by PCI-SIG that allows the vendor to create
their own and specific capability in the device config space.

Signed-off-by: Gustavo Pimentel <gustavo.pimentel@xxxxxxxxxxxx>
---
 drivers/pci/pci.c             | 29 +++++++++++++++++++++++++++++
 include/linux/pci.h           |  1 +
 include/uapi/linux/pci_regs.h |  5 +++++
 3 files changed, 35 insertions(+)

diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 6d4d5a2..235d0b2 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -623,6 +623,35 @@ u64 pci_get_dsn(struct pci_dev *dev)
 }
 EXPORT_SYMBOL_GPL(pci_get_dsn);
 
+/**
+ * pci_find_vsec_capability - Find a vendor-specific extended capability
+ * @dev: PCI device to query
+ * @cap: vendor-specific capability id code
+ *
+ * Returns the address of the vendor-specific structure that matches the
+ * requested capability id code within the device's PCI configuration space
+ * or 0 if it does not find a match.
+ */
+int pci_find_vsec_capability(struct pci_dev *dev, int vsec_cap_id)
+{
+	u32 header;
+	int vsec;
+
+	vsec = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_VNDR);
+	while (vsec) {
+		if (pci_read_config_dword(dev, vsec + 0x4,
+					  &header) == PCIBIOS_SUCCESSFUL &&
+		    PCI_VSEC_CAP_ID(header) == vsec_cap_id)
+			break;
+
+		vsec = pci_find_next_ext_capability(dev, vsec,
+						    PCI_EXT_CAP_ID_VNDR);
+	}
+
+	return vsec;
+}
+EXPORT_SYMBOL_GPL(pci_find_vsec_capability);
+
 static int __pci_find_next_ht_cap(struct pci_dev *dev, int pos, int ht_cap)
 {
 	int rc, ttl = PCI_FIND_CAP_TTL;
diff --git a/include/linux/pci.h b/include/linux/pci.h
index 22207a7..effecb0 100644
--- a/include/linux/pci.h
+++ b/include/linux/pci.h
@@ -1067,6 +1067,7 @@ int pci_find_capability(struct pci_dev *dev, int cap);
 int pci_find_next_capability(struct pci_dev *dev, u8 pos, int cap);
 int pci_find_ext_capability(struct pci_dev *dev, int cap);
 int pci_find_next_ext_capability(struct pci_dev *dev, int pos, int cap);
+int pci_find_vsec_capability(struct pci_dev *dev, int vsec_cap_id);
 int pci_find_ht_capability(struct pci_dev *dev, int ht_cap);
 int pci_find_next_ht_capability(struct pci_dev *dev, int pos, int ht_cap);
 struct pci_bus *pci_find_next_bus(const struct pci_bus *from);
diff --git a/include/uapi/linux/pci_regs.h b/include/uapi/linux/pci_regs.h
index a95d55f..f5d17be 100644
--- a/include/uapi/linux/pci_regs.h
+++ b/include/uapi/linux/pci_regs.h
@@ -730,6 +730,11 @@
 #define PCI_EXT_CAP_DSN_SIZEOF	12
 #define PCI_EXT_CAP_MCAST_ENDPOINT_SIZEOF 40
 
+/* Vendor-Specific Extended Capabilities */
+#define PCI_VSEC_CAP_ID(header)		(header & 0x0000ffff)
+#define PCI_VSEC_CAP_REV(header)	((header >> 16) & 0xf)
+#define PCI_VSEC_CAP_LEN(header)	((header >> 20) & 0xffc)
+
 /* Advanced Error Reporting */
 #define PCI_ERR_UNCOR_STATUS	4	/* Uncorrectable Error Status */
 #define  PCI_ERR_UNC_UND	0x00000001	/* Undefined */
-- 
2.7.4