[SeaBIOS] [PATCH v2 1/2] seabios: pci: introduce helper function to initialize a given device.

[Isaku Yamahata]

introduce helper function to initialize a given device,
This will be used later.

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

diff --git a/src/pci.c b/src/pci.c
index 1ab3c2c..c54b084 100644
--- a/src/pci.c
+++ b/src/pci.c
@@ -183,3 +183,23 @@ pci_find_class(u16 classid)
     }
     return -1;
 }
+
+int pci_init_device(const struct pci_device_id *ids, u16 bdf, void *arg)
+{
+    u16 vendor_id = pci_config_readw(bdf, PCI_VENDOR_ID);
+    u16 device_id = pci_config_readw(bdf, PCI_DEVICE_ID);
+    u16 class = pci_config_readw(bdf, PCI_CLASS_DEVICE);
+
+    while (ids->vendid || ids->class_mask) {
+        if ((ids->vendid == PCI_ANY_ID || ids->vendid == vendor_id) &&
+            (ids->devid == PCI_ANY_ID || ids->devid == device_id) &&
+            !((ids->class ^ class) & ids->class_mask)) {
+            if (ids->func) {
+                ids->func(bdf, arg);
+            }
+            return 0;
+        }
+        ids++;
+    }
+    return -1;
+}
diff --git a/src/pci.h b/src/pci.h
index e40e116..fa6a32d 100644
--- a/src/pci.h
+++ b/src/pci.h
@@ -60,6 +60,40 @@ int pci_next(int bdf, int *pmax);
          ; MAX = pci_bus_devfn_to_bdf(BUS, 0) + 0x0100,                 \
            BDF = pci_next(BDF + 1, &MAX))

+#define PCI_ANY_ID      (~0)
+struct pci_device_id {
+    u32 vendid;
+    u32 devid;
+    u32 class;
+    u32 class_mask;
+    void (*func)(u16 bdf, void *arg);
+};
+
+#define PCI_DEVICE(vendor_id, device_id, init_func)     \
+    {                                                   \
+        .vendid = (vendor_id),                          \
+        .devid = (device_id),                           \
+        .class = PCI_ANY_ID,                            \
+        .class_mask = 0,                                \
+        .func = (init_func)                             \
+    }
+
+#define PCI_DEVICE_CLASS(vendor_id, device_id, class_code, init_func)   \
+    {                                                                   \
+        .vendid = (vendor_id),                                          \
+        .devid = (device_id),                                           \
+        .class = (class_code),                                          \
+        .class_mask = ~0,                                               \
+        .func = (init_func)                                             \
+    }
+
+#define PCI_DEVICE_END                          \
+    {                                           \
+        .vendid = 0,                            \
+    }
+
+int pci_init_device(const struct pci_device_id *table, u16 bdf, void *arg);
+
 // pirtable.c
 void create_pirtable(void);