From f7cd7387dc517cb57a149da137efdf90308425b3 Mon Sep 17 00:00:00 2001 From: Ridha Noomane Date: Thu, 26 Dec 2024 10:47:08 +0100 Subject: [PATCH] Add Cooperative scheduler --- RidhaOs/Inc/ridhaOs.h | 14 +-- RidhaOs/Inc/ridhaOsScheduler.h | 7 +- RidhaOs/RIDHAOS_CONF.h | 24 ++--- RidhaOs/Src/ridhaOs.c | 94 ++++++++-------- RidhaOs/Src/ridhaOsScheduler.c | 189 +++++++++++++++++---------------- 5 files changed, 164 insertions(+), 164 deletions(-) diff --git a/RidhaOs/Inc/ridhaOs.h b/RidhaOs/Inc/ridhaOs.h index 5bdcf82..c6809c6 100644 --- a/RidhaOs/Inc/ridhaOs.h +++ b/RidhaOs/Inc/ridhaOs.h @@ -6,21 +6,17 @@ /* This Struct used to restore next thread and save the running thread */ typedef struct tControlBlock { - int32_t * stackPt; - struct tControlBlock * nextPt; -}tControlBlock_t; - + int32_t *stackPt; + struct tControlBlock *nextPt; +} tControlBlock_t; + /* Define typdef of static task to execute in the future */ #if (USE_STATIC_THREAD) -typedef void(*StaticTask)(void); +typedef void (*StaticTask)(void); #endif - - uint8_t ridhaOsAddThreads(StaticTask fn0, StaticTask fn1, StaticTask fn2); void ridhaOsStart(void); - - #endif /* __R_KERNEL_H__ */ \ No newline at end of file diff --git a/RidhaOs/Inc/ridhaOsScheduler.h b/RidhaOs/Inc/ridhaOsScheduler.h index b7fe034..16f9ec5 100644 --- a/RidhaOs/Inc/ridhaOsScheduler.h +++ b/RidhaOs/Inc/ridhaOsScheduler.h @@ -3,7 +3,6 @@ #include "ridhaOs.h" - #include "RIDHAOS_CONF.h" #include @@ -13,11 +12,11 @@ void ridhaOsSchedulerDelayMS(uint32_t delay); void ridhaOsSchedulerInit(void); inline void ridhaOsSchedulerStart(void) { - /* Enable Systick */ - SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk; + /* Enable Systick */ + SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk; } void ridhaOsSchedulerLaunch(void); - +void ridhaOsSchedulerThreadYield(void); #endif /* __R_SCHEDULER_H__ */ \ No newline at end of file diff --git a/RidhaOs/RIDHAOS_CONF.h b/RidhaOs/RIDHAOS_CONF.h index 7960ba0..da61e85 100644 --- a/RidhaOs/RIDHAOS_CONF.h +++ b/RidhaOs/RIDHAOS_CONF.h @@ -3,27 +3,23 @@ #warning Be Sure to configure this file, when you start your project. - /* Here Define your MCU REGISTER MAP FILE */ #include "fsl_device_registers.h" -#define CPU_ARM_VERSION 33 - +#define CPU_ARM_VERSION 33 /* SysTick Configuration for TimeBase */ extern uint32_t SystemCoreClock; -#define CPU_CLOCK_HZ SystemCoreClock // Use Default System Clock in Hz -#define MAX_DELAY 0xFFFFFFFFU // Max Wayt Delay -#define TICK_RATE_HZ 1000U // Getting SystemCoreClock divide by 1000 to get interrupt every 1ms -#define MAX_DELAY 0xFFFFFFFFU // MAX DELAY -#define ROUND_ROBIN_QUANTA 10U // QUANTA For round robin schedular change tasks. +#define CPU_CLOCK_HZ SystemCoreClock // Use Default System Clock in Hz +#define MAX_DELAY 0xFFFFFFFFU // Max Wayt Delay +#define TICK_RATE_HZ 1000U // Getting SystemCoreClock divide by 1000 to get interrupt every 1ms +#define MAX_DELAY 0xFFFFFFFFU // MAX DELAY +#define ROUND_ROBIN_QUANTA 10U // QUANTA in MS For round robin schedular change tasks. /* Thread Configuration Size and Number Max for TCB */ -#define USE_STATIC_THREAD 1 -#define USE_DYNAMIC_THREAD 0 -#define NUM_OF_THREADS 3 -#define STACKSIZE 100 - - +#define USE_STATIC_THREAD 1 +#define USE_DYNAMIC_THREAD 0 +#define NUM_OF_THREADS 3 +#define STACKSIZE 700 // As memory addressing orgonize by 4 so you need to mulitple by 4 #endif /* __R_CONFIGURATION_H__ */ \ No newline at end of file diff --git a/RidhaOs/Src/ridhaOs.c b/RidhaOs/Src/ridhaOs.c index eb53564..6815306 100644 --- a/RidhaOs/Src/ridhaOs.c +++ b/RidhaOs/Src/ridhaOs.c @@ -4,7 +4,7 @@ tControlBlock_t tcbs[NUM_OF_THREADS]; /* Define current Pointer to point to first of the list of Stack*/ -tControlBlock_t * currentPt; +tControlBlock_t *currentPt; /* Each Thread will have STACKSIZE * 4. */ int32_t TCB_STACK[NUM_OF_THREADS][STACKSIZE]; @@ -12,74 +12,74 @@ 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 + 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 + 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]; + /* 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 Stacks for first thread */ + ridhaOsStackInit(0); - /* Init Program counter (PC) to task functions */ - TCB_STACK[0][STACKSIZE-2] = (int32_t)(fn0); + /* Init Program counter (PC) to task functions */ + TCB_STACK[0][STACKSIZE - 2] = (int32_t)(fn0); - /* Init Stacks for first thread */ - ridhaOsStackInit(1); + /* Init Stacks for first thread */ + ridhaOsStackInit(1); - /* Init Program counter (PC) to task functions */ - TCB_STACK[1][STACKSIZE-2] = (int32_t)(fn1); + /* Init Program counter (PC) to task functions */ + TCB_STACK[1][STACKSIZE - 2] = (int32_t)(fn1); - /* Init Stacks for first thread */ - ridhaOsStackInit(2); + /* Init Stacks for first thread */ + ridhaOsStackInit(2); - /* Init Program counter (PC) to task functions */ - TCB_STACK[2][STACKSIZE-2] = (int32_t)(fn2); + /* Init Program counter (PC) to task functions */ + TCB_STACK[2][STACKSIZE - 2] = (int32_t)(fn2); - /* Start from thread 0 */ - currentPt = &tcbs[0]; + /* Start from thread 0 */ + currentPt = &tcbs[0]; - /* Enable global IRQ */ - __enable_irq(); + /* Enable global IRQ */ + __enable_irq(); - return 1; + return 1; } #endif void ridhaOsStart(void) -{ - /* Init the Time base */ - ridhaOsSchedulerInit(); - - /* Launch schedular */ - ridhaOsSchedulerLaunch(); - +{ + /* Init the Time base */ + ridhaOsSchedulerInit(); + + /* Launch schedular */ + ridhaOsSchedulerLaunch(); } + diff --git a/RidhaOs/Src/ridhaOsScheduler.c b/RidhaOs/Src/ridhaOsScheduler.c index 3421fdf..af91cc2 100644 --- a/RidhaOs/Src/ridhaOsScheduler.c +++ b/RidhaOs/Src/ridhaOsScheduler.c @@ -1,7 +1,7 @@ #include "ridhaOsScheduler.h" /* Define current Pointer to point to first of the list of Stack*/ -extern tControlBlock_t * currentPt; +extern tControlBlock_t *currentPt; volatile uint32_t g_curr_tick; volatile uint32_t g_curr_tick_p; volatile uint32_t tick_freq = 1; @@ -10,131 +10,140 @@ uint8_t tick_freq_div = 1; static void tick_increment(void) { - g_curr_tick += tick_freq; + g_curr_tick += tick_freq; } static inline void ridhaOsSchedulerReset(void) { - /* Reset Systick configuration*/ - SysTick->CTRL = 0x0000; + /* Reset Systick configuration*/ + SysTick->CTRL = 0x0000; - /* Clear Systick current Value */ - SysTick->VAL = 0; + /* Clear Systick current Value */ + SysTick->VAL = 0; } void ridhaOsSchedulerDelayS(uint32_t delay) { - ridhaOsSchedulerDelayMS(delay * 1000); + 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 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; + __disable_irq(); + g_curr_tick_p = g_curr_tick; + __enable_irq(); + return g_curr_tick_p; } void ridhaOsSchedulerInit(void) { - /* Reset registers */ - ridhaOsSchedulerReset(); + /* 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; + /* 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 Address of currentPt into R0 */ + __asm("LDR R0, =currentPt"); - /* Load R2 from address R0, i.e R2 =currentPt */ - __asm("LDR R2, [R0]"); + /* 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]"); + /* 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 r4, r5, r6, r7, r8, r9, r10, r11 */ + __asm("POP {R4-R11}"); - /* Restore r0, r1, r2, r3 */ - __asm("POP {R0-R3}"); + /* Restore R12 */ + __asm("POP {R12}"); - /* Skip LR */ - __asm("ADD SP, SP, #4"); + /* Restore r0, r1, r2, r3 */ + __asm("POP {R0-R3}"); - /* Create a new start location by poping LR */ - __asm("POP {LR}"); + /* Skip LR */ + __asm("ADD SP, SP, #4"); - /* Skip xPSR by adding 4 to SP */ - __asm("ADD SP,SP, #4"); + /* Create a new start location by poping LR */ + __asm("POP {LR}"); - /* Enable global interrupt */ - __asm("CPSIE I"); - - /* Return from exception and restore r0 to r3, lr, pc, psr */ - __asm("BX 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){ +__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]"); + /* 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"); +} + + +void ridhaOsSchedulerThreadYield(void) +{ + /* Clear Systick current value register */ + SysTick->VAL = 0; - /* 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"); - -} \ No newline at end of file + /* Trigger Systick */ + SCB->ICSR |= SCB_ICSR_PENDSTSET_Msk; + +} +