Added context switching and yield for cooperative multithreading.

4 files changed, 122 insertions, 3 deletions

diff --git a/main.cpp b/main.cpp
index d3c14b3..ea6490d 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,11 +1,15 @@
 #include "stm32.h"
 #include "interrupt.h"
+#include "thread.h"
 
 #include "ppmsum.h"
 #include "i2c.h"
 
 #include "itg3200.h"
 
+#include "usart.h"
+#include "xbee.h"
+
 template<class T>
 inline void saturate(T& var, T absmax) {
 	if(var > absmax) {
@@ -52,6 +56,18 @@ class PID {
 		}
 };
 
+void threadmain() {
+	while(1) {
+		GPIOB.ODR ^= 1 << 1;
+		
+		xbee_send(3, (uint8_t*)"hei");
+	}
+}
+
+uint32_t thstack[1024];
+
+Thread thread(thstack, sizeof(thstack), threadmain);
+
 int main() {
 	RCC.enable(RCC.AFIO);
 	RCC.enable(RCC.IOPA);
@@ -86,9 +102,13 @@ int main() {
 	PID pid_roll(6000, 0, 0);
 	PID pid_yaw(6000, 0, 0);
 	
+	usart_enable();
+	
 	while(1) {
 		// Wait for a new update.
-		while(!(TIM2.SR & 0x01));
+		while(!(TIM2.SR & 0x01)) {
+			Thread::yield();
+		}
 		TIM2.SR = 0;
 		
 		// Read sensors.
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();
+}
diff --git a/thread.h b/thread.h
new file mode 100644
index 0000000..1349ff6
--- /dev/null
+++ b/thread.h
@@ -0,0 +1,61 @@
+#ifndef THREAD_H
+#define THREAD_H
+
+#include <stdint.h>
+
+class Thread {
+	friend void switch_context();
+	
+	private:
+		struct int_frame_t {
+			// Software saved.
+			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 lr_ex;
+			
+			// Hardware saved.
+			uint32_t r0;
+			uint32_t r1;
+			uint32_t r2;
+			uint32_t r3;
+			uint32_t r12;
+			uint32_t lr;
+			uint32_t pc;
+			uint32_t psr;
+		};
+		
+		int_frame_t* sp;
+		
+		Thread* next;
+		
+		static Thread* active_thread;
+		static Thread main_thread;
+		
+		Thread() : next(this) {}
+	
+	public:
+		Thread(void* stack, uint32_t stack_size, void (*func)()) {
+			sp = (int_frame_t*)((uint8_t*)stack + stack_size - sizeof(int_frame_t));
+			
+			sp->lr_ex = 0xfffffff9;
+			
+			// frame->lr = thread exit handler
+			sp->pc = (uint32_t)func;
+			sp->psr = 0x01000000;
+			
+			next = active_thread->next;
+			active_thread->next = this;
+		}
+		
+		static inline void yield() {
+			asm volatile("svc 0");
+		}
+};
+
+#endif
diff --git a/usart.h b/usart.h
index 6c145c6..ab7fb3f 100644
--- a/usart.h
+++ b/usart.h
@@ -4,7 +4,7 @@
 template<>
 void interrupt<Interrupt::USART1>() {
 	USART1.DR;
-	GPIOA.ODR ^= 1 << 5;
+	//GPIOB.ODR ^= 1 << 1;
 }
 
 void usart_enable() {
@@ -16,7 +16,9 @@ void usart_enable() {
 }
 
 void usart_send(uint8_t data) {
-	while(!(USART1.SR & 0x80)); // Wait for TXE.
+	while(!(USART1.SR & 0x80)) {
+		Thread::yield();
+	} // Wait for TXE.
 	
 	USART1.DR = data;
 }