thermosphere: add actual breakpoint code

thermosphere/src/arm.h

@@ -14,5 +14,5 @@ void invalidate_icache_all(void);
 void set_memory_registers_enable_mmu(uintptr_t ttbr0, u64 tcr, u64 mair);
 void set_memory_registers_enable_stage2(uintptr_t vttbr, u64 vtcr);
 
-void initBreakpointRegs(size_t num);
+void reloadBreakpointRegs(size_t num);
 void initWatchpointRegs(size_t num);

thermosphere/src/arm.s

@@ -262,46 +262,3 @@ set_memory_registers_enable_stage2:
     isb
 
     ret
-
-
-// Precondition: x0 <= 16
-.section    .text.initBreakpointRegs, "ax", %progbits
-.type       initBreakpointRegs, %function
-.global     initBreakpointRegs
-initBreakpointRegs:
-    // x0 = number
-    adr     x16, 1f
-    mov     x1, #(16 * 8)
-    sub     x0, x1, x0,lsl #3
-    add     x16, x16, x0
-    br      x16
-
-    1:
-    .irp    count, 15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0
-    msr     dbgbcr\count\()_el1, xzr
-    msr     dbgbvr\count\()_el1, xzr
-    .endr
-    dsb     sy
-    isb
-    ret
-
-// Precondition: x0 <= 16
-.section    .text.initBreakpointRegs, "ax", %progbits
-.type       initWatchpointRegs, %function
-.global     initWatchpointRegs
-initWatchpointRegs:
-    // x0 = number
-    adr     x16, 1f
-    mov     x1, #(16 * 8)
-    sub     x0, x1, x0,lsl #3
-    add     x16, x16, x0
-    br      x16
-
-    1:
-    .irp    count, 15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0
-    msr     dbgwcr\count\()_el1, xzr
-    msr     dbgwvr\count\()_el1, xzr
-    .endr
-    dsb     sy
-    isb
-    ret

thermosphere/src/breakpoints.c

@@ -14,16 +14,118 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <string.h>
 #include "breakpoints.h"
+#include "breakpoints_watchpoints_save_restore.h"
 #include "utils.h"
 #include "sysreg.h"
 #include "arm.h"
 
-void enableAndResetBreakpoints(void)
-{
-    SET_SYSREG(mdscr_el1, GET_SYSREG(mdscr_el1) | MDSCR_EL1_MDE);
+BreakpointManager g_breakpointManager = {0};
 
-    // 20 for watchpoints
-    size_t num = ((GET_SYSREG(id_aa64dfr0_el1) >> 12) & 0xF) + 1;
-    initBreakpointRegs(num);
+// Init the structure (already in BSS, so already zero-initialized) and load the registers
+void initBreakpoints(void)
+{
+    recursiveSpinlockLock(&g_breakpointManager.lock);
+
+    if (currentCoreCtx->isBootCore && !currentCoreCtx->warmboot) {
+        size_t num = ((GET_SYSREG(id_aa64dfr0_el1) >> 12) & 0xF) + 1;
+        g_breakpointManager.maxBreakpoints = (u32)num;
+        g_breakpointManager.allocationBitmap = BIT(num) - 1;
+    }
+
+    loadBreakpointRegs(g_breakpointManager.breakpoints, g_breakpointManager.maxBreakpoints);
+
+    recursiveSpinlockUnlock(&g_breakpointManager.lock);
 }
+
+static DebugRegisterPair *allocateBreakpoint(void)
+{
+    u32 pos = __builtin_ffs(g_breakpointManager.allocationBitmap);
+    if (pos == 0) {
+        return NULL;
+    } else {
+        g_breakpointManager.allocationBitmap &= ~BIT(pos - 1);
+        return &g_breakpointManager.breakpoints[pos - 1];
+    }
+}
+
+static void freeBreakpoint(u32 pos)
+{
+    memset(&g_breakpointManager.breakpoints[pos], 0, sizeof(DebugRegisterPair));
+    g_breakpointManager.allocationBitmap |= BIT(pos);
+}
+
+static DebugRegisterPair *findBreakpoint(u64 addr)
+{
+    u16 bitmap = g_breakpointManager.allocationBitmap;
+    while (bitmap != 0) {
+        u32 pos = __builtin_ffs(bitmap);
+        if (pos == 0) {
+            return NULL;
+        } else {
+            bitmap &= ~BIT(pos - 1);
+            if (g_breakpointManager.breakpoints[pos - 1].vr == addr) {
+                return &g_breakpointManager.breakpoints[pos - 1];
+            }
+        }
+    }
+
+    return NULL;
+}
+
+// Note: A32/T32/T16 support intentionnally left out
+// Note: addresses are supposed to be well-formed regarding the sign extension bits
+
+bool addBreakpoint(u64 addr)
+{
+    recursiveSpinlockLock(&g_breakpointManager.lock);
+
+    // Reject misaligned addresses
+    if (addr & 3) {
+        return false;
+    }
+
+    // Breakpoint already added
+    if (findBreakpoint(addr) != NULL) {
+        return true;
+    }
+
+    DebugRegisterPair *regs = allocateBreakpoint();
+    regs->cr.bt = BreakpointType_AddressMatch;
+    regs->cr.linked = false;
+
+    // NS EL1&0 only
+    regs->cr.hmc = DebugHmc_LowerEl;
+    regs->cr.ssc = DebugSsc_NonSecure;
+    regs->cr.pmc = DebugPmc_El1And0;
+
+    regs->cr.bas = 0xF;
+    regs->cr.enabled = true;
+
+    regs->vr = addr;
+
+    recursiveSpinlockUnlock(&g_breakpointManager.lock);
+
+    // TODO commit & broadcast
+
+    return true;
+}
+
+bool removeBreakpoint(u64 addr)
+{
+    recursiveSpinlockLock(&g_breakpointManager.lock);
+
+    DebugRegisterPair *regs = findBreakpoint(addr);
+
+    if (findBreakpoint(addr) == NULL) {
+        return false;
+    }
+
+    freeBreakpoint(regs - &g_breakpointManager.breakpoints[0]);
+    recursiveSpinlockUnlock(&g_breakpointManager.lock);
+
+    // TODO commit & broadcast
+
+    return true;
+}

thermosphere/src/breakpoints.h

@@ -16,6 +16,17 @@
 
 #pragma once
 
-#include "types.h"
+#include "breakpoints_watchpoints_common.h"
+#include "spinlock.h"
 
-void enableAndResetBreakpoints(void);
+/// Structure to synchronize and keep track of breakpoints
+typedef struct BreakpointManager {
+    DebugRegisterPair breakpoints[16];
+    RecursiveSpinlock lock;
+    u32 maxBreakpoints;
+    u16 allocationBitmap;
+} BreakpointManager;
+
+extern BreakpointManager g_breakpointManager;
+
+void initBreakpoints(void);

thermosphere/src/breakpoints_watchpoints_common.h

@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2019 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 "types.h"
+#include "sysreg.h"
+
+/// BT[3:1] or res0. BT[0]/WT[0] is "is linked"
+typedef enum BreakpointType {
+    BreakpointType_AddressMatch = 0,
+    BreakpointType_VheContextIdMatch = 1,
+    BreakpointType_ContextIdMatch = 3,
+    BreakpointType_VmidMatch = 4,
+    BreakpointType_VmidContextIdMatch = 5,
+    BreakpointType_VmidVheContextIdMatch = 6,
+    BreakpointType_FullVheContextIdMatch = 7,
+} BreakpointType;
+
+// Note: some SSC HMC PMC combinations are invalid
+// Refer to "Table D2-9 Summary of breakpoint HMC, SSC, and PMC encodings"
+
+/// Debug Security State Control
+typedef enum DebugSsc {
+    DebugSsc_Both                  = 0,
+    DebugSsc_NonSecure             = 1,
+    DebugSsc_Secure                = 2,
+    DebugSsc_SecureIfLowerOrBoth   = 3,
+} DebugSsc;
+
+/// Debug Higher Mode Control
+typedef enum DebugHmc {
+    DebugHmc_LowerEl   = 0,
+    DebugHmc_HigherEl  = 1,
+} DebugHmc;
+
+/// Debug Privilege Mode Control (called PAC for watchpoints)
+typedef enum DebugPmc {
+    DebugPmc_NeitherEl1Nor0    = 0,
+    DebugPmc_El1               = 1,
+    DebugPmc_El0               = 2,
+    DebugPmc_El1And0           = 3,
+} DebugPmc;
+
+typedef enum WatchpointLoadStoreControl {
+    WatchpointLoadStoreControl_Load         = 0,
+    WatchpointLoadStoreControl_Store        = 2,
+    WatchpointLoadStoreControl_LoadStore    = 3,
+} WatchpointLoadStoreControl;
+
+// bas only 4 bits for breakpoints, other bits res0.
+// lsc, mask only for watchpoints, res0 for breakpoints
+// bt only from breakpoints, res0 for watchpoints
+typedef struct DebugControlRegister {
+    bool enabled                        : 1;
+    DebugPmc pmc                        : 2;
+    WatchpointLoadStoreControl lsc      : 2;
+    u32 bas                             : 8;
+    DebugHmc hmc                        : 1;
+    DebugSsc ssc                        : 2;
+    u32 lbn                             : 4;
+    bool linked                         : 1;
+    BreakpointType bt                   : 3;
+    u32 mask                            : 5;
+    u64 res0                            : 35;
+} DebugControlRegister;
+
+typedef struct DebugRegisterPair {
+    DebugControlRegister cr;
+    u64 vr;
+} DebugRegisterPair;
+
+static inline void enableBreakpointsAndWatchpoints(void)
+{
+    SET_SYSREG(mdscr_el1, GET_SYSREG(mdscr_el1) | MDSCR_EL1_MDE);
+}

thermosphere/src/breakpoints_watchpoints_save_restore.h

@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2018-2019 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 "breakpoints_watchpoints_common.h"
+
+void loadBreakpointRegs(const DebugRegisterPair *regs, size_t num);
+void saveBreakpointRegs(DebugRegisterPair *regs, size_t num);
+
+void loadWatchpointRegs(const DebugRegisterPair *regs, size_t num);
+void saveWatchpointRegs(DebugRegisterPair *regs, size_t num);

thermosphere/src/breakpoints_watchpoints_save_restore.s

@@ -0,0 +1,118 @@
+/*
+ * Copyright (c) 2018-2019 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/>.
+ */
+
+
+// Precondition: x1 <= 16
+.section    .text.loadBreakpointRegs, "ax", %progbits
+.type       loadBreakpointRegs, %function
+.global     loadBreakpointRegs
+loadBreakpointRegs:
+    // x1 = number
+    adr     x16, 1f
+    add     x0, x0, #(16 * 8)
+    mov     x4, #(16 * 12)
+    sub     x4, x4, x1,lsl #3
+    sub     x4, x4, x1,lsl #2
+    add     x16, x16, x4
+    br      x16
+
+    1:
+    .irp    count, 15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0
+    ldp     x2, x3, [x0, #-0x10]!
+    msr     dbgbcr\count\()_el1, x2
+    msr     dbgbvr\count\()_el1, x3
+    .endr
+    dsb     sy
+    isb
+    ret
+
+// Precondition: x1 <= 16
+.section    .text.storeBreakpointRegs, "ax", %progbits
+.type       storeBreakpointRegs, %function
+.global     storBreakpointRegs
+storeBreakpointRegs:
+    // x1 = number
+    dsb     sy
+    isb
+
+    adr     x16, 1f
+    add     x0, x0, #(16 * 8)
+    mov     x4, #(16 * 12)
+    sub     x4, x4, x1,lsl #3
+    sub     x4, x4, x1,lsl #2
+    add     x16, x16, x4
+    br      x16
+
+    1:
+    .irp    count, 15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0
+    mrs     x2, dbgbcr\count\()_el1
+    mrs     x3, dbgbvr\count\()_el1
+    stp     x2, x3, [x0, #-0x10]!
+
+    .endr
+    ret
+
+
+// Precondition: x1 <= 16
+.section    .text.loadWatchpointRegs, "ax", %progbits
+.type       loadWatchpointRegs, %function
+.global     loadWatchpointRegs
+loadWatchpointRegs:
+    // x1 = number
+    adr     x16, 1f
+    add     x0, x0, #(16 * 8)
+    mov     x4, #(16 * 12)
+    sub     x4, x4, x1,lsl #3
+    sub     x4, x4, x1,lsl #2
+    add     x16, x16, x4
+    br      x16
+
+    1:
+    .irp    count, 15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0
+    ldp     x2, x3, [x0, #-0x10]!
+    msr     dbgwcr\count\()_el1, x2
+    msr     dbgwvr\count\()_el1, x3
+    .endr
+    dsb     sy
+    isb
+    ret
+
+// Precondition: x1 <= 16
+.section    .text.storeWatchpointRegs, "ax", %progbits
+.type       storeWatchpointRegs, %function
+.global     storWatchpointRegs
+storeWatchpointRegs:
+    // x1 = number
+
+    dsb     sy
+    isb
+
+    adr     x16, 1f
+    add     x0, x0, #(16 * 8)
+    mov     x4, #(16 * 12)
+    sub     x4, x4, x1,lsl #3
+    sub     x4, x4, x1,lsl #2
+    add     x16, x16, x4
+    br      x16
+
+    1:
+    .irp    count, 15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0
+    mrs     x2, dbgwcr\count\()_el1
+    mrs     x3, dbgwvr\count\()_el1
+    stp     x2, x3, [x0, #-0x10]!
+
+    .endr
+    ret

thermosphere/src/core_ctx.h

@@ -24,7 +24,7 @@ typedef struct CoreCtx {
     u64 scratch;                    // @0x18
     u32 coreId;                     // @0x20
     bool isBootCore;                // @0x24
-    //bool wasSingleStepping;         // @0x25 (for pIRQ handler)
+    bool warmboot;                  // @0x25
 } CoreCtx;
 
 extern CoreCtx g_coreCtxs[4];

thermosphere/src/main.c

@@ -40,8 +40,10 @@ static void loadKernelViaSemihosting(void)
 void main(ExceptionStackFrame *frame)
 {
     enableTraps();
-    enableAndResetBreakpoints();
-    enableAndResetWatchpoints();
+    enableBreakpointsAndWatchpoints();
+
+    initBreakpoints();
+    initWatchpoints();
 
     if (currentCoreCtx->isBootCore) {
         uartInit(115200);

thermosphere/src/watchpoints.c

@@ -14,16 +14,26 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "breakpoints.h"
+#include "watchpoints.h"
+#include "breakpoints_watchpoints_save_restore.h"
 #include "utils.h"
 #include "sysreg.h"
 #include "arm.h"
 
-void enableAndResetWatchpoints(void)
-{
-    SET_SYSREG(mdscr_el1, GET_SYSREG(mdscr_el1) | MDSCR_EL1_MDE);
+WatchpointManager g_watchpointManager = {0};
 
-    // 20 for watchpoints
-    size_t num = ((GET_SYSREG(id_aa64dfr0_el1) >> 12) & 0xF) + 1;
-    initBreakpointRegs(num);
+// Init the structure (already in BSS, so already zero-initialized) and load the registers
+void initWatchpoints(void)
+{
+    recursiveSpinlockLock(&g_watchpointManager.lock);
+
+    if (currentCoreCtx->isBootCore && !currentCoreCtx->warmboot) {
+        size_t num = ((GET_SYSREG(id_aa64dfr0_el1) >> 12) & 0xF) + 1;
+        g_watchpointManager.maxWatchpoints = (u32)num;
+        g_watchpointManager.allocationBitmap = 0xFFFF;
+    }
+
+    loadWatchpointRegs(g_watchpointManager.watchpoints, g_watchpointManager.maxWatchpoints);
+
+    recursiveSpinlockUnlock(&g_watchpointManager.lock);
 }

thermosphere/src/watchpoints.h

@@ -16,6 +16,17 @@
 
 #pragma once
 
-#include "types.h"
+#include "breakpoints_watchpoints_common.h"
+#include "spinlock.h"
 
-void enableAndResetWatchpoints(void);
+/// Structure to synchronize and keep track of watchpoints
+typedef struct WatchpointManager {
+    DebugRegisterPair watchpoints[16];
+    RecursiveSpinlock lock;
+    u32 maxWatchpoints;
+    u16 allocationBitmap;
+} WatchpointManager;
+
+extern WatchpointManager g_watchpointManager;
+
+void initWatchpoints(void);