[OpenBIOS] [PATCHv2 08/14] sbus: implement map-in and map-out words

[Mark Cave-Ayland]

As these pass requests up the instance chain, make sure we do the real work
in the IOMMU map-in and map-in words too.

Signed-off-by: Mark Cave-Ayland <mark.cave-ayland at ilande.co.uk>
---
 openbios-devel/arch/sparc32/tree.fs |    2 ++
 openbios-devel/drivers/iommu.c      |   32 ++++++++++++++++++++++++++
 openbios-devel/drivers/sbus.fs      |   42 +++++++++++++++++++++++++++++++++++
 3 files changed, 76 insertions(+)

diff --git a/openbios-devel/arch/sparc32/tree.fs b/openbios-devel/arch/sparc32/tree.fs
index 0f31f4f..58e8339 100644
--- a/openbios-devel/arch/sparc32/tree.fs
+++ b/openbios-devel/arch/sparc32/tree.fs
@@ -55,6 +55,8 @@ new-device
   : close ;
   : encode-unit encode-unit-sbus ;
   : decode-unit decode-unit-sbus ;
+  : map-in map-in-sbus ;
+  : map-out map-out-sbus ;
 finish-device
 
 [IFDEF] CONFIG_BPP
diff --git a/openbios-devel/drivers/iommu.c b/openbios-devel/drivers/iommu.c
index 45005c1..090c155 100644
--- a/openbios-devel/drivers/iommu.c
+++ b/openbios-devel/drivers/iommu.c
@@ -6,6 +6,7 @@
  **/
 #include "config.h"
 #include "libopenbios/bindings.h"
+#include "libopenbios/ofmem.h"
 #include "drivers/drivers.h"
 #include "iommu.h"
 #include "arch/sparc32/ofmem_sparc32.h"
@@ -148,6 +149,34 @@ iommu_init(struct iommu *t, uint64_t base)
     return regs;
 }
 
+/* ( addr.lo addr.hi size -- virt ) */
+
+static void
+ob_iommu_map_in(void)
+{
+    phys_addr_t phys;
+    ucell size, virt;
+
+    size = POP();
+    POP();
+    phys = POP();
+
+    virt = ofmem_map_io(phys, size);
+
+    PUSH(virt);
+}
+
+/* ( virt size ) */
+
+static void
+ob_iommu_map_out(void)
+{
+    ucell size = POP();
+    ucell virt = POP();
+
+    ofmem_release_io(virt, size);
+}
+
 void
 ob_init_iommu(uint64_t base)
 {
@@ -172,4 +201,7 @@ ob_init_iommu(uint64_t base)
     fword("encode+");
     push_str("reg");
     fword("property");
+
+    bind_func("map-in", ob_iommu_map_in);
+    bind_func("map-out", ob_iommu_map_out);
 }
diff --git a/openbios-devel/drivers/sbus.fs b/openbios-devel/drivers/sbus.fs
index 409443b..73cd23b 100644
--- a/openbios-devel/drivers/sbus.fs
+++ b/openbios-devel/drivers/sbus.fs
@@ -16,3 +16,45 @@
   " ," pocket tmpstrcat >r
   rot pocket tohexstr r> tmpstrcat drop
 ;
+
+\ Convert sbus unit (from decode-unit) to physical address using
+\ sbus node ranges property
+
+: sbus-unit>addr ( phys.lo phys.hi -- phys.lo phys.hi -1 | 0 )
+  " ranges" my-self ihandle>phandle
+  get-package-property 0= if  ( phys.lo phys.hi prop prop-len )
+    begin
+      2over swap drop 0 swap  \ force phys.lo to zero for matching
+      2swap  ( unit.phys.lo unit.phys.hi 0 phys.hi res prop prop-len )
+      0 -rot  ( unit.phys.lo unit.phys.hi res prop prop-len )
+      2 0 do
+        decode-int -rot >r >r  ( unit.phys.lo unit.phys.hi res phys.x -- R: prop-len prop )
+        rot  ( unit.phys.lo res phys.x phys.hi )
+        = if
+          1+
+        then  ( unit.phys.lo res )
+        r> r>  ( unit.phys.lo res prop prop-len )
+      loop
+      rot  ( prop prop-len res )
+      2 = if  \ did we match the unit address? if so, return the physical address
+        decode-phys 2swap 2drop 2swap  ( unit.phys.lo unit.phys.hi phys.lo phys.hi )
+        drop 0 d+   \ force unit.phys.hi to zero and add address for final offset
+        -1 exit
+      else
+        decode-phys 2drop decode-int drop   \ drop the size and carry on
+      then
+    dup 0= until
+    2drop 2drop 0
+  then
+;
+
+: map-in-sbus ( phys.lo phys.hi size )
+  >r sbus-unit>addr if
+    r@ " map-in" $call-parent
+  then
+  r> drop
+;
+
+: map-out-sbus ( virt )
+  " map-out" $call-parent
+;
-- 
1.7.10.4