kern: implement debug register/vectors init

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

@@ -40,6 +40,8 @@ namespace ams::kern::arm64::cpu {
     MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(TcrEl1, tcr_el1)
     MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(MairEl1, mair_el1)
 
+    MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(VbarEl1, vbar_el1)
+
     MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(SctlrEl1, sctlr_el1)
 
     MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(CpuActlrEl1, s3_1_c15_c2_0)
@@ -47,6 +49,24 @@ namespace ams::kern::arm64::cpu {
 
     MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(CsselrEl1, csselr_el1)
 
+    MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(OslarEl1, oslar_el1)
+
+    #define FOR_I_IN_0_TO_15(HANDLER, ...)                                                                              \
+        HANDLER(0,  ## __VA_ARGS__) HANDLER(1,  ## __VA_ARGS__) HANDLER(2,  ## __VA_ARGS__) HANDLER(3,  ## __VA_ARGS__) \
+        HANDLER(4,  ## __VA_ARGS__) HANDLER(5,  ## __VA_ARGS__) HANDLER(6,  ## __VA_ARGS__) HANDLER(7,  ## __VA_ARGS__) \
+        HANDLER(8,  ## __VA_ARGS__) HANDLER(9,  ## __VA_ARGS__) HANDLER(10, ## __VA_ARGS__) HANDLER(11, ## __VA_ARGS__) \
+        HANDLER(12, ## __VA_ARGS__) HANDLER(13, ## __VA_ARGS__) HANDLER(14, ## __VA_ARGS__) HANDLER(15, ## __VA_ARGS__) \
+
+    #define MESOSPHERE_CPU_DEFINE_DBG_SYSREG_ACCESSORS(ID, ...)                     \
+        MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(DbgWcr##ID##El1, dbgwcr##ID##_el1)   \
+        MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(DbgWvr##ID##El1, dbgwvr##ID##_el1)   \
+        MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(DbgBcr##ID##El1, dbgbcr##ID##_el1)   \
+        MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS(DbgBvr##ID##El1, dbgbvr##ID##_el1)
+
+    FOR_I_IN_0_TO_15(MESOSPHERE_CPU_DEFINE_DBG_SYSREG_ACCESSORS)
+
+    #undef MESOSPHERE_CPU_DEFINE_DBG_SYSREG_ACCESSORS
+
     /* Base class for register accessors. */
     class GenericRegisterAccessorBase {
         NON_COPYABLE(GenericRegisterAccessorBase);
@@ -63,6 +83,21 @@ namespace ams::kern::arm64::cpu {
             constexpr ALWAYS_INLINE u64 GetBits(size_t offset, size_t count) const {
                 return (this->value >> offset) & ((1ul << count) - 1);
             }
+
+            constexpr ALWAYS_INLINE void SetBits(size_t offset, size_t count, u64 value) {
+                const u64 mask = ((1ul << count) - 1) << offset;
+                this->value &= ~mask;
+                this->value |= (value & mask) << offset;
+            }
+
+            constexpr ALWAYS_INLINE void SetBit(size_t offset, bool enabled) {
+                const u64 mask = 1ul << offset;
+                if (enabled) {
+                    this->value |= mask;
+                } else {
+                    this->value &= ~mask;
+                }
+            }
     };
 
     template<typename Derived>
@@ -98,6 +133,43 @@ namespace ams::kern::arm64::cpu {
                 return size_t(1) << (size_t(64) - shift_value);
             }
     };
+
+    MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS(DebugFeature) {
+        public:
+            MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS_FUNCTIONS(DebugFeature, id_aa64dfr0_el1)
+
+            constexpr ALWAYS_INLINE size_t GetNumWatchpoints() const {
+                return this->GetBits(20, 4);
+            }
+
+            constexpr ALWAYS_INLINE size_t GetNumBreakpoints() const {
+                return this->GetBits(12, 4);
+            }
+    };
+
+    MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS(MonitorDebugSystemControl) {
+        public:
+            MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS_FUNCTIONS(MonitorDebugSystemControl, mdscr_el1)
+
+            constexpr ALWAYS_INLINE bool GetMde() const {
+                return this->GetBits(15, 1) != 0;
+            }
+
+            constexpr ALWAYS_INLINE size_t GetTdcc() const {
+                return this->GetBits(12, 1) != 0;
+            }
+
+            constexpr ALWAYS_INLINE decltype(auto) SetMde(bool set) {
+                this->SetBit(15, set);
+                return *this;
+            }
+
+            constexpr ALWAYS_INLINE decltype(auto) SetTdcc(bool set) {
+                this->SetBit(12, set);
+                return *this;
+            }
+    };
+
     MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS(MultiprocessorAffinity) {
         public:
             MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS_FUNCTIONS(MultiprocessorAffinity, mpidr_el1)
@@ -129,6 +201,21 @@ namespace ams::kern::arm64::cpu {
             MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS_FUNCTIONS(ThreadId, tpidr_el1)
     };
 
+    MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS(OsLockAccess) {
+        public:
+            MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS_FUNCTIONS(OsLockAccess, oslar_el1)
+    };
+
+    MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS(ContextId) {
+        public:
+            MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS_FUNCTIONS(ContextId, contextidr_el1)
+
+            constexpr ALWAYS_INLINE decltype(auto) SetProcId(u32 proc_id) {
+                this->SetBits(0, BITSIZEOF(proc_id), proc_id);
+                return *this;
+            }
+    };
+
     MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS(MainId) {
         public:
             enum class Implementer {
@@ -197,6 +284,7 @@ namespace ams::kern::arm64::cpu {
             /* TODO: Other bitfield accessors? */
     };
 
+    #undef  FOR_I_IN_0_TO_15
     #undef  MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS_FUNCTIONS
     #undef  MESOSPHERE_CPU_SYSREG_ACCESSOR_CLASS
     #undef  MESOSPHERE_CPU_DEFINE_SYSREG_ACCESSORS

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

@@ -0,0 +1,141 @@
+/*
+ * 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/>.
+ */
+
+/* Some macros taken from https://github.com/ARM-software/arm-trusted-firmware/blob/master/include/common/aarch64/asm_macros.S */
+/*
+ * Copyright (c) 2013-2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * Declare the exception vector table, enforcing it is aligned on a
+ * 2KB boundary, as required by the ARMv8 architecture.
+ * Use zero bytes as the fill value to be stored in the padding bytes
+ * so that it inserts illegal AArch64 instructions. This increases
+ * security, robustness and potentially facilitates debugging.
+ */
+.macro vector_base  label, section_name=.vectors
+.section \section_name, "ax"
+.align 11, 0
+\label:
+.endm
+
+/*
+ * Create an entry in the exception vector table, enforcing it is
+ * aligned on a 128-byte boundary, as required by the ARMv8 architecture.
+ * Use zero bytes as the fill value to be stored in the padding bytes
+ * so that it inserts illegal AArch64 instructions. This increases
+ * security, robustness and potentially facilitates debugging.
+ */
+.macro vector_entry  label, section_name=.vectors
+.cfi_sections .debug_frame
+.section \section_name, "ax"
+.align 7, 0
+.type \label, %function
+.func \label
+.cfi_startproc
+\label:
+.endm
+
+/*
+ * This macro verifies that the given vector doesnt exceed the
+ * architectural limit of 32 instructions. This is meant to be placed
+ * immediately after the last instruction in the vector. It takes the
+ * vector entry as the parameter
+ */
+.macro check_vector_size since
+    .endfunc
+    .cfi_endproc
+    .if (. - \since) > (32 * 4)
+        .error "Vector exceeds 32 instructions"
+    .endif
+.endm
+
+/* Actual Vectors for Kernel. */
+.global _ZN3ams4kern16ExceptionVectorsEv
+vector_base _ZN3ams4kern16ExceptionVectorsEv
+
+/* Current EL, SP0 */
+.global unknown_exception
+unknown_exception:
+vector_entry synch_sp0
+    /* Just infinite loop. */
+    b unknown_exception
+    check_vector_size synch_sp0
+
+vector_entry irq_sp0
+    b unknown_exception
+    check_vector_size irq_sp0
+
+vector_entry fiq_sp0
+    b unknown_exception
+    check_vector_size fiq_sp0
+
+vector_entry serror_sp0
+    b unknown_exception
+    check_vector_size serror_sp0
+
+/* Current EL, SPx */
+vector_entry synch_spx
+    b unknown_exception
+    check_vector_size synch_spx
+
+vector_entry irq_spx
+    b unknown_exception
+    check_vector_size irq_spx
+
+vector_entry fiq_spx
+    b unknown_exception
+    check_vector_size fiq_spx
+
+vector_entry serror_spx
+    b unknown_exception
+    check_vector_size serror_spx
+
+/* Lower EL, A64 */
+vector_entry synch_a64
+    b unknown_exception
+    check_vector_size synch_a64
+
+vector_entry irq_a64
+    b unknown_exception
+    check_vector_size irq_a64
+
+vector_entry fiq_a64
+    b unknown_exception
+    check_vector_size fiq_a64
+
+vector_entry serror_a64
+    b unknown_exception
+    check_vector_size serror_a64
+
+/* Lower EL, A32 */
+vector_entry synch_a32
+    b unknown_exception
+    check_vector_size synch_a32
+
+vector_entry irq_a32
+    b unknown_exception
+    check_vector_size irq_a32
+
+vector_entry fiq_a32
+    b unknown_exception
+    check_vector_size fiq_a32
+
+vector_entry serror_a32
+    b unknown_exception
+    check_vector_size serror_a32

mesosphere/kernel/source/arch/arm64/init/kern_init_core.cpp

@@ -18,6 +18,12 @@
 extern "C" void _start();
 extern "C" void __end__();
 
+namespace ams::kern {
+
+    void ExceptionVectors();
+
+}
+
 namespace ams::kern::init {
 
     /* Prototypes for functions declared in ASM that we need to reference. */
@@ -310,11 +316,71 @@ namespace ams::kern::init {
     }
 
     void InitializeDebugRegisters() {
-        /* TODO */
+        /* Determine how many watchpoints and breakpoints we have */
+        cpu::DebugFeatureRegisterAccessor aa64dfr0;
+        const auto num_watchpoints = aa64dfr0.GetNumWatchpoints();
+        const auto num_breakpoints = aa64dfr0.GetNumBreakpoints();
+        cpu::EnsureInstructionConsistency();
+
+        /* Clear the debug monitor register and the os lock access register. */
+        cpu::MonitorDebugSystemControlRegisterAccessor(0).Store();
+        cpu::EnsureInstructionConsistency();
+        cpu::OsLockAccessRegisterAccessor(0).Store();
+        cpu::EnsureInstructionConsistency();
+
+        /* Clear all debug watchpoints/breakpoints. */
+        #define FOR_I_IN_15_TO_1(HANDLER, ...)                                                                              \
+            HANDLER(15, ## __VA_ARGS__) HANDLER(14, ## __VA_ARGS__) HANDLER(13, ## __VA_ARGS__) HANDLER(12, ## __VA_ARGS__) \
+            HANDLER(11, ## __VA_ARGS__) HANDLER(10, ## __VA_ARGS__) HANDLER(9,  ## __VA_ARGS__) HANDLER(8,  ## __VA_ARGS__) \
+            HANDLER(7,  ## __VA_ARGS__) HANDLER(6,  ## __VA_ARGS__) HANDLER(5,  ## __VA_ARGS__) HANDLER(4,  ## __VA_ARGS__) \
+            HANDLER(3,  ## __VA_ARGS__) HANDLER(2,  ## __VA_ARGS__) HANDLER(1,  ## __VA_ARGS__)
+
+        #define MESOSPHERE_INITIALIZE_WATCHPOINT_CASE(ID, ...) \
+            case ID:                                           \
+                cpu::SetDbgWcr##ID##El1(__VA_ARGS__);          \
+                cpu::SetDbgWvr##ID##El1(__VA_ARGS__);          \
+
+        #define MESOSPHERE_INITIALIZE_BREAKPOINT_CASE(ID, ...) \
+            case ID:                                           \
+                cpu::SetDbgBcr##ID##El1(__VA_ARGS__);          \
+                cpu::SetDbgBvr##ID##El1(__VA_ARGS__);          \
+            [[fallthrough]];
+
+
+        switch (num_watchpoints) {
+            FOR_I_IN_15_TO_1(MESOSPHERE_INITIALIZE_WATCHPOINT_CASE, 0)
+            default:
+                break;
+        }
+        cpu::SetDbgWcr0El1(0);
+        cpu::SetDbgWvr0El1(0);
+
+        switch (num_breakpoints) {
+            FOR_I_IN_15_TO_1(MESOSPHERE_INITIALIZE_BREAKPOINT_CASE, 0)
+            default:
+                break;
+        }
+        cpu::SetDbgBcr0El1(0);
+        cpu::SetDbgBvr0El1(0);
+
+        #undef MESOSPHERE_INITIALIZE_WATCHPOINT_CASE
+        #undef MESOSPHERE_INITIALIZE_BREAKPOINT_CASE
+        #undef FOR_I_IN_15_TO_1
+
+        cpu::EnsureInstructionConsistency();
+
+        /* Initialize the context id register to all 1s. */
+        cpu::ContextIdRegisterAccessor(0).SetProcId(std::numeric_limits<u32>::max()).Store();
+        cpu::EnsureInstructionConsistency();
+
+        /* Configure the debug monitor register. */
+        cpu::MonitorDebugSystemControlRegisterAccessor(0).SetMde(true).SetTdcc(true).Store();
+        cpu::EnsureInstructionConsistency();
     }
 
     void InitializeExceptionVectors() {
-        /* TODO */
+        cpu::SetVbarEl1(reinterpret_cast<uintptr_t>(::ams::kern::ExceptionVectors));
+        cpu::EnsureInstructionConsistency();
     }
 
 }