[SeaBIOS] [PATCH] pci: capability scanning function

[Michael S. Tsirkin]

Add pci_find_capability to scan capability list.
Return 0 on error, capability offset if found.

Signed-off-by: Michael S. Tsirkin

---

Not useful by itself, but should be handy if
we ever want to look at capabilities such as pci express.

diff --git a/src/pci.c b/src/pci.c
index 23a6878..324ac96 100644
--- a/src/pci.c
+++ b/src/pci.c
@@ -57,6 +57,30 @@ pci_config_maskw(u16 bdf, u32 addr, u16 off, u16 on)
     pci_config_writew(bdf, addr, val);
 }

+unsigned char pci_find_capability(u16 bdf, unsigned char cap_id)
+{
+    unsigned char next, prev;
+    int loop = 0;
+
+    if (!(pci_config_readb(bdf, PCI_STATUS) & PCI_STATUS_CAP_LIST))
+        return 0;
+
+    for (prev = PCI_CAPABILITY_LIST; (next = pci_config_readb(bdf, prev));
+         prev = next + PCI_CAP_LIST_NEXT) {
+        if (pci_config_readb(bdf, next + PCI_CAP_LIST_ID) == cap_id)
+            break;
+        if (loop++ > 0x100) {
+            dprintf(1, "ERROR: capability loop detected. "
+                       "PCI device %02x:%02x.%x\n"
+                    , pci_bdf_to_bus(bdf), pci_bdf_to_dev(bdf)
+                    , pci_bdf_to_fn(bdf));
+            return 0;
+        }
+    }
+
+    return next;
+}
+
 // Helper function for foreachbdf() macro - return next device
 int
 pci_next(int bdf, int bus)
diff --git a/src/pci.h b/src/pci.h
index c34e348..160cfc1 100644
--- a/src/pci.h
+++ b/src/pci.h
@@ -110,6 +110,8 @@ void pci_reboot(void);
 u32 pci_readl(u32 addr);
 void pci_writel(u32 addr, u32 val);

+unsigned char pci_find_capability(u16 bdf, unsigned char cap_id);
+
 // pirtable.c
 void create_pirtable(void);