[pci] Add pci_find_next_capability()

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@redhat.com>
Reviewed-by: Michael S. Tsirkin <mst@redhat.com>
Signed-off-by: Michael Brown <mcb30@ipxe.org>

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 ) {
+	uint8_t new_pos;
+
+	pci_read_config_byte ( pci, pos + PCI_CAP_NEXT, &new_pos );
+	return pci_find_capability_common ( pci, new_pos, cap );
 }
 
 /**

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 );
 
 /**