94 lines
2.4 KiB
Forth
94 lines
2.4 KiB
Forth
\ -------------------------------------------------------------------------
|
|
\ 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
|
|
;
|