Use intrusive list to keep track of ready threads.

3 files changed, 36 insertions, 8 deletions

diff --git a/interrupt/fault.cpp b/interrupt/fault.cpp
index 016b74b..9af0eb8 100644
--- a/interrupt/fault.cpp
+++ b/interrupt/fault.cpp
@@ -12,7 +12,11 @@ inline void __attribute__((naked)) switch_context() {
 	asm volatile ("str sp, [%0]" :: "r" (&Thread::active_thread->sp));
 	
 	// Update running thread.
-	Thread::active_thread = Thread::active_thread->next;
+	if(!Thread::reschedule()) {
+		// TODO: Set SLEEPONEXIT
+	} else {
+		// TODO: Clear SLEEPONEXIT
+	}
 	
 	// Fetch stack pointer for new thread.
 	asm volatile ("ldr sp, [%0]" :: "r" (&Thread::active_thread->sp));
diff --git a/os/thread.cpp b/os/thread.cpp
index 426fffd..0f2437d 100644
--- a/os/thread.cpp
+++ b/os/thread.cpp
@@ -1,4 +1,5 @@
 #include "thread.h"
 
-Thread Thread::main_thread __attribute__ ((init_priority (1000)));
+List<Thread> Thread::ready_queue __attribute__ ((init_priority (1000)));
+Thread Thread::main_thread __attribute__ ((init_priority (1001)));
 Thread* Thread::active_thread = &Thread::main_thread;
diff --git a/os/thread.h b/os/thread.h
index 7486d01..72ee275 100644
--- a/os/thread.h
+++ b/os/thread.h
@@ -2,6 +2,7 @@
 #define THREAD_H
 
 #include <stdint.h>
+#include <util/intrusive_list.h>
 
 class Thread {
 	friend void switch_context();
@@ -32,15 +33,38 @@ class Thread {
 		
 		int_frame_t* sp;
 		
-		Thread* next;
+		ListHandle<Thread> queue_handle;
+		
+		static List<Thread> ready_queue;
 		
 		static Thread* active_thread;
 		static Thread main_thread;
 		
-		Thread() : next(this) {}
+		static bool reschedule() {
+			//active_thread = active_thread->next;
+			
+			// TODO: Check whether active thread still is ready.
+			
+			// Move thread to end of ready queue.
+			ready_queue.append(active_thread->queue_handle);
+			
+			// Check whether any threads are ready to run.
+			if(ready_queue.empty()) {
+				return false;
+			}
+			
+			// Set front of ready queue as active.
+			active_thread = ready_queue.begin()->p;
+			
+			return true;
+		}
+		
+		Thread() : queue_handle(this) {
+			ready_queue.append(queue_handle);
+		}
 	
 	public:
-		Thread(void* stack, uint32_t stack_size, void (*func)()) {
+		Thread(void* stack, uint32_t stack_size, void (*func)()) : queue_handle(this) {
 			sp = (int_frame_t*)((uint8_t*)stack + stack_size - sizeof(int_frame_t));
 			
 			sp->lr_ex = 0xfffffff9;
@@ -51,12 +75,11 @@ class Thread {
 		}
 		
 		void start() {
-			next = active_thread->next;
-			active_thread->next = this;
+			ready_queue.append(queue_handle);
 		}
 		
 		static inline void yield() {
-			asm volatile("svc 0");
+			asm volatile("svc 0" ::: "memory");
 		}
 };