kern: implement SvcGetThreadContext3

libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_exception_context.hpp

@@ -22,7 +22,8 @@ namespace ams::kern::arch::arm64 {
         u64 x[(30 - 0) + 1];
         u64 sp;
         u64 pc;
-        u64 psr;
+        u32 psr;
+        u32 write;
         u64 tpidr;
         u64 reserved;
     };

libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_thread_context.hpp

@@ -74,7 +74,11 @@ namespace ams::kern::arch::arm64 {
 
             void CloneFpuStatus();
 
+            const u128 *GetFpuRegisters() const { return this->fpu_registers; }
+
             /* TODO: More methods (especially FPU management) */
     };
 
+    void GetUserContext(ams::svc::ThreadContext *out, const KThread *thread);
+
 }

libraries/libmesosphere/include/mesosphere/kern_k_thread.hpp

@@ -270,6 +270,11 @@ namespace ams::kern {
                 return this->GetStackParameters().is_in_exception_handler;
             }
 
+            ALWAYS_INLINE bool IsCallingSvc() const {
+                MESOSPHERE_ASSERT_THIS();
+                return this->GetStackParameters().is_calling_svc;
+            }
+
             ALWAYS_INLINE void RegisterDpc(DpcFlag flag) {
                 this->GetStackParameters().dpc_flags |= flag;
             }
@@ -455,6 +460,7 @@ namespace ams::kern {
             void Continue();
 
             Result SetActivity(ams::svc::ThreadActivity activity);
+            Result GetThreadContext3(ams::svc::ThreadContext *out);
 
             void ContinueIfHasKernelWaiters() {
                 if (this->GetNumKernelWaiters() > 0) {

libraries/libmesosphere/include/mesosphere/kern_k_thread_context.hpp

@@ -20,6 +20,8 @@
 
     namespace ams::kern {
         using ams::kern::arch::arm64::KThreadContext;
+
+        using ams::kern::arch::arm64::GetUserContext;
     }
 #else
     #error "Unknown architecture for KThreadContext"

libraries/libmesosphere/source/arch/arm64/kern_k_thread_context.cpp

@@ -40,6 +40,15 @@ namespace ams::kern::arch::arm64 {
 
     namespace {
 
+        /* TODO: Should these be elsewhere? (In a header)? */
+        ALWAYS_INLINE KExceptionContext *GetExceptionContext(KThread *thread) {
+            return reinterpret_cast<KExceptionContext *>(reinterpret_cast<uintptr_t>(thread->GetKernelStackTop()) - sizeof(KThread::StackParameters) - sizeof(KExceptionContext));
+        }
+
+        ALWAYS_INLINE const KExceptionContext *GetExceptionContext(const KThread *thread) {
+            return reinterpret_cast<const KExceptionContext *>(reinterpret_cast<uintptr_t>(thread->GetKernelStackTop()) - sizeof(KThread::StackParameters) - sizeof(KExceptionContext));
+        }
+
         ALWAYS_INLINE bool IsFpuEnabled() {
             return cpu::ArchitecturalFeatureAccessControlRegisterAccessor().IsFpEnabled();
         }
@@ -188,4 +197,78 @@ namespace ams::kern::arch::arm64 {
         this->SetFpsr(psr);
     }
 
+    void GetUserContext(ams::svc::ThreadContext *out, const KThread *thread) {
+        MESOSPHERE_ASSERT(KScheduler::IsSchedulerLockedByCurrentThread());
+        MESOSPHERE_ASSERT(thread->IsSuspended());
+        MESOSPHERE_ASSERT(thread->GetOwnerProcess() != nullptr);
+
+        /* Get the contexts. */
+        const KExceptionContext *e_ctx = GetExceptionContext(thread);
+        const KThreadContext    *t_ctx = std::addressof(thread->GetContext());
+
+        if (thread->GetOwnerProcess()->Is64Bit()) {
+            /* Set special registers. */
+            out->fp     = e_ctx->x[29];
+            out->lr     = e_ctx->x[30];
+            out->sp     = e_ctx->sp;
+            out->pc     = e_ctx->pc;
+            out->pstate = e_ctx->psr & 0xFF0FFE20;
+
+            /* Get the thread's general purpose registers. */
+            if (thread->IsCallingSvc()) {
+                for (size_t i = 19; i < 29; ++i) {
+                    out->r[i] = e_ctx->x[i];
+                }
+                if (e_ctx->write == 0) {
+                    out->pc -= sizeof(u32);
+                }
+            } else {
+                for (size_t i = 0; i < 29; ++i) {
+                    out->r[i] = e_ctx->x[i];
+                }
+            }
+
+            /* Copy tpidr. */
+            out->tpidr = e_ctx->tpidr;
+
+            /* Copy fpu registers. */
+            static_assert(util::size(ams::svc::ThreadContext{}.v) == KThreadContext::NumFpuRegisters);
+            const u128 *f = t_ctx->GetFpuRegisters();
+            for (size_t i = 0; i < KThreadContext::NumFpuRegisters; ++i) {
+                out->v[i] = f[i];
+            }
+        } else {
+            /* Set special registers. */
+            out->pc     = static_cast<u32>(e_ctx->pc);
+            out->pstate = e_ctx->psr & 0xFF0FFE20;
+
+            /* Get the thread's general purpose registers. */
+            for (size_t i = 0; i < 15; ++i) {
+                out->r[i] = static_cast<u32>(e_ctx->x[i]);
+            }
+
+            /* Adjust PC, if the thread is calling svc. */
+            if (thread->IsCallingSvc()) {
+                if (e_ctx->write == 0) {
+                    /* Adjust by 2 if thumb mode, 4 if arm mode. */
+                    out->pc -= ((e_ctx->psr & 0x20) == 0) ? sizeof(u32) : sizeof(u16);
+                }
+            }
+
+            /* Copy tpidr. */
+            out->tpidr = static_cast<u32>(e_ctx->tpidr);
+
+            /* Copy fpu registers. */
+            static_assert(util::size(ams::svc::ThreadContext{}.v) == KThreadContext::NumFpuRegisters);
+            const u128 *f = t_ctx->GetFpuRegisters();
+            for (size_t i = 0; i < KThreadContext::NumFpuRegisters / 2; ++i) {
+                out->v[i] = f[i];
+            }
+        }
+
+        /* Copy fpcr/fpsr. */
+        out->fpcr = t_ctx->GetFpcr();
+        out->fpsr = t_ctx->GetFpsr();
+    }
+
 }

libraries/libmesosphere/source/kern_k_thread.cpp

@@ -854,6 +854,27 @@ namespace ams::kern {
         return ResultSuccess();
     }
 
+    Result KThread::GetThreadContext3(ams::svc::ThreadContext *out) {
+        /* Lock ourselves. */
+        KScopedLightLock lk(this->activity_pause_lock);
+
+        /* Get the context. */
+        {
+            /* Lock the scheduler. */
+            KScopedSchedulerLock sl;
+
+            /* Verify that we're suspended. */
+            R_UNLESS(this->IsSuspendRequested(SuspendType_Thread), svc::ResultInvalidState());
+
+            /* If we're not terminating, get the thread's user context. */
+            if (!this->IsTerminationRequested()) {
+                GetUserContext(out, this);
+            }
+        }
+
+        return ResultSuccess();
+    }
+
     void KThread::AddWaiterImpl(KThread *thread) {
         MESOSPHERE_ASSERT_THIS();
         MESOSPHERE_ASSERT(KScheduler::IsSchedulerLockedByCurrentThread());

libraries/libmesosphere/source/svc/kern_svc_thread.cpp

@@ -195,6 +195,25 @@ namespace ams::kern::svc {
             return ResultSuccess();
         }
 
+        Result GetThreadContext3(KUserPointer<ams::svc::ThreadContext *> out_context, ams::svc::Handle thread_handle) {
+            /* Get the thread from its handle. */
+            KScopedAutoObject thread = GetCurrentProcess().GetHandleTable().GetObject<KThread>(thread_handle);
+            R_UNLESS(thread.IsNotNull(), svc::ResultInvalidHandle());
+
+            /* Require the handle be to a non-current thread in the current process. */
+            R_UNLESS(thread->GetOwnerProcess() == GetCurrentProcessPointer(), svc::ResultInvalidHandle());
+            R_UNLESS(thread.GetPointerUnsafe() != GetCurrentThreadPointer(),  svc::ResultBusy());
+
+            /* Get the thread context. */
+            ams::svc::ThreadContext context = {};
+            R_TRY(thread->GetThreadContext3(std::addressof(context)));
+
+            /* Copy the thread context to user space. */
+            R_TRY(out_context.CopyFrom(std::addressof(context)));
+
+            return ResultSuccess();
+        }
+
     }
 
     /* =============================    64 ABI    ============================= */
@@ -244,7 +263,7 @@ namespace ams::kern::svc {
     }
 
     Result GetThreadContext364(KUserPointer<ams::svc::ThreadContext *> out_context, ams::svc::Handle thread_handle) {
-        MESOSPHERE_PANIC("Stubbed SvcGetThreadContext364 was called.");
+        return GetThreadContext3(out_context, thread_handle);
     }
 
     Result GetThreadList64(int32_t *out_num_threads, KUserPointer<uint64_t *> out_thread_ids, int32_t max_out_count, ams::svc::Handle debug_handle) {
@@ -298,7 +317,7 @@ namespace ams::kern::svc {
     }
 
     Result GetThreadContext364From32(KUserPointer<ams::svc::ThreadContext *> out_context, ams::svc::Handle thread_handle) {
-        MESOSPHERE_PANIC("Stubbed SvcGetThreadContext364From32 was called.");
+        return GetThreadContext3(out_context, thread_handle);
     }
 
     Result GetThreadList64From32(int32_t *out_num_threads, KUserPointer<uint64_t *> out_thread_ids, int32_t max_out_count, ams::svc::Handle debug_handle) {