From f5d433f177a47fcacd2fe16c2a2972c04e2da4c7 Mon Sep 17 00:00:00 2001 From: Oleg Borodin Date: Tue, 30 Aug 2022 16:25:19 +0200 Subject: [PATCH] scheduler logic moved to semaphore.* --- Makefile | 2 + main.c | 151 +++++----------------------------------------------- scheduler.c | 124 ++++++++++++++++++++++++++++++++++++++++++ scheduler.h | 30 +++++++++++ 4 files changed, 168 insertions(+), 139 deletions(-) create mode 100644 scheduler.c create mode 100644 scheduler.h diff --git a/Makefile b/Makefile index 053411b..8024a4c 100644 --- a/Makefile +++ b/Makefile @@ -31,6 +31,8 @@ all: main.bin OBJS+= main.o OBJS+= syscall.o OBJS+= usartu.o +OBJS+= scheduler.o + main.elf: $(OBJS) $(TARGET)-gcc $(^F) $(LDFLAGS) -o $@ diff --git a/main.c b/main.c index 31c77c5..7df08d3 100644 --- a/main.c +++ b/main.c @@ -16,8 +16,10 @@ #include #include +#include "scheduler.h" #include "usartu.h" + void delay(uint32_t n) { for (volatile int i = 0; i < n * 800; i++) __asm__("nop"); @@ -55,146 +57,8 @@ static void systick_setup(void) { systick_counter_enable(); } - -void scheduler_yield(void); int g_uptime; -void sys_tick_handler(void) { - g_uptime++; - scheduler_yield(); -} - -#define SCHEDULER_NUM_TASKS 4 -#define TASK_STACK_SIZE 2048 - -typedef struct { - void (*entry)(void); - void *stack; - unsigned stack_size; - uint32_t sp; - bool runnable; -} task_t; - - -#define TASK_COUNT_REG 8 - -typedef struct __attribute__((packed)) { - struct { - uint32_t registers[TASK_COUNT_REG]; - } sw_frame; - struct { - uint32_t r0; - uint32_t r1; - uint32_t r2; - uint32_t r3; - uint32_t r12; - uint32_t lr; - uint32_t pc; - uint32_t psr; - } nvic_frame; -} stack_frame_t; - -typedef struct { - task_t tasks[SCHEDULER_NUM_TASKS]; - uint8_t current_task; -} scheduler_t; - -static scheduler_t g_scheduler = { - .current_task = 0 -}; - - -static void task_exit_func(void) { - while (true); -} - -#define RETURN_PSR 0x21000000 -#define STACK_FILL 0xA5 - -#define SCHEDULER_MAX_TASKS 16 - -void scheduler_init(scheduler_t *scheduler) { - for (int i = 0; i < SCHEDULER_NUM_TASKS; i++) { - task_t *task = &(scheduler->tasks[i]); - task->stack = NULL; - task->stack_size = 0; - task->sp = (uint32_t)NULL; - task->runnable = false; - } - scheduler->current_task = 0; -} - -void scheduler_task(scheduler_t *scheduler, int task_no, void (*entry)(void)) { - - task_t *task = &(scheduler->tasks[task_no]); - task->stack = (void*)malloc(TASK_STACK_SIZE); - task->stack_size = TASK_STACK_SIZE; - memset(task->stack, STACK_FILL, task->stack_size); - - task->sp = (uint32_t)(task->stack + task->stack_size); - task->sp -= sizeof(stack_frame_t); - - stack_frame_t *frame = (stack_frame_t*)task->sp; - frame->nvic_frame.lr = (uint32_t)task_exit_func; - frame->nvic_frame.pc = (uint32_t)entry; - frame->nvic_frame.psr = RETURN_PSR; - - task->runnable = true; -} - -static void scheduler_switch(scheduler_t *scheduler) { - do { - scheduler->current_task = (scheduler->current_task + 1) % SCHEDULER_NUM_TASKS; - } while (!scheduler->tasks[scheduler->current_task].runnable); -} - -#define EXC_RETURN_MODE_THREAD 0x00000004 -#define RETURN_ON_PSP_THREAD 0xFFFFFFFD - - -void __attribute__((naked)) pend_sv_handler(void) { - - const uint32_t RETURN_ON_PSP = RETURN_ON_PSP_THREAD; - - __asm__("cpsid if":::"memory"); - uint32_t lr; - __asm__("mov %0, lr\n":"=r"(lr)); - - if (lr & EXC_RETURN_MODE_THREAD) { - uint32_t psp; - __asm__( - "mrs %0, psp\n" - "stmdb %0!, {r4-r11}\n" - "msr psp, %0\n" : "=r"(psp) - ); - g_scheduler.tasks[g_scheduler.current_task].sp = psp; - } else { - __asm__( - "stmdb sp!, {r4-r11}\n"); - } - - scheduler_switch(&g_scheduler); - - uint32_t psp = (uint32_t)g_scheduler.tasks[g_scheduler.current_task].sp; - __asm__( - "ldmfd %0!, {r4-r11}\n" - "msr psp, %0\n"::"r"(psp) - ); - - __asm__("cpsie if" ::: "memory"); - __asm__("bx %0\n"::"r"(RETURN_ON_PSP)); -} - -void scheduler_yield(void) { - SCB_ICSR |= SCB_ICSR_PENDSVSET; - __asm__( - "nop\n" - "nop\n" - "nop\n" - "nop\n" - ); -} - void task1(void) { while (true) { printf("task 1 %d\r\n", g_uptime); @@ -223,8 +87,17 @@ void task4(void) { }; } +void sys_tick_handler(void) { + g_uptime++; + scheduler_yield(); +} + + int main(void) { - g_uptime = 0; + g_uptime = 0; + + static scheduler_t g_scheduler; + clock_setup(); usart_setup(); scheduler_init(&g_scheduler); diff --git a/scheduler.c b/scheduler.c new file mode 100644 index 0000000..8b4087b --- /dev/null +++ b/scheduler.c @@ -0,0 +1,124 @@ +/* + * Copyright 2022 Oleg Borodin + * + */ + +#include + +#include +#include +#include +#include + +#include "scheduler.h" + + +static scheduler_t* m_scheduler; + +#define TASK_COUNT_REG 8 + +typedef struct __attribute__((packed)) { + struct { + uint32_t registers[TASK_COUNT_REG]; + } sw_frame; + struct { + uint32_t r0; + uint32_t r1; + uint32_t r2; + uint32_t r3; + uint32_t r12; + uint32_t lr; + uint32_t pc; + uint32_t psr; + } nvic_frame; +} stack_frame_t; + +static void task_exit_func(void) { + while (true); +} + +#define RETURN_PSR 0x21000000 +#define STACK_FILL 0xA5 + +void scheduler_init(scheduler_t *scheduler) { + + for (int i = 0; i < SCHEDULER_NUM_TASKS; i++) { + task_t *task = &(scheduler->tasks[i]); + task->stack = NULL; + task->stack_size = 0; + task->sp = (uint32_t)NULL; + task->runnable = false; + } + scheduler->current_task = 0; + m_scheduler = scheduler; +} + +void scheduler_task(scheduler_t *scheduler, int task_no, void (*entry)(void)) { + + task_t *task = &(scheduler->tasks[task_no]); + task->stack = (void*)malloc(TASK_STACK_SIZE); + task->stack_size = TASK_STACK_SIZE; + memset(task->stack, STACK_FILL, task->stack_size); + + task->sp = (uint32_t)(task->stack + task->stack_size); + task->sp -= sizeof(stack_frame_t); + + stack_frame_t *frame = (stack_frame_t*)task->sp; + frame->nvic_frame.lr = (uint32_t)task_exit_func; + frame->nvic_frame.pc = (uint32_t)entry; + frame->nvic_frame.psr = RETURN_PSR; + + task->runnable = true; +} + +static void scheduler_switch(scheduler_t *scheduler) { + do { + scheduler->current_task = (scheduler->current_task + 1) % SCHEDULER_NUM_TASKS; + } while (!scheduler->tasks[scheduler->current_task].runnable); +} + +#define EXC_RETURN_MODE_THREAD 0x00000004 +#define RETURN_ON_PSP_THREAD 0xFFFFFFFD + +void __attribute__((naked)) pend_sv_handler(void) { + + const uint32_t RETURN_ON_PSP = RETURN_ON_PSP_THREAD; + + __asm__("cpsid if":::"memory"); + uint32_t lr; + __asm__("mov %0, lr\n":"=r"(lr)); + + if (lr & EXC_RETURN_MODE_THREAD) { + uint32_t psp; + __asm__( + "mrs %0, psp\n" + "stmdb %0!, {r4-r11}\n" + "msr psp, %0\n" : "=r"(psp) + ); + m_scheduler->tasks[m_scheduler->current_task].sp = psp; + } else { + __asm__( + "stmdb sp!, {r4-r11}\n"); + } + + scheduler_switch(m_scheduler); + + uint32_t psp = (uint32_t)m_scheduler->tasks[m_scheduler->current_task].sp; + __asm__( + "ldmfd %0!, {r4-r11}\n" + "msr psp, %0\n"::"r"(psp) + ); + + __asm__("cpsie if" ::: "memory"); + __asm__("bx %0\n"::"r"(RETURN_ON_PSP)); +} + +void scheduler_yield(void) { + SCB_ICSR |= SCB_ICSR_PENDSVSET; + __asm__( + "nop\n" + "nop\n" + "nop\n" + "nop\n" + ); +} diff --git a/scheduler.h b/scheduler.h new file mode 100644 index 0000000..c99e380 --- /dev/null +++ b/scheduler.h @@ -0,0 +1,30 @@ +/* + * Copyright 2022 Oleg Borodin + * + */ + +#ifndef SCHEDULER_H_QWERTY +#define SCHEDULER_H_QWERTY + +#define SCHEDULER_NUM_TASKS 4 +#define TASK_STACK_SIZE 2048 + +typedef struct { + void (*entry)(void); + void *stack; + unsigned stack_size; + uint32_t sp; + bool runnable; +} task_t; + +typedef struct { + task_t tasks[SCHEDULER_NUM_TASKS]; + uint8_t current_task; +} scheduler_t; + + +void scheduler_init(scheduler_t *scheduler); +void scheduler_task(scheduler_t *scheduler, int task_no, void (*entry)(void)); +void scheduler_yield(void); + +#endif