Initial threading in main firmware

inc/decls.h

@@ -31,6 +31,7 @@
 #include "intrinsics.h"
 
 #include "board.h"
+#include "thread.h"
 #include "time.h"
 #include "timer.h"
 #include "usb.h"

inc/thread.h

@@ -0,0 +1,35 @@
+/*
+ * thread.h
+ * 
+ * Cooperative multitasking.
+ * 
+ * Written & released by Keir Fraser <[email protected]> and Eric Anderson
+ * <[email protected]>
+ * 
+ * This is free and unencumbered software released into the public domain.
+ * See the file COPYING for more details, or visit <http://unlicense.org>.
+ */
+
+struct thread {
+    /* Internal bookkeeping */
+    bool_t exited;
+};
+
+/* Initialize a thread and queue it for execution. 'thread' must remain
+ * allocated for the lifetime of the thread. */
+void thread_start(struct thread *thread, uint32_t *stack, void (*func)(void*), void* arg);
+
+/* Yield execution to allow other threads to run. */
+void thread_yield(void);
+
+/* Returns true if provided thread has exited. A thread cannot be joined
+ * multiple times, unless it is started anew. */
+bool_t thread_tryjoin(struct thread *thread);
+
+/* Continuously yields until provided thread has exited. A thread cannot be
+ * joined multiple times, unless it is started anew. */
+void thread_join(struct thread *thread);
+
+/* Reinitializes threading subsystem to its initial state, throwing away all
+ * threads but the current. */
+void thread_reset(void);

src/Makefile

@@ -23,6 +23,7 @@ else
 OBJS += main.o
 OBJS += floppy.o
 OBJS += testmode.o
+OBJS += thread.o
 
 endif
 

src/main.c

@@ -11,6 +11,9 @@
 
 int EXC_reset(void) __attribute__((alias("main")));
 
+static uint32_t usb_stack[1024/4];
+static struct thread usb_thread;
+
 static void canary_init(void)
 {
     _irq_stackbottom[0] = _thread_stackbottom[0] = 0xdeadbeef;
@@ -22,6 +25,16 @@ static void canary_check(void)
     ASSERT(_thread_stackbottom[0] == 0xdeadbeef);
 }
 
+static void usb_thread_main(void *unused)
+{
+    usb_stack[0] = 0xdeadbeef;
+    for (;;) {
+        ASSERT(usb_stack[0] == 0xdeadbeef);
+        usb_process();
+        thread_yield();
+    }
+}
+
 int main(void)
 {
     /* Relocate DATA. Initialise BSS. */
@@ -43,10 +56,13 @@ int main(void)
     floppy_init();
     usb_init();
 
+    thread_start(&usb_thread, &usb_stack[ARRAY_SIZE(usb_stack)],
+                 usb_thread_main, NULL);
+
     for (;;) {
         canary_check();
-        usb_process();
         floppy_process();
+        thread_yield();
     }
 
     return 0;

src/thread.c

@@ -0,0 +1,96 @@
+/*
+ * thread.c
+ * 
+ * Cooperative multitasking.
+ * 
+ * Written & released by Keir Fraser <[email protected]> and Eric Anderson
+ * <[email protected]>
+ * 
+ * This is free and unencumbered software released into the public domain.
+ * See the file COPYING for more details, or visit <http://unlicense.org>.
+ */
+
+/* Holds stack pointer. */
+static uint32_t *waiting_thread;
+
+__attribute__((naked))
+static void _thread_yield(uint32_t *new_stack, uint32_t **save_stack_pointer)
+{
+    asm (
+        "    stmdb sp!,{r4-r11,lr}\n"
+        "    str   sp,[r1]\n"
+        "    b     resume\n"
+        );
+}
+
+void thread_yield(void)
+{
+    if (!waiting_thread)
+        return;
+    _thread_yield(waiting_thread, &waiting_thread);
+}
+
+__attribute__((naked))
+static void resume(uint32_t *stack)
+{
+    asm (
+        "    mov   sp,r0\n"
+        "    ldmia sp!,{r4-r11,lr}\n"
+        "    bx    lr\n"
+        );
+}
+
+void _thread_main(struct thread *thread, void (*func)(void*), void *arg)
+{
+    uint32_t *other_thread;
+    func(arg);
+    thread->exited = TRUE;
+
+    other_thread = waiting_thread;
+    waiting_thread = 0;
+    resume(other_thread);
+    ASSERT(0); /* unreachable */
+}
+
+__attribute__((naked))
+static void _main(void)
+{
+    asm (
+        "    mov r0,r9\n"
+        "    mov r1,r10\n"
+        "    mov r2,r11\n"
+        "    b   _thread_main\n"
+        );
+}
+
+void thread_start(struct thread *thread, uint32_t *stack,
+                  void (*func)(void *), void* arg)
+{
+    memset(thread, 0, sizeof(*thread));
+    ASSERT(!waiting_thread);
+    /* Fake thread_yield storage */
+    *(--stack) = (uint32_t)_main; /* rl */
+    *(--stack) = (uint32_t)arg; /* r11 */
+    *(--stack) = (uint32_t)func; /* r10 */
+    *(--stack) = (uint32_t)thread; /* r9 */
+    stack -= 5; /* r4-r8 */
+    waiting_thread = stack;
+}
+
+bool_t thread_tryjoin(struct thread *thread)
+{
+    bool_t exited = thread->exited;
+    thread->exited = FALSE;
+    return exited;
+}
+
+void thread_join(struct thread *thread)
+{
+    while (!thread->exited)
+        thread_yield();
+}
+
+void thread_reset(void)
+{
+    waiting_thread = NULL;
+}