smp: fixup threading

smp/internal.c

@@ -8,5 +8,7 @@ bool     __smp_PauseTicker = false;
 
 __smp_Thread **__smp_Current;
 
+smp_thread_ID __smp_Idallo;
+
 tree_Tree *__smp_Threads;
 tree_Tree *__smp_ThreadsWaiting;

smp/internal.h

@@ -35,6 +35,9 @@ extern bool __smp_PauseTicker;
 // __smp_Thread*[], current thread for each core
 extern __smp_Thread **__smp_Current;
 
+// largest thread ID
+extern smp_thread_ID __smp_Idallo;
+
 /* Priority = lastTick + nice
  * So the average value is in fact constantly growing */
 

smp/kthread.c

@@ -6,6 +6,8 @@
 #include "../util/tree.h"
 #include "../interrupt/interrupt.h"
 #include "../driver/irq/pic/rtc/rtc.h"
+#include "../memory/memory.h"
+#include "../memory/paging_internal.h"
 
 
 smp_thread_ID smp_thread_Init() {
@@ -14,13 +16,16 @@ smp_thread_ID smp_thread_Init() {
 	__smp_ThreadsWaiting = tree_Create(sizeof(void *));
 	__smp_Now            = 1;
 
-	smp_thread_ID id   = 1;
-	tree_Node *   node = tree_Insert(__smp_Threads, 1, NULL);
+	smp_thread_ID id   = ++__smp_Idallo;
+	tree_Node *   node = tree_InsertNode(__smp_Threads, id, NULL);
 	__smp_Thread *t    = (__smp_Thread *)node->data;
 	t->nice            = SMP_NICENESS_DEFAULT;
 	t->id              = id;
 	t->lastTick        = 1;
-	__smp_Current[0]   = t;
+	__smp_Count        = 1;
+
+	__smp_Current    = kMalloc(sizeof(void *) * __smp_Count);
+	__smp_Current[0] = t;
 
 	if (!pic_rtc_Enabled)
 		pic_rtc_Init();
@@ -30,6 +35,39 @@ smp_thread_ID smp_thread_Init() {
 	return id;
 }
 
+smp_thread_ID smp_thread_Start(void *entry, const smp_thread_Arguments *args, unsigned int nice) {
+	INTERRUPT_DISABLE;
+
+	smp_thread_ID id   = ++__smp_Idallo;
+	tree_Node *   node = tree_InsertNode(__smp_Threads, id, NULL);
+	__smp_Thread *t    = (__smp_Thread *)node->data;
+	t->nice            = nice;
+	t->id              = id;
+	t->lastTick        = __smp_Now;
+
+	t->state.cs  = GDT_EXEC_SELECTOR;
+	t->state.ss  = 0;
+	t->state.rip = (uint64_t)entry;
+
+	t->state.rdi = args->a;
+	t->state.rsi = args->b;
+	t->state.rdx = args->c;
+	t->state.rcx = args->d;
+	t->state.r8  = args->e;
+	t->state.r9  = args->f;
+
+	// allocate a new 4K stack
+	uint64_t newstack = memory_AllocateKernelMapping(4096, 4096);
+	paging_map_PageAllocated(newstack, 1, MAP_PROT_READ | MAP_PROT_WRITE);
+	t->state.rsp = newstack + 4096;
+
+	// insert the thread into the waiting queue
+	*((void **)tree_Insert(__smp_ThreadsWaiting, t->lastTick + t->nice, 0)) = t;
+
+	INTERRUPT_RESTORE;
+	return id;
+}
+
 int smp_thread_Nice(smp_thread_ID id, int newnice) {
 	INTERRUPT_DISABLE;
 	__smp_Thread *t = tree_Find(__smp_Threads, id);
@@ -49,4 +87,12 @@ int smp_thread_Nice(smp_thread_ID id, int newnice) {
 
 void smp_thread_Yield() {
 	__smp_PauseTicker = true;
+	asm volatile("int $0x28"); // TODO This is just quick and dirty to get into the scheduler
+}
+
+void smp_thread_Sleep(int ticks) {
+	INTERRUPT_DISABLE;
+	__smp_Current[0]->lastTick = __smp_Now + ticks;
+	INTERRUPT_RESTORE;
+	smp_thread_Yield();
 }

smp/kthread_switch.c

@@ -6,10 +6,24 @@
 #include "../runtime/stdio.h"
 #include <string.h>
 
+#include "../util/tree_internal.h"
+
 
 // defined in assembly
 SYSV_ABI void __smp_Switch_Idle();
 
+#define NODE_POINTER(node) (*((__smp_Thread **)(node->data)))
+
+static void printTree(tree_Node *root, int level, __tree_ConnectType type) {
+	if (!root)
+		return;
+	for (int i = 0; i < level * 4; i++)
+		io_WriteConsoleASCII(" ");
+	io_Printf("%s, key=%d, data=%llx, &data=%llx\n", type == __tree_Left ? "Left" : "Right", root->key, (uint64_t)NODE_POINTER(root), root->data);
+	printTree(root->left, level + 1, __tree_Left);
+	printTree(root->right, level + 1, __tree_Right);
+}
+
 SYSV_ABI uintptr_t __smp_Switch() {
 	// the calling function smp_IntSwitch already CLI-ed for us
 
@@ -21,33 +35,42 @@ SYSV_ABI uintptr_t __smp_Switch() {
 	} else
 		__smp_PauseTicker = false;
 
-	io_Printf("__smp_Switch: Tick: %d, switching\n", __smp_Now);
+	//io_Printf("__smp_Switch: Tick: %d, switching\n", __smp_Now);
 
 	__smp_Thread *t = __smp_Current[0];
 
 	tree_Node *node = 0;
 	// insert the current thread back into the waiting queue
-	if (!t) {
-		uint64_t priority = t->nice + __smp_Now; // new priority for the thread
-		node              = tree_InsertNode(__smp_ThreadsWaiting, priority, 0);
+	if (t) {
+		uint64_t priority = t->nice + (t->lastTick > __smp_Now ? t->lastTick : __smp_Now); // new priority for the thread
+		bool     ok       = false;
+		do {
+			node = tree_InsertNode(__smp_ThreadsWaiting, priority, &ok);
+			priority++;
+		} while (!ok);
+		NODE_POINTER(node) = t;
 	}
+	//printTree(__smp_Threads->root, 0, 0);
+	//printTree(__smp_ThreadsWaiting->root, 0, 0);
 	tree_Node *first = tree_FirstNode(__smp_ThreadsWaiting);
 
-	while (first && (*((__smp_Thread **)node->data))->lastTick > __smp_Now)
+	//io_Printf("    first0.id=%d\n", NODE_POINTER(first) ? NODE_POINTER(first)->id : 0);
+	while (first && NODE_POINTER(first)->lastTick > __smp_Now) {
+		//io_Printf("      iterating, .id=%d\n", NODE_POINTER(first) ? NODE_POINTER(first)->id : 0);
 		first = tree_Node_Next(first);
+	}
 
 	if (first == node) {
 		// the current thread is still the first, return
-		io_Printf("    Not context switching, still running %d\n", t ? t->id : 0);
-		if (!t)
+		//io_Printf("    Not context switching, still running %d\n", t ? t->id : 0);
+		if (node)
 			tree_Delete(__smp_ThreadsWaiting, node);
 		return 0;
 	}
 
 	// we need a real context switch
 	// first save the current thread context
-	*((void **)node->data) = t;
-	t->lastTick            = __smp_Now;
+	t->lastTick = __smp_Now;
 	memcpy(&t->state, &__smp_IntSwitch_LastState, sizeof(smp_thread_State));
 
 	if (!first) {
@@ -57,10 +80,10 @@ SYSV_ABI uintptr_t __smp_Switch() {
 		io_WriteConsoleASCII("__smp_Switch: Entering idle\n");
 	} else {
 		// load the new context
-		io_Printf("    Context switching, from %d to %d\n", t ? t->id : 0, ((__smp_Thread *)first->data) ? ((__smp_Thread *)first->data)->id : 0);
-		memcpy(&__smp_IntSwitch_LastState, &((__smp_Thread *)first->data)->state, sizeof(smp_thread_State));
+		io_Printf("    Context switching, from %d to %d\n", t ? t->id : 0, NODE_POINTER(first)->id);
+		memcpy(&__smp_IntSwitch_LastState, &NODE_POINTER(first)->state, sizeof(smp_thread_State));
+		__smp_Current[0] = NODE_POINTER(first);
 		tree_Delete(__smp_ThreadsWaiting, first);
-		__smp_Current[0] = (__smp_Thread *)first->data;
 	}
 
 	return 1;