Colony

 * Based on avrOS and 18348 Lab9 code

#include "time.h"

static uint8_t STACK[MAXTASKS][STACKSIZE];

static uint8_t nactive_tasks = 1;

static uint8_t current_task = 0; //Default to main.

//Internal functions 

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

typedef struct PCB_t

{

	void (*exec)(void);

	uint8_t* sp;

	char running;

	uint16_t period; //Interval in clock cycles.

	uint32_t next;

} PCB_t;

PCB_t PCB[MAXTASKS + 1];

void scheduler_init() { }

void task_terminate(void) { 

	PCB[current_task].running = 0;

	yield();

}

void yield() {

	//TODO Actually implement.

	for(;;){}

}

int register_task(void (*exec)(void), uint16_t period)

{

	if(nactive_tasks >= MAXTASKS) { 

		return -1;

	}

	PCB[nactive_tasks].exec = exec;

	PCB[nactive_tasks].period = period;

	PCB[nactive_tasks].next = rtc_get() + period;

	PCB[nactive_tasks].running = 0;	

	//Don't need to initialize SP, it will get done later.

	nactive_tasks++;

	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){

	static uint8_t task_i;

	uint8_t i;

	int current_time = rtc_get();

	//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) { 

			current_task = task_i;	

			PCB[task_i].next += PCB[task_i].period;

			task_i++;

			break;

		}

		//Loop back to 0 if necessary.

		task_i++;

		if(task_i >= nactive_tasks) 

			task_i = 0;

	}

}

void store_task(void) { 

	//Store all state for this task.

	/* 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));

}

//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");

	scheduler();

	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.