diff --git a/thermosphere/src/arm.h b/thermosphere/src/arm.h
index 5d88a5b35..993aac94a 100644
--- a/thermosphere/src/arm.h
+++ b/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);
diff --git a/thermosphere/src/arm.s b/thermosphere/src/arm.s
index 587d58724..c5b59b9ca 100644
--- a/thermosphere/src/arm.s
+++ b/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
diff --git a/thermosphere/src/breakpoints.c b/thermosphere/src/breakpoints.c
index 5efb71a83..94e03a199 100644
--- a/thermosphere/src/breakpoints.c
+++ b/thermosphere/src/breakpoints.c
@@ -14,16 +14,118 @@
* along with this program. If not, see .
*/
+#include
#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;
+}
\ No newline at end of file
diff --git a/thermosphere/src/breakpoints.h b/thermosphere/src/breakpoints.h
index 1f8b888b0..c2d6cf34b 100644
--- a/thermosphere/src/breakpoints.h
+++ b/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);
diff --git a/thermosphere/src/breakpoints_watchpoints_common.h b/thermosphere/src/breakpoints_watchpoints_common.h
new file mode 100644
index 000000000..36e4f3dc0
--- /dev/null
+++ b/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 .
+ */
+
+#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);
+}
\ No newline at end of file
diff --git a/thermosphere/src/breakpoints_watchpoints_save_restore.h b/thermosphere/src/breakpoints_watchpoints_save_restore.h
new file mode 100644
index 000000000..8ca0b12f7
--- /dev/null
+++ b/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 .
+ */
+
+#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);
\ No newline at end of file
diff --git a/thermosphere/src/breakpoints_watchpoints_save_restore.s b/thermosphere/src/breakpoints_watchpoints_save_restore.s
new file mode 100644
index 000000000..e938d5948
--- /dev/null
+++ b/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 .
+ */
+
+
+// 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
diff --git a/thermosphere/src/core_ctx.h b/thermosphere/src/core_ctx.h
index be3b1575c..2f73bc939 100644
--- a/thermosphere/src/core_ctx.h
+++ b/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];
diff --git a/thermosphere/src/main.c b/thermosphere/src/main.c
index a0d71bc52..59687ad31 100644
--- a/thermosphere/src/main.c
+++ b/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);
diff --git a/thermosphere/src/watchpoints.c b/thermosphere/src/watchpoints.c
index 872de8922..32ca585fa 100644
--- a/thermosphere/src/watchpoints.c
+++ b/thermosphere/src/watchpoints.c
@@ -14,16 +14,26 @@
* along with this program. If not, see .
*/
-#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);
}
diff --git a/thermosphere/src/watchpoints.h b/thermosphere/src/watchpoints.h
index d2c7de677..12a6a5e89 100644
--- a/thermosphere/src/watchpoints.h
+++ b/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);