int rtos_AddThreads (void(*task0)(void),void(*task1)(void),void(*task2)(void))
{
//Using Circular Linked-List for chaining Threads
tcbs[0].nextThread = &tcbs[1]; //Thread 0 points to Thread 1
tcbs[1].nextThread = &tcbs[2]; //Thread 1 points to Thread 2
tcbs[2].nextThread = &tcbs[0]; //Thread 2 points to Thread 0
setInitialStack(0);//Set Software Stack for Thread 0
stacks[0][STACKSIZE-2] = (uint32_t)(task0);//PC for Thread 0
setInitialStack(1);//Set Software Stack for Thread 1
stacks[1][STACKSIZE-2] = (uint32_t)(task1);//PC for Thread 1
setInitialStack(2);//Set Software Stack for Thread 2
stacks[2][STACKSIZE-2] = (uint32_t)(task2);//PC for Thread 2
currentThread = &tcbs[0];//Thread 0 will run first
return 1;
}
int OS_AddThread(void(*task)(void), unsigned long stackSize, unsigned long priority) {
long status;
int i;
int index;
status = StartCritical();
if(NumThreads == 0) {
// First thread no TCBs yet
tcbs[0].next = &tcbs[0];
tcbs[0].prev = &tcbs[0];
Head = &tcbs[0];
Tail = &tcbs[0];
index = 0;
} else {
// Look for open spot in tcbs array
for(i = 0; i < MAXTHREADS; i++) {
if(tcbs[i].valid == INVALID) {
index = i;
i = MAXTHREADS; // Exit loop
} else {
index = -1; // Sentinel to detect no invalid spots
}
}
if(index == -1) {
EndCritical(status);
return FAILURE; // No space in tcbs
}
tcbs[index].next = Head; // New tcb points to head
tcbs[index].prev = Tail; // Point back to current tail
(*Tail).next = &tcbs[index]; // Tail now points to new tcb
Tail = &tcbs[index]; // New tcb becomes the tail
(*Head).prev = &tcbs[index]; // Head now points backwards to new tcb
}
// Initilizing the stack for debugging
setInitialStack(index);
// Set PC for stack to point to function to run
tcbs[index].stack[STACKSIZE - 2] = (long) (task);
// Set inital values for sleep status and id
tcbs[index].sleepState = 0;
tcbs[index].priority = priority;
tcbs[index].blockedState = '\0';
tcbs[index].id = index;
tcbs[index].valid = VALID;
NumThreads++;
EndCritical(status);
return SUCCESS;
}