From e9f8369ce8b464f178c778ff8bcc3f54fe70b30c Mon Sep 17 00:00:00 2001
From: Vegard Storheil Eriksen <zyp@jvnv.net>
Date: Sat, 3 Sep 2011 18:19:57 +0200
Subject: Added context switching and yield for cooperative multithreading.

---
 thread.cpp | 36 ++++++++++++++++++++++++++++++++++++
 1 file changed, 36 insertions(+)
 create mode 100644 thread.cpp

(limited to 'thread.cpp')

diff --git a/thread.cpp b/thread.cpp
new file mode 100644
index 0000000..7839a19
--- /dev/null
+++ b/thread.cpp
@@ -0,0 +1,36 @@
+#include "thread.h"
+#include "interrupt.h"
+
+Thread Thread::main_thread __attribute__ ((init_priority (1000)));
+Thread* Thread::active_thread = &Thread::main_thread;
+
+inline void __attribute__((naked)) switch_context() {
+	asm volatile ("cpsid i");
+	
+	// Save unsaved registers.
+	asm volatile ("push {r4, r5, r6, r7, r8, r9, r10, r11, lr}" ::: "memory");
+	
+	// Store stack pointer for old thread.
+	asm volatile ("str sp, [%0]" :: "r" (&Thread::active_thread->sp));
+	
+	// Update running thread.
+	Thread::active_thread = Thread::active_thread->next;
+	
+	// Fetch stack pointer for new thread.
+	asm volatile ("ldr sp, [%0]" :: "r" (&Thread::active_thread->sp));
+	
+	asm volatile ("cpsie i");
+	
+	// Load registers and return.
+	asm volatile ("pop {r4, r5, r6, r7, r8, r9, r10, r11, pc}" ::: "memory");
+}
+
+template<>
+void interrupt<Interrupt::SysTick>() {
+	switch_context();
+}
+
+template<>
+void interrupt<Interrupt::SVCall>() {
+	switch_context();
+}
-- 
cgit v1.2.3