root / trunk / code / projects / scheduler / scheduler.c @ 1545
History | View | Annotate | Download (4.93 KB)
1 | 1479 | jsexton | /**
|
---|---|---|---|
2 | * @file scheduler.c
|
||
3 | * @brief Scheduler
|
||
4 | *
|
||
5 | * Implementation of functions for scheduler
|
||
6 | *
|
||
7 | * @author Colony Project, CMU Robotics Club
|
||
8 | 1544 | justin | * Based on avrOS and 18348 Lab9 code
|
9 | 1479 | jsexton | **/
|
10 | 16 | bcoltin | |
11 | 1479 | jsexton | #include "scheduler.h" |
12 | 1544 | justin | #include "time.h" |
13 | |||
14 | static uint8_t STACK[MAXTASKS][STACKSIZE];
|
||
15 | static uint8_t nactive_tasks = 1; |
||
16 | static uint8_t current_task = 0; //Default to main. |
||
17 | |||
18 | //Internal functions
|
||
19 | void task_terminate(void); |
||
20 | |||
21 | //Scheduler gets called from the interrupt.
|
||
22 | void scheduler(void); |
||
23 | void create_launch_stack(uint8_t task);
|
||
24 | void store_task(void); |
||
25 | void restore_task(void); |
||
26 | void restore_next_task(void); |
||
27 | |||
28 | typedef struct PCB_t |
||
29 | { |
||
30 | void (*exec)(void); |
||
31 | uint8_t* sp; |
||
32 | char running;
|
||
33 | uint16_t period; //Interval in clock cycles.
|
||
34 | uint32_t next; |
||
35 | } PCB_t; |
||
36 | |||
37 | PCB_t PCB[MAXTASKS + 1];
|
||
38 | |||
39 | void scheduler_init() { }
|
||
40 | |||
41 | void task_terminate(void) { |
||
42 | PCB[current_task].running = 0;
|
||
43 | yield(); |
||
44 | } |
||
45 | |||
46 | void yield() {
|
||
47 | //TODO Actually implement.
|
||
48 | for(;;){}
|
||
49 | } |
||
50 | |||
51 | int register_task(void (*exec)(void), uint16_t period) |
||
52 | { |
||
53 | if(nactive_tasks >= MAXTASKS) {
|
||
54 | return -1; |
||
55 | } |
||
56 | |||
57 | PCB[nactive_tasks].exec = exec; |
||
58 | PCB[nactive_tasks].period = period; |
||
59 | PCB[nactive_tasks].next = rtc_get() + period; |
||
60 | PCB[nactive_tasks].running = 0;
|
||
61 | |||
62 | //Don't need to initialize SP, it will get done later.
|
||
63 | nactive_tasks++; |
||
64 | return nactive_tasks - 1; |
||
65 | } |
||
66 | |||
67 | //Create a fresh launch stack.
|
||
68 | void create_launch_stack(uint8_t task) {
|
||
69 | uint8_t* sp = &STACK[task][STACKSIZE - 1];
|
||
70 | |||
71 | //Put task terminate and the task to execute on the stack.
|
||
72 | *(sp--) = (uint8_t)(uint16_t) *task_terminate; |
||
73 | *(sp--) = (uint8_t)(uint16_t) *task_terminate >> 8;
|
||
74 | *(sp--) = (uint8_t)(uint16_t) *PCB[nactive_tasks].exec; |
||
75 | *(sp--) = (uint8_t)(uint16_t) *PCB[nactive_tasks].exec >> 8;
|
||
76 | |||
77 | //This is going to get me in trouble,
|
||
78 | //but store_task already does everything else we need
|
||
79 | //(since we only care about void-void functions)
|
||
80 | store_task(); |
||
81 | } |
||
82 | |||
83 | void scheduler(void){ |
||
84 | 1545 | justin | static uint8_t task_i = 1; |
85 | 1544 | justin | uint8_t i; |
86 | int current_time = rtc_get();
|
||
87 | |||
88 | //Loop over registered tasks, like in round robin order.
|
||
89 | for(i = nactive_tasks; i > 0; i--) { |
||
90 | if( !PCB[task_i].running
|
||
91 | && PCB[task_i].next <= current_time) { |
||
92 | |||
93 | current_task = task_i; |
||
94 | PCB[task_i].next += PCB[task_i].period; |
||
95 | 1545 | justin | create_launch_stack(task_i); |
96 | 1544 | justin | task_i++; |
97 | 1545 | justin | break;
|
98 | } |
||
99 | 1544 | justin | |
100 | 1545 | justin | if( PCB[task_i].running ) {
|
101 | current_task = task_i; |
||
102 | task_i++ |
||
103 | 1544 | justin | break;
|
104 | } |
||
105 | |||
106 | //Loop back to 0 if necessary.
|
||
107 | task_i++; |
||
108 | if(task_i >= nactive_tasks)
|
||
109 | 1545 | justin | task_i = 1;
|
110 | 1544 | justin | } |
111 | 1545 | justin | |
112 | //If no task was selected to run,
|
||
113 | if(i == 0) { |
||
114 | //Return to main.
|
||
115 | current_task = 0;
|
||
116 | } |
||
117 | 1544 | justin | } |
118 | |||
119 | void store_task(void) { |
||
120 | //Store all state for this task.
|
||
121 | |||
122 | /* store general purpose registers */
|
||
123 | asm volatile( \ |
||
124 | "push r31 \n" \
|
||
125 | "push r30 \n" \
|
||
126 | "push r29 \n" \
|
||
127 | "push r28 \n" \
|
||
128 | "push r27 \n" \
|
||
129 | "push r26 \n" \
|
||
130 | "push r25 \n" \
|
||
131 | "push r24 \n" \
|
||
132 | "push r23 \n" \
|
||
133 | "push r22 \n" \
|
||
134 | "push r21 \n" \
|
||
135 | "push r20 \n" \
|
||
136 | "push r19 \n" \
|
||
137 | "push r18 \n" \
|
||
138 | "push r17 \n" \
|
||
139 | "push r16 \n" \
|
||
140 | "push r15 \n" \
|
||
141 | "push r14 \n" \
|
||
142 | "push r13 \n" \
|
||
143 | "push r12 \n" \
|
||
144 | "push r11 \n" \
|
||
145 | "push r10 \n" \
|
||
146 | "push r9 \n" \
|
||
147 | "push r8 \n" \
|
||
148 | "push r7 \n" \
|
||
149 | "push r6 \n" \
|
||
150 | "push r5 \n" \
|
||
151 | "push r4 \n" \
|
||
152 | "push r3 \n" \
|
||
153 | "push r2 \n" \
|
||
154 | "push r1 \n" \
|
||
155 | "push r0 \n");
|
||
156 | |||
157 | //Store status register.
|
||
158 | asm volatile( \ |
||
159 | "in r0, __SREG__ \n" \
|
||
160 | "push r0 \n");
|
||
161 | |||
162 | //Store sp.
|
||
163 | uint16_t sploc = (uint16_t)&(PCB[current_task].sp); |
||
164 | asm volatile( \ |
||
165 | "in r0, __SP_L__ \n" \
|
||
166 | "st Z+, r0 \n" \
|
||
167 | "in r0, __SP_H__ \n" \
|
||
168 | "st Z, r0 \n" : : "e" (sploc)); |
||
169 | |||
170 | } |
||
171 | |||
172 | //Figure out which task to run next, then run it.
|
||
173 | void restore_task(void) { |
||
174 | |||
175 | //Again, we never return.
|
||
176 | asm volatile( \ |
||
177 | "pop r0 \n" \
|
||
178 | "pop r0 \n");
|
||
179 | |||
180 | /* restore stack pointer */
|
||
181 | uint16_t sp = (uint16_t)PCB[current_task].sp; |
||
182 | asm volatile( \ |
||
183 | "out __SP_L__, %A0 \n" \
|
||
184 | "out __SP_H__, %B0 \n" : : "d" (sp)); |
||
185 | |||
186 | /* restore status register */
|
||
187 | asm volatile( \ |
||
188 | "pop r0 \n" \
|
||
189 | "out __SREG__, r0 \n");
|
||
190 | |||
191 | /* restore general purpose registers */
|
||
192 | asm volatile( \ |
||
193 | "pop r0 \n" \
|
||
194 | "pop r1 \n" \
|
||
195 | "pop r2 \n" \
|
||
196 | "pop r3 \n" \
|
||
197 | "pop r4 \n" \
|
||
198 | "pop r5 \n" \
|
||
199 | "pop r6 \n" \
|
||
200 | "pop r7 \n" \
|
||
201 | "pop r8 \n" \
|
||
202 | "pop r9 \n" \
|
||
203 | "pop r10 \n" \
|
||
204 | "pop r11 \n" \
|
||
205 | "pop r12 \n" \
|
||
206 | "pop r13 \n" \
|
||
207 | "pop r14 \n" \
|
||
208 | "pop r15 \n" \
|
||
209 | "pop r16 \n" \
|
||
210 | "pop r17 \n" \
|
||
211 | "pop r18 \n" \
|
||
212 | "pop r19 \n" \
|
||
213 | "pop r20 \n" \
|
||
214 | "pop r21 \n" \
|
||
215 | "pop r22 \n" \
|
||
216 | "pop r23 \n" \
|
||
217 | "pop r24 \n" \
|
||
218 | "pop r25 \n" \
|
||
219 | "pop r26 \n" \
|
||
220 | "pop r27 \n" \
|
||
221 | "pop r28 \n" \
|
||
222 | "pop r29 \n" \
|
||
223 | "pop r30 \n" \
|
||
224 | "pop r31 \n");
|
||
225 | |||
226 | /* start process and enable interrupts */
|
||
227 | //Note that either create_launch_stack or a timer interrupt
|
||
228 | //already has stored the value to load into PC
|
||
229 | asm volatile("reti \n"); |
||
230 | } |
||
231 | |||
232 | void restore_next_task(void) { |
||
233 | //Again, we never return.
|
||
234 | asm volatile( \ |
||
235 | "pop r0 \n" \
|
||
236 | "pop r0 \n");
|
||
237 | |||
238 | 1545 | justin | PCB[current_task].running = 0;
|
239 | 1544 | justin | scheduler(); |
240 | 1545 | justin | PCB[current_task].running = 1;
|
241 | 1544 | justin | restore_task(); |
242 | } |
||
243 | |||
244 | //I need a timer to do the rest. Thinking about stealing it from rtc.
|
||
245 | //ISR needs to
|
||
246 | //a) store_task
|
||
247 | //b) restore_next_task.
|
||
248 |