Colony - / - Diff - Robotics Club

Revision 1573

      * @brief Scheduler
+     *
      * Implementation of functions for scheduler
      * Currently relies on avr-gcc's interrupt call convention.
+     *
      * @author Colony Project, CMU Robotics Club
      * Based on avrOS and 18348 Lab9 code
      **/
     #include <avr/interrupt.h>
     #include <util/delay.h>
     #include "scheduler.h"
     #include "time.h"
     static uint8_t STACK[MAXTASKS][STACKSIZE];
     #define DEBUG
     #ifdef DEBUG
     #include <serial.h>
     #endif //DEBUG
     static uint8_t STACK[MAXTASKS + 1][STACKSIZE];
     static uint8_t nactive_tasks = 1;
     static uint8_t current_task = 0; //Default to main.
     //Internal functions
     void task_terminate(void);
     static void task_terminate(void);
     //Scheduler gets called from the interrupt.
     void scheduler(void);
     void create_launch_stack(uint8_t task);
     void store_task(void);
     void restore_task(void);
     void restore_next_task(void);
     //TODO Use a context struct instead of pushing onto the stack.
     //so we don't need to worry so much about stack overflow.
     typedef struct context_t {
     	uint8_t reg[32];
     	uint8_t sreg;
     } context_t;
     //Define some PCB's
     typedef struct PCB_t
+    {
     	void (*exec)(void);
-...
     PCB_t PCB[MAXTASKS + 1];
     void scheduler_init() { }
     //Keep track of global time.
     static uint32_t current_time = 0;
     void task_terminate(void) {
     unsigned int time_now() {
     	return current_time;
+    }
     void scheduler_init() {
     	//Set the counter to 0.
     	TCNT3 = 0;
     	//Clear timer on compare (CTC) mode.
     	TCCR3B |= _BV(WGM32);
     	//Set a prescalar of 8. 8 / (8MHz) = 1us per tick.
     	TCCR3B |= _BV(CS31);
     	//Trigger an interrupt every 16th second for now.
     	OCR3A = 0xF424;
     	//Enable Output Compare A Interrupt. (Begins immediately).
       	ETIMSK |= _BV(OCIE3A);
     #ifdef DEBUG
     	usb_init();
     #endif //DEBUG
     	sei();
+    }
     static void task_terminate(void) {
     #ifdef DEBUG
     	usb_puts("In task terminate!.\n\r");
     #endif //DEBUG
     	PCB[current_task].running = 0;
     	yield();
+    }
-...
     	PCB[nactive_tasks].exec = exec;
     	PCB[nactive_tasks].period = period;
     	PCB[nactive_tasks].next = rtc_get() + period;
     	PCB[nactive_tasks].next = current_time + period;
     	PCB[nactive_tasks].running = 0;
     	//Don't need to initialize SP, it will get done later.
-...
     	return nactive_tasks - 1;
+    }
     //Create a fresh launch stack.
     void create_launch_stack(uint8_t task) {
     	uint8_t* sp = &STACK[task][STACKSIZE - 1];
     	//Put task terminate  and the task to execute on the stack.
     	*(sp--) = (uint8_t)(uint16_t) *task_terminate;
     	*(sp--) = (uint8_t)(uint16_t) *task_terminate >> 8;
     	*(sp--) = (uint8_t)(uint16_t) *PCB[nactive_tasks].exec;
     	*(sp--) = (uint8_t)(uint16_t) *PCB[nactive_tasks].exec >> 8;
     	//This is going to get me in trouble,
     	//but store_task already does everything else we need
     	//(since we only care about void-void functions)
     	store_task();
+    }
     void scheduler(void){
     //I need a timer to do the rest.  Thinking about stealing it from rtc.
     SIGNAL(TIMER3_COMPA_vect) {
     	static uint8_t task_i = 1;
     	volatile uint8_t** sploc;
     	volatile uint8_t* sp;
     	uint8_t i;
     	int current_time = rtc_get();
     #ifdef DEBUG
     	char test_buf[128];
     #endif //DEBUG
     	//Store sp.
     	sploc = &(PCB[current_task].sp);
     	asm volatile( \
     		"in r0, __SP_L__ \n" \
     	   	"st %a0+, r0 \n" \
     	    "in r0, __SP_H__ \n" \
     		"st %a0, r0 \n"  : : "e" (sploc));
     #ifdef DEBUG
     	sprintf(test_buf, "current_task = %d, current_time = %d, sp = %x\n\r", current_task, current_time, sploc);
     	usb_puts(test_buf);
     #endif //DEBUG
     	//The current task may no longer be running when we exit.
     	PCB[current_task].running = 0;
     	//Keep track of the number of 1/16 second's that have passed.
     	current_time++;
     	/******** scheduler ********/
     	//Loop over registered tasks, like in round robin order.
     	for(i = nactive_tasks; i > 0; i--) {
     		if(		!PCB[task_i].running
     			&&   PCB[task_i].next <= current_time) {
     	for(i = nactive_tasks - 1; i > 0; i--) {
     		//Launch a new task if it is due to run.
     		if(	!PCB[task_i].running
     		  && PCB[task_i].next <= current_time) {
     			task_i++;
     			current_task = task_i;
     			PCB[task_i].next += PCB[task_i].period;
     			create_launch_stack(task_i);
     			task_i++;
     			/***** create launch stack ***/
     #ifdef DEBUG
     			usb_puts("Switching tasks..... (cross fingers)\n\r");
     #endif //DEBUG
     			sp = &(STACK[task_i][STACKSIZE - 1]);
     			//Put task terminate  and the task to execute on the stack.
     			*(sp--) = (uint8_t)(uint16_t) task_terminate;
     			*(sp--) = (uint8_t)(uint16_t) task_terminate >> 8;
     			*(sp--) = (uint8_t)(uint16_t) PCB[nactive_tasks].exec;
     			*(sp--) = (uint8_t)(uint16_t) PCB[nactive_tasks].exec >> 8;
     			//The current state of the registers is what the tasks will
     			//see on entry (shouldn't matter).
     			asm volatile( \
     				"out __SP_L__, %A0 \n" \
     				"out __SP_H__, %B0 \n" : : "d" (sp));
     #ifdef DEBUG
     			sprintf(test_buf, "Switching tasks,  sp = %x\n\r", sp);
     			usb_puts(test_buf);
     #endif //DEBUG
     	 		/* start process and enable interrupts */
     	 		asm volatile("reti \n");
     			break;
+    		}
     		if( 	PCB[task_i].running ) {
     		//Continue an old task.
     		if(PCB[task_i].running) {
     			current_task = task_i;
     			//Reset the stack pointer.
     			sp = PCB[current_task].sp;
     			asm volatile( \
     				"out __SP_L__, %A0 \n" \
     				"out __SP_H__, %B0 \n" : : "d" (sp));
     			task_i++;
     			break;
+    		}
     		//Loop back to 0 if necessary.
     		//Loop back to 1 if necessary.
     		task_i++;
     		if(task_i >= nactive_tasks)
     		if(task_i >= nactive_tasks)
     			task_i = 1;
+    	}
     	//If no task was selected to run,
     	//If no task was selected to run, go to main
     	if(i == 0) {
     		//Return to main.
     #ifdef DEBUG
     		usb_puts("Resetting main.\n\r");
     #endif //DEBUG
     		//Set the SP to the original stack.
     		sp = PCB[current_task].sp;
     		asm volatile( \
     			"out __SP_L__, %A0 \n" \
     			"out __SP_H__, %B0 \n" : : "d" (sp));
     		current_task = 0;
+    	}
+    }
     void store_task(void) {
     	//Store all state for this task.
     	//Set the running task.*/
     	PCB[current_task].running = 1;
     	/* store general purpose registers */
     	asm volatile( \
     	 	"push r31 \n" \
     	   	"push r30 \n" \
     	    "push r29 \n" \
     		"push r28 \n" \
     		"push r27 \n" \
     		"push r26 \n" \
     		"push r25 \n" \
     		"push r24 \n" \
     		"push r23 \n" \
     		"push r22 \n" \
     		"push r21 \n" \
     		"push r20 \n" \
     		"push r19 \n" \
     		"push r18 \n" \
     		"push r17 \n" \
     		"push r16 \n" \
     		"push r15 \n" \
     		"push r14 \n" \
     		"push r13 \n" \
     		"push r12 \n" \
     		"push r11 \n" \
     		"push r10 \n" \
     		"push r9 \n" \
     		"push r8 \n" \
     		"push r7 \n" \
     		"push r6 \n" \
     		"push r5 \n" \
     		"push r4 \n" \
     		"push r3 \n" \
     		"push r2 \n" \
     		"push r1 \n" \
     		"push r0 \n");
     	 //Store status register.
     	asm volatile( \
     	  	"in r0, __SREG__ \n" \
     	   	"push r0 \n");
     	//Store sp.
     	uint16_t sploc = (uint16_t)&(PCB[current_task].sp);
     	asm volatile( \
     		"in r0, __SP_L__ \n" \
     	   	"st Z+, r0 \n" \
     	    "in r0, __SP_H__ \n" \
     		"st Z, r0 \n"  : : "e" (sploc));
     	//avr-gcc inserts all of the necessary pops and the reti here.
+    }
     //Figure out which task to run next, then run it.
     void restore_task(void) {
     	//Again, we never return.
     	asm volatile( \
     	 	"pop r0 \n" \
     	 	"pop r0 \n");
     	/* restore stack pointer */
     	uint16_t sp = (uint16_t)PCB[current_task].sp;
     	asm volatile( \
     		"out __SP_L__, %A0 \n" \
     		"out __SP_H__, %B0 \n" : : "d" (sp));
     	/* restore status register */
     	 asm volatile( \
     	  	"pop r0 \n" \
     	   	"out __SREG__, r0 \n");
     	 /* restore general purpose registers */
     	 asm volatile( \
     	 	"pop r0 \n" \
     	 	"pop r1 \n" \
     		"pop r2 \n" \
     		"pop r3 \n" \
     		"pop r4 \n" \
     		"pop r5 \n" \
     		"pop r6 \n" \
     		"pop r7 \n" \
     		"pop r8 \n" \
     		"pop r9 \n" \
     		"pop r10 \n" \
     		"pop r11 \n" \
     		"pop r12 \n" \
     		"pop r13 \n" \
     		"pop r14 \n" \
     		"pop r15 \n" \
     		"pop r16 \n" \
     		"pop r17 \n" \
     		"pop r18 \n" \
     		"pop r19 \n" \
     		"pop r20 \n" \
     		"pop r21 \n" \
     		"pop r22 \n" \
     		"pop r23 \n" \
     		"pop r24 \n" \
     		"pop r25 \n" \
     		"pop r26 \n" \
     		"pop r27 \n" \
     		"pop r28 \n" \
     		"pop r29 \n" \
     		"pop r30 \n" \
     		"pop r31 \n");
     	 /* start process and enable interrupts */
     	 //Note that either create_launch_stack or a timer interrupt
     	 //already has stored the value to load into PC
     	 asm volatile("reti \n");
+    }
     void restore_next_task(void) {
     	//Again, we never return.
     	asm volatile( \
     	 	"pop r0 \n" \
     	 	"pop r0 \n");
     	PCB[current_task].running = 0;
     	scheduler();
     	PCB[current_task].running = 1;
     	restore_task();
+    }
     //I need a timer to do the rest.  Thinking about stealing it from rtc.
     //ISR needs to
     //a) store_task
     //b) restore_next_task.

     #ifndef _SCHEDULER_H_
     #define _SCHEDULER_H_
     #define STACKSIZE 64
     #define MAXTASKS 16
     #define STACKSIZE 128
     #define MAXTASKS 3
     #define PRIORITY_LOWEST 8
     #include <stdint.h>
     void scheduler_init(void);
     void yield(void);
     int register_task(void (*exec)(void), uint16_t period);
     unsigned int time_now(void);
     #endif

     #include <dragonfly_lib.h>
     #include <serial.h>
     #include <avr/interrupt.h>
     #include "scheduler.h"
     void task0(void);
     void task1(void);
     void task2(void);
     int main(void)
+    {
     	//Granularity is currently seconds,
     	usb_init();
     	/* initialize components, set wireless channel */
     	dragonfly_init(ALL_ON);
     	int val;
     	usb_puts("Oh no!!! Reset!!\n\r");
     	scheduler_init();
     	register_task(task0, 22);
     	//register_task(task1, 2);
     	//register_task(task2, 3);
     	while (1) {
     		usb_puts("Val: ");
     		bom_refresh(BOM_ALL);
     		usb_puti(bom_get_max());
     		usb_putc('\n');
     		//usb_puti(time_now());
     		usb_puts("main\n\r");
     		delay_ms(200);
+    	}
     	return 0;
+    }
     void task0() {
     	sei();
     	usb_puts("0\n\r");
     	cli();
+    }
     void task1() {
     	sei();
     	usb_puts("1\n\r");
     	cli();
+    }
     void task2() {
     	sei();
     	usb_puts("2\n\r");
     	cli();
+    }

Also available in: Unified diff