[ipxe-devel] [PATCH v2 1/4] [pci] Add pci_find_next_capability

[Ladi Prosek]

PCI devices may support more capabilities of the same type (for
example PCI_CAP_ID_VNDR) and there was no way to discover all of
them. This commit adds a new API pci_find_next_capability which
provides this functionality. It would typically be used like so:

  for (pos = pci_find_capability(pci, PCI_CAP_ID_VNDR);
       pos > 0;
       pos = pci_find_next_capability(pci, pos, PCI_CAP_ID_VNDR)) {
    ...
  }

Signed-off-by: Ladi Prosek <lprosek at redhat.com>
Reviewed-by: Michael S. Tsirkin <mst at redhat.com>
---
 src/drivers/bus/pciextra.c | 54 +++++++++++++++++++++++++++++++++++-----------
 src/include/ipxe/pci.h     |  2 ++
 2 files changed, 43 insertions(+), 13 deletions(-)

diff --git a/src/drivers/bus/pciextra.c b/src/drivers/bus/pciextra.c
index 82287fb..15f79c9 100644
--- a/src/drivers/bus/pciextra.c
+++ b/src/drivers/bus/pciextra.c
@@ -3,6 +3,24 @@ FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
 #include <stdint.h>
 #include <ipxe/pci.h>
 
+static int pci_find_capability_common ( struct pci_device *pci,
+		   uint8_t pos, int cap ) {
+	uint8_t id;
+	int ttl = 48;
+
+	while ( ttl-- && pos >= 0x40 ) {
+		pos &= ~3;
+		pci_read_config_byte ( pci, pos + PCI_CAP_ID, &id );
+		DBG ( "PCI Capability: %d\n", id );
+		if ( id == 0xff )
+			break;
+		if ( id == cap )
+			return pos;
+		pci_read_config_byte ( pci, pos + PCI_CAP_NEXT, &pos );
+	}
+	return 0;
+}
+
 /**
  * Look for a PCI capability
  *
@@ -17,9 +35,8 @@ FILE_LICENCE ( GPL2_OR_LATER_OR_UBDL );
  */
 int pci_find_capability ( struct pci_device *pci, int cap ) {
 	uint16_t status;
-	uint8_t pos, id;
+	uint8_t pos;
 	uint8_t hdr_type;
-	int ttl = 48;
 
 	pci_read_config_word ( pci, PCI_STATUS, &status );
 	if ( ! ( status & PCI_STATUS_CAP_LIST ) )
@@ -36,17 +53,28 @@ int pci_find_capability ( struct pci_device *pci, int cap ) {
 		pci_read_config_byte ( pci, PCI_CB_CAPABILITY_LIST, &pos );
 		break;
 	}
-	while ( ttl-- && pos >= 0x40 ) {
-		pos &= ~3;
-		pci_read_config_byte ( pci, pos + PCI_CAP_ID, &id );
-		DBG ( "PCI Capability: %d\n", id );
-		if ( id == 0xff )
-			break;
-		if ( id == cap )
-			return pos;
-		pci_read_config_byte ( pci, pos + PCI_CAP_NEXT, &pos );
-	}
-	return 0;
+	return pci_find_capability_common ( pci, pos, cap );
+}
+
+/**
+ * Look for another PCI capability
+ *
+ * @v pci		PCI device to query
+ * @v pos		Address of the current capability
+ * @v cap		Capability code
+ * @ret address		Address of capability, or 0 if not found
+ *
+ * Determine whether or not a device supports a given PCI capability
+ * starting the search at a given address within the device's PCI
+ * configuration space. Returns the address of the next capability
+ * structure within the device's PCI configuration space, or 0 if the
+ * device does not support another such capability.
+ */
+int pci_find_next_capability ( struct pci_device *pci, int pos, int cap ) {
+	u8 new_pos;
+
+	pci_read_config_byte ( pci, pos + PCI_CAP_NEXT, &new_pos );
+	return pci_find_capability_common ( pci, new_pos, cap );
 }
 
 /**
diff --git a/src/include/ipxe/pci.h b/src/include/ipxe/pci.h
index 89d9d80..0c6bc7e 100644
--- a/src/include/ipxe/pci.h
+++ b/src/include/ipxe/pci.h
@@ -286,6 +286,8 @@ extern int pci_find_driver ( struct pci_device *pci );
 extern int pci_probe ( struct pci_device *pci );
 extern void pci_remove ( struct pci_device *pci );
 extern int pci_find_capability ( struct pci_device *pci, int capability );
+extern int pci_find_next_capability ( struct pci_device *pci,
+				      int pos, int capability );
 extern unsigned long pci_bar_size ( struct pci_device *pci, unsigned int reg );
 
 /**
-- 
2.5.0