1 #asm
2 .cseg
3 .equ oldpcos=pc
4 .org 0x2000
5 #endasm
6
7 void os_schedule(char mode)
8 {
9 os_thread *temp_thread, *top_thread;
10
11 #asm("cli");
12
13 temp_thread = g_usedthreads;
14 top_thread = g_currthread;
15
16 while (temp_thread != NULL)
17 {
18 if (
19 (temp_thread->state == THREADSTATE_RUNNING) &&
20 (temp_thread->sleep_duration == 0 || --temp_thread->sleep_duration == 0) &&
21 (temp_thread->priority <= top_thread->priority || top_thread->state != THREADSTATE_RUNNING || (top_thread->sleep_duration != 0))
22 )
23 {
24 top_thread = temp_thread;
25 }
26 temp_thread = temp_thread->next;
27 }
28 if (top_thread != g_currthread)
29 {
30 // we need to move the thread about to be run to front of chain
31 // this will allow 2 threads with same priority to alternate
32 if (top_thread != g_usedthreads)
33 {
34 if (top_thread->prev != NULL)
35 {
36 // Hook left to right
37 top_thread->prev->next = top_thread->next;
38 }
39 if (top_thread->next != NULL)
40 {
41 // Hook right to left
42 top_thread->next->prev = top_thread->prev;
43 }
44 // Put current at front
45 g_usedthreads->prev = top_thread;
46 top_thread->next = g_usedthreads;
47 g_usedthreads = top_thread;
48 }
49
50 temp_thread = g_usedthreads;
51
52 g_prevthread = g_currthread;
53 g_currthread = top_thread;
54
55 os_contextswitch();
56 }
57 else if (g_currthread != g_usedthreads)
58 {
59 if (g_currthread->prev != NULL)
60 {
61 // Hook left to right
62 g_currthread->prev->next = g_currthread->next;
63 }
64 if (g_currthread->next != NULL)
65 {
66 // Hook right to left
67 g_currthread->next->prev = g_currthread->prev;
68 }
69 // Put current at front
70 g_usedthreads->prev = g_currthread;
71 g_currthread->next = g_usedthreads;
72 g_usedthreads = g_currthread;
73 }
74 #asm("sei");
75 }
76 interrupt [TIM0_COMP] void os_timer0_comp(void)
77 {
78 #asm("cli");
79 if (g_threadcount > 0)
80 {
81 os_schedule(0);
82 }
83 #asm("sei");
84 }
85
86 /*
87 * This context swtiching algortim was borrowed from aOS by Anssi Ylätalo
88 */
89 void os_contextswitch(void)
90 {
91 UCHAR *new_swstack, *new_hwstack;
92 os_thread *curr_thread;
93
94 #asm("cli");
95
96 curr_thread = g_prevthread; /* Get address to current TCB */
97 new_swstack = g_currthread->swstack; /* Get highest priority ready tasks data stack */
98 new_hwstack = g_currthread->hwstack; /* Get highest priority ready task's hw stack */
99
100 /*
101 Context switch.
102
103 ; *dstk_highrdy -> Y+4
104 ; *hstk_highrdy -> Y+2
105 ; *curr_tcb -> Y+0
106
107 1. Save HW SP and data SP to current task's PCB
108 2. Load SP from highest priority task's TCB
109 3. Restore context and return to new task
110 */
111
112 #asm
113 ; Save all registers except SW stack pointer
114 PUSH R0
115 PUSH R1
116 PUSH R2
117 PUSH R3
118 PUSH R4
119 PUSH R5
120 PUSH R6
121 PUSH R7
122 PUSH R8
123 PUSH R9
124 PUSH R10
125 PUSH R11
126 PUSH R12
127 PUSH R13
128 PUSH R14
129 PUSH R15
130 PUSH R16
131 PUSH R17
132 PUSH R18
133 PUSH R19
134 PUSH R20
135 PUSH R21
136 PUSH R22
137 PUSH R23
138 PUSH R24
139 PUSH R25
140 PUSH R26
141 PUSH R27
142 PUSH R30
143 PUSH R31
144 IN R0,SREG
145 PUSH R0
146
147 ; Save current HW stack pointer, low first
148 LDD R26,Y+0 ; hstk_current -> X
149 LDD R27,Y+1
150
151 IN R30,SPL ; *hstk_current = SP;
152 ST X+,R30
153 IN R30,SPH
154 ST X+,R30
155
156 ; Save current SW stack pointer, low first
157 MOV R30,R28 ; LOW(Y) -> R30
158 ADIW R30,6 ; Restore Y value (3 pointers * 2 bytes each)
159 ST X+,R30 ; *dstk_current = Y;
160 ST X,R29
161
162 ; Load new HW stack pointer, low first
163 LDD R30,Y+2
164 OUT SPL,R30
165 LDD R30,Y+3
166 OUT SPH,R30
167
168 ; Load new SW stack pointer, low first
169 LDD R30,Y+4
170 LDD R31,Y+5
171 MOV R28,R30
172 MOV R29,R31
173
174 ; Pop all registers except SW stack pointer
175 POP R0
176 OUT SREG,R0
177 POP R31
178 POP R30
179 POP R27
180 POP R26
181 POP R25
182 POP R24
183 POP R23
184 POP R22
185 POP R21
186 POP R20
187 POP R19
188 POP R18
189 POP R17
190 POP R16
191 POP R15
192 POP R14
193 POP R13
194 POP R12
195 POP R11
196 POP R10
197 POP R9
198 POP R8
199 POP R7
200 POP R6
201 POP R5
202 POP R4
203 POP R3
204 POP R2
205 POP R1
206 POP R0
207 RETI
208 #endasm
209 }
210 void *os_alloc_stack(UINT stkSize)
211 {
212 if (g_stkCurrent + stkSize <= g_stkEnd)
213 {
214 g_stkCurrent = (g_stkCurrent+stkSize);
215 return (void*)(g_stkCurrent);
216 }
217 return NULL;
218 }
219 void os_reset_stack(void)
220 {
221 g_stkCurrent = g_stkStart;
222 }
223 #asm
224 .org oldpcos
225 #endasm