razzor/sam7fc
1
0
Fork 0
Code Issues Pull Requests Releases Activity

context, spinlock and semaphore functions

Browse Source
This commit is contained in:
Olaf Rempel 2008-02-24 20:07:46 +01:00
parent 991b088b0d
commit 9e8158277c
9 changed files with 419 additions and 152 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@ -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

60
include/rtos/context.h
View File

@ -4,24 +4,62 @@
#include <stdint.h>
#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_ */

10
include/rtos/critical.h
View File

@ -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_ */

6
include/rtos/semaphore.h
View File

@ -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_ */

7
include/rtos/spinlock.h
View File

@ -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_ */

127
main.c
View File

@ -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);
}

264
src/rtos/context.c
View File

@ -1 +1,265 @@
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#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;
}

56
src/rtos/semaphore.c Normal file
View File

@ -0,0 +1,56 @@
#include <stdint.h>
#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;
}

39
src/rtos/spinlock.c Normal file
View File

@ -0,0 +1,39 @@
#include <stdint.h>
#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;
}