3 files changed, 53 insertions, 12 deletions

diff --git a/os/thread.cpp b/os/thread.cpp
index 0f2437d..432727a 100644
--- a/os/thread.cpp
+++ b/os/thread.cpp
@@ -1,5 +1,6 @@
 #include "thread.h"
 
 List<Thread> Thread::ready_queue __attribute__ ((init_priority (1000)));
+List<Thread> Thread::sleep_queue;
 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 72ee275..9270727 100644
--- a/os/thread.h
+++ b/os/thread.h
@@ -4,6 +4,8 @@
 #include <stdint.h>
 #include <util/intrusive_list.h>
 
+#include "time.h"
+
 class Thread {
 	friend void switch_context();
 	
@@ -33,20 +35,42 @@ class Thread {
 		
 		int_frame_t* sp;
 		
+		enum State {
+			Running,
+			Sleeping,
+		};
+		
+		State state;
+		uint32_t sleep_deadline;
+		
 		ListHandle<Thread> queue_handle;
 		
 		static List<Thread> ready_queue;
+		static List<Thread> sleep_queue;
 		
 		static Thread* active_thread;
 		static Thread main_thread;
 		
 		static bool reschedule() {
-			//active_thread = active_thread->next;
+			uint32_t now = Time::time();
 			
-			// TODO: Check whether active thread still is ready.
+			// Go through sleep queue.
+			for(auto h : sleep_queue) {
+				// Move to ready queue if deadline has expired.
+				if(h->p->sleep_deadline <= now) {
+					h->p->state = Running;
+					ready_queue.append(*h);
+					
+					// Moving one element makes the iterator unusable, so break here.
+					break;
+				}
+			}
 			
-			// Move thread to end of ready queue.
-			ready_queue.append(active_thread->queue_handle);
+			// Check whether active thread still is ready.
+			if(active_thread->state == Running) {
+				// 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()) {
@@ -78,6 +102,29 @@ class Thread {
 			ready_queue.append(queue_handle);
 		}
 		
+		void set_sleeping(uint32_t until) {
+			sleep_deadline = until;
+			state = Sleeping;
+			
+			// Search for thread with later deadline.
+			for(auto h : sleep_queue) {
+				// Insert before if found.
+				if(h->p->sleep_deadline > sleep_deadline) {
+					queue_handle.insert_before(*h);
+					return;
+				}
+			}
+			
+			// Otherwise append to end of queue.
+			sleep_queue.append(queue_handle);
+		}
+		
+		static void sleep(uint32_t delay) {
+			active_thread->set_sleeping(Time::time() + delay);
+			
+			yield();
+		}
+		
 		static inline void yield() {
 			asm volatile("svc 0" ::: "memory");
 		}
diff --git a/os/time.h b/os/time.h
index f4e3ce6..6287834 100644
--- a/os/time.h
+++ b/os/time.h
@@ -1,7 +1,7 @@
 #ifndef TIME_H
 #define TIME_H
 
-#include "thread.h"
+#include <stdint.h>
 
 struct STK_t {
 	volatile uint32_t CTRL;
@@ -24,13 +24,6 @@ class Time {
 		inline static uint32_t time() {
 			return systime;
 		}
-		
-		inline static void sleep(uint32_t ms) {
-			ms += systime;
-			while(systime < ms) {
-				Thread::yield();
-			}
-		}
 };
 
 #endif