1 files changed, 87 insertions, 0 deletions
diff --git a/c/src/lib/libbsp/powerpc/mvme2307/pci/pci.c b/c/src/lib/libbsp/powerpc/mvme2307/pci/pci.c
new file mode 100644
index 0000000000..e9bebd3f78
--- /dev/null
+++ b/c/src/lib/libbsp/powerpc/mvme2307/pci/pci.c
@@ -0,0 +1,87 @@
+#include <bsp.h>
+
+static const void *PciConfAddr = IO_TO_LOCAL(0x0cf8);
+static const void *PciConfData = IO_TO_LOCAL(0x0cfc);
+
+int pci_make_devsig(int bus, int device, int function) {
+    return ((bus & 255) << 16) | ((device & 31) << 11) | 
+           ((function & 7) << 8) | 0x80000000;
+}
+
+int pci_conf_read8(int sig, int off) {
+    st_le32(PciConfAddr, sig + (off & 252));
+    synchronize_io();
+    return *(unsigned char *)(PciConfData + (off & 3));
+}
+
+int pci_conf_read16(int sig, int off) {
+    st_le32(PciConfAddr, sig + (off & 252));
+    synchronize_io();
+    return ld_le16(PciConfData + (off & 3));
+}
+
+int pci_conf_read32(int sig, int off) {
+    st_le32(PciConfAddr, sig + (off & 252));
+    synchronize_io();
+    return ld_le32(PciConfData);
+}
+
+void pci_conf_write8(int sig, int off, unsigned int data) {
+    st_le32(PciConfAddr, sig + (off & 252));
+    synchronize_io();
+    *(unsigned char *)(PciConfData + (off & 3)) = data;
+}
+
+void pci_conf_write16(int sig, int off, unsigned int data) {
+    st_le32(PciConfAddr, sig + (off & 252));
+    synchronize_io();
+    st_le16(PciConfData + (off & 3), data);
+}
+
+void pci_conf_write32(int sig, int off, unsigned int data) {
+    st_le32(PciConfAddr, sig + (off & 252));
+    synchronize_io();
+    st_le32(PciConfData + (off & 3), data);
+}
+
+static int recursive_find(int bus, int devid_venid, int idx, int *count) {
+    int devsig, dev, func, max_func, idreg, class, secondary_bus;
+    char header_type;
+
+    for (dev = 0; dev < 32; dev++) {
+        devsig = pci_make_devsig(bus, dev, 0);
+        header_type = pci_conf_read8(devsig, 0x0e);
+        max_func = (header_type & 0x80) ? 8 : 1;   /* multi-function device? */
+        for (func = 0; func < max_func; func++) {
+            devsig = pci_make_devsig(bus, dev, func);
+            idreg = pci_conf_read32(devsig, 0x00);
+            if (idreg == 0xffffffff) { 
+                break;      /* empty slot */
+            }
+            if (idreg == devid_venid) {
+                /* id's match - is it the right instance? */
+                if (*count == idx) {
+                    return devsig;
+                } else {
+                    (*count)++;
+                }
+            }
+            class = pci_conf_read32(devsig, 0x08) & 0xffff0000;
+            if (class == 0x06040000) {      /* pci-pci bridge? */
+                secondary_bus = pci_conf_read8(devsig, 0x19);
+                devsig = recursive_find(secondary_bus, devid_venid, idx, count);
+                if (devsig != PCI_NOT_FOUND) {
+                    return devsig;
+                }
+            }
+        }
+    }
+    return PCI_NOT_FOUND;
+}
+
+int pci_find_by_devid(int vendorId, int devId, int idx) {
+    int count = 0;
+    int devid_venid = ((devId << 16) & 0xffff0000) | (vendorId & 0xffff);
+
+    return recursive_find(0, devid_venid, idx, &count);
+}