Statistics
| Revision:

root / trunk / code / projects / scheduler / scheduler.c @ 1573

History | View | Annotate | Download (4.71 KB)

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

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

    
16
#define DEBUG
17

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

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

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

    
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;
35

    
36
//Define some PCB's
37
typedef struct PCB_t
38
{
39
        void (*exec)(void);
40
        uint8_t* sp;
41
        char running;
42
        uint16_t period; //Interval in clock cycles.
43
        uint32_t next;
44
} PCB_t;
45

    
46
PCB_t PCB[MAXTASKS + 1];
47

    
48
//Keep track of global time.
49
static uint32_t current_time = 0;
50

    
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
82
        PCB[current_task].running = 0;
83
        yield();
84
}
85

    
86
void yield() {
87
        //TODO Actually implement.
88
        for(;;){}
89
}
90

    
91
int register_task(void (*exec)(void), uint16_t period)
92
{
93
        if(nactive_tasks >= MAXTASKS) { 
94
                return -1;
95
        }
96
        
97
        PCB[nactive_tasks].exec = exec;
98
        PCB[nactive_tasks].period = period;
99
        PCB[nactive_tasks].next = current_time + period;
100
        PCB[nactive_tasks].running = 0;        
101
        
102
        //Don't need to initialize SP, it will get done later.
103
        nactive_tasks++;
104
        return nactive_tasks - 1;
105
}
106

    
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;
112
        
113
        uint8_t i;
114
        
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 ********/
140

    
141
        //Loop over registered tasks, like in round robin order.
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) { 
147
                        
148
                        task_i++;
149
                        
150
                        current_task = task_i;        
151
                        PCB[task_i].next += PCB[task_i].period;
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");
180
                        break;
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));
192

    
193
                        task_i++;
194
                        break;
195
                }
196
                
197
                //Loop back to 1 if necessary.
198
                task_i++;
199
                if(task_i >= nactive_tasks)  
200
                        task_i = 1;
201
        }
202
        
203
        //If no task was selected to run, go to main
204
        if(i == 0) { 
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
                
214
                current_task = 0;        
215
        }
216
                
217
        //Set the running task.*/
218
        PCB[current_task].running = 1;
219
        
220
        //avr-gcc inserts all of the necessary pops and the reti here.
221
}
222