kern: add hardware single step extension

libraries/libmesosphere/include/mesosphere/arch/arm64/kern_assembly_offsets.h

@@ -14,6 +14,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 #pragma once
+#include <mesosphere/kern_build_config.hpp>
 
 /* TODO: Different header for this? */
 #define AMS_KERN_NUM_SUPERVISOR_CALLS 0xC0
@@ -30,6 +31,10 @@
 #define THREAD_STACK_PARAMETERS_IS_IN_EXCEPTION_HANDLER 0x2D
 #define THREAD_STACK_PARAMETERS_IS_PINNED               0x2E
 
+#if defined(MESOSPHERE_ENABLE_HARDWARE_SINGLE_STEP)
+#define THREAD_STACK_PARAMETERS_IS_SINGLE_STEP          0x2F
+#endif
+
 /* ams::kern::arch::arm64::KThreadContext, https://github.com/Atmosphere-NX/Atmosphere/blob/master/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_thread_context.hpp */
 #define THREAD_CONTEXT_SIZE          0x290
 #define THREAD_CONTEXT_CPU_REGISTERS 0x000

libraries/libmesosphere/include/mesosphere/kern_build_config.hpp

@@ -31,3 +31,4 @@
 
 //#define MESOSPHERE_BUILD_FOR_TRACING
 #define MESOSPHERE_ENABLE_PANIC_REGISTER_DUMP
+#define MESOSPHERE_ENABLE_HARDWARE_SINGLE_STEP

libraries/libmesosphere/include/mesosphere/kern_k_thread.hpp

@@ -92,6 +92,9 @@ namespace ams::kern {
                 bool is_calling_svc;
                 bool is_in_exception_handler;
                 bool is_pinned;
+                #if defined(MESOSPHERE_ENABLE_HARDWARE_SINGLE_STEP)
+                bool is_single_step;
+                #endif
             };
             static_assert(alignof(StackParameters) == 0x10);
             static_assert(sizeof(StackParameters) == THREAD_STACK_PARAMETERS_SIZE);
@@ -106,6 +109,10 @@ namespace ams::kern {
             static_assert(__builtin_offsetof(StackParameters, is_in_exception_handler) == THREAD_STACK_PARAMETERS_IS_IN_EXCEPTION_HANDLER);
             static_assert(__builtin_offsetof(StackParameters, is_pinned)               == THREAD_STACK_PARAMETERS_IS_PINNED);
 
+            #if defined(MESOSPHERE_ENABLE_HARDWARE_SINGLE_STEP)
+            static_assert(__builtin_offsetof(StackParameters, is_single_step)          == THREAD_STACK_PARAMETERS_IS_SINGLE_STEP);
+            #endif
+
             struct QueueEntry {
                 private:
                     KThread *m_prev;
@@ -325,6 +332,25 @@ namespace ams::kern {
                 return this->GetStackParameters().current_svc_id;
             }
 
+            #if defined(MESOSPHERE_ENABLE_HARDWARE_SINGLE_STEP)
+
+            ALWAYS_INLINE void SetSingleStep() {
+                MESOSPHERE_ASSERT_THIS();
+                this->GetStackParameters().is_single_step = true;
+            }
+
+            ALWAYS_INLINE void ClearSingleStep() {
+                MESOSPHERE_ASSERT_THIS();
+                this->GetStackParameters().is_single_step = false;
+            }
+
+            ALWAYS_INLINE bool IsSingleStep() const {
+                MESOSPHERE_ASSERT_THIS();
+                return this->GetStackParameters().is_single_step;
+            }
+
+            #endif
+
             ALWAYS_INLINE void RegisterDpc(DpcFlag flag) {
                 this->GetStackParameters().dpc_flags.fetch_or(flag);
             }

libraries/libmesosphere/source/arch/arm64/svc/kern_svc_exception_asm.s

@@ -15,6 +15,39 @@
  */
 #include <mesosphere/kern_select_assembly_offsets.h>
 
+#if defined(MESOSPHERE_ENABLE_HARDWARE_SINGLE_STEP)
+    .macro disable_single_step, scratch
+    /* Clear MDSCR_EL1.SS. */
+    mrs \scratch, mdscr_el1
+    bic \scratch, \scratch, #1
+    msr mdscr_el1, \scratch
+    .endm
+
+    .macro check_enable_single_step, scratch1, scratch2, spsr_value
+    /* Check if single-step is requested. */
+    ldrb    \scratch1, [sp, #(EXCEPTION_CONTEXT_SIZE + THREAD_STACK_PARAMETERS_IS_SINGLE_STEP)]
+    tbz     \scratch1, #0, .skip_single_step\@
+
+    /* If single-step is requested, enable the single-step machine by setting MDSCR_EL1.SS. */
+    mrs     \scratch2, mdscr_el1
+    orr     \scratch2, \scratch2, #1
+    msr     mdscr_el1, \scratch2
+
+    /* Since we're returning from an exception, set SPSR.SS so we actually advance an instruction. */
+    orr \spsr_value, \spsr_value, #(1 << 21)
+
+    isb
+
+.skip_single_step\@:
+    .endm
+#else
+    .macro disable_single_step, scratch
+    .endm
+
+    .macro check_enable_single_step, scratch1, scratch2, spsr_value
+    .endm
+#endif
+
 /* ams::kern::svc::CallReturnFromException64(Result result) */
 .section    .text._ZN3ams4kern3svc25CallReturnFromException64Ev, "ax", %progbits
 .global     _ZN3ams4kern3svc25CallReturnFromException64Ev
@@ -89,6 +122,9 @@ _ZN3ams4kern3svc14RestoreContextEm:
     ldp     x30, x8,  [sp, #(EXCEPTION_CONTEXT_X30_SP)]
     ldp     x9,  x10, [sp, #(EXCEPTION_CONTEXT_PC_PSR)]
     ldr     x11,      [sp, #(EXCEPTION_CONTEXT_TPIDR)]
+
+    check_enable_single_step w0, x0, x10
+
     msr     sp_el0, x8
     msr     elr_el1, x9
     msr     spsr_el1, x10

libraries/libmesosphere/source/arch/arm64/svc/kern_svc_handlers_asm.s

@@ -16,6 +16,39 @@
 #include <mesosphere/kern_build_config.hpp>
 #include <mesosphere/kern_select_assembly_offsets.h>
 
+#if defined(MESOSPHERE_ENABLE_HARDWARE_SINGLE_STEP)
+    .macro disable_single_step, scratch
+    /* Clear MDSCR_EL1.SS. */
+    mrs \scratch, mdscr_el1
+    bic \scratch, \scratch, #1
+    msr mdscr_el1, \scratch
+    .endm
+
+    .macro check_enable_single_step, scratch1, scratch2, spsr_value
+    /* Check if single-step is requested. */
+    ldrb    \scratch1, [sp, #(EXCEPTION_CONTEXT_SIZE + THREAD_STACK_PARAMETERS_IS_SINGLE_STEP)]
+    tbz     \scratch1, #0, .skip_single_step\@
+
+    /* If single-step is requested, enable the single-step machine by setting MDSCR_EL1.SS. */
+    mrs     \scratch2, mdscr_el1
+    orr     \scratch2, \scratch2, #1
+    msr     mdscr_el1, \scratch2
+
+    /* Since we're returning from an SVC, make sure SPSR.SS is cleared so we break instantly on the instruction after the SVC. */
+    bic \spsr_value, \spsr_value, #(1 << 21)
+
+    isb
+
+.skip_single_step\@:
+    .endm
+#else
+    .macro disable_single_step, scratch
+    .endm
+
+    .macro check_enable_single_step, scratch1, scratch2, spsr_value
+    .endm
+#endif
+
 /* ams::kern::arch::arm64::SvcHandler64() */
 .section    .text._ZN3ams4kern4arch5arm6412SvcHandler64Ev, "ax", %progbits
 .global     _ZN3ams4kern4arch5arm6412SvcHandler64Ev
@@ -51,6 +84,9 @@ _ZN3ams4kern4arch5arm6412SvcHandler64Ev:
     stp     x8,  x9,  [sp, #(EXCEPTION_CONTEXT_SP_PC)]
     stp     x10, x11, [sp, #(EXCEPTION_CONTEXT_PSR_TPIDR)]
 
+    /* Disable single-step. */
+    disable_single_step x8
+
     /* Check if the SVC index is out of range. */
     mrs     x8, esr_el1
     and     x8, x8, #0xFF
@@ -154,6 +190,9 @@ _ZN3ams4kern4arch5arm6412SvcHandler64Ev:
     ldp     x30, x8,  [sp, #(EXCEPTION_CONTEXT_X30_SP)]
     ldp     x9,  x10, [sp, #(EXCEPTION_CONTEXT_PC_PSR)]
     ldr     x11,      [sp, #(EXCEPTION_CONTEXT_TPIDR)]
+
+    check_enable_single_step w0, x0, x10
+
     msr     sp_el0, x8
     msr     elr_el1, x9
     msr     spsr_el1, x10
@@ -203,6 +242,9 @@ _ZN3ams4kern4arch5arm6412SvcHandler64Ev:
     ldp     x9,  x10, [sp, #(EXCEPTION_CONTEXT_PC_PSR)]
     ldr     x11,      [sp, #(EXCEPTION_CONTEXT_TPIDR)]
     ldr     x18,      [sp, #(EXCEPTION_CONTEXT_X18)]
+
+    check_enable_single_step w12, x12, x10
+
     msr     sp_el0, x8
     msr     elr_el1, x9
     msr     spsr_el1, x10
@@ -260,6 +302,9 @@ _ZN3ams4kern4arch5arm6412SvcHandler32Ev:
     stp     x12, x13, [sp, #(EXCEPTION_CONTEXT_X12_X13)]
     stp     x14, xzr, [sp, #(EXCEPTION_CONTEXT_X14_X15)]
 
+    /* Disable single-step. */
+    disable_single_step x8
+
     /* Check if the SVC index is out of range. */
     mrs     x16, esr_el1
     and     x16, x16, #0xFF
@@ -359,6 +404,9 @@ _ZN3ams4kern4arch5arm6412SvcHandler32Ev:
     /* Restore registers. */
     ldp     x17, x20, [sp, #(EXCEPTION_CONTEXT_PC_PSR)]
     ldr     x19,      [sp, #(EXCEPTION_CONTEXT_TPIDR)]
+
+    check_enable_single_step w0, x0, x20
+
     msr     elr_el1, x17
     msr     spsr_el1, x20
     msr     tpidr_el0, x19
@@ -402,6 +450,9 @@ _ZN3ams4kern4arch5arm6412SvcHandler32Ev:
     ldp     x14, xzr, [sp, #(EXCEPTION_CONTEXT_X14_X15)]
     ldp     x17, x20, [sp, #(EXCEPTION_CONTEXT_PC_PSR)]
     ldr     x19,      [sp, #(EXCEPTION_CONTEXT_TPIDR)]
+
+    check_enable_single_step w21, x21, x20
+
     msr     elr_el1, x17
     msr     spsr_el1, x20
     msr     tpidr_el0, x19

libraries/libmesosphere/source/kern_k_debug_base.cpp

@@ -372,6 +372,14 @@ namespace ams::kern {
                     new_state = state;
                 }
 
+                /* Clear single step on all threads. */
+                {
+                    auto end = target->GetThreadList().end();
+                    for (auto it = target->GetThreadList().begin(); it != end; ++it) {
+                        it->ClearSingleStep();
+                    }
+                }
+
                 /* Detach from the process. */
                 target->ClearDebugObject(new_state);
                 m_process = nullptr;
@@ -479,6 +487,25 @@ namespace ams::kern {
                 }
             }
 
+            /* Update thread single-step state. */
+            #if defined(MESOSPHERE_ENABLE_HARDWARE_SINGLE_STEP)
+            {
+                if ((context_flags & ams::svc::ThreadContextFlag_SetSingleStep) != 0) {
+                    /* Set single step. */
+                    thread->SetSingleStep();
+
+                    /* If no other thread flags are present, we're done. */
+                    R_SUCCEED_IF((context_flags & ~ams::svc::ThreadContextFlag_SetSingleStep) == 0);
+                } else if ((context_flags & ams::svc::ThreadContextFlag_ClearSingleStep) != 0) {
+                    /* Clear single step. */
+                    thread->ClearSingleStep();
+
+                    /* If no other thread flags are present, we're done. */
+                    R_SUCCEED_IF((context_flags & ~ams::svc::ThreadContextFlag_ClearSingleStep) == 0);
+                }
+            }
+            #endif
+
             /* Verify that the thread's svc state is valid. */
             if (thread->IsCallingSvc()) {
                 const u8 svc_id         = thread->GetSvcId();
@@ -873,6 +900,9 @@ namespace ams::kern {
                             {
                                 auto end = process->GetThreadList().end();
                                 for (auto it = process->GetThreadList().begin(); it != end; ++it) {
+                                    /* Clear the thread's single-step state. */
+                                    it->ClearSingleStep();
+
                                     if (resume) {
                                         /* If the process isn't crashed, resume threads. */
                                         it->Resume(KThread::SuspendType_Debug);
@@ -960,9 +990,10 @@ namespace ams::kern {
             /* Set the process as breaked. */
             process->SetDebugBreak();
 
-            /* If the event is an exception, set the result. */
+            /* If the event is an exception, set the result and clear single step. */
             if (event == ams::svc::DebugEvent_Exception) {
                 GetCurrentThread().SetDebugExceptionResult(ResultSuccess());
+                GetCurrentThread().ClearSingleStep();
             }
 
             /* Exit our retry loop. */

libraries/libmesosphere/source/svc/kern_svc_debug.cpp

@@ -186,7 +186,24 @@ namespace ams::kern::svc {
             R_UNLESS(KTargetSystem::IsDebugMode(), svc::ResultNotImplemented());
 
             /* Validate the context flags. */
+            #if defined(MESOSPHERE_ENABLE_HARDWARE_SINGLE_STEP)
+            {
+                /* Check that the flags are a subset of the allowable. */
+                constexpr u32 AllFlagsMask = ams::svc::ThreadContextFlag_All | ams::svc::ThreadContextFlag_SetSingleStep | ams::svc::ThreadContextFlag_ClearSingleStep;
+                R_UNLESS((context_flags | AllFlagsMask) == AllFlagsMask, svc::ResultInvalidEnumValue());
+
+                /* Check that thread isn't both setting and clearing single step. */
+                const bool set_ss   = (context_flags & ams::svc::ThreadContextFlag_SetSingleStep) != 0;
+                const bool clear_ss = (context_flags & ams::svc::ThreadContextFlag_ClearSingleStep) != 0;
+
+                R_UNLESS(!(set_ss && clear_ss), svc::ResultInvalidEnumValue());
+            }
+            #else
+            {
+                /* Check that the flags are a subset of the allowable. */
                 R_UNLESS((context_flags | ams::svc::ThreadContextFlag_All) == ams::svc::ThreadContextFlag_All, svc::ResultInvalidEnumValue());
+            }
+            #endif
 
             /* Copy the thread context from userspace. */
             ams::svc::ThreadContext context;

libraries/libmesosphere/source/svc/kern_svc_info.cpp

@@ -271,6 +271,16 @@ namespace ams::kern::svc {
                                     *out = KTraceValue;
                                 }
                                 break;
+                            case ams::svc::MesosphereMetaInfo_IsSingleStepEnabled:
+                                {
+                                    /* Return whether the kernel supports hardware single step. */
+                                    #if defined(MESOSPHERE_ENABLE_HARDWARE_SINGLE_STEP)
+                                    *out = 1;
+                                    #else
+                                    *out = 0;
+                                    #endif
+                                }
+                                break;
                             default:
                                 return svc::ResultInvalidCombination();
                         }

libraries/libvapours/include/vapours/svc/svc_types_common.hpp

@@ -176,6 +176,7 @@ namespace ams::svc {
     enum MesosphereMetaInfo : u64 {
         MesosphereMetaInfo_KernelVersion       = 0,
         MesosphereMetaInfo_IsKTraceEnabled     = 1,
+        MesosphereMetaInfo_IsSingleStepEnabled = 2,
     };
 
     enum SystemInfoType : u32 {
@@ -299,6 +300,9 @@ namespace ams::svc {
         ThreadContextFlag_FpuControl = (1 << 3),
 
         ThreadContextFlag_All = (ThreadContextFlag_General | ThreadContextFlag_Control | ThreadContextFlag_Fpu | ThreadContextFlag_FpuControl),
+
+        ThreadContextFlag_SetSingleStep   = (1u << 30),
+        ThreadContextFlag_ClearSingleStep = (1u << 31),
     };
 
     enum ContinueFlag : u32 {

mesosphere/kernel/source/arch/arm64/kern_exception_handlers_asm.s

@@ -15,6 +15,39 @@
  */
 #include <mesosphere/kern_select_assembly_offsets.h>
 
+#if defined(MESOSPHERE_ENABLE_HARDWARE_SINGLE_STEP)
+    .macro disable_single_step, scratch
+    /* Clear MDSCR_EL1.SS. */
+    mrs \scratch, mdscr_el1
+    bic \scratch, \scratch, #1
+    msr mdscr_el1, \scratch
+    .endm
+
+    .macro check_enable_single_step, scratch1, scratch2, spsr_value
+    /* Check if single-step is requested. */
+    ldrb    \scratch1, [sp, #(EXCEPTION_CONTEXT_SIZE + THREAD_STACK_PARAMETERS_IS_SINGLE_STEP)]
+    tbz     \scratch1, #0, .skip_single_step\@
+
+    /* If single-step is requested, enable the single-step machine by setting MDSCR_EL1.SS. */
+    mrs     \scratch2, mdscr_el1
+    orr     \scratch2, \scratch2, #1
+    msr     mdscr_el1, \scratch2
+
+    /* Since we're returning from an exception, set SPSR.SS so we actually advance an instruction. */
+    orr \spsr_value, \spsr_value, #(1 << 21)
+
+    isb
+
+.skip_single_step\@:
+    .endm
+#else
+    .macro disable_single_step, scratch
+    .endm
+
+    .macro check_enable_single_step, scratch1, scratch2, spsr_value
+    .endm
+#endif
+
 /* ams::kern::arch::arm64::EL1IrqExceptionHandler() */
 .section    .text._ZN3ams4kern4arch5arm6422EL1IrqExceptionHandlerEv, "ax", %progbits
 .global     _ZN3ams4kern4arch5arm6422EL1IrqExceptionHandlerEv
@@ -100,6 +133,8 @@ _ZN3ams4kern4arch5arm6422EL0IrqExceptionHandlerEv:
     stp     x21, x22, [sp, #(EXCEPTION_CONTEXT_PC_PSR)]
     str     x23,      [sp, #(EXCEPTION_CONTEXT_TPIDR)]
 
+    disable_single_step x0
+
     /* Invoke KInterruptManager::HandleInterrupt(bool user_mode). */
     ldr     x18, [sp, #(EXCEPTION_CONTEXT_SIZE + THREAD_STACK_PARAMETERS_CUR_THREAD)]
     mov     x0, #1
@@ -110,6 +145,8 @@ _ZN3ams4kern4arch5arm6422EL0IrqExceptionHandlerEv:
     ldp     x21, x22, [sp, #(EXCEPTION_CONTEXT_PC_PSR)]
     ldr     x23,      [sp, #(EXCEPTION_CONTEXT_TPIDR)]
 
+    check_enable_single_step w0, x0, x22
+
     msr     sp_el0, x20
     msr     elr_el1, x21
     msr     spsr_el1, x22
@@ -202,6 +239,8 @@ _ZN3ams4kern4arch5arm6430EL0SynchronousExceptionHandlerEv:
     stp     x21, x22, [sp, #(EXCEPTION_CONTEXT_PC_PSR)]
     str     x23,      [sp, #(EXCEPTION_CONTEXT_TPIDR)]
 
+    disable_single_step x16
+
     /* Call ams::kern::arch::arm64::HandleException(ams::kern::arch::arm64::KExceptionContext *) */
     ldr     x18, [sp, #(EXCEPTION_CONTEXT_SIZE + THREAD_STACK_PARAMETERS_CUR_THREAD)]
     mov     x0,  sp
@@ -212,6 +251,8 @@ _ZN3ams4kern4arch5arm6430EL0SynchronousExceptionHandlerEv:
     ldp     x21, x22, [sp, #(EXCEPTION_CONTEXT_PC_PSR)]
     ldr     x23,      [sp, #(EXCEPTION_CONTEXT_TPIDR)]
 
+    check_enable_single_step w0, x0, x22
+
     msr     sp_el0, x20
     msr     elr_el1, x21
     msr     spsr_el1, x22