kern: implement KThreadContext

libraries/libmesosphere/include/mesosphere.hpp

@@ -25,6 +25,7 @@
 /* Primitive types. */
 #include <mesosphere/kern_k_typed_address.hpp>
 #include <mesosphere/kern_initial_process.hpp>
+#include <mesosphere/kern_k_exception_context.hpp>
 
 /* Core pre-initialization includes. */
 #include <mesosphere/kern_select_cpu.hpp>

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

@@ -0,0 +1,31 @@
+/*
+ * Copyright (c) 2018-2020 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+#include <mesosphere/kern_common.hpp>
+
+namespace ams::kern::arm64 {
+
+    struct KExceptionContext {
+        u64 x[(30 - 0) + 1];
+        u64 sp;
+        u64 pc;
+        u64 psr;
+        u64 tpidr;
+        u64 reserved;
+    };
+    static_assert(sizeof(KExceptionContext) == 0x120);
+
+}

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

@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) 2018-2020 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+#include <mesosphere/kern_common.hpp>
+#include <mesosphere/kern_k_typed_address.hpp>
+
+namespace ams::kern {
+
+    class KThread;
+
+}
+
+namespace ams::kern::arm64 {
+
+    class KThreadContext {
+        public:
+            static constexpr size_t NumCalleeSavedRegisters = (29 - 19) + 1;
+            static constexpr size_t NumFpuRegisters = 32;
+        private:
+            union {
+                u64 registers[NumCalleeSavedRegisters];
+                struct {
+                    u64 x19;
+                    u64 x20;
+                    u64 x21;
+                    u64 x22;
+                    u64 x23;
+                    u64 x24;
+                    u64 x25;
+                    u64 x26;
+                    u64 x27;
+                    u64 x28;
+                    u64 x29;
+                };
+            } callee_saved;
+            u64 lr;
+            u64 sp;
+            u64 cpacr;
+            u64 fpcr;
+            u64 fpsr;
+            alignas(0x10) u128 fpu_registers[NumFpuRegisters];
+            bool locked;
+        private:
+            static void RestoreFpuRegisters64(const KThreadContext &);
+            static void RestoreFpuRegisters32(const KThreadContext &);
+        public:
+            constexpr explicit KThreadContext() : callee_saved(), lr(), sp(), cpacr(), fpcr(), fpsr(), fpu_registers(), locked() { /* ... */ }
+
+            Result Initialize(KVirtualAddress u_pc, KVirtualAddress k_sp, KVirtualAddress u_sp, uintptr_t arg, bool is_user, bool is_64_bit, bool is_main);
+            Result Finalize();
+
+            /* TODO: More methods (especially FPU management) */
+    };
+
+}

libraries/libmesosphere/include/mesosphere/kern_k_exception_context.hpp

@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2018-2020 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#ifdef ATMOSPHERE_BOARD_NINTENDO_SWITCH
+    #include <mesosphere/arch/arm64/kern_k_exception_context.hpp>
+
+    namespace ams::kern {
+        using ams::kern::arm64::KExceptionContext;
+    }
+#else
+    #error "Unknown board for KExceptionContext"
+#endif

libraries/libmesosphere/include/mesosphere/kern_k_thread.hpp

@@ -17,6 +17,7 @@
 #include <mesosphere/kern_slab_helpers.hpp>
 #include <mesosphere/kern_k_synchronization_object.hpp>
 #include <mesosphere/kern_k_affinity_mask.hpp>
+#include <mesosphere/kern_k_thread_context.hpp>
 
 namespace ams::kern {
 
@@ -31,7 +32,7 @@ namespace ams::kern {
                 bool is_in_exception_handler;
                 bool has_exception_svc_perms;
                 s32 disable_count;
-                void *context; /* TODO: KThreadContext * */
+                KThreadContext *context;
             };
             static_assert(alignof(StackParameters) == 0x10);
 

libraries/libmesosphere/include/mesosphere/kern_k_thread_context.hpp

@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2018-2020 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#ifdef ATMOSPHERE_BOARD_NINTENDO_SWITCH
+    #include <mesosphere/arch/arm64/kern_k_thread_context.hpp>
+
+    namespace ams::kern {
+        using ams::kern::arm64::KThreadContext;
+    }
+#else
+    #error "Unknown board for KThreadContext"
+#endif

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

@@ -0,0 +1,131 @@
+/*
+ * Copyright (c) 2018-2020 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <mesosphere.hpp>
+
+namespace ams::kern::arm64 {
+
+    /* These are implemented elsewhere (asm). */
+    void UserModeThreadStarter();
+    void SupervisorModeThreadStarter();
+
+    void OnThreadStart() {
+        /* TODO: Implement this. */
+    }
+
+    namespace {
+
+        uintptr_t SetupStackForUserModeThreadStarter(KVirtualAddress pc, KVirtualAddress k_sp, KVirtualAddress u_sp, uintptr_t arg, bool is_64_bit) {
+            /* NOTE: Stack layout on entry looks like following:                         */
+            /* SP                                                                        */
+            /* |                                                                         */
+            /* v                                                                         */
+            /* | KExceptionContext (size 0x120) | KThread::StackParameters (size 0x30) | */
+            KExceptionContext *ctx = GetPointer<KExceptionContext>(k_sp) - 1;
+
+            /* Clear context. */
+            std::memset(ctx, 0, sizeof(*ctx));
+
+            /* Set PC and argument. */
+            ctx->pc = GetInteger(pc);
+            ctx->x[0] = arg;
+
+            /* Set PSR. */
+            if (is_64_bit) {
+                ctx->psr = 0;
+            } else {
+                constexpr u64 PsrArmValue   = 0x20;
+                constexpr u64 PsrThumbValue = 0x00;
+                ctx->psr = ((pc & 1) == 0 ? PsrArmValue : PsrThumbValue) | (0x10);
+            }
+
+            /* Set stack pointer. */
+            if (is_64_bit) {
+                ctx->sp    = GetInteger(u_sp);
+            } else {
+                ctx->x[13] = GetInteger(u_sp);
+            }
+
+            return reinterpret_cast<uintptr_t>(ctx);
+        }
+
+        uintptr_t SetupStackForSupervisorModeThreadStarter(KVirtualAddress pc, KVirtualAddress sp, uintptr_t arg) {
+            /* NOTE: Stack layout on entry looks like following:                        */
+            /* SP                                                                       */
+            /* |                                                                        */
+            /* v                                                                        */
+            /* | u64 argument | u64 entrypoint | KThread::StackParameters (size 0x30) | */
+            static_assert(sizeof(KThread::StackParameters) == 0x30);
+
+            u64 *stack = GetPointer<u64>(sp);
+            *(--stack) = GetInteger(pc);
+            *(--stack) = arg;
+            return reinterpret_cast<uintptr_t>(stack);
+        }
+
+    }
+
+    Result KThreadContext::Initialize(KVirtualAddress u_pc, KVirtualAddress k_sp, KVirtualAddress u_sp, uintptr_t arg, bool is_user, bool is_64_bit, bool is_main) {
+        MESOSPHERE_ASSERT(k_sp != Null<KVirtualAddress>);
+
+        /* Ensure that the stack pointers are aligned. */
+        k_sp = util::AlignDown(GetInteger(k_sp), 16);
+        u_sp = util::AlignDown(GetInteger(u_sp), 16);
+
+        /* Determine LR and SP. */
+        if (is_user) {
+            /* Usermode thread. */
+            this->lr = reinterpret_cast<uintptr_t>(::ams::kern::arm64::UserModeThreadStarter);
+            this->sp = SetupStackForUserModeThreadStarter(u_pc, k_sp, u_sp, arg, is_64_bit);
+        } else {
+            /* Kernel thread. */
+            MESOSPHERE_ASSERT(is_64_bit);
+
+            if (is_main) {
+                /* Main thread. */
+                this->lr = GetInteger(u_pc);
+                this->sp = GetInteger(k_sp);
+            } else {
+                /* Generic Kernel thread. */
+                this->lr = reinterpret_cast<uintptr_t>(::ams::kern::arm64::SupervisorModeThreadStarter);
+                this->sp = SetupStackForSupervisorModeThreadStarter(u_pc, k_sp, arg);
+            }
+        }
+
+        /* Clear callee-saved registers. */
+        for (size_t i = 0; i < util::size(this->callee_saved.registers); i++) {
+            this->callee_saved.registers[i] = 0;
+        }
+
+        /* Clear FPU state. */
+        this->fpcr = 0;
+        this->fpsr = 0;
+        this->cpacr = 0;
+        for (size_t i = 0; i < util::size(this->fpu_registers); i++) {
+            this->fpu_registers[i] = 0;
+        }
+
+        /* Lock the context, if we're a main thread. */
+        this->locked = is_main;
+
+        return ResultSuccess();
+    }
+
+    Result KThreadContext::Finalize() {
+        /* This doesn't actually do anything. */
+        return ResultSuccess();
+    }
+
+}

libraries/libmesosphere/source/arch/arm64/kern_k_thread_context_asm.s

@@ -0,0 +1,143 @@
+/*
+ * Copyright (c) 2018-2020 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* ams::kern::arm64::UserModeThreadStarter() */
+.section    .text._ZN3ams4kern5arm6421UserModeThreadStarterEv, "ax", %progbits
+.global     _ZN3ams4kern5arm6421UserModeThreadStarterEv
+.type       _ZN3ams4kern5arm6421UserModeThreadStarterEv, %function
+_ZN3ams4kern5arm6421UserModeThreadStarterEv:
+    /* NOTE: Stack layout on entry looks like following:                         */
+    /* SP                                                                        */
+    /* |                                                                         */
+    /* v                                                                         */
+    /* | KExceptionContext (size 0x120) | KThread::StackParameters (size 0x30) | */
+
+    /* Clear the disable count for this thread's stack parameters. */
+    str wzr, [sp, #(0x120 + 0x18)]
+
+    /* Call ams::kern::arm64::OnThreadStart() */
+    bl _ZN3ams4kern5arm6413OnThreadStartEv
+
+    /* Restore thread state from the KExceptionContext on stack  */
+    ldp x30, x19, [sp, #(8 * 30)]       /* x30 = lr,   x19 = sp  */
+    ldp x20, x21, [sp, #(8 * 30 + 16)]  /* x20 = pc,   x21 = psr */
+    ldr x22,      [sp, #(8 * 30 + 32)]  /* x22 = tpidr           */
+
+    msr sp_el0,    x19
+    msr elr_el1,   x20
+    msr spsr_el1,  x21
+    msr tpidr_el1, x22
+
+    ldp x0,  x1,  [sp, #(8 *  0)]
+    ldp x2,  x3,  [sp, #(8 *  2)]
+    ldp x4,  x5,  [sp, #(8 *  4)]
+    ldp x6,  x7,  [sp, #(8 *  6)]
+    ldp x8,  x9,  [sp, #(8 *  8)]
+    ldp x10, x11, [sp, #(8 * 10)]
+    ldp x12, x13, [sp, #(8 * 12)]
+    ldp x14, x15, [sp, #(8 * 14)]
+    ldp x16, x17, [sp, #(8 * 16)]
+    ldp x18, x19, [sp, #(8 * 18)]
+    ldp x20, x21, [sp, #(8 * 20)]
+    ldp x22, x23, [sp, #(8 * 22)]
+    ldp x24, x25, [sp, #(8 * 24)]
+    ldp x26, x27, [sp, #(8 * 26)]
+    ldp x28, x29, [sp, #(8 * 28)]
+
+    /* Increment stack pointer above the KExceptionContext */
+    add sp, sp, #0x120
+
+    /* Return to EL0 */
+    eret
+
+/* ams::kern::arm64::SupervisorModeThreadStarter() */
+.section    .text._ZN3ams4kern5arm6427SupervisorModeThreadStarterEv, "ax", %progbits
+.global     _ZN3ams4kern5arm6427SupervisorModeThreadStarterEv
+.type       _ZN3ams4kern5arm6427SupervisorModeThreadStarterEv, %function
+_ZN3ams4kern5arm6427SupervisorModeThreadStarterEv:
+    /* NOTE: Stack layout on entry looks like following:                        */
+    /* SP                                                                       */
+    /* |                                                                        */
+    /* v                                                                        */
+    /* | u64 argument | u64 entrypoint | KThread::StackParameters (size 0x30) | */
+
+    /* Load the argument and entrypoint. */
+    ldp x0, x1, [sp], #0x10
+
+    /* Clear the disable count for this thread's stack parameters. */
+    str wzr, [sp, #(0x18)]
+
+    /*  Mask I bit in DAIF */
+    msr daifclr, #2
+    br x1
+
+    /* This should never execute, but Nintendo includes an ERET here. */
+    eret
+
+
+/* ams::kern::arm64::KThreadContext::RestoreFpuRegisters64(const KThreadContext &) */
+.section    .text._ZN3ams4kern5arm6414KThreadContext21RestoreFpuRegisters64ERKS2_, "ax", %progbits
+.global     _ZN3ams4kern5arm6414KThreadContext21RestoreFpuRegisters64ERKS2_
+.type       _ZN3ams4kern5arm6414KThreadContext21RestoreFpuRegisters64ERKS2_, %function
+_ZN3ams4kern5arm6414KThreadContext21RestoreFpuRegisters64ERKS2_:
+    /* Load and restore FPCR and FPSR from the context. */
+    ldr x1, [x0, #0x70]
+    msr fpcr, x1
+    ldr x1, [x0, #0x78]
+    msr fpsr, x1
+
+    /* Restore the FPU registers. */
+    ldp q0,  q1,  [sp, #(16 *  0 + 0x80)]
+    ldp q2,  q3,  [sp, #(16 *  2 + 0x80)]
+    ldp q4,  q5,  [sp, #(16 *  4 + 0x80)]
+    ldp q6,  q7,  [sp, #(16 *  6 + 0x80)]
+    ldp q8,  q9,  [sp, #(16 *  8 + 0x80)]
+    ldp q10, q11, [sp, #(16 * 10 + 0x80)]
+    ldp q12, q13, [sp, #(16 * 12 + 0x80)]
+    ldp q14, q15, [sp, #(16 * 14 + 0x80)]
+    ldp q16, q17, [sp, #(16 * 16 + 0x80)]
+    ldp q18, q19, [sp, #(16 * 18 + 0x80)]
+    ldp q20, q21, [sp, #(16 * 20 + 0x80)]
+    ldp q22, q23, [sp, #(16 * 22 + 0x80)]
+    ldp q24, q25, [sp, #(16 * 24 + 0x80)]
+    ldp q26, q27, [sp, #(16 * 26 + 0x80)]
+    ldp q28, q29, [sp, #(16 * 28 + 0x80)]
+    ldp q30, q31, [sp, #(16 * 30 + 0x80)]
+
+    ret
+
+/* ams::kern::arm64::KThreadContext::RestoreFpuRegisters32(const KThreadContext &) */
+.section    .text._ZN3ams4kern5arm6414KThreadContext21RestoreFpuRegisters32ERKS2_, "ax", %progbits
+.global     _ZN3ams4kern5arm6414KThreadContext21RestoreFpuRegisters32ERKS2_
+.type       _ZN3ams4kern5arm6414KThreadContext21RestoreFpuRegisters32ERKS2_, %function
+_ZN3ams4kern5arm6414KThreadContext21RestoreFpuRegisters32ERKS2_:
+    /* Load and restore FPCR and FPSR from the context. */
+    ldr x1, [x0, #0x70]
+    msr fpcr, x1
+    ldr x1, [x0, #0x78]
+    msr fpsr, x1
+
+    /* Restore the FPU registers. */
+    ldp q0,  q1,  [sp, #(16 *  0 + 0x80)]
+    ldp q2,  q3,  [sp, #(16 *  2 + 0x80)]
+    ldp q4,  q5,  [sp, #(16 *  4 + 0x80)]
+    ldp q6,  q7,  [sp, #(16 *  6 + 0x80)]
+    ldp q8,  q9,  [sp, #(16 *  8 + 0x80)]
+    ldp q10, q11, [sp, #(16 * 10 + 0x80)]
+    ldp q12, q13, [sp, #(16 * 12 + 0x80)]
+    ldp q14, q15, [sp, #(16 * 14 + 0x80)]
+
+    ret