From 021be8058924d96bc3a5227eda97a17bccccb3eb Mon Sep 17 00:00:00 2001 From: Olaf Rempel Date: Mon, 25 Feb 2008 18:16:42 +0100 Subject: [PATCH] working semaphore & threading --- include/rtos/context.h | 7 ++++- main.c | 34 ++++++++++++++++----- src/at91_pio.c | 69 ++++++++++++++++++++++++++++++++++++++++++ src/rtos/context.c | 59 ++++++++++++++++++++++++++++++------ 4 files changed, 151 insertions(+), 18 deletions(-) create mode 100644 src/at91_pio.c diff --git a/include/rtos/context.h b/include/rtos/context.h index b6bf4d5..8140b2f 100644 --- a/include/rtos/context.h +++ b/include/rtos/context.h @@ -47,7 +47,8 @@ struct context { extern struct context *current_context; -struct context * create_ctx(uint32_t stacksize, uint8_t priority, void (* code)(void *arg), void *arg); +void isr_context_yield(void); +void context_yield(void); uint8_t isr_context_wait(struct spinlock *lock); uint8_t context_wait(struct spinlock *lock); @@ -62,4 +63,8 @@ void isr_context_interrupt(struct context *c); void context_interrupt(struct context *c); uint32_t context_interrupt_queue(struct context *c, struct context **queue); +struct context * create_ctx(uint32_t stacksize, uint8_t priority, void (* code)(void *arg), void *arg); + +void init_context(void); + #endif /* _CONTEXT_H_ */ diff --git a/main.c b/main.c index 90b9bdb..233c50a 100644 --- a/main.c +++ b/main.c @@ -24,17 +24,25 @@ #include "board.h" #include "at91_sysc.h" #include "at91_dbgu.h" +#include "at91_pio.h" #include "rtos/context.h" +#include "rtos/semaphore.h" + +static struct semaphore sem; + +void testisr(uint32_t status, uint32_t input) +{ + printf("testisr: sem_post()\n\r"); + sem_post(&sem); +} void testfunc(void *p) { - printf("testfunc: %p\n\r", p); - - asm volatile ("swi"); - - printf("testfunc: %p\n\r", p); - while(1); + while (1) { + printf("testfunc(%ld): sem_wait()\n\r", (uint32_t)p); + sem_wait(&sem); + } } int main(void) @@ -47,5 +55,17 @@ int main(void) at91_dbgu_init(); at91_dbgu_puts("==========================================================\n\rGood morning Dave\n\r"); - create_ctx(1024, 0x80, testfunc, 0); + at91_pio_init(); + + struct context *text_ctx1 = create_ctx(256, 0x80, testfunc, (void *)1); + printf("test_ctx(1)=%p\n\r", text_ctx1); + + struct context *text_ctx2 = create_ctx(256, 0x80, testfunc, (void *)2); + printf("test_ctx(2)=%p\n\r", text_ctx2); + + sem_init(&sem, 0); + + init_context(); } + +PIO_PINCHANGE_ISR(TAST1, testisr); diff --git a/src/at91_pio.c b/src/at91_pio.c new file mode 100644 index 0000000..6a990d4 --- /dev/null +++ b/src/at91_pio.c @@ -0,0 +1,69 @@ +/*************************************************************************** + * Copyright (C) 01/2008 by Olaf Rempel * + * razzor@kopf-tisch.de * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; version 2 of the License * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program; if not, write to the * + * Free Software Foundation, Inc., * + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * + ***************************************************************************/ +#include +#include +#include "AT91SAM7S256.h" +#include "board.h" +#include "at91_pio.h" + +/* extern symbols, defined in ldscript */ +extern struct pio_pinchange_isr _pio_isr_table; +extern struct pio_pinchange_isr _pio_isr_table_end; + +static void pio_isr(void) +{ + uint32_t status = *AT91C_PIOA_ISR; + uint32_t input = *AT91C_PIOA_PDSR; + + struct pio_pinchange_isr *isr; + for (isr = &_pio_isr_table; isr < &_pio_isr_table_end; isr++) + if (isr->mask & status) + isr->func(status, input); +} + +void pio_trigger_isr(uint32_t mask) +{ + uint32_t input = *AT91C_PIOA_PDSR; + + struct pio_pinchange_isr *isr; + for (isr = &_pio_isr_table; isr < &_pio_isr_table_end; isr++) + if (isr->mask & mask) + isr->func(mask, input); +} + +void at91_pio_init(void) +{ + /* enable PIO clock */ + *AT91C_PMC_PCER = (1 << AT91C_ID_PIOA); + + /* enable pinchange interrupts */ + struct pio_pinchange_isr *isr; + for (isr = &_pio_isr_table; isr < &_pio_isr_table_end; isr++) + *AT91C_PIOA_IER = isr->mask; + + /* dummy read to clear interrupts */ + uint32_t dummy = *AT91C_PIOA_ISR; + dummy = dummy; + + /* low priority, level triggered, own vector */ + AT91S_AIC *aic = AT91C_BASE_AIC; + aic->AIC_SMR[AT91C_ID_PIOA] = IRQPRIO_PIOA | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL; + aic->AIC_SVR[AT91C_ID_PIOA] = (uint32_t)pio_isr; + aic->AIC_IECR = (1 << AT91C_ID_PIOA); +} diff --git a/src/rtos/context.c b/src/rtos/context.c index 65b66f5..a04edcc 100644 --- a/src/rtos/context.c +++ b/src/rtos/context.c @@ -45,12 +45,10 @@ __attribute__((naked)) void SWI_Handler(void) if (current_context != run_queue) current_context = run_queue; + current_context->state = CONTEXT_RUNNING; + /* 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" @@ -70,6 +68,13 @@ __attribute__((naked)) void SWI_Handler(void) ); } +static uint8_t isr_context_switch(void) +{ + asm volatile ("swi"); + + // TODO: return previous state of now running thread + return CONTEXT_READY; +} /* inserts context into run_queue */ static void isr_context_ready(struct context *ctx) @@ -86,14 +91,24 @@ static void isr_context_ready(struct context *ctx) *qprev = ctx; } -static uint8_t isr_context_switch(void) +/* process yields, try to switch to process with lower or same prio */ +void isr_context_yield(void) { - // TODO: when called from ISR.. this will go boom :) - asm volatile ("swi"); + run_queue = current_context->run_queue; + isr_context_ready(current_context); - return current_context->state; + if (current_context != run_queue) { + current_context->state = CONTEXT_READY; + isr_context_switch(); + } } +void context_yield(void) +{ + disable_irqs(); + isr_context_yield(); + restore_irqs(); +} static uint8_t __isr_context_wait(struct spinlock *lock, uint8_t sleepstate) { @@ -229,10 +244,13 @@ uint32_t context_interrupt_queue(struct context *c, struct context **queue) struct context * create_ctx(uint32_t stacksize, uint8_t priority, void (* code)(void *arg), void *arg) { - void *stack = static_alloc(sizeof(struct context) + stacksize); + uint32_t *stack = static_alloc(sizeof(struct context) + stacksize); memset(stack, 0, stacksize); + // TODO: check on context-switch for corruption */ + stack[0] = 0xdeadbeef; + struct context *ctx = (struct context *)((uint8_t *)stack + stacksize); ctx->regs.r0 = (uint32_t)arg; @@ -257,9 +275,30 @@ struct context * create_ctx(uint32_t stacksize, uint8_t priority, void (* code)( ctx->stacksize = stacksize; ctx->priority = priority; - // TODO: disable irqs? + disable_irqs(); isr_context_ready(ctx); + restore_irqs(); return ctx; } +void init_context(void) +{ + /* + * create shallow idle context + * idle context runs in SVC mode, so no real stack is needed + */ + current_context = create_ctx(4, 255, NULL, NULL); + printf("idle_ctx=%p\n\r", current_context); + + /* For now, we're the only thread, so this simply safes our context */ + disable_irqs(); + isr_context_switch(); + restore_irqs(); + + /* idle loop */ + while (1) { + // TODO: this sucks, context switch after interrupt are way better.. + context_yield(); + } +}