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);