Project

General

Profile

Revision 1573

svn is being wonky...

Changes to the scheduler, still doesn't work.
avr-gcc has its own convention for saving state
on entry and exit that's the same every time, so
I lean on this now to restore the context.

I think that it's very close,
my guess is that when new tasks are launched, RETI is
sending it somewhere random and it's causing a reset.

View differences:

trunk/code/projects/scheduler/scheduler.c
3 3
 * @brief Scheduler
4 4
 *
5 5
 * Implementation of functions for scheduler
6
 * Currently relies on avr-gcc's interrupt call convention.
6 7
 *
7 8
 * @author Colony Project, CMU Robotics Club
8 9
 * Based on avrOS and 18348 Lab9 code
9 10
 **/
10 11

  
12
#include <avr/interrupt.h>
13
#include <util/delay.h>
11 14
#include "scheduler.h"
12
#include "time.h"
13 15

  
14
static uint8_t STACK[MAXTASKS][STACKSIZE];
16
#define DEBUG
17

  
18
#ifdef DEBUG
19
#include <serial.h>
20
#endif //DEBUG
21

  
22
static uint8_t STACK[MAXTASKS + 1][STACKSIZE];
15 23
static uint8_t nactive_tasks = 1;
16 24
static uint8_t current_task = 0; //Default to main.
17 25

  
18 26
//Internal functions 
19
void task_terminate(void);
27
static void task_terminate(void);
20 28

  
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); 
29
//TODO Use a context struct instead of pushing onto the stack.
30
//so we don't need to worry so much about stack overflow.
31
typedef struct context_t {
32
	uint8_t reg[32];
33
	uint8_t sreg;
34
} context_t;
27 35

  
36
//Define some PCB's
28 37
typedef struct PCB_t
29 38
{
30 39
	void (*exec)(void);
......
36 45

  
37 46
PCB_t PCB[MAXTASKS + 1];
38 47

  
39
void scheduler_init() { }
48
//Keep track of global time.
49
static uint32_t current_time = 0;
40 50

  
41
void task_terminate(void) { 
51
unsigned int time_now() { 
52
	return current_time;
53
}
54

  
55
void scheduler_init() { 
56
	//Set the counter to 0.
57
	TCNT3 = 0;
58
	
59
	//Clear timer on compare (CTC) mode.
60
	TCCR3B |= _BV(WGM32);
61

  
62
	//Set a prescalar of 8. 8 / (8MHz) = 1us per tick.
63
	TCCR3B |= _BV(CS31);
64

  
65
	//Trigger an interrupt every 16th second for now.
66
	OCR3A = 0xF424;
67

  
68
	//Enable Output Compare A Interrupt. (Begins immediately).
69
  	ETIMSK |= _BV(OCIE3A);
70

  
71
#ifdef DEBUG
72
	usb_init();
73
#endif //DEBUG
74
	
75
	sei();
76
}
77

  
78
static void task_terminate(void) { 
79
#ifdef DEBUG
80
	usb_puts("In task terminate!.\n\r");
81
#endif //DEBUG
42 82
	PCB[current_task].running = 0;
43 83
	yield();
44 84
}
......
56 96
	
57 97
	PCB[nactive_tasks].exec = exec;
58 98
	PCB[nactive_tasks].period = period;
59
	PCB[nactive_tasks].next = rtc_get() + period;
99
	PCB[nactive_tasks].next = current_time + period;
60 100
	PCB[nactive_tasks].running = 0;	
61 101
	
62 102
	//Don't need to initialize SP, it will get done later.
......
64 104
	return nactive_tasks - 1;
65 105
}
66 106

  
67
//Create a fresh launch stack. 
68
void create_launch_stack(uint8_t task) {
69
	uint8_t* sp = &STACK[task][STACKSIZE - 1];
107
//I need a timer to do the rest.  Thinking about stealing it from rtc.
108
SIGNAL(TIMER3_COMPA_vect) {
109
	static uint8_t task_i = 1;
110
	volatile uint8_t** sploc;
111
	volatile uint8_t* sp;
70 112
	
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;
113
	uint8_t i;
76 114
	
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
}
115
#ifdef DEBUG
116
	char test_buf[128];
117
#endif //DEBUG
118
	
119
	//Store sp.
120
	sploc = &(PCB[current_task].sp);
121
	
122
	asm volatile( \
123
		"in r0, __SP_L__ \n" \
124
	   	"st %a0+, r0 \n" \
125
	    "in r0, __SP_H__ \n" \
126
		"st %a0, r0 \n"  : : "e" (sploc));
127
	
128
#ifdef DEBUG
129
	sprintf(test_buf, "current_task = %d, current_time = %d, sp = %x\n\r", current_task, current_time, sploc);
130
	usb_puts(test_buf);
131
#endif //DEBUG
132
	
133
	//The current task may no longer be running when we exit.
134
	PCB[current_task].running = 0;
135
	
136
	//Keep track of the number of 1/16 second's that have passed.
137
	current_time++;
138
	
139
	/******** scheduler ********/
82 140

  
83
void scheduler(void){
84
	static uint8_t task_i = 1;
85
	uint8_t i;
86
	int current_time = rtc_get();
87
	
88 141
	//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) { 
142
	for(i = nactive_tasks - 1; i > 0; i--) { 
143
		
144
		//Launch a new task if it is due to run.
145
		if(	!PCB[task_i].running
146
		  && PCB[task_i].next <= current_time) { 
92 147
			
148
			task_i++;
149
			
93 150
			current_task = task_i;	
94 151
			PCB[task_i].next += PCB[task_i].period;
95
			create_launch_stack(task_i);
96
			task_i++;
152
		
153
			/***** create launch stack ***/
154
#ifdef DEBUG
155
			usb_puts("Switching tasks..... (cross fingers)\n\r");
156
#endif //DEBUG
157

  
158
			sp = &(STACK[task_i][STACKSIZE - 1]);
159
			
160
			//Put task terminate  and the task to execute on the stack.
161
			*(sp--) = (uint8_t)(uint16_t) task_terminate;
162
			*(sp--) = (uint8_t)(uint16_t) task_terminate >> 8;
163
			*(sp--) = (uint8_t)(uint16_t) PCB[nactive_tasks].exec;
164
			*(sp--) = (uint8_t)(uint16_t) PCB[nactive_tasks].exec >> 8;
165
			
166
			//The current state of the registers is what the tasks will
167
			//see on entry (shouldn't matter).
168

  
169
			asm volatile( \
170
				"out __SP_L__, %A0 \n" \
171
				"out __SP_H__, %B0 \n" : : "d" (sp));
172

  
173
#ifdef DEBUG
174
			sprintf(test_buf, "Switching tasks,  sp = %x\n\r", sp);
175
			usb_puts(test_buf);
176
#endif //DEBUG
177

  
178
	 		/* start process and enable interrupts */
179
	 		asm volatile("reti \n");
97 180
			break;
98 181
		}
182
		
183
		//Continue an old task.
184
		if(PCB[task_i].running) {
185
			current_task = task_i;
186
			
187
			//Reset the stack pointer.
188
			sp = PCB[current_task].sp;
189
			asm volatile( \
190
				"out __SP_L__, %A0 \n" \
191
				"out __SP_H__, %B0 \n" : : "d" (sp));
99 192

  
100
		if( 	PCB[task_i].running ) {
101
			current_task = task_i;
102 193
			task_i++;
103 194
			break;
104 195
		}
105 196
		
106
		//Loop back to 0 if necessary.
197
		//Loop back to 1 if necessary.
107 198
		task_i++;
108
		if(task_i >= nactive_tasks) 
199
		if(task_i >= nactive_tasks)  
109 200
			task_i = 1;
110 201
	}
111 202
	
112
	//If no task was selected to run,
203
	//If no task was selected to run, go to main
113 204
	if(i == 0) { 
114
		//Return to main.
205
#ifdef DEBUG
206
		usb_puts("Resetting main.\n\r");
207
#endif //DEBUG
208
		//Set the SP to the original stack.
209
		sp = PCB[current_task].sp;
210
		asm volatile( \
211
			"out __SP_L__, %A0 \n" \
212
			"out __SP_H__, %B0 \n" : : "d" (sp));
213
		
115 214
		current_task = 0;	
116 215
	}
117
}
118

  
119
void store_task(void) { 
120
	//Store all state for this task.
216
		
217
	//Set the running task.*/
218
	PCB[current_task].running = 1;
121 219
	
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

  
220
	//avr-gcc inserts all of the necessary pops and the reti here.
170 221
}
171 222

  
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

  
trunk/code/projects/scheduler/scheduler.h
12 12
#ifndef _SCHEDULER_H_
13 13
#define _SCHEDULER_H_
14 14

  
15
#define STACKSIZE 64
16
#define MAXTASKS 16
15
#define STACKSIZE 128
16
#define MAXTASKS 3
17 17

  
18
#define PRIORITY_LOWEST 8
19

  
20 18
#include <stdint.h>
21 19

  
22 20
void scheduler_init(void);
23 21
void yield(void);
22

  
24 23
int register_task(void (*exec)(void), uint16_t period);
25 24

  
25
unsigned int time_now(void);
26

  
26 27
#endif
trunk/code/projects/scheduler/main.c
1
#include <dragonfly_lib.h>
1
#include <serial.h>
2
#include <avr/interrupt.h>
2 3
#include "scheduler.h"
3 4

  
5
void task0(void);
6
void task1(void);
7
void task2(void);
8

  
4 9
int main(void)
5 10
{
11
	//Granularity is currently seconds, 
12
	usb_init();
6 13

  
7
	/* initialize components, set wireless channel */
8
	dragonfly_init(ALL_ON);
9
	
10
	int val;
14
	usb_puts("Oh no!!! Reset!!\n\r");
11 15

  
16
	scheduler_init();
17
	register_task(task0, 22);
18
	//register_task(task1, 2);
19
	//register_task(task2, 3);
20

  
12 21
	while (1) {
13

  
14
		usb_puts("Val: ");
15
		bom_refresh(BOM_ALL);
16
		usb_puti(bom_get_max());
17

  
18
		usb_putc('\n');
19

  
22
		//usb_puti(time_now());
23
		usb_puts("main\n\r");
20 24
		delay_ms(200);
21

  
22 25
	}
23 26

  
24 27
	return 0;
25 28
}
26 29

  
30
void task0() { 
31
	sei();
32
	usb_puts("0\n\r");
33
	
34
	cli();
35
}
36

  
37
void task1() { 
38
	sei();
39
	usb_puts("1\n\r");
40
	cli();
41
}
42

  
43
void task2() { 
44
	sei();
45
	usb_puts("2\n\r");
46
	cli();
47
}

Also available in: Unified diff