[PATCH v5] drivers/pci: Implement config access words

[BALATON Zoltan]

Signed-off-by: BALATON Zoltan 
---
v5: Different approach to add call parent words for bridge
v4: Added call parent word for bridge devices

drivers/pci.c | 69 +++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 69 insertions(+)

diff --git a/drivers/pci.c b/drivers/pci.c
index f30e427..1d6d973 100644
--- a/drivers/pci.c
+++ b/drivers/pci.c
@@ -424,6 +424,60 @@ ob_pci_bus_map_in(int *idx)
    PUSH(virt);
 }

+static void
+ob_pci_config_read8(int *idx)
+{
+    cell hi = POP();
+    pci_addr addr = PCI_ADDR(PCI_BUS(hi), PCI_DEV(hi), PCI_FN(hi));
+    uint8_t val = pci_config_read8(addr, hi & 0xff);
+    PUSH(val);
+}
+
+static void
+ob_pci_config_write8(int *idx)
+{
+    cell hi = POP();
+    pci_addr addr = PCI_ADDR(PCI_BUS(hi), PCI_DEV(hi), PCI_FN(hi));
+    cell val = POP();
+    pci_config_write8(addr, hi & 0xff, val);
+}
+
+static void
+ob_pci_config_read16(int *idx)
+{
+    cell hi = POP();
+    pci_addr addr = PCI_ADDR(PCI_BUS(hi), PCI_DEV(hi), PCI_FN(hi));
+    uint16_t val = pci_config_read16(addr, hi & 0xff);
+    PUSH(val);
+}
+
+static void
+ob_pci_config_write16(int *idx)
+{
+    cell hi = POP();
+    pci_addr addr = PCI_ADDR(PCI_BUS(hi), PCI_DEV(hi), PCI_FN(hi));
+    cell val = POP();
+    pci_config_write16(addr, hi & 0xff, val);
+}
+
+static void
+ob_pci_config_read32(int *idx)
+{
+    cell hi = POP();
+    pci_addr addr = PCI_ADDR(PCI_BUS(hi), PCI_DEV(hi), PCI_FN(hi));
+    uint32_t val = pci_config_read32(addr, hi & 0xff);
+    PUSH(val);
+}
+
+static void
+ob_pci_config_write32(int *idx)
+{
+    cell hi = POP();
+    pci_addr addr = PCI_ADDR(PCI_BUS(hi), PCI_DEV(hi), PCI_FN(hi));
+    cell val = POP();
+    pci_config_write32(addr, hi & 0xff, val);
+}
+
 static void
 ob_pci_dma_alloc(int *idx)
 {
@@ -460,6 +514,12 @@ NODE_METHODS(ob_pci_bus_node) = {
    { "decode-unit",	ob_pci_decode_unit	},
    { "encode-unit",	ob_pci_encode_unit	},
    { "pci-map-in",		ob_pci_bus_map_in	},
+	{ "config-b@",		ob_pci_config_read8	},
+	{ "config-b!",		ob_pci_config_write8	},
+	{ "config-w@",		ob_pci_config_read16	},
+	{ "config-w!",		ob_pci_config_write16	},
+	{ "config-l@",		ob_pci_config_read32	},
+	{ "config-l!",		ob_pci_config_write32	},
    { "dma-alloc",		ob_pci_dma_alloc	},
    { "dma-free",		ob_pci_dma_free		},
    { "dma-map-in",		ob_pci_dma_map_in	},
@@ -494,6 +554,14 @@ NODE_METHODS(ob_pci_simple_node) = {
    { "close",		ob_pci_close		},
 };

+static void pci_config_call_parent_methods(void)
+{
+	/* As per the IEEE-1275 PCI specification, chain up to the parent */
+	feval("" config-b@" is-call-parent " config-b!" is-call-parent "
+	      "" config-w@" is-call-parent " config-w!" is-call-parent "
+	      "" config-l@" is-call-parent " config-l!" is-call-parent ");
+}
+
 static void pci_set_bus_range(const pci_config_t *config)
 {
    phandle_t dev = find_dev(config->path);
@@ -1831,6 +1899,7 @@ static phandle_t ob_configure_pci_device(const char* parent_path,
     case PCI_BASE_CLASS_BRIDGE:
         if (subclass != PCI_SUBCLASS_BRIDGE_HOST) {
             BIND_NODE_METHODS(phandle, ob_pci_bridge_node);
+            pci_config_call_parent_methods();
         } else {
             BIND_NODE_METHODS(phandle, ob_pci_bus_node);
         }