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);
+}