thermosphere: add watchpoint + watchpoint merging code

thermosphere/Makefile

@@ -80,7 +80,7 @@ CXXFLAGS	:= $(CFLAGS) -fno-rtti -fno-exceptions -std=gnu++11
 ASFLAGS	:=	-g $(ARCH)
 LDFLAGS	=	-specs=$(TOPDIR)/linker.specs -nostartfiles -nostdlib -g $(ARCH) -Wl,-Map,$(notdir $*.map)
 
-LIBS	:=
+LIBS	:=	-lgcc
 
 #---------------------------------------------------------------------------------
 # list of directories containing libraries, this must be the top level containing

thermosphere/src/breakpoints.c

@@ -58,7 +58,7 @@ static void freeBreakpoint(u32 pos)
 
 static DebugRegisterPair *findBreakpoint(u64 addr)
 {
-    u16 bitmap = g_breakpointManager.allocationBitmap;
+    u16 bitmap = ~g_breakpointManager.allocationBitmap & 0xFFFF;
     while (bitmap != 0) {
         u32 pos = __builtin_ffs(bitmap);
         if (pos == 0) {
@@ -119,6 +119,7 @@ bool removeBreakpoint(u64 addr)
     DebugRegisterPair *regs = findBreakpoint(addr);
 
     if (findBreakpoint(addr) == NULL) {
+        recursiveSpinlockUnlock(&g_breakpointManager.lock);
         return false;
     }
 

thermosphere/src/main.c

@@ -42,12 +42,15 @@ void main(ExceptionStackFrame *frame)
     enableTraps();
     enableBreakpointsAndWatchpoints();
 
+    if (currentCoreCtx->isBootCore) {
+        uartInit(115200);
+        DEBUG("EL2: core %u reached main first!\n", currentCoreCtx->coreId);
+    }
+
     initBreakpoints();
     initWatchpoints();
 
     if (currentCoreCtx->isBootCore) {
-        uartInit(115200);
-        DEBUG("EL2: core %u reached main first!\n", currentCoreCtx->coreId);
         if (currentCoreCtx->kernelEntrypoint == 0) {
             if (semihosting_connection_supported()) {
                 loadKernelViaSemihosting();

thermosphere/src/watchpoints.c

@@ -14,13 +14,18 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <string.h>
 #include "watchpoints.h"
 #include "breakpoints_watchpoints_save_restore.h"
 #include "utils.h"
 #include "sysreg.h"
 #include "arm.h"
+#include "debug_log.h"
 
 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
 void initWatchpoints(void)
@@ -30,10 +35,266 @@ 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.allocationBitmap = 0xFFFF;
+        g_watchpointManager.maxSplitWatchpoints = 8 * (u32)num;
+        g_watchpointManager.allocationBitmap = BIT(num) - 1;
+    } else if (currentCoreCtx->isBootCore) {
+        combineAllCurrentWatchpoints();
     }
 
-    loadWatchpointRegs(g_watchpointManager.watchpoints, g_watchpointManager.maxWatchpoints);
+    loadWatchpointRegs(g_combinedWatchpoints, g_watchpointManager.maxWatchpoints);
 
     recursiveSpinlockUnlock(&g_watchpointManager.lock);
 }
+
+static DebugRegisterPair *findCombinedWatchpoint(u64 addr)
+{
+    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);
+    if (pos == 0) {
+        return NULL;
+    } else {
+        *bitmap &= ~BIT(pos - 1);
+        return &g_combinedWatchpoints[pos - 1];
+    }
+}
+
+// Precondition: not a MASK-based watchpoint
+static bool checkNormalWatchpointRange(u64 addr, size_t size)
+{
+    u16 bitmap = g_watchpointManager.allocationBitmap;
+    if (findCombinedWatchpoint(addr) == NULL) {
+        if (allocateCombinedWatchpoint(&bitmap) == NULL) {
+            return false;
+        }
+    }
+
+    // if it overlaps...
+    u64 addr2 = (addr + size) & ~7ull;
+
+    if (addr2 != (addr & ~7ull)) {
+        if (findCombinedWatchpoint(addr2) == NULL) {
+            return allocateCombinedWatchpoint(&bitmap) != NULL;
+        }
+    }
+
+    return true;
+}
+
+static inline bool isRangeMaskWatchpoint(u64 addr, size_t size)
+{
+    // size needs to be a power of 2, at least 8 (we'll only allow 16+ though), addr needs to be aligned.
+    bool ret = (size & (size - 1)) == 0 && size >= 16 && (addr & (size - 1)) == 0;
+    return ret;
+}
+
+static bool combineWatchpoint(const DebugRegisterPair *wp)
+{
+    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(u64 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) {
+            off = 0;
+            sz = size;
+        } 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);
+            }
+        }
+
+        u64 wpaddr = (wp->vr & ~7ull) + off;
+        if (strict) {
+            if (addr == wpaddr && direction == wp->cr.lsc && sz == size) {
+                return wp;
+            }
+        } else if (overlaps(wpaddr, sz, addr, size) && (direction & wp->cr.lsc) != 0) {
+            return wp;
+        }
+    }
+
+    return NULL;
+}
+
+DebugRegisterPair *findSplitWatchpoint(u64 addr, size_t size, WatchpointLoadStoreControl direction, bool strict)
+{
+    recursiveSpinlockLock(&g_watchpointManager.lock);
+    DebugRegisterPair *ret = doFindSplitWatchpoint(addr, size, direction, strict);
+    recursiveSpinlockUnlock(&g_watchpointManager.lock);
+    return ret;
+}
+
+bool addWatchpoint(u64 addr, size_t size, WatchpointLoadStoreControl direction)
+{
+    if (size == 0) {
+        return false;
+    }
+
+    recursiveSpinlockLock(&g_watchpointManager.lock);
+
+    if (doFindSplitWatchpoint(addr, size, direction, true)) {
+        recursiveSpinlockUnlock(&g_watchpointManager.lock);
+        return true;
+    }
+
+    if (g_watchpointManager.numSplitWatchpoints == g_watchpointManager.maxSplitWatchpoints) {
+        recursiveSpinlockUnlock(&g_watchpointManager.lock);
+        return false;
+    }
+
+    size_t oldNumSplitWatchpoints = g_watchpointManager.numSplitWatchpoints;
+    DebugRegisterPair *wp = &g_watchpointManager.splitWatchpoints[g_watchpointManager.numSplitWatchpoints++], *wp2 = NULL;
+
+    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 false;
+        }
+    } 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 false;
+        }
+
+        u64 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 false;
+        }
+    } else {
+        recursiveSpinlockUnlock(&g_watchpointManager.lock);
+        return false;
+    }
+
+    recursiveSpinlockUnlock(&g_watchpointManager.lock);
+
+    // TODO: commit and broadcast
+
+    return true;
+}
+
+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]);
+    }
+}
+
+bool removeWatchpoint(u64 addr, size_t size, WatchpointLoadStoreControl direction)
+{
+    if (size == 0) {
+        return false;
+    }
+
+    recursiveSpinlockLock(&g_watchpointManager.lock);
+
+    DebugRegisterPair *wp = doFindSplitWatchpoint(addr, size, direction, true);
+    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();
+    } else {
+        DEBUG("watchpoint not found 0x%016llx, size %llu, direction %d\n", addr, size, direction);
+    }
+    recursiveSpinlockUnlock(&g_watchpointManager.lock);
+
+    // TODO: commit and broadcast
+
+    return true;
+}

thermosphere/src/watchpoints.h

@@ -21,12 +21,17 @@
 
 /// Structure to synchronize and keep track of watchpoints
 typedef struct WatchpointManager {
-    DebugRegisterPair watchpoints[16];
+    DebugRegisterPair splitWatchpoints[16 * 8];
     RecursiveSpinlock lock;
+    u32 numSplitWatchpoints;
     u32 maxWatchpoints;
+    u32 maxSplitWatchpoints;
     u16 allocationBitmap;
 } 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);