root / trunk / code / projects / scheduler / scheduler.c @ 1544
History  View  Annotate  Download (4.66 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;

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 
task_i++; 
96  
97 
break;

98 
} 
99 

100 
//Loop back to 0 if necessary.

101 
task_i++; 
102 
if(task_i >= nactive_tasks)

103 
task_i = 0;

104 
} 
105 
} 
106  
107 
void store_task(void) { 
108 
//Store all state for this task.

109 

110 
/* store general purpose registers */

111 
asm volatile( \ 
112 
"push r31 \n" \

113 
"push r30 \n" \

114 
"push r29 \n" \

115 
"push r28 \n" \

116 
"push r27 \n" \

117 
"push r26 \n" \

118 
"push r25 \n" \

119 
"push r24 \n" \

120 
"push r23 \n" \

121 
"push r22 \n" \

122 
"push r21 \n" \

123 
"push r20 \n" \

124 
"push r19 \n" \

125 
"push r18 \n" \

126 
"push r17 \n" \

127 
"push r16 \n" \

128 
"push r15 \n" \

129 
"push r14 \n" \

130 
"push r13 \n" \

131 
"push r12 \n" \

132 
"push r11 \n" \

133 
"push r10 \n" \

134 
"push r9 \n" \

135 
"push r8 \n" \

136 
"push r7 \n" \

137 
"push r6 \n" \

138 
"push r5 \n" \

139 
"push r4 \n" \

140 
"push r3 \n" \

141 
"push r2 \n" \

142 
"push r1 \n" \

143 
"push r0 \n");

144 

145 
//Store status register.

146 
asm volatile( \ 
147 
"in r0, __SREG__ \n" \

148 
"push r0 \n");

149  
150 
//Store sp.

151 
uint16_t sploc = (uint16_t)&(PCB[current_task].sp); 
152 
asm volatile( \ 
153 
"in r0, __SP_L__ \n" \

154 
"st Z+, r0 \n" \

155 
"in r0, __SP_H__ \n" \

156 
"st Z, r0 \n" : : "e" (sploc)); 
157  
158 
} 
159  
160 
//Figure out which task to run next, then run it.

161 
void restore_task(void) { 
162 

163 
//Again, we never return.

164 
asm volatile( \ 
165 
"pop r0 \n" \

166 
"pop r0 \n");

167  
168 
/* restore stack pointer */

169 
uint16_t sp = (uint16_t)PCB[current_task].sp; 
170 
asm volatile( \ 
171 
"out __SP_L__, %A0 \n" \

172 
"out __SP_H__, %B0 \n" : : "d" (sp)); 
173  
174 
/* restore status register */

175 
asm volatile( \ 
176 
"pop r0 \n" \

177 
"out __SREG__, r0 \n");

178  
179 
/* restore general purpose registers */

180 
asm volatile( \ 
181 
"pop r0 \n" \

182 
"pop r1 \n" \

183 
"pop r2 \n" \

184 
"pop r3 \n" \

185 
"pop r4 \n" \

186 
"pop r5 \n" \

187 
"pop r6 \n" \

188 
"pop r7 \n" \

189 
"pop r8 \n" \

190 
"pop r9 \n" \

191 
"pop r10 \n" \

192 
"pop r11 \n" \

193 
"pop r12 \n" \

194 
"pop r13 \n" \

195 
"pop r14 \n" \

196 
"pop r15 \n" \

197 
"pop r16 \n" \

198 
"pop r17 \n" \

199 
"pop r18 \n" \

200 
"pop r19 \n" \

201 
"pop r20 \n" \

202 
"pop r21 \n" \

203 
"pop r22 \n" \

204 
"pop r23 \n" \

205 
"pop r24 \n" \

206 
"pop r25 \n" \

207 
"pop r26 \n" \

208 
"pop r27 \n" \

209 
"pop r28 \n" \

210 
"pop r29 \n" \

211 
"pop r30 \n" \

212 
"pop r31 \n");

213  
214 
/* start process and enable interrupts */

215 
//Note that either create_launch_stack or a timer interrupt

216 
//already has stored the value to load into PC

217 
asm volatile("reti \n"); 
218 
} 
219  
220 
void restore_next_task(void) { 
221 
//Again, we never return.

222 
asm volatile( \ 
223 
"pop r0 \n" \

224 
"pop r0 \n");

225 

226 
scheduler(); 
227 
restore_task(); 
228 
} 
229  
230 
//I need a timer to do the rest. Thinking about stealing it from rtc.

231 
//ISR needs to

232 
//a) store_task

233 
//b) restore_next_task.

234  
235 