From 7fbcceb657f6fc0b2d549a628497861e1104fca1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?S=C3=B6nke=20Holz?= <sholz8530@gmail.com>
Date: Thu, 15 Feb 2024 16:37:00 +0100
Subject: [PATCH] Kernel/riscv64: Implement enter_thread_context

This code is based on the aarch64 implementation.
---
 Kernel/Arch/riscv64/CSR.h         |  5 +++++
 Kernel/Arch/riscv64/Processor.cpp | 30 +++++++++++++++++++++++++++++-
 2 files changed, 34 insertions(+), 1 deletion(-)

diff --git a/Kernel/Arch/riscv64/CSR.h b/Kernel/Arch/riscv64/CSR.h
index a1f3cff971..7829474444 100644
--- a/Kernel/Arch/riscv64/CSR.h
+++ b/Kernel/Arch/riscv64/CSR.h
@@ -93,6 +93,11 @@ struct [[gnu::packed]] alignas(u64) SATP {
     {
         return bit_cast<SATP>(CSR::read(CSR::Address::SATP));
     }
+
+    bool operator==(SATP const& other) const
+    {
+        return bit_cast<u64>(*this) == bit_cast<u64>(other);
+    }
 };
 static_assert(AssertSize<SATP, 8>());
 
diff --git a/Kernel/Arch/riscv64/Processor.cpp b/Kernel/Arch/riscv64/Processor.cpp
index 9e8c355c1b..dd24dff145 100644
--- a/Kernel/Arch/riscv64/Processor.cpp
+++ b/Kernel/Arch/riscv64/Processor.cpp
@@ -58,7 +58,7 @@ static void store_fpu_state(FPUState* fpu_state)
         : "t0", "memory");
 }
 
-[[maybe_unused]] static void load_fpu_state(FPUState* fpu_state)
+static void load_fpu_state(FPUState* fpu_state)
 {
     asm volatile(
         "fld f0, 0*8(%0) \n"
@@ -276,6 +276,34 @@ ErrorOr<Vector<FlatPtr, 32>> ProcessorBase<T>::capture_stack_trace(Thread&, size
     return Vector<FlatPtr, 32> {};
 }
 
+extern "C" void enter_thread_context(Thread* from_thread, Thread* to_thread);
+extern "C" void enter_thread_context(Thread* from_thread, Thread* to_thread)
+{
+    VERIFY(from_thread == to_thread || from_thread->state() != Thread::State::Running);
+    VERIFY(to_thread->state() == Thread::State::Running);
+
+    Processor::set_current_thread(*to_thread);
+
+    store_fpu_state(&from_thread->fpu_state());
+
+    auto& from_regs = from_thread->regs();
+    auto& to_regs = to_thread->regs();
+    if (from_regs.satp != to_regs.satp) {
+        RISCV64::CSR::SATP::write(to_regs.satp);
+        Processor::flush_entire_tlb_local();
+    }
+
+    to_thread->set_cpu(Processor::current().id());
+
+    Processor::set_thread_specific_data(to_thread->thread_specific_data());
+
+    auto in_critical = to_thread->saved_critical();
+    VERIFY(in_critical > 0);
+    Processor::restore_critical(in_critical);
+
+    load_fpu_state(&to_thread->fpu_state());
+}
+
 NAKED void thread_context_first_enter(void)
 {
     asm("unimp");