From 52893a08feccdbaf08065fedde61b221ee5190c0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bj=C3=B6rn=20Gaier?= <Bjoern.Gaier@horiba.com>
Date: Fri, 14 Jun 2024 20:53:30 +0200
Subject: [PATCH] Support CD Callback

---
 .../System/callbacks_internal.hpp             | 24 +++++++++++++------
 .../internal-include/System/threads.hpp       |  8 +++++--
 src/Library/src/BootLoader/callbacks_boot.cpp |  4 +---
 src/Library/src/CD/cd.cpp                     |  5 ++--
 4 files changed, 26 insertions(+), 15 deletions(-)

diff --git a/src/Library/internal-include/System/callbacks_internal.hpp b/src/Library/internal-include/System/callbacks_internal.hpp
index b3250b4a..a921682f 100644
--- a/src/Library/internal-include/System/callbacks_internal.hpp
+++ b/src/Library/internal-include/System/callbacks_internal.hpp
@@ -4,14 +4,20 @@
 namespace JabyEngine {
     namespace Callback {
         namespace internal {
-            static void execute_callback(Thread::Handle thread_handle) {
+            static void execute_callback(Thread::Handle thread_handle, uint32_t parm) {
                 if(CurrentThread::is_me(MainThread::Handle)) {
                     CurrentThread::replace_with(thread_handle);
+                    CurrentThread::force_a0(parm);
                 }
-
                 SysCall::ReturnFromException();
             }
 
+            static uint32_t resume_callback(Thread::Handle handle) {
+                SysCall::ChangeThread(handle);
+                asm("sw $a0, %0" : "=m"(handle));
+                return handle;
+            }
+
             namespace VSync {
                 static constexpr size_t StackSize = 64;
 
@@ -20,18 +26,22 @@ namespace JabyEngine {
                 void                         routine();
 
                 static void [[deprecated("Currently not in use")]] execute() {
-                    execute_callback(VSync::thread_handle);
+                    execute_callback(VSync::thread_handle, 0);
                 }
             }
 
             namespace CD {
                 static constexpr size_t StackSize = 256;
 
-                extern SysCall::ThreadHandle thread_handle;
-                extern uint32_t              stack[StackSize];
+                extern Thread::Handle thread_handle;
+                extern uint32_t       stack[StackSize];
 
-                static void execute() {
-                    execute_callback(CD::thread_handle);
+                static void execute(uint32_t parm) {
+                    execute_callback(CD::thread_handle, parm);
+                }
+
+                static uint32_t resume() {
+                    return resume_callback(MainThread::Handle);
                 }
             }
         }
diff --git a/src/Library/internal-include/System/threads.hpp b/src/Library/internal-include/System/threads.hpp
index 155ecbdd..22f7450b 100644
--- a/src/Library/internal-include/System/threads.hpp
+++ b/src/Library/internal-include/System/threads.hpp
@@ -14,8 +14,8 @@ namespace JabyEngine {
         }
 
         template<size_t N>
-        static Handle create(void(*function)(), uint32_t(&stack)[N]) {
-            return SysCall::OpenThread(function, &stack[N - 1], SysCall::get_gp());
+        static Handle create(void(*function)(uint32_t), uint32_t(&stack)[N]) {
+            return SysCall::OpenThread(reinterpret_cast<void(*)()>(function), &stack[N - 1], SysCall::get_gp());
         }
 
         static void set_kernel_mode_for(SysCall::ThreadHandle handle) {
@@ -32,6 +32,10 @@ namespace JabyEngine {
             return table_of_tables.processes->current_tcb->epc;
         }
 
+        static void force_a0(uint32_t a0) {
+            table_of_tables.processes->current_tcb->reg[4] = a0;
+        }
+
         static bool is_me(Thread::Handle handle) {
             return table_of_tables.processes->current_tcb == &table_of_tables.threads[Thread::idx_from_handle(handle)];
         }
diff --git a/src/Library/src/BootLoader/callbacks_boot.cpp b/src/Library/src/BootLoader/callbacks_boot.cpp
index f9a421dd..4fa7e058 100644
--- a/src/Library/src/BootLoader/callbacks_boot.cpp
+++ b/src/Library/src/BootLoader/callbacks_boot.cpp
@@ -16,9 +16,7 @@ namespace JabyEngine {
                     );
                     Thread::set_user_mode_for(InternalCallback::VSync::thread_handle);*/
 
-                    InternalCallback::CD::thread_handle = Thread::create(
-                        reinterpret_cast<void(*)()>(JabyEngine::CD::internal::IRQ::data_ready_handler),
-                        InternalCallback::CD::stack);                    
+                    InternalCallback::CD::thread_handle = Thread::create(JabyEngine::CD::internal::IRQ::data_ready_handler, InternalCallback::CD::stack);                    
                     Thread::set_user_mode_for(InternalCallback::CD::thread_handle);
                 SysCall::ExitCriticalSection();
             }
diff --git a/src/Library/src/CD/cd.cpp b/src/Library/src/CD/cd.cpp
index a4bef6b3..487f3b44 100644
--- a/src/Library/src/CD/cd.cpp
+++ b/src/Library/src/CD/cd.cpp
@@ -104,7 +104,6 @@ namespace JabyEngine {
 
                 static void handler(uint32_t) {
                     const auto old_status  = CD_IO::IndexStatus.read();
-                    void (*exit_routine)() = Callback::internal::CD::execute;
 
                     CD_IO::PortIndex1::change_to();
                     const auto cur_irq = CD_IO::Interrupt::get_type(CD_IO::PortIndex1::InterruptFlag);
@@ -151,13 +150,13 @@ namespace JabyEngine {
 
                     // No masking required because we can only write bit 0 - 2
                     CD_IO::IndexStatus.write(old_status);
-                    exit_routine();
+                    Callback::internal::CD::execute(cur_irq);
                 }
 
                 void data_ready_handler(uint32_t data) {
                     while(true) {
                         printf("Data: %i\n", data);
-                        SysCall::ChangeThread(MainThread::Handle);
+                        data = Callback::internal::CD::resume();
                     }
                 }
             }