Statistics
| Revision:

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