thermosphere: rewrite watchpoints.c

This commit is contained in:
TuxSH 2020-02-03 19:03:11 +00:00
parent f2edaee5b6
commit 833d88bb90
3 changed files with 69 additions and 189 deletions

View file

@ -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. // Note: exception info doesn't provide us with the access size. Use 1.
bool wnr = (info->frame->esr_el2.iss & BIT(6)) != 0; bool wnr = (info->frame->esr_el2.iss & BIT(6)) != 0;
WatchpointLoadStoreControl dr = wnr ? WatchpointLoadStoreControl_Store : WatchpointLoadStoreControl_Load; 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) { if (!cr.enabled) {
DEBUG("GDB: oops, unhandled watchpoint for core id %u, far=%016lx\n", info->coreId, info->frame->far_el2); DEBUG("GDB: oops, unhandled watchpoint for core id %u, far=%016lx\n", info->coreId, info->frame->far_el2);
} else { } else {

View file

@ -22,10 +22,7 @@
#include "execute_function.h" #include "execute_function.h"
#include "debug_log.h" #include "debug_log.h"
WatchpointManager g_watchpointManager = {0}; static WatchpointManager g_watchpointManager = {0};
static TEMPORARY DebugRegisterPair g_combinedWatchpoints[16] = {0};
static void combineAllCurrentWatchpoints(void);
// Init the structure (already in BSS, so already zero-initialized) and load the registers // Init the structure (already in BSS, so already zero-initialized) and load the registers
void initWatchpoints(void) void initWatchpoints(void)
@ -35,13 +32,10 @@ void initWatchpoints(void)
if (currentCoreCtx->isBootCore && !currentCoreCtx->warmboot) { if (currentCoreCtx->isBootCore && !currentCoreCtx->warmboot) {
size_t num = ((GET_SYSREG(id_aa64dfr0_el1) >> 20) & 0xF) + 1; size_t num = ((GET_SYSREG(id_aa64dfr0_el1) >> 20) & 0xF) + 1;
g_watchpointManager.maxWatchpoints = (u32)num; g_watchpointManager.maxWatchpoints = (u32)num;
g_watchpointManager.maxSplitWatchpoints = 8 * (u32)num;
g_watchpointManager.allocationBitmap = BIT(num) - 1; 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); recursiveSpinlockUnlock(&g_watchpointManager.lock);
} }
@ -50,7 +44,7 @@ static void commitAndBroadcastWatchpointHandler(void *p)
{ {
(void)p; (void)p;
u64 flags = maskIrq(); u64 flags = maskIrq();
loadWatchpointRegs(g_combinedWatchpoints, g_watchpointManager.maxWatchpoints); loadWatchpointRegs(g_watchpointManager.watchpoints, g_watchpointManager.maxWatchpoints);
restoreInterruptFlags(flags); restoreInterruptFlags(flags);
} }
@ -60,56 +54,21 @@ static inline void commitAndBroadcastWatchpoints(void)
executeFunctionOnAllCores(commitAndBroadcastWatchpointHandler, NULL, true); executeFunctionOnAllCores(commitAndBroadcastWatchpointHandler, NULL, true);
} }
static DebugRegisterPair *findCombinedWatchpoint(uintptr_t addr) static DebugRegisterPair *allocateWatchpoint(void)
{ {
addr &= ~7ull; u32 pos = __builtin_ffs(g_watchpointManager.allocationBitmap);
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);
if (pos == 0) { if (pos == 0) {
return NULL; return NULL;
} else { } else {
*bitmap &= ~BIT(pos - 1); g_watchpointManager.allocationBitmap &= ~BIT(pos - 1);
return &g_combinedWatchpoints[pos - 1]; return &g_watchpointManager.watchpoints[pos - 1];
} }
} }
// Precondition: not a MASK-based watchpoint static void freeWatchpoint(u32 pos)
static bool checkNormalWatchpointRange(uintptr_t addr, size_t size)
{ {
u16 bitmap = g_watchpointManager.allocationBitmap; memset(&g_watchpointManager.watchpoints[pos], 0, sizeof(DebugRegisterPair));
if (findCombinedWatchpoint(addr) == NULL) { g_watchpointManager.allocationBitmap |= BIT(pos);
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;
} }
static inline bool isRangeMaskWatchpoint(uintptr_t addr, size_t size) 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; 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; u64 bitmap = ~g_watchpointManager.allocationBitmap & 0xFFFF;
FOREACH_BIT (tmp, i, bitmap) {
DebugRegisterPair *wp = &g_watchpointManager.watchpoints[i];
wpSlot = findCombinedWatchpoint(wp->vr & ~7ull); size_t off;
size_t sz;
// To simplify, don't allow combining for wps that use MASK (except if only perms needs to be combined) size_t nmask;
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;
if (wp->cr.mask != 0) { if (wp->cr.mask != 0) {
off = 0; off = 0;
sz = size; sz = MASK(wp->cr.mask);
nmask = ~sz;
} else { } else {
off = __builtin_ffs(wp->cr.bas) - 1; off = __builtin_ffs(wp->cr.bas) - 1;
sz = __builtin_popcount(wp->cr.bas); sz = __builtin_popcount(wp->cr.bas);
if (wp->vr & 1) { nmask = ~7ul;
DebugRegisterPair *wp2 = &g_watchpointManager.splitWatchpoints[i + 1];
sz += __builtin_popcount(wp2->cr.bas);
}
} }
u64 wpaddr = (wp->vr & ~7ull) + off; if (size != 0) {
if (strict) { // Strict watchpoint check
if (addr == wpaddr && direction == wp->cr.lsc && sz == size) { 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; return wp;
} }
} else if (overlaps(wpaddr, sz, addr, size) && (direction & wp->cr.lsc) != 0) {
return wp;
} }
} }
return NULL; 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); recursiveSpinlockLock(&g_watchpointManager.lock);
DebugRegisterPair *wp = doFindSplitWatchpoint(addr, size, direction, strict); DebugRegisterPair *wp = findWatchpoint(addr, 0, direction);
DebugControlRegister ret = { 0 }; DebugControlRegister ret = { 0 };
if (wp != NULL) { if (wp != NULL) {
ret = wp->cr; ret = wp->cr;
@ -201,79 +127,57 @@ DebugControlRegister retrieveSplitWatchpointConfig(uintptr_t addr, size_t size,
return ret; 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) { 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; return -EINVAL;
} }
recursiveSpinlockLock(&g_watchpointManager.lock); recursiveSpinlockLock(&g_watchpointManager.lock);
if (doFindSplitWatchpoint(addr, size, direction, true)) { if (g_watchpointManager.allocationBitmap == 0) {
recursiveSpinlockUnlock(&g_watchpointManager.lock);
return -EEXIST;
}
if (g_watchpointManager.numSplitWatchpoints == g_watchpointManager.maxSplitWatchpoints) {
recursiveSpinlockUnlock(&g_watchpointManager.lock); recursiveSpinlockUnlock(&g_watchpointManager.lock);
return -EBUSY; return -EBUSY;
} }
size_t oldNumSplitWatchpoints = g_watchpointManager.numSplitWatchpoints; if (findWatchpoint(addr, size, direction)) {
DebugRegisterPair *wp = &g_watchpointManager.splitWatchpoints[g_watchpointManager.numSplitWatchpoints++], *wp2 = NULL; recursiveSpinlockUnlock(&g_watchpointManager.lock);
return -EEXIST;
}
DebugRegisterPair *wp = allocateWatchpoint();
memset(wp, 0, sizeof(DebugRegisterPair)); memset(wp, 0, sizeof(DebugRegisterPair));
wp->cr.lsc = direction; wp->cr.lsc = direction;
if (isRangeMaskWatchpoint(addr, size)) { if (isRangeMaskWatchpoint(addr, size)) {
wp->vr = addr; wp->vr = addr;
wp->cr.bas = 0xFF; // TRM-mandated wp->cr.bas = 0xFF; // TRM-mandated
wp->cr.mask = (u32)__builtin_ffsl(size) - 1; 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 { } else {
recursiveSpinlockUnlock(&g_watchpointManager.lock); size_t off = addr & 7ull;
return -EINVAL; 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(); commitAndBroadcastWatchpoints();
recursiveSpinlockUnlock(&g_watchpointManager.lock); recursiveSpinlockUnlock(&g_watchpointManager.lock);
@ -281,40 +185,23 @@ int addWatchpoint(uintptr_t addr, size_t size, WatchpointLoadStoreControl direct
return 0; 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) int removeWatchpoint(uintptr_t addr, size_t size, WatchpointLoadStoreControl direction)
{ {
if (size == 0) { if (!checkWatchpointAddressAndSizeParams(addr, size)) {
return -EINVAL; return -EINVAL;
} }
recursiveSpinlockLock(&g_watchpointManager.lock); recursiveSpinlockLock(&g_watchpointManager.lock);
DebugRegisterPair *wp = doFindSplitWatchpoint(addr, size, direction, true); DebugRegisterPair *wp = findWatchpoint(addr, size, direction);
if (wp != NULL) { if (wp != NULL) {
size_t pos = wp - &g_watchpointManager.splitWatchpoints[0]; freeWatchpoint(wp - &g_watchpointManager.watchpoints[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();
} else { } else {
DEBUG("watchpoint not found 0x%016llx, size %llu, direction %d\n", addr, size, direction);
recursiveSpinlockUnlock(&g_watchpointManager.lock); recursiveSpinlockUnlock(&g_watchpointManager.lock);
return -ENOENT; return -ENOENT;
} }
commitAndBroadcastWatchpoints(); commitAndBroadcastWatchpoints();
recursiveSpinlockUnlock(&g_watchpointManager.lock); recursiveSpinlockUnlock(&g_watchpointManager.lock);
return 0; return 0;
@ -327,12 +214,9 @@ int removeAllWatchpoints(void)
recursiveSpinlockLock(&g_watchpointManager.lock); recursiveSpinlockLock(&g_watchpointManager.lock);
g_watchpointManager.allocationBitmap = BIT(g_watchpointManager.maxWatchpoints) - 1; g_watchpointManager.allocationBitmap = BIT(g_watchpointManager.maxWatchpoints) - 1;
g_watchpointManager.numSplitWatchpoints = 0; memset(g_watchpointManager.watchpoints, 0, sizeof(g_watchpointManager.watchpoints));
memset(g_watchpointManager.splitWatchpoints, 0, sizeof(g_watchpointManager.splitWatchpoints));
memset(g_combinedWatchpoints, 0, sizeof(g_combinedWatchpoints));
commitAndBroadcastWatchpoints(); commitAndBroadcastWatchpoints();
recursiveSpinlockUnlock(&g_watchpointManager.lock); recursiveSpinlockUnlock(&g_watchpointManager.lock);
return 0; return 0;

View file

@ -24,18 +24,14 @@
/// Structure to synchronize and keep track of watchpoints /// Structure to synchronize and keep track of watchpoints
typedef struct WatchpointManager { typedef struct WatchpointManager {
DebugRegisterPair watchpoints[MAX_WCR];
RecursiveSpinlock lock; RecursiveSpinlock lock;
u32 numSplitWatchpoints;
u32 maxWatchpoints; u32 maxWatchpoints;
u32 maxSplitWatchpoints;
u16 allocationBitmap; u16 allocationBitmap;
DebugRegisterPair splitWatchpoints[MAX_WCR * 8];
} WatchpointManager; } WatchpointManager;
extern WatchpointManager g_watchpointManager;
void initWatchpoints(void); 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 addWatchpoint(uintptr_t addr, size_t size, WatchpointLoadStoreControl direction);
int removeWatchpoint(uintptr_t addr, size_t size, WatchpointLoadStoreControl direction); int removeWatchpoint(uintptr_t addr, size_t size, WatchpointLoadStoreControl direction);
int removeAllWatchpoints(void); int removeAllWatchpoints(void);