1 files changed, 24 insertions, 0 deletions
diff --git a/src/hw/pci.c b/src/hw/pci.c
index 8e3d6179..9855badb 100644
--- a/src/hw/pci.c
+++ b/src/hw/pci.c
@@ -58,6 +58,30 @@ pci_config_maskw(u16 bdf, u32 addr, u16 off, u16 on)
     pci_config_writew(bdf, addr, val);
 }
 
+u8 pci_find_capability(u16 bdf, u8 cap_id, u8 cap)
+{
+    int i;
+    u16 status = pci_config_readw(bdf, PCI_STATUS);
+
+    if (!(status & PCI_STATUS_CAP_LIST))
+        return 0;
+
+    if (cap == 0) {
+        /* find first */
+        cap = pci_config_readb(bdf, PCI_CAPABILITY_LIST);
+    } else {
+        /* find next */
+        cap = pci_config_readb(bdf, cap + PCI_CAP_LIST_NEXT);
+    }
+    for (i = 0; cap && i <= 0xff; i++) {
+        if (pci_config_readb(bdf, cap + PCI_CAP_LIST_ID) == cap_id)
+            return cap;
+        cap = pci_config_readb(bdf, cap + PCI_CAP_LIST_NEXT);
+    }
+
+    return 0;
+}
+
 // Helper function for foreachbdf() macro - return next device
 int
 pci_next(int bdf, int bus)