From 9e8158277c7501e1072ccd2c867b2379862546b0 Mon Sep 17 00:00:00 2001 From: Olaf Rempel Date: Sun, 24 Feb 2008 20:07:46 +0100 Subject: [PATCH] context, spinlock and semaphore functions --- Makefile | 2 +- include/rtos/context.h | 60 +++++++-- include/rtos/critical.h | 10 -- include/rtos/semaphore.h | 6 +- include/rtos/spinlock.h | 7 +- main.c | 127 +------------------ src/rtos/context.c | 264 +++++++++++++++++++++++++++++++++++++++ src/rtos/semaphore.c | 56 +++++++++ src/rtos/spinlock.c | 39 ++++++ 9 files changed, 419 insertions(+), 152 deletions(-) delete mode 100644 include/rtos/critical.h create mode 100644 src/rtos/semaphore.c create mode 100644 src/rtos/spinlock.c diff --git a/Makefile b/Makefile index df4855d..4e66c67 100644 --- a/Makefile +++ b/Makefile @@ -20,7 +20,7 @@ TARGET = sam7fc ASFLAGS = -mcpu=arm7tdmi -Wa,-adhlns=$(BUILD)/$(*D)/$(*F).lst,--gdwarf-2 -Iinclude -CFLAGS = -gdwarf-2 -mcpu=arm7tdmi -Os -std=gnu99 +CFLAGS = -gdwarf-2 -mcpu=arm7tdmi -Os -std=gnu99 -dr CFLAGS += -Wa,-adhlns=$(BUILD)/$(*D)/$(*F).lst CFLAGS += -nostdinc $(patsubst %,-I%,$(INCDIRS)) CFLAGS += -MD -MP -MF $(BUILD)/$(*D)/$(*F).d diff --git a/include/rtos/context.h b/include/rtos/context.h index 82d0520..b6bf4d5 100644 --- a/include/rtos/context.h +++ b/include/rtos/context.h @@ -4,24 +4,62 @@ #include #include "rtos/spinlock.h" -enum state_t { - CONTEXT_NULL = 0x00, - CONTEXT_RUNNING, - CONTEXT_READY, - CONTEXT_INTERRUPTED, - CONTEXT_SLEEP, - CONTEXT_SLEEP_QUEUE, +struct register_context { + uint32_t r0; + uint32_t r1; + uint32_t pc; + uint32_t cpsr; + uint32_t r2; + uint32_t r3; + uint32_t r4; + uint32_t r5; + uint32_t r6; + uint32_t r7; + uint32_t r8; + uint32_t r9; + uint32_t r10; + uint32_t r11; + uint32_t r12; + uint32_t sp; + uint32_t r14; +}; + +enum ctx_state { + CONTEXT_NULL = 0, + CONTEXT_RUNNING = 1, + CONTEXT_READY = 2, + CONTEXT_INTERRUPTED = 3, + CONTEXT_SLEEP = 4, + CONTEXT_SLEEP_QUEUE = 5, }; struct context { - state_t state; + /* regs *MUST* be first in struct! */ + struct register_context regs; + uint32_t stacksize; + uint8_t state; uint8_t priority; - uint32_t spinlock_held; - uint32_t dummy; + + /* next pointers for run & sleep queues */ + struct context *run_queue; + struct context *sleep_queue; }; -uint32_t context_wait_priority_queue(struct spinlock *lock, struct context **queue); +extern struct context *current_context; + +struct context * create_ctx(uint32_t stacksize, uint8_t priority, void (* code)(void *arg), void *arg); + +uint8_t isr_context_wait(struct spinlock *lock); +uint8_t context_wait(struct spinlock *lock); +uint8_t context_wait_queue(struct spinlock *lock, struct context **queue); +uint8_t context_wait_pri_queue(struct spinlock *lock, struct context **queue); + +void isr_context_signal(struct context *c); +void context_signal(struct context *c); uint32_t context_signal_queue(struct context **queue); + +void isr_context_interrupt(struct context *c); +void context_interrupt(struct context *c); uint32_t context_interrupt_queue(struct context *c, struct context **queue); #endif /* _CONTEXT_H_ */ diff --git a/include/rtos/critical.h b/include/rtos/critical.h deleted file mode 100644 index a5e62fb..0000000 --- a/include/rtos/critical.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef _CRITICAL_H_ -#define _CRITICAL_H_ - -/* disable Interrupts */ -void enter_critical_section(void); - -/* enable Interrupts */ -void exit_critical_section(void); - -#endif /* _CRITICAL_H_ */ diff --git a/include/rtos/semaphore.h b/include/rtos/semaphore.h index 6bdf7f1..a0d6900 100644 --- a/include/rtos/semaphore.h +++ b/include/rtos/semaphore.h @@ -9,14 +9,14 @@ struct semaphore { struct context *sleep_queue; struct spinlock lock; - uint32_t count; + int32_t count; }; -state_t sem_wait(struct semaphore *sem); +uint8_t sem_wait(struct semaphore *sem); void sem_post(struct semaphore *sem); void sem_interrupt(struct semaphore *sem, struct context *c); int32_t sem_get_count(struct semaphore *sem); -void sem_init(struct semaphore *sem, uint8_t priority, int32_t count); +void sem_init(struct semaphore *sem, int32_t count); #endif /* _SEMSPHORE_H_ */ diff --git a/include/rtos/spinlock.h b/include/rtos/spinlock.h index 18da564..4252fd1 100644 --- a/include/rtos/spinlock.h +++ b/include/rtos/spinlock.h @@ -5,12 +5,15 @@ struct spinlock { uint8_t locked; - uint8_t priority_locked; uint8_t priority_unlocked; }; +void isr_spinlock_lock(struct spinlock *lock); void spinlock_lock(struct spinlock *lock); + +void isr_spinlock_unlock(struct spinlock *lock); void spinlock_unlock(struct spinlock *lock); -void spinlock_init(struct spinlock *lock, uint8_t priority); + +void spinlock_init(struct spinlock *lock); #endif /* _SPINLOCK_H_ */ diff --git a/main.c b/main.c index 813f6f3..90b9bdb 100644 --- a/main.c +++ b/main.c @@ -24,130 +24,8 @@ #include "board.h" #include "at91_sysc.h" #include "at91_dbgu.h" -#include "memalloc.h" -struct register_context { - uint32_t r0; - uint32_t r1; - uint32_t pc; - uint32_t cpsr; - uint32_t r2; - uint32_t r3; - uint32_t r4; - uint32_t r5; - uint32_t r6; - uint32_t r7; - uint32_t r8; - uint32_t r9; - uint32_t r10; - uint32_t r11; - uint32_t r12; - uint32_t sp; - uint32_t r14; -}; - -struct context { - struct register_context regs; - uint32_t stacksize; -}; - -struct context * create_ctx(uint32_t stacksize, void *code, void *arg) -{ - void *stack = static_alloc(sizeof(struct context) + stacksize); - memset(stack, 0, stacksize); - - struct context *ctx = (struct context *)((uint8_t *)stack + stacksize); - ctx->regs.r0 = (uint32_t)arg; - ctx->regs.pc = (uint32_t)code; - ctx->regs.cpsr = 0x0000001F; - ctx->regs.sp = (uint32_t)ctx; - - ctx->regs.r1 = 0x01010101; - ctx->regs.r2 = 0x02020202; - ctx->regs.r3 = 0x03030303; - ctx->regs.r4 = 0x04040404; - ctx->regs.r5 = 0x05050505; - ctx->regs.r6 = 0x06060606; - ctx->regs.r7 = 0x07070707; - ctx->regs.r8 = 0x08080808; - ctx->regs.r9 = 0x09090909; - ctx->regs.r10 = 0x10101010; - ctx->regs.r11 = 0x11111111; - ctx->regs.r12 = 0x12121212; - ctx->regs.r14 = 0x14141414; - - return ctx; -} - -struct context *current_ctx; - -/* we're in the scheduler, SVC mode */ -void restore_ctx(void) -{ - asm volatile ( - // debug: assume we're in svc mode - "msr cpsr_c, 0x13 \n\r" - - "ldr r0, =current_ctx \n\t" - "ldr r0, [r0] \n\t" - - // get r0, r1 and pc(in lr) - "ldmia r0!, {r1-r2,r14} \n\t" - - // put r0, r1 on (svc)stack - "stmdb sp!, {r1-r2} \n\t" - - // get spsr(in r1), r2-r14 (usermode!) - "ldmia r0, {r1-r14}^ \n\t" - "nop \n\t" - - // store spsr - "msr spsr, r1 \n\t" - - // get r0 & r1 from (svc)stack - "ldmia sp!, {r0, r1} \n\t" - - // jump & restore cpsr from spsr - "movs pc, lr \n\t" - ); -} - -__attribute__((naked)) void SWI_Handler(void) -{ - asm volatile ( - "stmdb sp!, {r0-r1} \n\t" - - "ldr r0, =current_ctx \n\t" - "ldr r0, [r0] \n\t" - "add r0, r0, #68 \n\t" - - "mrs r1, spsr \n\t" - - "stmdb r0, {r1-r14}^ \n\t" - "nop \n\t" - "sub r0, r0, #56 \n\t" - - "ldmia sp!, {r1, r2} \n\t" - "stmdb r0!, {r1, r2, r14} \n\t" - ); - - printf("swi: ok\n\r"); - - asm volatile ( - "ldr r0, =current_ctx \n\t" - "ldr r0, [r0] \n\t" - - "ldmia r0!, {r1-r2,r14} \n\t" - "stmdb sp!, {r1-r2} \n\t" - - "ldmia r0, {r1-r14}^ \n\t" - "nop \n\t" - - "msr spsr, r1 \n\t" - "ldmia sp!, {r0, r1} \n\t" - "movs pc, lr \n\t" - ); -} +#include "rtos/context.h" void testfunc(void *p) { @@ -169,6 +47,5 @@ int main(void) at91_dbgu_init(); at91_dbgu_puts("==========================================================\n\rGood morning Dave\n\r"); - current_ctx = create_ctx(1024, testfunc, 0); - restore_ctx(); + create_ctx(1024, 0x80, testfunc, 0); } diff --git a/src/rtos/context.c b/src/rtos/context.c index 9a6118b..65b66f5 100644 --- a/src/rtos/context.c +++ b/src/rtos/context.c @@ -1 +1,265 @@ #include +#include +#include + +#include "atomic.h" +#include "memalloc.h" + +#include "rtos/context.h" +#include "rtos/spinlock.h" + +/* linked list of ready contexts, #1 is the running thread */ +struct context *run_queue = NULL; + +/* pointer to the running thread */ +struct context *current_context = NULL; + +/* + * SWI_Handler: + * Entry with SVC mode, IRQs are disabled, FIQ is enabled + */ +__attribute__((naked)) void SWI_Handler(void) +{ + /* store register context to current_context */ + asm volatile ( + /* r0-1 -> svc_stack */ + "stmdb sp!, {r0-r1} \n\t" + + /* get top of struct register_context */ + "ldr r0, =current_context \n\t" + "ldr r0, [r0] \n\t" + "add r0, r0, #68 \n\t" + + /* save usermode cpsr & r2-14 */ + "mrs r1, spsr \n\t" + "stmdb r0, {r1-r14}^ \n\t" + "nop \n\t" + "sub r0, r0, #56 \n\t" + + /* save r0-1 and svc_lr (= pc) */ + "ldmia sp!, {r1, r2} \n\t" + "stmdb r0!, {r1, r2, r14} \n\t" + ); + + /* we're no longer #1 in run_queue, switch to new #1 */ + if (current_context != run_queue) + current_context = run_queue; + + /* restore register context from current_context */ + asm volatile ( + /* label for first thread creation */ + ".global __restore_ctx \n\t" + "__restore_ctx: \n\t" + + /* get pointer to struct register_context */ + "ldr r0, =current_context \n\t" + "ldr r0, [r0] \n\t" + + /* get values of r0-1 and pc (= svc_lr) */ + "ldmia r0!, {r1-r2,r14} \n\t" + "stmdb sp!, {r1-r2} \n\t" + + /* restore usermode cpsr & r2-14 */ + "ldmia r0, {r1-r14}^ \n\t" + "nop \n\t" + "msr spsr, r1 \n\t" + + /* get r0-1 from svc_stack, jump back */ + "ldmia sp!, {r0, r1} \n\t" + "movs pc, lr \n\t" + ); +} + + +/* inserts context into run_queue */ +static void isr_context_ready(struct context *ctx) +{ + struct context *q = run_queue; + struct context **qprev = &run_queue; + + while (q && (q->priority <= ctx->priority)) { + qprev = &q->run_queue; + q = q->run_queue; + } + + ctx->run_queue = q; + *qprev = ctx; +} + +static uint8_t isr_context_switch(void) +{ + // TODO: when called from ISR.. this will go boom :) + asm volatile ("swi"); + + return current_context->state; +} + + +static uint8_t __isr_context_wait(struct spinlock *lock, uint8_t sleepstate) +{ + isr_spinlock_unlock(lock); + + run_queue = current_context->run_queue; + current_context->state = sleepstate; + + uint8_t retval = isr_context_switch(); + + isr_spinlock_lock(lock); + + return retval; +} + +uint8_t isr_context_wait(struct spinlock *lock) +{ + return __isr_context_wait(lock, CONTEXT_SLEEP); +} + +uint8_t context_wait(struct spinlock *lock) +{ + disable_irqs(); + uint8_t retval = __isr_context_wait(lock, CONTEXT_SLEEP); + restore_irqs(); + + return retval; +} + +uint8_t context_wait_queue(struct spinlock *lock, struct context **queue) +{ + disable_irqs(); + current_context->sleep_queue = *queue; + *queue = current_context; + + uint8_t retval = __isr_context_wait(lock, CONTEXT_SLEEP_QUEUE); + restore_irqs(); + + return retval; +} + +uint8_t context_wait_pri_queue(struct spinlock *lock, struct context **queue) +{ + disable_irqs(); + + struct context *q = *queue; + while (q && (q->priority <= lock->priority_unlocked)) { + queue = &q->sleep_queue; + q = q->sleep_queue; + } + + current_context->sleep_queue = q; + *queue = current_context; + + uint8_t retval = __isr_context_wait(lock, CONTEXT_SLEEP_QUEUE); + restore_irqs(); + + return retval; +} + +void isr_context_signal(struct context *c) +{ + if (c->state == CONTEXT_SLEEP) { + c->state = CONTEXT_READY; + isr_context_ready(c); + } +} + +void context_signal(struct context *c) +{ + disable_irqs(); + isr_context_signal(c); + restore_irqs(); +} + +uint32_t context_signal_queue(struct context **queue) +{ + disable_irqs(); + + uint32_t retval = 0; + if (*queue) { + struct context *c = *queue; + *queue = c->sleep_queue; + c->state = CONTEXT_READY; + isr_context_ready(c); + retval = 1; + } + + restore_irqs(); + + return retval; +} + +void isr_context_interrupt(struct context *c) +{ + if (c->state == CONTEXT_SLEEP) { + c->state = CONTEXT_INTERRUPTED; + isr_context_ready(c); + } + +} + +void context_interrupt(struct context *c) +{ + disable_irqs(); + isr_context_signal(c); + restore_irqs(); +} + +uint32_t context_interrupt_queue(struct context *c, struct context **queue) +{ + disable_irqs(); + + struct context *q = *queue; + while (q && (q != c)) { + queue = &q->sleep_queue; + q = q->sleep_queue; + } + + uint32_t retval = 0; + + if (q) { + *queue = c->sleep_queue; + c->state = CONTEXT_INTERRUPTED; + isr_context_ready(c); + retval = 1; + } + + restore_irqs(); + + return retval; +} + +struct context * create_ctx(uint32_t stacksize, uint8_t priority, void (* code)(void *arg), void *arg) +{ + void *stack = static_alloc(sizeof(struct context) + stacksize); + + memset(stack, 0, stacksize); + + struct context *ctx = (struct context *)((uint8_t *)stack + stacksize); + + ctx->regs.r0 = (uint32_t)arg; + ctx->regs.pc = (uint32_t)code; + ctx->regs.cpsr = 0x0000001F; + ctx->regs.sp = (uint32_t)ctx; + + ctx->regs.r1 = 0x01010101; + ctx->regs.r2 = 0x02020202; + ctx->regs.r3 = 0x03030303; + ctx->regs.r4 = 0x04040404; + ctx->regs.r5 = 0x05050505; + ctx->regs.r6 = 0x06060606; + ctx->regs.r7 = 0x07070707; + ctx->regs.r8 = 0x08080808; + ctx->regs.r9 = 0x09090909; + ctx->regs.r10 = 0x10101010; + ctx->regs.r11 = 0x11111111; + ctx->regs.r12 = 0x12121212; + ctx->regs.r14 = 0x14141414; + + ctx->stacksize = stacksize; + ctx->priority = priority; + + // TODO: disable irqs? + isr_context_ready(ctx); + + return ctx; +} + diff --git a/src/rtos/semaphore.c b/src/rtos/semaphore.c new file mode 100644 index 0000000..2e06e55 --- /dev/null +++ b/src/rtos/semaphore.c @@ -0,0 +1,56 @@ +#include + +#include "rtos/context.h" +#include "rtos/semaphore.h" +#include "rtos/spinlock.h" + +uint8_t sem_wait(struct semaphore *sem) +{ + uint32_t retval = CONTEXT_READY; + + spinlock_lock(&sem->lock); + + sem->count--; + if (sem->count < 0) + retval = context_wait_pri_queue(&sem->lock, &sem->sleep_queue); + + spinlock_unlock(&sem->lock); + + return retval; +} + +void sem_post(struct semaphore *sem) +{ + spinlock_lock(&sem->lock); + + sem->count++; + if (sem->count <= 0) + context_signal_queue(&sem->sleep_queue); + + spinlock_unlock(&sem->lock); +} + +void sem_interrupt(struct semaphore *sem, struct context *c) +{ + spinlock_lock(&sem->lock); + context_interrupt_queue(c, &sem->sleep_queue); + spinlock_unlock(&sem->lock); +} + +int32_t sem_get_count(struct semaphore *sem) +{ + int32_t retval; + + spinlock_lock(&sem->lock); + retval = sem->count; + spinlock_unlock(&sem->lock); + + return retval; +} + +void sem_init(struct semaphore *sem, int32_t count) +{ + sem->sleep_queue = 0; + spinlock_init(&sem->lock); + sem->count = 0; +} diff --git a/src/rtos/spinlock.c b/src/rtos/spinlock.c new file mode 100644 index 0000000..265d8d0 --- /dev/null +++ b/src/rtos/spinlock.c @@ -0,0 +1,39 @@ +#include + +#include "atomic.h" + +#include "rtos/context.h" +#include "rtos/spinlock.h" + +void isr_spinlock_lock(struct spinlock *lock) +{ + lock->priority_unlocked = current_context->priority; + current_context->priority = 0; + lock->locked = 1; +} + +void spinlock_lock(struct spinlock *lock) +{ + disable_irqs() + isr_spinlock_lock(lock); + restore_irqs(); +} + +void isr_spinlock_unlock(struct spinlock *lock) +{ + current_context->priority = lock->priority_unlocked; + lock->locked = 0; + // TODO: check for context-switch... +} + +void spinlock_unlock(struct spinlock *lock) +{ + disable_irqs() + isr_spinlock_unlock(lock); + restore_irqs(); +} + +void spinlock_init(struct spinlock *lock) +{ + lock->locked = 0; +}