root / trunk / code / projects / scheduler / scheduler.c @ 1546
History  View  Annotate  Download (4.93 KB)
1 
/**


2 
* @file scheduler.c

3 
* @brief Scheduler

4 
*

5 
* Implementation of functions for scheduler

6 
*

7 
* @author Colony Project, CMU Robotics Club

8 
* Based on avrOS and 18348 Lab9 code

9 
**/

10  
11 
#include "scheduler.h" 
12 
#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 voidvoid functions)

80 
store_task(); 
81 
} 
82  
83 
void scheduler(void){ 
84 
static uint8_t task_i = 1; 
85 
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 
create_launch_stack(task_i); 
96 
task_i++; 
97 
break;

98 
} 
99  
100 
if( PCB[task_i].running ) {

101 
current_task = task_i; 
102 
task_i++; 
103 
break;

104 
} 
105 

106 
//Loop back to 0 if necessary.

107 
task_i++; 
108 
if(task_i >= nactive_tasks)

109 
task_i = 1;

110 
} 
111 

112 
//If no task was selected to run,

113 
if(i == 0) { 
114 
//Return to main.

115 
current_task = 0;

116 
} 
117 
} 
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 
PCB[current_task].running = 0;

239 
scheduler(); 
240 
PCB[current_task].running = 1;

241 
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  
249 