context, spinlock and semaphore functions

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

include/rtos/context.h

@@ -4,24 +4,62 @@
 #include <stdint.h>
 #include "rtos/spinlock.h"
 
-enum state_t {
+struct register_context {
-	CONTEXT_NULL = 0x00,
+	uint32_t r0;
-	CONTEXT_RUNNING,
+	uint32_t r1;
-	CONTEXT_READY,
+	uint32_t pc;
-	CONTEXT_INTERRUPTED,
+	uint32_t cpsr;
-	CONTEXT_SLEEP,
+	uint32_t r2;
-	CONTEXT_SLEEP_QUEUE,
+	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_ */

include/rtos/critical.h

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

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

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

main.c

@@ -24,130 +24,8 @@
 #include "board.h"
 #include "at91_sysc.h"
 #include "at91_dbgu.h"
-#include "memalloc.h"
 
-struct register_context {
+#include "rtos/context.h"
-	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"
-	);
-}
 
 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);
+	create_ctx(1024, 0x80, testfunc, 0);
-	restore_ctx();
 }

src/rtos/context.c

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

src/rtos/semaphore.c

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

src/rtos/spinlock.c

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