1 files changed, 94 insertions, 0 deletions
diff --git a/qemu/roms/openbios/drivers/sbus.fs b/qemu/roms/openbios/drivers/sbus.fs
new file mode 100644
index 000000000..b84a3ac72
--- /dev/null
+++ b/qemu/roms/openbios/drivers/sbus.fs
@@ -0,0 +1,94 @@
+\ -------------------------------------------------------------------------
+\ SBus encode/decode unit
+\ -------------------------------------------------------------------------
+
+: decode-unit-sbus ( str len -- id lun )
+  ascii , left-split
+  ( addr-R len-R addr-L len-L )
+  parse-hex
+  -rot parse-hex
+  swap
+;
+
+: encode-unit-sbus ( id lun -- str len)
+  swap
+  pocket tohexstr
+  " ," 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 size )
+  " map-out" $call-parent
+;
+
+\ -------------------------------------------------------------------------
+\ SBus probe
+\ -------------------------------------------------------------------------
+
+: probe-self-sbus ( arg-adr arg-len reg-adr reg-len fcode-adr fcode-len -- )
+
+  0 to probe-fcode?
+
+  ['] decode-unit-sbus catch if
+    2drop 2drop 2drop 2drop
+    exit
+  then
+
+  h# 10000 map-in-sbus
+
+  dup cpeek if
+    dup h# f1 = swap h# fd = or if
+      new-device
+      >r set-args r>
+      dup 1 byte-load
+      finish-device
+
+      -1 to probe-fcode?
+    else
+      nip nip nip nip
+      ." Invalid FCode start byte" cr
+    then
+  else
+    nip nip nip nip
+  then
+
+  h# 10000 map-out-sbus
+;