drivers/pci: Add function to find an extended capability

PCIe extends device's configuration space to 4k and provides
extended capability. The patch adds function to find them.
The code is ported from Linux PCIe driver.

Signed-off-by: Minghuan Lian <Minghuan.Lian@freescale.com>
Reviewed-by: Bin Meng <bmeng.cn@gmail.com>
Reviewed-by: York Sun <yorksun@freescale.com>

1 files changed, 53 insertions, 0 deletions

diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 157491c52d..df50b48003 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -526,3 +526,56 @@ int pci_find_cap(struct pci_controller *hose, pci_dev_t dev, int pos, int cap)
 	}
 	return 0;
 }
+
+/**
+ * pci_find_next_ext_capability - Find an extended capability
+ *
+ * Returns the address of the next matching extended capability structure
+ * within the device's PCI configuration space or 0 if the device does
+ * not support it.  Some capabilities can occur several times, e.g., the
+ * vendor-specific capability, and this provides a way to find them all.
+ */
+int pci_find_next_ext_capability(struct pci_controller *hose, pci_dev_t dev,
+				 int start, int cap)
+{
+	u32 header;
+	int ttl, pos = PCI_CFG_SPACE_SIZE;
+
+	/* minimum 8 bytes per capability */
+	ttl = (PCI_CFG_SPACE_EXP_SIZE - PCI_CFG_SPACE_SIZE) / 8;
+
+	if (start)
+		pos = start;
+
+	pci_hose_read_config_dword(hose, dev, pos, &header);
+	if (header == 0xffffffff || header == 0)
+		return 0;
+
+	while (ttl-- > 0) {
+		if (PCI_EXT_CAP_ID(header) == cap && pos != start)
+			return pos;
+
+		pos = PCI_EXT_CAP_NEXT(header);
+		if (pos < PCI_CFG_SPACE_SIZE)
+			break;
+
+		pci_hose_read_config_dword(hose, dev, pos, &header);
+		if (header == 0xffffffff || header == 0)
+			break;
+	}
+
+	return 0;
+}
+
+/**
+ * pci_hose_find_ext_capability - Find an extended capability
+ *
+ * Returns the address of the requested extended capability structure
+ * within the device's PCI configuration space or 0 if the device does
+ * not support it.
+ */
+int pci_hose_find_ext_capability(struct pci_controller *hose, pci_dev_t dev,
+				 int cap)
+{
+	return pci_find_next_ext_capability(hose, dev, 0, cap);
+}