ziggi/stm32nanoos
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

scheduler logic moved to semaphore.*

Browse Source
This commit is contained in:
Oleg Borodin
2022-08-30 16:25:19 +02:00
parent 4393e89624
commit f5d433f177
4 changed files with 168 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@@ -31,6 +31,8 @@ all: main.bin
OBJS+= main.o OBJS+= main.o
OBJS+= syscall.o OBJS+= syscall.o
OBJS+= usartu.o OBJS+= usartu.o
OBJS+= scheduler.o
main.elf: $(OBJS) main.elf: $(OBJS)
$(TARGET)-gcc $(^F) $(LDFLAGS) -o $@ $(TARGET)-gcc $(^F) $(LDFLAGS) -o $@

149
main.c
View File

@@ -16,8 +16,10 @@
#include <stdbool.h> #include <stdbool.h>
#include <string.h> #include <string.h>
#include "scheduler.h"
#include "usartu.h" #include "usartu.h"
void delay(uint32_t n) { void delay(uint32_t n) {
for (volatile int i = 0; i < n * 800; i++) for (volatile int i = 0; i < n * 800; i++)
__asm__("nop"); __asm__("nop");
@@ -55,146 +57,8 @@ static void systick_setup(void) {
systick_counter_enable(); systick_counter_enable();
} }
void scheduler_yield(void);
int g_uptime; 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) { void task1(void) {
while (true) { while (true) {
printf("task 1 %d\r\n", g_uptime); 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) { int main(void) {
g_uptime = 0; g_uptime = 0;
static scheduler_t g_scheduler;
clock_setup(); clock_setup();
usart_setup(); usart_setup();
scheduler_init(&g_scheduler); scheduler_init(&g_scheduler);

124
scheduler.c Normal file
View File

@@ -0,0 +1,124 @@
/*
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
*
*/
#include <libopencm3/cm3/scb.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <stdint.h>
#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"
);
}

30
scheduler.h Normal file
View File

@@ -0,0 +1,30 @@
/*
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
*
*/
#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