Minimal RTOS

/* === file: include/rtos.h === */
#ifndef RTOS_H
#define RTOS_H
 
#include <stdint.h>
 
#define RTOS_MAX_TASKS 16
#define RTOS_MAX_PRIORITIES 32
 
typedef enum { TASK_READY, TASK_RUNNING, TASK_BLOCKED, TASK_SUSPENDED, TASK_TERMINATED } task_state_t;
 
typedef struct tcb {
    uint32_t *stack_ptr;    // saved PSP
    uint32_t *stack_base;   // pointer to stack buffer base (for checks)
    uint32_t stack_size;
    uint8_t priority;       // 0 = highest
    task_state_t state;
    struct tcb *next;       // ready list link
    uint32_t id;
} tcb_t;
 
int rtos_task_create(void (*entry)(void*), void *arg, uint32_t *stack_mem, uint32_t stack_size_words, uint8_t priority);
void rtos_start(void);
void rtos_task_yield(void);
void rtos_delay(uint32_t ms);
 
extern volatile uint32_t sys_ticks;
 
#endif /* RTOS_H */
 
 
/* === file: src/rtos.c === */
 
#include "rtos.h"
#include <string.h>
#include "stm32f4xx.h" // for SCB, NVIC, SysTick -- adjust include to your HAL
 
/* Simple minimal preemptive RTOS core
   - uses PSP for tasks
   - PendSV for context switch (assembly)
   - SysTick for tick
   - fixed-size arrays for TCBs and stacks
*/
 
#define STACK_MAGIC 0xDEADBEEF
 
static tcb_t tcbs[RTOS_MAX_TASKS];
static uint32_t task_count = 0;
static tcb_t *current_tcb = NULL;
static tcb_t *idle_tcb = NULL;
 
/* Ready lists by priority (0 = highest). Each is a singly linked list.
   ready_bitmap bit i == 1 => list non-empty
*/
static tcb_t *ready_lists[RTOS_MAX_PRIORITIES];
static uint32_t ready_bitmap = 0;
 
volatile uint32_t sys_ticks = 0;
 
/* Forward declarations */
void PendSV_Handler(void);
void SysTick_Handler(void);
 
/* helper: set bit in bitmap */
static inline void ready_set(uint8_t prio) {
    ready_bitmap |= (1u << prio);
}
static inline void ready_clear(uint8_t prio) {
    ready_bitmap &= ~(1u << prio);
}
 
static void ready_push(tcb_t **list, tcb_t *t) {
    t->next = NULL;
    if (!*list) {
        *list = t;
        ready_set(t->priority);
    } else {
        /* push to tail */
        tcb_t *it = *list;
        while (it->next) it = it->next;
        it->next = t;
    }
}
 
static tcb_t* ready_pop(uint8_t prio) {
    tcb_t *l = ready_lists[prio];
    if (!l) return NULL;
    tcb_t *res = l;
    ready_lists[prio] = res->next;
    if (!ready_lists[prio]) ready_clear(prio);
    res->next = NULL;
    return res;
}
 
static int highest_ready_prio(void) {
    if (!ready_bitmap) return -1;
    /* find highest priority (lowest index) with bit set */
    /* builtin_clz counts leading zeros on 32-bit; we invert interpretation
       We'll scan from 0..31 since priorities are small. Simpler approach:
    */
    for (int i = 0; i < RTOS_MAX_PRIORITIES; ++i) {
        if (ready_bitmap & (1u << i)) return i;
    }
    return -1;
}
 
/* scheduler: select next ready task (simple priority + round-robin) */
static tcb_t* scheduler_select_next(void) {
    int prio = highest_ready_prio();
    if (prio < 0) return idle_tcb;
    return ready_pop(prio);
}
 
/* low-level: initialize task stack (stack_mem is words count) */
static void init_task_stack(tcb_t *tcb, void (*entry)(void*), void *arg, uint32_t *stack_mem, uint32_t stack_size_words) {
    // stack grows downwards; ensure 8-byte alignment
    uint32_t *stack_top = stack_mem + stack_size_words; // points one past end
    uintptr_t sp = (uintptr_t)stack_top;
    sp &= ~((uintptr_t)0x7); // align 8
    uint32_t *stk = (uint32_t*)sp;
 
    // Cortex-M exception stack frame (automatically stacked on exception entry)
    // xPSR, PC, LR, R12, R3, R2, R1, R0
    *(--stk) = (uint32_t)0x01000000;    // xPSR (Thumb bit)
    *(--stk) = (uint32_t)entry;         // PC
    *(--stk) = (uint32_t)0xFFFFFFFD;    // LR (EXC_RETURN to use PSP, return to Thread mode)
    *(--stk) = 0; // R12
    *(--stk) = 0; // R3
    *(--stk) = 0; // R2
    *(--stk) = 0; // R1
    *(--stk) = (uint32_t)arg; // R0 -> argument
    // then callee saved regs r4-r11 will be stored by PendSV handler
    // prepare space for r4-r11
    for (int i = 0; i < 8; ++i) *(--stk) = 0xCCCCCCCC;
 
    // place magic for overflow detection
    *(stack_mem) = STACK_MAGIC;
 
    tcb->stack_ptr = stk;
    tcb->stack_base = stack_mem;
    tcb->stack_size = stack_size_words * sizeof(uint32_t);
}
 
int rtos_task_create(void (*entry)(void*), void *arg, uint32_t *stack_mem, uint32_t stack_size_words, uint8_t priority) {
    if (task_count >= RTOS_MAX_TASKS) return -1;
    if (priority >= RTOS_MAX_PRIORITIES) return -2;
    tcb_t *t = &tcbs[task_count];
    memset(t, 0, sizeof(tcb_t));
    t->priority = priority;
    t->state = TASK_READY;
    t->id = task_count;
    init_task_stack(t, entry, arg, stack_mem, stack_size_words);
    ready_push(&ready_lists[priority], t);
    task_count++;
    return (int)t->id;
}
 
/* Called from assembly PendSV to perform scheduler switch selection.
   Returns pointer to next task's stack_ptr in r0 by convention.
*/
 
// This function will be called with interrupts disabled in PendSV context.
uint32_t* rtos_scheduler_select_next_stack(void) {
    // save current into ready list if still runnable
    if (current_tcb && current_tcb->state == TASK_RUNNING) {
        current_tcb->state = TASK_READY;
        ready_push(&ready_lists[current_tcb->priority], current_tcb);
    }
    tcb_t *next = scheduler_select_next();
    if (!next) next = idle_tcb;
    next->state = TASK_RUNNING;
    current_tcb = next;
    return current_tcb->stack_ptr;
}
 
/* start scheduler: arrange MSP/PSP and start first task */
void rtos_start(void) {
    // create idle TCB if not existing
    static uint32_t idle_stack[128];
    static int idle_created = 0;
    if (!idle_created) {
        memset(idle_stack, 0, sizeof(idle_stack));
        idle_tcb = &tcbs[RTOS_MAX_TASKS-1]; // reserved slot
        memset(idle_tcb, 0, sizeof(tcb_t));
        init_task_stack(idle_tcb, (void(*)(void*))0, 0, idle_stack, sizeof(idle_stack)/4);
        idle_tcb->priority = RTOS_MAX_PRIORITIES-1;
        idle_tcb->state = TASK_READY;
        idle_created = 1;
    }
 
    // configure PendSV and SysTick priorities
    NVIC_SetPriority(PendSV_IRQn, (1UL << __NVIC_PRIO_BITS) - 1); // lowest
    NVIC_SetPriority(SysTick_IRQn, (1UL << __NVIC_PRIO_BITS) - 2);
 
    // start SysTick: 1ms tick (assumes SystemCoreClock defined)
    SysTick_Config(SystemCoreClock / 1000U);
 
    // pick first task
    tcb_t *first = scheduler_select_next();
    if (!first) first = idle_tcb;
    first->state = TASK_RUNNING;
    current_tcb = first;
 
    // set PSP to first->stack_ptr and switch to use PSP
    __set_PSP((uint32_t)first->stack_ptr);
    // set CONTROL to use PSP and unprivileged Thread mode (optionally keep privileged)
    __set_CONTROL(0x02);
    __ISB();
 
    // start first task by popping exception frame (simulated by exception return)
    // use assembler to perform exception return to thread using PSP
    __enable_irq();
    asm volatile (
        "bx %0\n"
        : : "r" (0xFFFFFFFD) :
    );
}
 
void rtos_task_yield(void) {
    // trigger PendSV
    SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
}
 
/* millisecond delay (simple blocking) */
void rtos_delay(uint32_t ms) {
    uint32_t start = sys_ticks;
    while ((sys_ticks - start) < ms) {
        rtos_task_yield();
    }
}
 
/* SysTick handler: increment tick and request context switch if needed */
void SysTick_Handler(void) {
    sys_ticks++;
    // simple preemption: if highest ready priority is higher (lower number) than current -> pend PendSV
    int pr = highest_ready_prio();
    if (pr >= 0 && current_tcb && pr < current_tcb->priority) {
        SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
    }
}
 
/* Hook for assembly PendSV to get next stack */
__attribute__((used)) uint32_t* rtos_get_next_sp(void) {
    return rtos_scheduler_select_next_stack();
}
 
/* optional: minimal check for stack overflow */
int rtos_check_stack_overflow(tcb_t *t) {
    if (!t) return 0;
    if (*(t->stack_base) != STACK_MAGIC) return 1; // overflow detected
    return 0;
}
 
/* === file: src/context_switch.s === */
    .syntax unified
    .cpu cortex-m4
    .thumb
 
    .global PendSV_Handler
    .type PendSV_Handler, %function
PendSV_Handler:
    cpsid i
    mrs r0, PSP                /* r0 = PSP (current process SP) */
    /* store r4-r11 on process stack */
    stmdb r0!, {r4-r11}
    /* save updated PSP into current_tcb->stack_ptr */
    ldr r1, =current_tcb
    ldr r2, [r1]
    cbz r2, 1f
    str r0, [r2]
1:
    /* call C function to pick next task and get its stack */
    bl rtos_get_next_sp
    /* r0 contains new stack_ptr */
    /* load saved stack_ptr into r0 */
    mov r1, r0
    /* restore r4-r11 from new stack */
    ldmia r1!, {r4-r11}
    msr PSP, r1
    cpsie i
    bx lr
 
    .size PendSV_Handler, .-PendSV_Handler
 
/* === file: src/main.c === */
 
#include "rtos.h"
#include <stdint.h>
#include "stm32f4xx.h"
 
/* example tasks */
static uint32_t task1_stack[128];
static uint32_t task2_stack[128];
 
void task1(void *arg) {
    (void)arg;
    while (1) {
        // toggle LED or do work
        for (volatile int i=0;i<1000000;i++);
        rtos_task_yield();
    }
}
 
void task2(void *arg) {
    (void)arg;
    while (1) {
        for (volatile int i=0;i<500000;i++);
        rtos_task_yield();
    }
}
 
int main(void) {
    SystemInit();
    // setup GPIO for LED here if desired
 
    rtos_task_create(task1, NULL, task1_stack, sizeof(task1_stack)/4, 1);
    rtos_task_create(task2, NULL, task2_stack, sizeof(task2_stack)/4, 2);
 
    rtos_start(); // does not return
    while(1);
}
 
/* === file: src/startup_stm32.s === */
/* Minimal vector table stub. Replace with your project's real startup code. */
    .section .isr_vector,"a",%progbits
    .word __StackTop
    .word Reset_Handler
    .word NMI_Handler
    .word HardFault_Handler
    .word MemManage_Handler
    .word BusFault_Handler
    .word UsageFault_Handler
    .word 0
    .word 0
    .word 0
    .word 0
    .word SVC_Handler
    .word DebugMon_Handler
    .word 0
    .word PendSV_Handler
    .word SysTick_Handler
 
/* Handlers are weak and can be implemented in C elsewhere */
 
/* === file: Makefile === */
# Simple Makefile (adjust paths and flags to your toolchain)
 
TARGET = rtos_min
SRCS = src/rtos.c src/main.c src/context_switch.s
INCS = -Iinclude
CC = arm-none-eabi-gcc
LD = arm-none-eabi-gcc
OBJCOPY = arm-none-eabi-objcopy
CFLAGS = -mcpu=cortex-m4 -mthumb -mfloat-abi=soft -O2 -g -ffreestanding -fno-common $(INCS)
LDFLAGS = -Tstm32f4_linker.ld -nostartfiles
 
all: $(TARGET).bin
 
$(TARGET).elf: $(SRCS)
	$(CC) $(CFLAGS) $(SRCS) -o $(TARGET).elf $(LDFLAGS)
	$(OBJCOPY) -O binary $(TARGET).elf $(TARGET).bin
 
clean:
	rm -f $(TARGET).elf $(TARGET).bin