diff --git a/thermosphere/src/breakpoints.c b/thermosphere/src/breakpoints.c index e02797119..5465f9905 100644 --- a/thermosphere/src/breakpoints.c +++ b/thermosphere/src/breakpoints.c @@ -77,18 +77,20 @@ static DebugRegisterPair *findBreakpoint(u64 addr) // 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) +int addBreakpoint(u64 addr) { recursiveSpinlockLock(&g_breakpointManager.lock); // Reject misaligned addresses if (addr & 3) { - return false; + recursiveSpinlockUnlock(&g_breakpointManager.lock); + return -EINVAL; } // Breakpoint already added if (findBreakpoint(addr) != NULL) { - return true; + recursiveSpinlockUnlock(&g_breakpointManager.lock); + return -EEXIST; } DebugRegisterPair *regs = allocateBreakpoint(); @@ -109,10 +111,10 @@ bool addBreakpoint(u64 addr) // TODO commit & broadcast - return true; + return 0; } -bool removeBreakpoint(u64 addr) +int removeBreakpoint(u64 addr) { recursiveSpinlockLock(&g_breakpointManager.lock); @@ -120,7 +122,7 @@ bool removeBreakpoint(u64 addr) if (findBreakpoint(addr) == NULL) { recursiveSpinlockUnlock(&g_breakpointManager.lock); - return false; + return -ENOENT; } freeBreakpoint(regs - &g_breakpointManager.breakpoints[0]); @@ -128,5 +130,18 @@ bool removeBreakpoint(u64 addr) // TODO commit & broadcast - return true; + return 0; } + +int removeAllBreakpoints(void) +{ + recursiveSpinlockLock(&g_breakpointManager.lock); + g_breakpointManager.allocationBitmap = BIT(g_breakpointManager.maxBreakpoints) - 1; + memset(g_breakpointManager.breakpoints, 0, sizeof(g_breakpointManager.breakpoints)); + + // TODO: commit & broadcast + + recursiveSpinlockUnlock(&g_breakpointManager.lock); + + return 0; +} \ No newline at end of file diff --git a/thermosphere/src/breakpoints.h b/thermosphere/src/breakpoints.h index c2d6cf34b..82e0b9984 100644 --- a/thermosphere/src/breakpoints.h +++ b/thermosphere/src/breakpoints.h @@ -19,6 +19,9 @@ #include "breakpoints_watchpoints_common.h" #include "spinlock.h" +#define _REENT_ONLY +#include + /// Structure to synchronize and keep track of breakpoints typedef struct BreakpointManager { DebugRegisterPair breakpoints[16]; @@ -30,3 +33,6 @@ typedef struct BreakpointManager { extern BreakpointManager g_breakpointManager; void initBreakpoints(void); +int addBreakpoint(u64 addr); +int removeBreakpoint(u64 addr); +int removeAllBreakpoints(void); diff --git a/thermosphere/src/software_breakpoints.c b/thermosphere/src/software_breakpoints.c new file mode 100644 index 000000000..4921ab400 --- /dev/null +++ b/thermosphere/src/software_breakpoints.c @@ -0,0 +1,204 @@ +/* + * 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 . + */ + +#include +#include "software_breakpoints.h" +#include "utils.h" +#include "arm.h" + +SoftwareBreakpointManager g_softwareBreakpointManager = {0}; + +/* + Consider the following: + - Breakpoints are based on VA + - Translation tables may change + - Translation tables may differ from core to core + + We also define sw breakpoints on invalid addresses (for one or more cores) UNPREDICTABLE. +*/ + +static size_t findClosestSoftwareBreakpointSlot(u64 address) +{ + if(g_softwareBreakpointManager.numBreakpoints == 0 || address <= g_softwareBreakpointManager.breakpoints[0].address) { + return 0; + } else if(address > g_softwareBreakpointManager.breakpoints[g_softwareBreakpointManager.numBreakpoints - 1].address) { + return g_softwareBreakpointManager.numBreakpoints; + } + + size_t a = 0, b = g_softwareBreakpointManager.numBreakpoints - 1, m; + + do { + m = (a + b) / 2; + if(g_softwareBreakpointManager.breakpoints[m].address < address) { + a = m; + } else if(g_softwareBreakpointManager.breakpoints[m].address > address) { + b = m; + } else { + return m; + } + } while(b - a > 1); + + return b; +} + +static bool doApplySoftwareBreakpoint(size_t id) +{ + SoftwareBreakpoint *bp = &g_softwareBreakpointManager.breakpoints[id]; + if (bp->applied) { + return true; + } + + u32 brkInst = 0xF2000000 | bp->uid; + + if (readEl1Memory(&bp->savedInstruction, bp->address, 4) && writeEl1Memory(bp->address, &brkInst, 4)) { + bp->applied = true; + return true; + } + + return false; +} + +static bool doRevertSoftwareBreakpoint(size_t id) +{ + SoftwareBreakpoint *bp = &g_softwareBreakpointManager.breakpoints[id]; + if (!bp->applied) { + return true; + } + + if (writeEl1Memory(bp->address, &bp->savedInstruction, 4)) { + bp->applied = false; + return true; + } + + return false; +} + +// TODO write SGI handlers for those ^ + +bool applyAllSoftwareBreakpoints(void) +{ + recursiveSpinlockLock(&g_softwareBreakpointManager.lock); + bool ret = true; + + for (size_t i = 0; i < g_softwareBreakpointManager.numBreakpoints; i++) { + ret = ret && doApplySoftwareBreakpoint(i); + } + + recursiveSpinlockUnlock(&g_softwareBreakpointManager.lock); + return ret; +} + +bool revertAllSoftwareBreakpoints(void) +{ + recursiveSpinlockLock(&g_softwareBreakpointManager.lock); + bool ret = true; + for (size_t i = 0; i < g_softwareBreakpointManager.numBreakpoints; i++) { + ret = ret && doRevertSoftwareBreakpoint(i); + } + + recursiveSpinlockUnlock(&g_softwareBreakpointManager.lock); + return ret; +} + +int addSoftwareBreakpoint(u64 addr, bool persistent) +{ + if ((addr & 3) != 0) { + return -EINVAL; + } + + recursiveSpinlockLock(&g_softwareBreakpointManager.lock); + + size_t id = findClosestSoftwareBreakpointSlot(addr); + + if(id != g_softwareBreakpointManager.numBreakpoints && g_softwareBreakpointManager.breakpoints[id].address == addr) { + recursiveSpinlockUnlock(&g_softwareBreakpointManager.lock); + return -EEXIST; + } else if(g_softwareBreakpointManager.numBreakpoints == MAX_SW_BREAKPOINTS) { + recursiveSpinlockUnlock(&g_softwareBreakpointManager.lock); + return -EBUSY; + } + + for(size_t i = g_softwareBreakpointManager.numBreakpoints; i > id && i != 0; i--) { + g_softwareBreakpointManager.breakpoints[i] = g_softwareBreakpointManager.breakpoints[i - 1]; + } + ++g_softwareBreakpointManager.numBreakpoints; + + SoftwareBreakpoint *bp = &g_softwareBreakpointManager.breakpoints[id]; + bp->address = addr; + bp->persistent = persistent; + bp->applied = false; + bp->uid = 0x2000 + g_softwareBreakpointManager.bpUniqueCounter++; + + // TODO: write broadcast for apply + // Note: no way to handle breakpoint failing to apply on 1+ core but not all, we need to assume operation succeeds + recursiveSpinlockUnlock(&g_softwareBreakpointManager.lock); + + return 0; +} + +int removeSoftwareBreakpoint(u64 addr, bool keepPersistent) +{ + if ((addr & 3) != 0) { + return -EINVAL; + } + + recursiveSpinlockLock(&g_softwareBreakpointManager.lock); + + size_t id = findClosestSoftwareBreakpointSlot(addr); + + if(id == g_softwareBreakpointManager.numBreakpoints || g_softwareBreakpointManager.breakpoints[id].address != addr) { + recursiveSpinlockUnlock(&g_softwareBreakpointManager.lock); + return -ENOENT; + } + + SoftwareBreakpoint *bp = &g_softwareBreakpointManager.breakpoints[id]; + if (!keepPersistent || !bp->persistent) { + // TODO: write broadcast for 'revert' + // Note: no way to handle breakpoint failing to revert on 1+ core but not all, we need to assume operation succeeds + } + + for(size_t i = id; i < g_softwareBreakpointManager.numBreakpoints - 1; i++) { + g_softwareBreakpointManager.breakpoints[i] = g_softwareBreakpointManager.breakpoints[i + 1]; + } + + memset(&g_softwareBreakpointManager.breakpoints[--g_softwareBreakpointManager.numBreakpoints], 0, sizeof(SoftwareBreakpoint)); + + recursiveSpinlockUnlock(&g_softwareBreakpointManager.lock); + + return 0; +} + +int removeAllSoftwareBreakpoints(bool keepPersistent) +{ + int ret = 0; + recursiveSpinlockLock(&g_softwareBreakpointManager.lock); + + for (size_t id = 0; id < g_softwareBreakpointManager.numBreakpoints; id++) { + SoftwareBreakpoint *bp = &g_softwareBreakpointManager.breakpoints[id]; + if (!keepPersistent || !bp->persistent) { + // TODO: write broadcast for 'revert' + // Note: no way to handle breakpoint failing to revert on 1+ core but not all, we need to assume operation succeeds + } + } + + g_softwareBreakpointManager.numBreakpoints = 0; + g_softwareBreakpointManager.bpUniqueCounter = 0; + memset(g_softwareBreakpointManager.breakpoints, 0, sizeof(g_softwareBreakpointManager.breakpoints)); + + recursiveSpinlockUnlock(&g_softwareBreakpointManager.lock); + + return ret; +} \ No newline at end of file diff --git a/thermosphere/src/software_breakpoints.h b/thermosphere/src/software_breakpoints.h new file mode 100644 index 000000000..7d6374f74 --- /dev/null +++ b/thermosphere/src/software_breakpoints.h @@ -0,0 +1,48 @@ +/* + * 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 + +#define _REENT_ONLY +#include + +#include "spinlock.h" + +#define MAX_SW_BREAKPOINTS 32 + +typedef struct SoftwareBreakpoint { + u64 address; // VA + u32 savedInstruction; + u32 uid; + bool persistent; + bool applied; +} SoftwareBreakpoint; + +typedef struct SoftwareBreakpointManager { + RecursiveSpinlock lock; + size_t numBreakpoints; + SoftwareBreakpoint breakpoints[MAX_SW_BREAKPOINTS]; + u32 bpUniqueCounter; +} SoftwareBreakpointManager; + +extern SoftwareBreakpointManager g_softwareBreakpointManager; + +bool revertAllSoftwareBreakpoints(void); +bool applyAllSoftwareBreakpoints(void); + +int addSoftwareBreakpoint(u64 addr, bool persistent); +int removeSoftwareBreakpoint(u64 addr, bool keepPersistent); +int removeAllSoftwareBreakpoints(bool keepPersistent); \ No newline at end of file diff --git a/thermosphere/src/spinlock.h b/thermosphere/src/spinlock.h index c6b948bf4..4fff76cc4 100644 --- a/thermosphere/src/spinlock.h +++ b/thermosphere/src/spinlock.h @@ -27,20 +27,20 @@ typedef struct Spinlock { typedef struct RecursiveSpinlock { Spinlock lock; u32 count; - u32 tag; + vu32 tag; } RecursiveSpinlock; static inline u64 maskIrq(void) { u64 ret = GET_SYSREG(daif); - SET_SYSREG(daifset, BITL(1)); + SET_SYSREG_IMM(daifset, BITL(1)); return ret; } static inline u64 unmaskIrq(void) { u64 ret = GET_SYSREG(daif); - SET_SYSREG(daifclr, BITL(1)); + SET_SYSREG_IMM(daifclr, BITL(1)); return ret; } diff --git a/thermosphere/src/sysreg.h b/thermosphere/src/sysreg.h index 53b9e7d6d..69f77dfd3 100644 --- a/thermosphere/src/sysreg.h +++ b/thermosphere/src/sysreg.h @@ -423,7 +423,8 @@ __val; \ }) -#define SET_SYSREG(reg, val) do { u64 temp_reg = (val); __asm__ __volatile__ ("msr " #reg ", %0" :: "r"(temp_reg) : "memory"); } while(false) +#define SET_SYSREG(reg, val) do { u64 temp_reg = (val); __asm__ __volatile__ ("msr " #reg ", %0" :: "r"(temp_reg) : "memory"); } while(false) +#define SET_SYSREG_IMM(reg, imm) do { __asm__ __volatile__ ("msr " #reg ", %0" :: "I"(imm) : "memory"); } while(false) #define SYSREG_OP1_AARCH32_AUTO 0 #define SYSREG_OP1_AARCH64_EL1 0 diff --git a/thermosphere/src/sysreg_traps.c b/thermosphere/src/sysreg_traps.c index f1fe0e64a..ac13d9acd 100644 --- a/thermosphere/src/sysreg_traps.c +++ b/thermosphere/src/sysreg_traps.c @@ -18,6 +18,7 @@ #include "sysreg.h" #include "arm.h" #include "debug_log.h" +#include "software_breakpoints.h" static void doSystemRegisterRwImpl(u64 *val, u32 iss) { @@ -67,13 +68,32 @@ void doSystemRegisterWrite(ExceptionStackFrame *frame, u32 iss, u32 reg) val = frame->x[reg]; + bool reevalSoftwareBreakpoints = false; + // Hooks go here: switch (iss) { + case ENCODE_SYSREG_ISS(TTBR0_EL1): + case ENCODE_SYSREG_ISS(TTBR1_EL1): + case ENCODE_SYSREG_ISS(TCR_EL1): + case ENCODE_SYSREG_ISS(SCTLR_EL1): + reevalSoftwareBreakpoints = true; + break; default: break; } + if (reevalSoftwareBreakpoints) { + revertAllSoftwareBreakpoints(); + } + doSystemRegisterRwImpl(&val, iss); + + if (reevalSoftwareBreakpoints) { + __dsb_sy(); + __isb(); + applyAllSoftwareBreakpoints(); + } + skipFaultingInstruction(frame, 4); } diff --git a/thermosphere/src/traps.c b/thermosphere/src/traps.c index 9b015cb64..b552b513c 100644 --- a/thermosphere/src/traps.c +++ b/thermosphere/src/traps.c @@ -34,6 +34,9 @@ void enableTraps(void) { u64 hcr = GET_SYSREG(hcr_el2); + // Trap memory-related sysreg writes (note: not supported by QEMU yet) + hcr |= HCR_TVM; + // Trap SMC instructions hcr |= HCR_TSC; diff --git a/thermosphere/src/utils.c b/thermosphere/src/utils.c index 86b7c8726..bdd2fa6d2 100644 --- a/thermosphere/src/utils.c +++ b/thermosphere/src/utils.c @@ -16,6 +16,8 @@ #include #include "utils.h" +#include "arm.h" +#include "spinlock.h" __attribute__((noinline)) bool overlaps(u64 as, u64 ae, u64 bs, u64 be) { @@ -25,3 +27,46 @@ __attribute__((noinline)) bool overlaps(u64 as, u64 ae, u64 bs, u64 be) return true; return false; } + +// TODO: put that elsewhere +bool readEl1Memory(void *dst, uintptr_t addr, size_t size) +{ + bool valid; + + u64 flags = maskIrq(); + uintptr_t pa = get_physical_address_el1_stage12(&valid, addr); + restoreInterruptFlags(flags); + + if (!valid) { + return false; + } + + flush_dcache_range((const void *)pa, (const void *)(pa + size)); + memcpy(dst, (const void *)pa, size); + + return true; +} + +bool writeEl1Memory(uintptr_t addr, const void *src, size_t size) +{ + bool valid; + + u64 flags = maskIrq(); + uintptr_t pa = get_physical_address_el1_stage12(&valid, addr); + restoreInterruptFlags(flags); + + if (!valid) { + return false; + } + + flush_dcache_range((const void *)pa, (const void *)(pa + size)); + memcpy((void *)pa, src, size); + flush_dcache_range((const void *)pa, (const void *)(pa + size)); + invalidate_icache_all(); + + __tlb_invalidate_el1_stage12(); + __dsb_sy(); + __isb(); + + return true; +} \ No newline at end of file diff --git a/thermosphere/src/utils.h b/thermosphere/src/utils.h index a102f4f81..9da9fe02a 100644 --- a/thermosphere/src/utils.h +++ b/thermosphere/src/utils.h @@ -45,16 +45,25 @@ static inline void __isb(void) __asm__ __volatile__ ("isb" ::: "memory"); } +static inline void __tlb_invalidate_el1_stage12(void) +{ + __asm__ __volatile__ ("tlbi alle1" ::: "memory"); +} + bool overlaps(u64 as, u64 ae, u64 bs, u64 be); -static inline uintptr_t get_physical_address_el1_stage12(const uintptr_t el1_vaddr) { +static inline uintptr_t get_physical_address_el1_stage12(bool *valid, const uintptr_t el1_vaddr) { // NOTE: interrupt must be disabled when calling this func uintptr_t PAR; - __asm__ __volatile__ ("at s12e1r, %0" :: "r"(el1_vaddr)); + __asm__ __volatile__ ("at s12e1r, %0" :: "r"(el1_vaddr)); // note: we don't care whether it's writable in EL1&0 translation regime __asm__ __volatile__ ("mrs %0, par_el1" : "=r"(PAR)); + *valid = (PAR & 1) == 0ull; return (PAR & 1) ? 0ull : (PAR & MASK2L(40, 12)) | ((uintptr_t)el1_vaddr & MASKL(12)); } +bool readEl1Memory(void *dst, uintptr_t addr, size_t size); +bool writeEl1Memory(uintptr_t addr, const void *src, size_t size); + static inline u32 read32le(const volatile void *dword, size_t offset) { return *(u32 *)((uintptr_t)dword + offset); } diff --git a/thermosphere/src/watchpoints.c b/thermosphere/src/watchpoints.c index 116202755..060b31f83 100644 --- a/thermosphere/src/watchpoints.c +++ b/thermosphere/src/watchpoints.c @@ -183,22 +183,22 @@ DebugRegisterPair *findSplitWatchpoint(u64 addr, size_t size, WatchpointLoadStor return ret; } -bool addWatchpoint(u64 addr, size_t size, WatchpointLoadStoreControl direction) +int addWatchpoint(u64 addr, size_t size, WatchpointLoadStoreControl direction) { if (size == 0) { - return false; + return -EINVAL; } recursiveSpinlockLock(&g_watchpointManager.lock); if (doFindSplitWatchpoint(addr, size, direction, true)) { recursiveSpinlockUnlock(&g_watchpointManager.lock); - return true; + return -EEXIST; } if (g_watchpointManager.numSplitWatchpoints == g_watchpointManager.maxSplitWatchpoints) { recursiveSpinlockUnlock(&g_watchpointManager.lock); - return false; + return -EBUSY; } size_t oldNumSplitWatchpoints = g_watchpointManager.numSplitWatchpoints; @@ -213,7 +213,7 @@ bool addWatchpoint(u64 addr, size_t size, WatchpointLoadStoreControl direction) if (!combineWatchpoint(wp)) { g_watchpointManager.numSplitWatchpoints = oldNumSplitWatchpoints; recursiveSpinlockUnlock(&g_watchpointManager.lock); - return false; + return -EBUSY; } } else if (size <= 9) { // Normal one or 2 up-to-9-bytes wp(s) (ie. never exceeeds two combined wp) @@ -221,7 +221,7 @@ bool addWatchpoint(u64 addr, size_t size, WatchpointLoadStoreControl direction) if (!checkNormalWatchpointRange(addr, size)) { g_watchpointManager.numSplitWatchpoints = oldNumSplitWatchpoints; recursiveSpinlockUnlock(&g_watchpointManager.lock); - return false; + return -EINVAL; } u64 addr2 = (addr + size) & ~7ull; @@ -249,18 +249,18 @@ bool addWatchpoint(u64 addr, size_t size, WatchpointLoadStoreControl direction) if (!combineWatchpoint(wp) || (size2 != 0 && !combineWatchpoint(wp2))) { g_watchpointManager.numSplitWatchpoints = oldNumSplitWatchpoints; recursiveSpinlockUnlock(&g_watchpointManager.lock); - return false; + return -EBUSY; } } else { recursiveSpinlockUnlock(&g_watchpointManager.lock); - return false; + return -EINVAL; } recursiveSpinlockUnlock(&g_watchpointManager.lock); // TODO: commit and broadcast - return true; + return 0; } static void combineAllCurrentWatchpoints(void) @@ -272,10 +272,10 @@ static void combineAllCurrentWatchpoints(void) } } -bool removeWatchpoint(u64 addr, size_t size, WatchpointLoadStoreControl direction) +int removeWatchpoint(u64 addr, size_t size, WatchpointLoadStoreControl direction) { if (size == 0) { - return false; + return -EINVAL; } recursiveSpinlockLock(&g_watchpointManager.lock); @@ -291,10 +291,30 @@ bool removeWatchpoint(u64 addr, size_t size, WatchpointLoadStoreControl directio combineAllCurrentWatchpoints(); } else { DEBUG("watchpoint not found 0x%016llx, size %llu, direction %d\n", addr, size, direction); + recursiveSpinlockUnlock(&g_watchpointManager.lock); + return -ENOENT; } recursiveSpinlockUnlock(&g_watchpointManager.lock); // TODO: commit and broadcast - return true; + return 0; } + +int removeAllWatchpoints(void) +{ + // Yeet it all + + recursiveSpinlockLock(&g_watchpointManager.lock); + + g_watchpointManager.allocationBitmap = BIT(g_watchpointManager.maxWatchpoints) - 1; + g_watchpointManager.numSplitWatchpoints = 0; + memset(g_watchpointManager.splitWatchpoints, 0, sizeof(g_watchpointManager.splitWatchpoints)); + memset(g_combinedWatchpoints, 0, sizeof(g_combinedWatchpoints)); + + // TODO: commit and broadcast + + recursiveSpinlockUnlock(&g_watchpointManager.lock); + + return 0; +} \ No newline at end of file diff --git a/thermosphere/src/watchpoints.h b/thermosphere/src/watchpoints.h index 899510a50..1ab79fafa 100644 --- a/thermosphere/src/watchpoints.h +++ b/thermosphere/src/watchpoints.h @@ -16,22 +16,26 @@ #pragma once +#define _REENT_ONLY +#include + #include "breakpoints_watchpoints_common.h" #include "spinlock.h" /// Structure to synchronize and keep track of watchpoints typedef struct WatchpointManager { - DebugRegisterPair splitWatchpoints[16 * 8]; RecursiveSpinlock lock; u32 numSplitWatchpoints; u32 maxWatchpoints; u32 maxSplitWatchpoints; u16 allocationBitmap; + DebugRegisterPair splitWatchpoints[16 * 8]; } WatchpointManager; extern WatchpointManager g_watchpointManager; void initWatchpoints(void); DebugRegisterPair *findSplitWatchpoint(u64 addr, size_t size, WatchpointLoadStoreControl direction, bool strict); -bool addWatchpoint(u64 addr, size_t size, WatchpointLoadStoreControl direction); -bool removeWatchpoint(u64 addr, size_t size, WatchpointLoadStoreControl direction); +int addWatchpoint(u64 addr, size_t size, WatchpointLoadStoreControl direction); +int removeWatchpoint(u64 addr, size_t size, WatchpointLoadStoreControl direction); +int removeAllWatchpoints(void);