blob: b84a3ac72f3a515b4b3d69204b4b6b88c7976783 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
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
;
|
