140 lines
3.3 KiB
C
140 lines
3.3 KiB
C
#include "ridhaOsScheduler.h"
|
|
|
|
/* Define current Pointer to point to first of the list of Stack*/
|
|
extern tControlBlock_t * currentPt;
|
|
volatile uint32_t g_curr_tick;
|
|
volatile uint32_t g_curr_tick_p;
|
|
volatile uint32_t tick_freq = 1;
|
|
|
|
uint8_t tick_freq_div = 1;
|
|
|
|
static void tick_increment(void)
|
|
{
|
|
g_curr_tick += tick_freq;
|
|
}
|
|
|
|
static inline void ridhaOsSchedulerReset(void)
|
|
{
|
|
/* Reset Systick configuration*/
|
|
SysTick->CTRL = 0x0000;
|
|
|
|
/* Clear Systick current Value */
|
|
SysTick->VAL = 0;
|
|
}
|
|
|
|
void ridhaOsSchedulerDelayS(uint32_t delay)
|
|
{
|
|
ridhaOsSchedulerDelayMS(delay * 1000);
|
|
}
|
|
|
|
void ridhaOsSchedulerDelayMS(uint32_t delay)
|
|
{
|
|
uint32_t tickstart = ridhaOsSchedulerGetTick();
|
|
uint32_t wait = delay;
|
|
if(wait < MAX_DELAY)
|
|
{
|
|
wait += (uint32_t)(tick_freq);
|
|
}
|
|
|
|
while((ridhaOsSchedulerGetTick() - tickstart) < wait){}
|
|
|
|
}
|
|
|
|
uint32_t ridhaOsSchedulerGetTick(void)
|
|
{
|
|
__disable_irq();
|
|
g_curr_tick_p = g_curr_tick;
|
|
__enable_irq();
|
|
return g_curr_tick_p;
|
|
}
|
|
|
|
void ridhaOsSchedulerInit(void)
|
|
{
|
|
/* Reset registers */
|
|
ridhaOsSchedulerReset();
|
|
|
|
/* Reload the timer with number of MS */
|
|
SysTick->LOAD = ((SystemCoreClock / TICK_RATE_HZ)
|
|
* ROUND_ROBIN_QUANTA) - 1;
|
|
|
|
/* Clear Systick current value register */
|
|
SysTick->VAL = 0x00;
|
|
|
|
/* Select Internal Clock source */
|
|
SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | SysTick_CTRL_ENABLE_Msk;
|
|
|
|
/* Set Systick to low priority */
|
|
NVIC_SetPriority(SysTick_IRQn, 15);
|
|
|
|
/* Enable SysTick interrupt */
|
|
SysTick->CTRL |= SysTick_CTRL_TICKINT_Msk;
|
|
|
|
}
|
|
|
|
|
|
void ridhaOsSchedulerLaunch(void)
|
|
{
|
|
/* Load Address of currentPt into R0 */
|
|
__asm("LDR R0, =currentPt");
|
|
|
|
/* Load R2 from address R0, i.e R2 =currentPt */
|
|
__asm("LDR R2, [R0]");
|
|
|
|
/* Load Cortex M SP from address equals R2, i.e. SP = currentPt->stackPt */
|
|
__asm("LDR SP, [R2]");
|
|
|
|
/* Restore r4, r5, r6, r7, r8, r9, r10, r11 */
|
|
__asm("POP {R4-R11}");
|
|
|
|
/* Restore R12 */
|
|
__asm("POP {R12}");
|
|
|
|
/* Restore r0, r1, r2, r3 */
|
|
__asm("POP {R0-R3}");
|
|
|
|
/* Skip LR */
|
|
__asm("ADD SP, SP, #4");
|
|
|
|
/* Create a new start location by poping LR */
|
|
__asm("POP {LR}");
|
|
|
|
/* Skip xPSR by adding 4 to SP */
|
|
__asm("ADD SP,SP, #4");
|
|
|
|
/* Enable global interrupt */
|
|
__asm("CPSIE I");
|
|
|
|
/* Return from exception and restore r0 to r3, lr, pc, psr */
|
|
__asm("BX LR");
|
|
|
|
}
|
|
|
|
__attribute__((naked)) void SysTick_Handler(void){
|
|
|
|
/* SUSPEND CURRENT THREAD */
|
|
/* DISABLE GLOBAL INTERRUPTS */
|
|
__asm("CPSID I");
|
|
/* Save Other Register not pushed by interrupt (R4->R11) */
|
|
__asm("PUSH {R4-R11}");
|
|
/* Load Address of currentPT int R0 */
|
|
__asm("LDR R0, =currentPt");
|
|
/* Load r1 from address equals r0, i.e r1 =currentPt */
|
|
__asm("LDR R1, [R0]");
|
|
/* Store Cortex-M SP at address equales to R1, i.e save SP into tcb */
|
|
__asm("STR SP, [R1]");
|
|
|
|
/* CHOOSE NEXT THREAD */
|
|
/* Load r1 from a location 4 bytes above address r1, i.e next r1 =currentPt->next*/
|
|
__asm("LDR R1, [R1,#4]");
|
|
/* Store r1 @Address equals r0, i.e currentPt = r1 */
|
|
__asm("STR R1, [R0]");
|
|
/* Load Cortext M SP from @ equals r1, i.e SP = current->stackPt */
|
|
__asm("LDR SP, [R1]");
|
|
/* Restore R4-R11 registers */
|
|
__asm("POP {R4-R11}");
|
|
/* Enable global interrupt */
|
|
__asm("CPSIE I");
|
|
/* Return from exception and restore r0 to r3, lr, pc, psr */
|
|
__asm("BX LR");
|
|
|
|
} |