[SeaBIOS] [PATCH 1/8] seabios: pci: introduce foreachpci_in_bus() helper macro.

[Isaku Yamahata]

This patch introduces foreachpci_in_bus() helper macro for
depth first recursion. foreachpci() is for width first recursion.
The macro will be used later to initialize pci bridge
that requires depth first recursion.

Signed-off-by: Isaku Yamahata 
---
 src/pci.c |   30 ++++++++++++++++++++++++++++++
 src/pci.h |   11 +++++++++++
 2 files changed, 41 insertions(+), 0 deletions(-)

diff --git a/src/pci.c b/src/pci.c
index 1ab3c2c..d418b4b 100644
--- a/src/pci.c
+++ b/src/pci.c
@@ -157,6 +157,36 @@ pci_find_vga(void)
     }
 }

+// Helper function for foreachpci_in_bus() macro - return next devfn
+int
+pci_next_in_bus(int bus, int devfn)
+{
+    int bdf = pci_bus_devfn_to_bdf(bus, devfn);
+    if (pci_bdf_to_fn(bdf) == 1
+        && (pci_config_readb(bdf-1, PCI_HEADER_TYPE) & 0x80) == 0)
+        // Last found device wasn't a multi-function device - skip to
+        // the next device.
+        devfn += 7;
+
+    for (;;) {
+        if (devfn >= 0x100)
+            return -1;
+
+        bdf = pci_bus_devfn_to_bdf(bus, devfn);
+        u16 v = pci_config_readw(bdf, PCI_VENDOR_ID);
+        if (v != 0x0000 && v != 0xffff)
+            // Device is present.
+            break;
+
+        if (pci_bdf_to_fn(bdf) == 0)
+            devfn += 8;
+        else
+            devfn += 1;
+    }
+
+    return devfn;
+}
+
 // Search for a device with the specified vendor and device ids.
 int
 pci_find_device(u16 vendid, u16 devid)
diff --git a/src/pci.h b/src/pci.h
index 8a21c06..26bfd40 100644
--- a/src/pci.h
+++ b/src/pci.h
@@ -21,6 +21,9 @@ static inline u8 pci_bdf_to_fn(u16 bdf) {
 static inline u16 pci_to_bdf(int bus, int dev, int fn) {
     return (bus<<8) | (dev<<3) | fn;
 }
+static inline u16 pci_bus_devfn_to_bdf(int bus, u16 devfn) {
+    return (bus << 8) | devfn;
+}

static inline u32 pci_vd(u16 vendor, u16 device) {
     return (device << 16) | vendor;
@@ -50,6 +53,14 @@ int pci_next(int bdf, int *pmax);
          ; BDF >= 0                             \
          ; BDF=pci_next(BDF+1, &MAX))

+int pci_next_in_bus(int bus, int devfn);
+#define foreachpci_in_bus(BUS, DEVFN, BDF)              \
+    for (DEVFN = pci_next_in_bus(BUS, 0),               \
+         BDF = pci_bus_devfn_to_bdf(BUS, DEVFN)         \
+         ; DEVFN >= 0                                   \
+         ; DEVFN = pci_next_in_bus(BUS, DEVFN + 1),     \
+           BDF = pci_bus_devfn_to_bdf(BUS, DEVFN))
+
 // pirtable.c
 void create_pirtable(void);