working semaphore & threading

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

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);
+	while (1) {
-
+		printf("testfunc(%ld): sem_wait()\n\r", (uint32_t)p);
-	asm volatile ("swi");
+		sem_wait(&sem);
-
+	}
-	printf("testfunc: %p\n\r", p);
-	while(1);
 }
 
 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);

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 <stdint.h>
+#include <stdio.h>
+#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);
+}

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 :)
+	run_queue = current_context->run_queue;
-	asm volatile ("swi");
+	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();
+	}
+}