diff --git a/smp/internal.c b/smp/internal.c index 32d30ab..854f075 100644 --- a/smp/internal.c +++ b/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; diff --git a/smp/internal.h b/smp/internal.h index 5f3455c..d751c53 100644 --- a/smp/internal.h +++ b/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 */ diff --git a/smp/kthread.c b/smp/kthread.c index f1c8c7d..d57e24e 100644 --- a/smp/kthread.c +++ b/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(); } diff --git a/smp/kthread_switch.c b/smp/kthread_switch.c index d3af6d7..b615b2d 100644 --- a/smp/kthread_switch.c +++ b/smp/kthread_switch.c @@ -6,10 +6,24 @@ #include "../runtime/stdio.h" #include +#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;