From 4baf0e8cce68a2cb96dca455839c59281f0d345c Mon Sep 17 00:00:00 2001
From: Michael Scire <SciresM@gmail.com>
Date: Wed, 9 Oct 2024 22:01:45 -0700
Subject: [PATCH] kern: invoke supervisor mode thread functions from C++
 context with valid stack frame

---
 .../source/arch/arm64/kern_k_thread_context.cpp          | 9 +++++++++
 .../kernel/source/arch/arm64/kern_k_thread_context_asm.s | 7 ++++++-
 2 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/libraries/libmesosphere/source/arch/arm64/kern_k_thread_context.cpp b/libraries/libmesosphere/source/arch/arm64/kern_k_thread_context.cpp
index b3cef6755..76e91cff7 100644
--- a/libraries/libmesosphere/source/arch/arm64/kern_k_thread_context.cpp
+++ b/libraries/libmesosphere/source/arch/arm64/kern_k_thread_context.cpp
@@ -21,6 +21,15 @@ namespace ams::kern::arch::arm64 {
     void UserModeThreadStarter();
     void SupervisorModeThreadStarter();
 
+    void InvokeSupervisorModeThread(uintptr_t argument, uintptr_t entrypoint) {
+        /* Invoke the function. */
+        using SupervisorModeFunctionType = void (*)(uintptr_t);
+        reinterpret_cast<SupervisorModeFunctionType>(entrypoint)(argument);
+
+        /* Wait forever. */
+        AMS_INFINITE_LOOP();
+    }
+
     void OnThreadStart() {
         MESOSPHERE_ASSERT(!KInterruptManager::AreInterruptsEnabled());
         /* Send KDebug event for this thread's creation. */
diff --git a/mesosphere/kernel/source/arch/arm64/kern_k_thread_context_asm.s b/mesosphere/kernel/source/arch/arm64/kern_k_thread_context_asm.s
index 362adc712..4b4b1bd3a 100644
--- a/mesosphere/kernel/source/arch/arm64/kern_k_thread_context_asm.s
+++ b/mesosphere/kernel/source/arch/arm64/kern_k_thread_context_asm.s
@@ -76,6 +76,9 @@ _ZN3ams4kern4arch5arm6427SupervisorModeThreadStarterEv:
     /* v                                                                        */
     /* | u64 argument | u64 entrypoint | KThread::StackParameters (size 0x30) | */
 
+    /* Clear the link register. */
+    mov x30, #0
+
     /* Load the argument and entrypoint. */
     ldp x0, x1, [sp], #0x10
 
@@ -84,4 +87,6 @@ _ZN3ams4kern4arch5arm6427SupervisorModeThreadStarterEv:
 
     /*  Mask I bit in DAIF */
     msr daifclr, #2
-    br x1
+
+    /* Invoke the function (by calling ams::kern::arch::arm64::InvokeSupervisorModeThread(argument, entrypoint)). */
+    b _ZN3ams4kern4arch5arm6426InvokeSupervisorModeThreadEmm