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