diff --git a/thermosphere/src/gdb/debug.c b/thermosphere/src/gdb/debug.c index 4d54e25e9..3ec858c2e 100644 --- a/thermosphere/src/gdb/debug.c +++ b/thermosphere/src/gdb/debug.c @@ -162,7 +162,7 @@ int GDB_SendStopReply(GDBContext *ctx, const DebugEventInfo *info, bool asNotifi // Note: exception info doesn't provide us with the access size. Use 1. bool wnr = (info->frame->esr_el2.iss & BIT(6)) != 0; WatchpointLoadStoreControl dr = wnr ? WatchpointLoadStoreControl_Store : WatchpointLoadStoreControl_Load; - DebugControlRegister cr = retrieveSplitWatchpointConfig(info->frame->far_el2, 1, dr, false); + DebugControlRegister cr = retrieveWatchpointConfig(info->frame->far_el2, dr); if (!cr.enabled) { DEBUG("GDB: oops, unhandled watchpoint for core id %u, far=%016lx\n", info->coreId, info->frame->far_el2); } else { diff --git a/thermosphere/src/watchpoints.c b/thermosphere/src/watchpoints.c index 6d7a93520..a10d8ede0 100644 --- a/thermosphere/src/watchpoints.c +++ b/thermosphere/src/watchpoints.c @@ -22,10 +22,7 @@ #include "execute_function.h" #include "debug_log.h" -WatchpointManager g_watchpointManager = {0}; -static TEMPORARY DebugRegisterPair g_combinedWatchpoints[16] = {0}; - -static void combineAllCurrentWatchpoints(void); +static WatchpointManager g_watchpointManager = {0}; // Init the structure (already in BSS, so already zero-initialized) and load the registers void initWatchpoints(void) @@ -35,13 +32,10 @@ void initWatchpoints(void) if (currentCoreCtx->isBootCore && !currentCoreCtx->warmboot) { size_t num = ((GET_SYSREG(id_aa64dfr0_el1) >> 20) & 0xF) + 1; g_watchpointManager.maxWatchpoints = (u32)num; - g_watchpointManager.maxSplitWatchpoints = 8 * (u32)num; g_watchpointManager.allocationBitmap = BIT(num) - 1; - } else if (currentCoreCtx->isBootCore) { - combineAllCurrentWatchpoints(); } - loadWatchpointRegs(g_combinedWatchpoints, g_watchpointManager.maxWatchpoints); + loadWatchpointRegs(g_watchpointManager.watchpoints, g_watchpointManager.maxWatchpoints); recursiveSpinlockUnlock(&g_watchpointManager.lock); } @@ -50,7 +44,7 @@ static void commitAndBroadcastWatchpointHandler(void *p) { (void)p; u64 flags = maskIrq(); - loadWatchpointRegs(g_combinedWatchpoints, g_watchpointManager.maxWatchpoints); + loadWatchpointRegs(g_watchpointManager.watchpoints, g_watchpointManager.maxWatchpoints); restoreInterruptFlags(flags); } @@ -60,56 +54,21 @@ static inline void commitAndBroadcastWatchpoints(void) executeFunctionOnAllCores(commitAndBroadcastWatchpointHandler, NULL, true); } -static DebugRegisterPair *findCombinedWatchpoint(uintptr_t addr) +static DebugRegisterPair *allocateWatchpoint(void) { - addr &= ~7ull; - u16 bitmap = ~g_watchpointManager.allocationBitmap & 0xFFFF; - while (bitmap != 0) { - u32 pos = __builtin_ffs(bitmap); - if (pos == 0) { - return NULL; - } else { - bitmap &= ~BIT(pos - 1); - if (g_combinedWatchpoints[pos - 1].vr == addr) { - return &g_combinedWatchpoints[pos - 1]; - } - } - } - - return NULL; -} - -static DebugRegisterPair *allocateCombinedWatchpoint(u16 *bitmap) -{ - u32 pos = __builtin_ffs(*bitmap); + u32 pos = __builtin_ffs(g_watchpointManager.allocationBitmap); if (pos == 0) { return NULL; } else { - *bitmap &= ~BIT(pos - 1); - return &g_combinedWatchpoints[pos - 1]; + g_watchpointManager.allocationBitmap &= ~BIT(pos - 1); + return &g_watchpointManager.watchpoints[pos - 1]; } } -// Precondition: not a MASK-based watchpoint -static bool checkNormalWatchpointRange(uintptr_t addr, size_t size) +static void freeWatchpoint(u32 pos) { - u16 bitmap = g_watchpointManager.allocationBitmap; - if (findCombinedWatchpoint(addr) == NULL) { - if (allocateCombinedWatchpoint(&bitmap) == NULL) { - return false; - } - } - - // if it overlaps... - uintptr_t addr2 = (addr + size) & ~7ull; - - if (addr2 != (addr & ~7ull)) { - if (findCombinedWatchpoint(addr2) == NULL) { - return allocateCombinedWatchpoint(&bitmap) != NULL; - } - } - - return true; + memset(&g_watchpointManager.watchpoints[pos], 0, sizeof(DebugRegisterPair)); + g_watchpointManager.allocationBitmap |= BIT(pos); } static inline bool isRangeMaskWatchpoint(uintptr_t addr, size_t size) @@ -119,80 +78,47 @@ static inline bool isRangeMaskWatchpoint(uintptr_t addr, size_t size) return ret; } -static bool combineWatchpoint(const DebugRegisterPair *wp) +// Size = 0 means nonstrict +static DebugRegisterPair *findWatchpoint(uintptr_t addr, size_t size, WatchpointLoadStoreControl direction) { - DebugRegisterPair *wpSlot = NULL; - - wpSlot = findCombinedWatchpoint(wp->vr & ~7ull); - - // To simplify, don't allow combining for wps that use MASK (except if only perms needs to be combined) - if (wp->cr.mask != 0 && wpSlot != NULL && (wp->cr.mask != wpSlot->cr.mask || wp->vr != wpSlot->vr)) { - wpSlot = NULL; - } - - if (wpSlot == NULL) { - wpSlot = allocateCombinedWatchpoint(&g_watchpointManager.allocationBitmap); - memset(wpSlot, 0, sizeof(DebugRegisterPair)); - wpSlot->vr = wp->vr & ~7ull; - } - - if (wpSlot == NULL) { - return false; - } - - wpSlot->cr.hmc = DebugHmc_LowerEl; - wpSlot->cr.ssc = DebugSsc_NonSecure; - wpSlot->cr.pmc = DebugPmc_El1And0; - wpSlot->cr.enabled = true; - - // Merge 8-byte selection mask and access permissions (possibly broadening them) - wpSlot->cr.bas |= wp->cr.bas; - wpSlot->cr.lsc |= wp->cr.lsc; - - return true; -} - -static DebugRegisterPair *doFindSplitWatchpoint(uintptr_t addr, size_t size, WatchpointLoadStoreControl direction, bool strict) -{ - // Note: we will use RES0 bit0_1 of wr in case of overlapping - for (u32 i = 0; i < g_watchpointManager.numSplitWatchpoints; i++) { - DebugRegisterPair *wp = &g_watchpointManager.splitWatchpoints[i]; - if (wp->vr & 2) { - continue; - } - - size_t off = 0; - size_t sz = 0; + u64 bitmap = ~g_watchpointManager.allocationBitmap & 0xFFFF; + FOREACH_BIT (tmp, i, bitmap) { + DebugRegisterPair *wp = &g_watchpointManager.watchpoints[i]; + size_t off; + size_t sz; + size_t nmask; + if (wp->cr.mask != 0) { off = 0; - sz = size; + sz = MASK(wp->cr.mask); + nmask = ~sz; } else { off = __builtin_ffs(wp->cr.bas) - 1; sz = __builtin_popcount(wp->cr.bas); - if (wp->vr & 1) { - DebugRegisterPair *wp2 = &g_watchpointManager.splitWatchpoints[i + 1]; - sz += __builtin_popcount(wp2->cr.bas); - } + nmask = ~7ul; } - u64 wpaddr = (wp->vr & ~7ull) + off; - if (strict) { - if (addr == wpaddr && direction == wp->cr.lsc && sz == size) { + if (size != 0) { + // Strict watchpoint check + if (addr == wp->vr + off && direction == wp->cr.lsc && sz == size) { + return wp; + } + } else { + // Return first wp that could have triggered the exception + if ((addr & nmask) == wp->vr && (direction & wp->cr.lsc) != 0) { return wp; } - } else if (overlaps(wpaddr, sz, addr, size) && (direction & wp->cr.lsc) != 0) { - return wp; } } return NULL; } -DebugControlRegister retrieveSplitWatchpointConfig(uintptr_t addr, size_t size, WatchpointLoadStoreControl direction, bool strict) +DebugControlRegister retrieveWatchpointConfig(uintptr_t addr, WatchpointLoadStoreControl direction) { recursiveSpinlockLock(&g_watchpointManager.lock); - DebugRegisterPair *wp = doFindSplitWatchpoint(addr, size, direction, strict); + DebugRegisterPair *wp = findWatchpoint(addr, 0, direction); DebugControlRegister ret = { 0 }; if (wp != NULL) { ret = wp->cr; @@ -201,79 +127,57 @@ DebugControlRegister retrieveSplitWatchpointConfig(uintptr_t addr, size_t size, return ret; } -int addWatchpoint(uintptr_t addr, size_t size, WatchpointLoadStoreControl direction) +static inline bool checkWatchpointAddressAndSizeParams(uintptr_t addr, size_t size) { if (size == 0) { + return false; + } else if (size > 8) { + return isRangeMaskWatchpoint(addr, size); + } else { + return ((addr + size) & ~7ul) == (addr & ~7ul); + } +} + +int addWatchpoint(uintptr_t addr, size_t size, WatchpointLoadStoreControl direction) +{ + if (!checkWatchpointAddressAndSizeParams(addr, size)) { return -EINVAL; } recursiveSpinlockLock(&g_watchpointManager.lock); - if (doFindSplitWatchpoint(addr, size, direction, true)) { - recursiveSpinlockUnlock(&g_watchpointManager.lock); - return -EEXIST; - } - - if (g_watchpointManager.numSplitWatchpoints == g_watchpointManager.maxSplitWatchpoints) { + if (g_watchpointManager.allocationBitmap == 0) { recursiveSpinlockUnlock(&g_watchpointManager.lock); return -EBUSY; } - size_t oldNumSplitWatchpoints = g_watchpointManager.numSplitWatchpoints; - DebugRegisterPair *wp = &g_watchpointManager.splitWatchpoints[g_watchpointManager.numSplitWatchpoints++], *wp2 = NULL; + if (findWatchpoint(addr, size, direction)) { + recursiveSpinlockUnlock(&g_watchpointManager.lock); + return -EEXIST; + } + DebugRegisterPair *wp = allocateWatchpoint(); memset(wp, 0, sizeof(DebugRegisterPair)); + wp->cr.lsc = direction; if (isRangeMaskWatchpoint(addr, size)) { wp->vr = addr; wp->cr.bas = 0xFF; // TRM-mandated wp->cr.mask = (u32)__builtin_ffsl(size) - 1; - if (!combineWatchpoint(wp)) { - g_watchpointManager.numSplitWatchpoints = oldNumSplitWatchpoints; - recursiveSpinlockUnlock(&g_watchpointManager.lock); - return -EBUSY; - } - } else if (size <= 9) { - // Normal one or 2 up-to-9-bytes wp(s) (ie. never exceeeds two combined wp) - // Note: we will use RES0 bit0_1 of wr in case of overlapping - if (!checkNormalWatchpointRange(addr, size)) { - g_watchpointManager.numSplitWatchpoints = oldNumSplitWatchpoints; - recursiveSpinlockUnlock(&g_watchpointManager.lock); - return -EINVAL; - } - - uintptr_t addr2 = (addr + size) & ~7ull; - size_t off1 = addr & 7ull; - size_t size1 = (addr != addr2) ? 8 - off1 : size; - size_t size2 = size - size1; - wp->vr = addr & ~7ull; - wp->cr.bas = MASK2(off1 + size1, off1); - - if (size2 != 0) { - if (g_watchpointManager.numSplitWatchpoints == g_watchpointManager.maxSplitWatchpoints) { - g_watchpointManager.numSplitWatchpoints = oldNumSplitWatchpoints; - recursiveSpinlockUnlock(&g_watchpointManager.lock); - return false; - } - wp2 = &g_watchpointManager.splitWatchpoints[g_watchpointManager.numSplitWatchpoints++]; - wp2->cr.lsc = direction; - wp2->cr.bas = MASK2(size2, 0); - - // Note: we will use RES0 bit0_1 of wr in case of overlapping - wp->vr |= 1; - wp2->vr = addr2 | 2; - } - - if (!combineWatchpoint(wp) || (size2 != 0 && !combineWatchpoint(wp2))) { - g_watchpointManager.numSplitWatchpoints = oldNumSplitWatchpoints; - recursiveSpinlockUnlock(&g_watchpointManager.lock); - return -EBUSY; - } } else { - recursiveSpinlockUnlock(&g_watchpointManager.lock); - return -EINVAL; + size_t off = addr & 7ull; + size_t sz = 8 - off; + wp->vr = addr & ~7ul; + wp->cr.bas = MASK2(off + sz, off); } + wp->cr.linked = false; + + wp->cr.hmc = DebugHmc_LowerEl; + wp->cr.ssc = DebugSsc_NonSecure; + wp->cr.pmc = DebugPmc_El1And0; + wp->cr.enabled = true; + commitAndBroadcastWatchpoints(); recursiveSpinlockUnlock(&g_watchpointManager.lock); @@ -281,40 +185,23 @@ int addWatchpoint(uintptr_t addr, size_t size, WatchpointLoadStoreControl direct return 0; } -static void combineAllCurrentWatchpoints(void) -{ - memset(g_combinedWatchpoints, 0, sizeof(g_combinedWatchpoints)); - g_watchpointManager.allocationBitmap = BIT(g_watchpointManager.maxWatchpoints) - 1; - for (u32 i = 0; i < g_watchpointManager.numSplitWatchpoints; i++) { - combineWatchpoint(&g_watchpointManager.splitWatchpoints[i]); - } -} - int removeWatchpoint(uintptr_t addr, size_t size, WatchpointLoadStoreControl direction) { - if (size == 0) { + if (!checkWatchpointAddressAndSizeParams(addr, size)) { return -EINVAL; } recursiveSpinlockLock(&g_watchpointManager.lock); - DebugRegisterPair *wp = doFindSplitWatchpoint(addr, size, direction, true); + DebugRegisterPair *wp = findWatchpoint(addr, size, direction); if (wp != NULL) { - size_t pos = wp - &g_watchpointManager.splitWatchpoints[0]; - size_t num = (wp->vr & 1) ? 2 : 1; - for (size_t i = pos + num; i < g_watchpointManager.numSplitWatchpoints; i ++) { - g_watchpointManager.splitWatchpoints[i - num] = g_watchpointManager.splitWatchpoints[i]; - } - g_watchpointManager.numSplitWatchpoints -= num; - combineAllCurrentWatchpoints(); + freeWatchpoint(wp - &g_watchpointManager.watchpoints[0]); } else { - DEBUG("watchpoint not found 0x%016llx, size %llu, direction %d\n", addr, size, direction); recursiveSpinlockUnlock(&g_watchpointManager.lock); return -ENOENT; } commitAndBroadcastWatchpoints(); - recursiveSpinlockUnlock(&g_watchpointManager.lock); return 0; @@ -327,12 +214,9 @@ int removeAllWatchpoints(void) 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)); + memset(g_watchpointManager.watchpoints, 0, sizeof(g_watchpointManager.watchpoints)); commitAndBroadcastWatchpoints(); - recursiveSpinlockUnlock(&g_watchpointManager.lock); return 0; diff --git a/thermosphere/src/watchpoints.h b/thermosphere/src/watchpoints.h index 41eaf02f6..c6f4615fe 100644 --- a/thermosphere/src/watchpoints.h +++ b/thermosphere/src/watchpoints.h @@ -24,18 +24,14 @@ /// Structure to synchronize and keep track of watchpoints typedef struct WatchpointManager { + DebugRegisterPair watchpoints[MAX_WCR]; RecursiveSpinlock lock; - u32 numSplitWatchpoints; u32 maxWatchpoints; - u32 maxSplitWatchpoints; u16 allocationBitmap; - DebugRegisterPair splitWatchpoints[MAX_WCR * 8]; } WatchpointManager; -extern WatchpointManager g_watchpointManager; - void initWatchpoints(void); -DebugControlRegister retrieveSplitWatchpointConfig(uintptr_t addr, size_t size, WatchpointLoadStoreControl direction, bool strict); +DebugControlRegister retrieveWatchpointConfig(uintptr_t addr, WatchpointLoadStoreControl direction); int addWatchpoint(uintptr_t addr, size_t size, WatchpointLoadStoreControl direction); int removeWatchpoint(uintptr_t addr, size_t size, WatchpointLoadStoreControl direction); int removeAllWatchpoints(void);