86 lines
2.3 KiB
C
86 lines
2.3 KiB
C
#include "ridhaOs.h"
|
|
|
|
/* Define a fixed linked list of Stack */
|
|
tControlBlock_t tcbs[NUM_OF_THREADS];
|
|
|
|
/* Define current Pointer to point to first of the list of Stack*/
|
|
tControlBlock_t * currentPt;
|
|
|
|
/* Each Thread will have STACKSIZE * 4. */
|
|
int32_t TCB_STACK[NUM_OF_THREADS][STACKSIZE];
|
|
|
|
void ridhaOsStackInit(int i)
|
|
{
|
|
tcbs[i].stackPt = &TCB_STACK[i][STACKSIZE - 16];
|
|
|
|
/* Set bit24 Thumb state bit to one, operate in thumb mode */
|
|
TCB_STACK[i][STACKSIZE-1] = (1U << 24); // PSR register
|
|
|
|
#ifdef DEBUG
|
|
/* @Note: Block below need to delete after for debug purpose */
|
|
/* Dummy stack content */
|
|
TCB_STACK[i][STACKSIZE-3] = 0xAAAAAAAA; // R14(LR)
|
|
TCB_STACK[i][STACKSIZE-4] = 0xAAAAAAAA; // R12
|
|
TCB_STACK[i][STACKSIZE-5] = 0xAAAAAAAA; // R3
|
|
TCB_STACK[i][STACKSIZE-6] = 0xAAAAAAAA; // R2
|
|
TCB_STACK[i][STACKSIZE-7] = 0xAAAAAAAA; // R1
|
|
TCB_STACK[i][STACKSIZE-8] = 0xAAAAAAAA; // R0
|
|
|
|
TCB_STACK[i][STACKSIZE-9] = 0xAAAAAAAA; // R11
|
|
TCB_STACK[i][STACKSIZE-10] = 0xAAAAAAAA; // R10
|
|
TCB_STACK[i][STACKSIZE-11] = 0xAAAAAAAA; // R9
|
|
TCB_STACK[i][STACKSIZE-12] = 0xAAAAAAAA; // R8
|
|
TCB_STACK[i][STACKSIZE-13] = 0xAAAAAAAA; // R6
|
|
TCB_STACK[i][STACKSIZE-14] = 0xAAAAAAAA; // R7
|
|
TCB_STACK[i][STACKSIZE-15] = 0xAAAAAAAA; // R5
|
|
TCB_STACK[i][STACKSIZE-16] = 0xAAAAAAAA; // R4
|
|
#endif
|
|
}
|
|
|
|
#if (USE_STATIC_THREAD)
|
|
uint8_t ridhaOsAddThreads(StaticTask fn0, StaticTask fn1, StaticTask fn2)
|
|
{
|
|
/* Disable Global interrupts */
|
|
__disable_irq();
|
|
tcbs[0].nextPt = &tcbs[1];
|
|
tcbs[1].nextPt = &tcbs[2];
|
|
tcbs[2].nextPt = &tcbs[0];
|
|
|
|
/* Init Stacks for first thread */
|
|
ridhaOsStackInit(0);
|
|
|
|
/* Init Program counter (PC) to task functions */
|
|
TCB_STACK[0][STACKSIZE-2] = (int32_t)(fn0);
|
|
|
|
/* Init Stacks for first thread */
|
|
ridhaOsStackInit(1);
|
|
|
|
/* Init Program counter (PC) to task functions */
|
|
TCB_STACK[1][STACKSIZE-2] = (int32_t)(fn1);
|
|
|
|
/* Init Stacks for first thread */
|
|
ridhaOsStackInit(2);
|
|
|
|
/* Init Program counter (PC) to task functions */
|
|
TCB_STACK[2][STACKSIZE-2] = (int32_t)(fn2);
|
|
|
|
/* Start from thread 0 */
|
|
currentPt = &tcbs[0];
|
|
|
|
/* Enable global IRQ */
|
|
__enable_irq();
|
|
|
|
return 1;
|
|
}
|
|
#endif
|
|
|
|
void ridhaOsStart(void)
|
|
{
|
|
/* Init the Time base */
|
|
ridhaOsSchedulerInit();
|
|
|
|
/* Launch schedular */
|
|
ridhaOsSchedulerLaunch();
|
|
|
|
}
|