From 6d910559b3855e3b09f7475a42c67b5bcf39327f Mon Sep 17 00:00:00 2001
From: Michael Brown <mcb30@ipxe.org>
Date: Thu, 1 Aug 2013 16:52:28 +0100
Subject: [PATCH] [pci] Add pci_find_next() to iterate over existent PCI
 devices

Signed-off-by: Michael Brown <mcb30@ipxe.org>
---
 src/drivers/bus/pci.c  | 65 ++++++++++++++++++++++++++++--------------
 src/include/ipxe/pci.h |  2 ++
 2 files changed, 46 insertions(+), 21 deletions(-)

diff --git a/src/drivers/bus/pci.c b/src/drivers/bus/pci.c
index 7bd353d8..4a8d00b5 100644
--- a/src/drivers/bus/pci.c
+++ b/src/drivers/bus/pci.c
@@ -171,8 +171,20 @@ void adjust_pci_device ( struct pci_device *pci ) {
  * @ret rc		Return status code
  */
 int pci_read_config ( struct pci_device *pci ) {
+	uint16_t busdevfn;
+	uint8_t hdrtype;
 	uint32_t tmp;
 
+	/* Ignore all but the first function on non-multifunction devices */
+	if ( PCI_FUNC ( pci->busdevfn ) != 0 ) {
+		busdevfn = pci->busdevfn;
+		pci->busdevfn = PCI_FIRST_FUNC ( pci->busdevfn );
+		pci_read_config_byte ( pci, PCI_HEADER_TYPE, &hdrtype );
+		pci->busdevfn = busdevfn;
+		if ( ! ( hdrtype & 0x80 ) )
+			return -ENODEV;
+	}
+
 	/* Check for physical device presence */
 	pci_read_config_dword ( pci, PCI_VENDOR_ID, &tmp );
 	if ( ( tmp == 0xffffffff ) || ( tmp == 0 ) )
@@ -203,6 +215,32 @@ int pci_read_config ( struct pci_device *pci ) {
 	return 0;
 }
 
+/**
+ * Find next device on PCI bus
+ *
+ * @v pci		PCI device to fill in
+ * @v busdevfn		Starting bus:dev.fn address
+ * @ret busdevfn	Bus:dev.fn address of next PCI device, or negative error
+ */
+int pci_find_next ( struct pci_device *pci, unsigned int busdevfn ) {
+	static unsigned int end;
+	int rc;
+
+	/* Determine number of PCI buses */
+	if ( ! end )
+		end = PCI_BUSDEVFN ( pci_num_bus(), 0, 0 );
+
+	/* Find next PCI device, if any */
+	for ( ; busdevfn < end ; busdevfn++ ) {
+		memset ( pci, 0, sizeof ( *pci ) );
+		pci_init ( pci, busdevfn );
+		if ( ( rc = pci_read_config ( pci ) ) == 0 )
+			return busdevfn;
+	}
+
+	return -ENODEV;
+}
+
 /**
  * Find driver for PCI device
  *
@@ -276,14 +314,10 @@ void pci_remove ( struct pci_device *pci ) {
  */
 static int pcibus_probe ( struct root_device *rootdev ) {
 	struct pci_device *pci = NULL;
-	unsigned int num_bus;
-	unsigned int busdevfn;
-	uint8_t hdrtype = 0;
+	int busdevfn = 0;
 	int rc;
 
-	num_bus = pci_num_bus();
-	for ( busdevfn = 0 ; busdevfn < PCI_BUSDEVFN ( num_bus, 0, 0 ) ;
-	      busdevfn++ ) {
+	for ( busdevfn = 0 ; 1 ; busdevfn++ ) {
 
 		/* Allocate struct pci_device */
 		if ( ! pci )
@@ -292,22 +326,11 @@ static int pcibus_probe ( struct root_device *rootdev ) {
 			rc = -ENOMEM;
 			goto err;
 		}
-		memset ( pci, 0, sizeof ( *pci ) );
-		pci_init ( pci, busdevfn );
-			
-		/* Skip all but the first function on
-		 * non-multifunction cards
-		 */
-		if ( PCI_FUNC ( busdevfn ) == 0 ) {
-			pci_read_config_byte ( pci, PCI_HEADER_TYPE,
-					       &hdrtype );
-		} else if ( ! ( hdrtype & 0x80 ) ) {
-			continue;
-		}
 
-		/* Read device configuration */
-		if ( ( rc = pci_read_config ( pci ) ) != 0 )
-			continue;
+		/* Find next PCI device, if any */
+		busdevfn = pci_find_next ( pci, busdevfn );
+		if ( busdevfn < 0 )
+			break;
 
 		/* Look for a driver */
 		if ( ( rc = pci_find_driver ( pci ) ) != 0 ) {
diff --git a/src/include/ipxe/pci.h b/src/include/ipxe/pci.h
index a6ed484f..692771eb 100644
--- a/src/include/ipxe/pci.h
+++ b/src/include/ipxe/pci.h
@@ -351,6 +351,7 @@ struct pci_driver {
 #define PCI_FUNC( busdevfn )		( ( (busdevfn) >> 0 ) & 0x07 )
 #define PCI_BUSDEVFN( bus, slot, func )	\
 	( ( (bus) << 8 ) | ( (slot) << 3 ) | ( (func) << 0 ) )
+#define PCI_FIRST_FUNC( busdevfn )	( (busdevfn) & ~0x07 )
 
 #define PCI_BASE_CLASS( class )		( (class) >> 16 )
 #define PCI_SUB_CLASS( class )		( ( (class) >> 8 ) & 0xff )
@@ -385,6 +386,7 @@ extern void adjust_pci_device ( struct pci_device *pci );
 extern unsigned long pci_bar_start ( struct pci_device *pci,
 				     unsigned int reg );
 extern int pci_read_config ( struct pci_device *pci );
+extern int pci_find_next ( struct pci_device *pci, unsigned int busdevfn );
 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 );