Revision 1544 trunk/code/projects/scheduler/scheduler.c

View differences:

scheduler.c
5 5
 * Implementation of functions for scheduler
6 6
 *
7 7
 * @author Colony Project, CMU Robotics Club
8
 * Based on avrOS and 18348 Lab9 code
8 9
 **/
9 10

  
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 void-void 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

  

Also available in: Unified diff