Improve thread code

src/Library/internal-include/System/callbacks_internal.hpp

@@ -1,12 +1,15 @@
 #pragma once
-#include <PSX/System/threads.hpp>
+#include "threads.hpp"
 
 namespace JabyEngine {
     namespace Callback {
         namespace internal {
-            static void execute_thread(SysCall::ThreadHandle thread_handle) {
-                MainThread::prepare_if_main(thread_handle);
-                Thread::execute_next();    
+            static void execute_callback(Thread::Handle thread_handle) {
+                if(CurrentThread::is_me(MainThread::Handle)) {
+                    CurrentThread::replace_with(thread_handle);
+                }
+
+                SysCall::ReturnFromException();
             }
 
             namespace VSync {
@@ -17,7 +20,7 @@ namespace JabyEngine {
                 void                         routine();
 
                 static void [[deprecated("Currently not in use")]] execute() {
-                    execute_thread(VSync::thread_handle);
+                    execute_callback(VSync::thread_handle);
                 }
             }
 
@@ -28,7 +31,7 @@ namespace JabyEngine {
                 extern uint32_t              stack[StackSize];
 
                 static void execute() {
-                    execute_thread(CD::thread_handle);
+                    execute_callback(CD::thread_handle);
                 }
             }
         }

src/Library/internal-include/System/threads.hpp

@@ -0,0 +1,51 @@
+#pragma once
+#include <PSX/System/syscalls.hpp>
+
+namespace JabyEngine {
+    struct Thread {
+        using Handle = SysCall::ThreadHandle;
+
+        static constexpr uint32_t idx_from_handle(SysCall::ThreadHandle thread) {
+            return thread & 0xFFFF;
+        }
+
+        static uintptr_t get_pic_of(Handle handle) {
+            return table_of_tables.threads[idx_from_handle(handle)].epc;
+        }
+
+        template<size_t N>
+        static Handle create(void(*function)(), uint32_t(&stack)[N]) {
+            return SysCall::OpenThread(function, &stack[N - 1], SysCall::get_gp());
+        }
+
+        static void set_kernel_mode_for(SysCall::ThreadHandle handle) {
+            table_of_tables.threads[idx_from_handle(handle)].sr = 0x0;
+        }
+
+        static void set_user_mode_for(SysCall::ThreadHandle handle) {
+            table_of_tables.threads[idx_from_handle(handle)].sr = 0x40000404;
+        }
+    };
+
+    struct CurrentThread {
+        static uintptr_t get_pic() {
+            return table_of_tables.processes->current_tcb->epc;
+        }
+
+        static bool is_me(Thread::Handle handle) {
+            return table_of_tables.processes->current_tcb == &table_of_tables.threads[Thread::idx_from_handle(handle)];
+        }
+
+        static void replace_with(Thread::Handle handle) {
+            table_of_tables.processes->current_tcb = &table_of_tables.threads[Thread::idx_from_handle(handle)];
+        }
+    };
+
+    struct MainThread {
+        static constexpr const Thread::Handle Handle = 0xFF000000;
+
+        static uintptr_t get_pic() {
+            return table_of_tables.threads[0].epc;
+        }
+    };
+}

src/Library/src/BootLoader/callbacks_boot.cpp

@@ -16,11 +16,9 @@ namespace JabyEngine {
                     );
                     Thread::set_user_mode_for(InternalCallback::VSync::thread_handle);*/
 
-                    InternalCallback::CD::thread_handle = SysCall::OpenThread(
+                    InternalCallback::CD::thread_handle = Thread::create(
                         reinterpret_cast<void(*)()>(JabyEngine::CD::internal::IRQ::data_ready_handler),
-                        &InternalCallback::CD::stack[InternalCallback::CD::StackSize - 1],
-                        SysCall::get_gp()
-                    );                    
+                        InternalCallback::CD::stack);                    
                     Thread::set_user_mode_for(InternalCallback::CD::thread_handle);
                 SysCall::ExitCriticalSection();
             }

src/Library/src/CD/cd.cpp

@@ -157,7 +157,7 @@ namespace JabyEngine {
                 void data_ready_handler(uint32_t data) {
                     while(true) {
                         printf("Data: %i\n", data);
-                        SysCall::ChangeThread(0xFF000000);
+                        SysCall::ChangeThread(MainThread::Handle);
                     }
                 }
             }

src/Library/src/System/callbacks.cpp

@@ -19,7 +19,7 @@ namespace JabyEngine {
                         /*if(VSyncCallback::callback) {
                             VSyncCallback::callback();
                         }*/
-                        MainThread::restore();
+                        SysCall::ChangeThread(MainThread::Handle);
                     }
                 }
             }