-
Notifications
You must be signed in to change notification settings - Fork 9
/
Copy pathpriotasker.fs
167 lines (143 loc) · 3.52 KB
/
priotasker.fs
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
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
\
\ PicForth library file
\
\ This library file has been written by Samuel Tardieu <sam@rfc1149.net>.
\ It belongs to the public domain. Do whatever you want with it.
\
\ ----------------------------------------------------------------------
\ Priority-based cooperative multitasker (state-machine based)
\ ----------------------------------------------------------------------
host
\ Task structure:
\ - address of task semaphore address (1 cell) (or 0 for a bit task)
\ - address of task entry point (1 cell)
\ - task priority (1 cell)
\ - previous task (1 cell)
\ - task byte (1 cell) (if task semaphore is 0)
\ (byte is inverted is condition is inverted)
\ or bit number to check for run (1 cell)
\ - task bit (1 cell) (if task semaphore is 0)
variable last-task
variable prio
variable mtorig
variable task-count
variable last-opt
variable last-byte
variable last-bit
255 constant idleprio
: allocate-bit ( -- )
last-bit @ if -1 last-bit +! exit then
data-here dup last-byte ! 1+ data-org 7 last-bit !
;
: new-task ( -- previous )
1 task-count +!
align here last-task @ swap last-task !
;
variable w-set?
: tasks-init ( -- )
0 last-byte !
last-task @
begin dup while dup @ ?dup if
dup 0< if
abs clrf
else
dup last-byte @ <> if
last-byte @ 0= if ff movlw then
dup last-byte ! movwf
else
drop
then
then
then 3 cells + @ repeat drop
;
' tasks-init add-to-init-chain
variable jump-address
: act-last ( -- )
task-count @ current-bank @ or if
meta> ahead
jump-address !
else
mtorig @ goto
0 jump-address !
then
reachable
;
: restore-last ( -- )
jump-address @ ?dup if
meta> then
then
;
: sameprio? ( addr -- flag )
begin
3 cells + @ dup while
dup 2 cells + @ prio @ = if drop true exit then
repeat
drop false ;
: task-prio ( n -- )
prio !
last-task @
begin dup while
dup 2 cells + @ prio @ = if
-1 task-count +!
prio @ idleprio = if
dup cell+ @ call
else
dup @ if
\ Semaphore test
dup @ abs adjust-bank over 4 cells + @ btfsc act-last
current-bank @ >r restore-bank
dup @ 0< if dup @ ,f decf then dup cell+ @ call
else
\ Bit test
dup 5 cells + @ over 4 cells + @ dup abs adjust-bank
swap 0< if
swap btfsc
else
swap btfss
then
act-last
current-bank @ >r restore-bank
dup cell+ @ call
then
dup sameprio? 0= if mtorig @ goto then
restore-last
r> current-bank ! restore-bank
then
then
3 cells + @
repeat
;
: tasks-schedule ( n -- )
tcshere mtorig !
clrwdt
256 0 do i task-prio loop
\ If the last test was optimized, no need to add an extra goto data-here
jump-address @ if mtorig @ goto then
unreachable ;
: (task) ( prio addr -- )
new-task >r
, tcshere , , r> ,
;
meta
: task ( prio "name" -- )
create allocate-bit last-byte @ tuck (task) 1+ data-org last-bit @ , ;
: task-cond ( prio "name" -- ) create data-here tuck negate (task) 1+ data-org ;
: task-idle ( -- ) idleprio 0 (task) ;
: task-set ( port bit prio -- ) 0 (task) swap , , ;
: task-clear ( port bit prio -- ) 2>r invert 2r> task-set ;
: start ( addr -- )
dup @ (literal) 4 cells + @ (literal)
meta> bit-set
;
: stop ( addr -- )
dup @ (literal) 4 cells + @ (literal)
meta> bit-clr
;
: signal ( addr -- )
1 (literal) @ abs (literal)
meta> +!
;
: multitasker ( -- )
tasks-schedule
;
target