From 4c78239721ffb219866757bfb19b7c962cab610a Mon Sep 17 00:00:00 2001
From: Jonas 'Sortie' Termansen <sortie@maxsi.org>
Date: Thu, 2 Oct 2014 15:38:06 +0200
Subject: [PATCH] Implement yielding a timeslice to another thread.

---
 kernel/include/sortix/kernel/scheduler.h |  1 +
 kernel/include/sortix/kernel/thread.h    |  4 ++
 kernel/scheduler.cpp                     | 71 ++++++++++++++++++++----
 kernel/thread.cpp                        |  2 +
 4 files changed, 66 insertions(+), 12 deletions(-)

diff --git a/kernel/include/sortix/kernel/scheduler.h b/kernel/include/sortix/kernel/scheduler.h
index 74f8b994..d14f2f51 100644
--- a/kernel/include/sortix/kernel/scheduler.h
+++ b/kernel/include/sortix/kernel/scheduler.h
@@ -67,6 +67,7 @@ void SaveInterruptedContext(const struct interrupt_context* intctx,
                             struct thread_registers* registers);
 void LoadInterruptedContext(struct interrupt_context* intctx,
                             const struct thread_registers* registers);
+void ScheduleTrueThread();
 
 } // namespace Scheduler
 } // namespace Sortix
diff --git a/kernel/include/sortix/kernel/thread.h b/kernel/include/sortix/kernel/thread.h
index 58255da3..8e8d37c2 100644
--- a/kernel/include/sortix/kernel/thread.h
+++ b/kernel/include/sortix/kernel/thread.h
@@ -25,6 +25,8 @@
 #ifndef INCLUDE_SORTIX_KERNEL_THREAD_H
 #define INCLUDE_SORTIX_KERNEL_THREAD_H
 
+#include <stdint.h>
+
 #include <sortix/sigaction.h>
 #include <sortix/signal.h>
 #include <sortix/sigset.h>
@@ -65,6 +67,8 @@ public:
 	~Thread();
 
 public:
+	uintptr_t system_tid;
+	uintptr_t yield_to_tid;
 	struct thread_registers registers;
 	uint8_t* self_allocation;
 	size_t id;
diff --git a/kernel/scheduler.cpp b/kernel/scheduler.cpp
index 4277c9ba..4afc59a1 100644
--- a/kernel/scheduler.cpp
+++ b/kernel/scheduler.cpp
@@ -193,23 +193,51 @@ void SwitchThread(struct interrupt_context* intctx, Thread* prev, Thread* next)
 
 static Thread* idle_thread;
 static Thread* first_runnable_thread;
+static Thread* true_current_thread;
 static Process* init_process;
 
-static Thread* PopNextThread()
+static Thread* FindRunnableThreadWithSystemTid(uintptr_t system_tid)
 {
-	if ( first_runnable_thread )
+	Thread* begun_thread = current_thread;
+	Thread* iter = begun_thread;
+	do
 	{
-		Thread* result = first_runnable_thread;
-		first_runnable_thread = first_runnable_thread->scheduler_list_next;
-		return result;
-	}
-
-	return idle_thread;
+		if ( iter->system_tid == system_tid )
+			return iter;
+		iter = iter->scheduler_list_next;
+	} while ( iter != begun_thread );
+	return NULL;
 }
 
-void Switch(struct interrupt_context* intctx)
+static Thread* PopNextThread(bool yielded)
 {
-	SwitchThread(intctx, CurrentThread(), PopNextThread());
+	Thread* result;
+
+	uintptr_t yield_to_tid = current_thread->yield_to_tid;
+	if ( yielded && yield_to_tid != 0 )
+	{
+		if ( (result = FindRunnableThreadWithSystemTid(yield_to_tid)) )
+			return result;
+	}
+
+	if ( first_runnable_thread )
+	{
+		result = first_runnable_thread;
+		first_runnable_thread = first_runnable_thread->scheduler_list_next;
+	}
+	else
+	{
+		result = idle_thread;
+	}
+
+	true_current_thread = result;
+
+	return result;
+}
+
+static void RealSwitch(struct interrupt_context* intctx, bool yielded)
+{
+	SwitchThread(intctx, CurrentThread(), PopNextThread(yielded));
 
 	if ( intctx->signal_pending && InUserspace(intctx) )
 	{
@@ -218,15 +246,20 @@ void Switch(struct interrupt_context* intctx)
 	}
 }
 
+void Switch(struct interrupt_context* intctx)
+{
+	RealSwitch(intctx, false);
+}
+
 void InterruptYieldCPU(struct interrupt_context* intctx, void* /*user*/)
 {
-	Switch(intctx);
+	RealSwitch(intctx, true);
 }
 
 void ThreadExitCPU(struct interrupt_context* intctx, void* /*user*/)
 {
 	SetThreadState(current_thread, ThreadState::DEAD);
-	InterruptYieldCPU(intctx, NULL);
+	RealSwitch(intctx, false);
 }
 
 // The idle thread serves no purpose except being an infinite loop that does
@@ -237,6 +270,7 @@ void SetIdleThread(Thread* thread)
 	idle_thread = thread;
 	SetThreadState(thread, ThreadState::NONE);
 	current_thread = thread;
+	true_current_thread = thread;
 }
 
 void SetInitProcess(Process* init)
@@ -304,9 +338,22 @@ static int sys_sched_yield(void)
 	return kthread_yield(), 0;
 }
 
+void ScheduleTrueThread()
+{
+	bool wasenabled = Interrupt::SetEnabled(false);
+	if ( true_current_thread != current_thread )
+	{
+		current_thread->yield_to_tid = 0;
+		first_runnable_thread = true_current_thread;
+		kthread_yield();
+	}
+	Interrupt::SetEnabled(wasenabled);
+}
+
 void Init()
 {
 	first_runnable_thread = NULL;
+	true_current_thread = NULL;
 	idle_thread = NULL;
 
 	Syscall::Register(SYSCALL_SCHED_YIELD, (void*) sys_sched_yield);
diff --git a/kernel/thread.cpp b/kernel/thread.cpp
index f63c3e9a..83215f93 100644
--- a/kernel/thread.cpp
+++ b/kernel/thread.cpp
@@ -78,6 +78,8 @@ void FreeThread(Thread* thread)
 Thread::Thread()
 {
 	assert(!((uintptr_t) registers.fpuenv & 0xFUL));
+	system_tid = (uintptr_t) this;
+	yield_to_tid = 0;
 	id = 0; // TODO: Make a thread id.
 	process = NULL;
 	prevsibling = NULL;