diff --git a/thermosphere/src/breakpoints.c b/thermosphere/src/breakpoints.c
index 70ab2cbaa..adbdffe85 100644
--- a/thermosphere/src/breakpoints.c
+++ b/thermosphere/src/breakpoints.c
@@ -30,7 +30,8 @@ void initBreakpoints(void)
     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;
+        g_breakpointManager.freeBitmap = BIT(num) - 1;
+        g_breakpointManager.usedBitmap = 0;
     }
 
     loadBreakpointRegs(g_breakpointManager.breakpoints, g_breakpointManager.maxBreakpoints);
@@ -54,11 +55,12 @@ static inline void commitAndBroadcastBreakpoints(void)
 
 static DebugRegisterPair *allocateBreakpoint(void)
 {
-    u32 pos = __builtin_ffs(g_breakpointManager.allocationBitmap);
+    u32 pos = __builtin_ffs(g_breakpointManager.freeBitmap);
     if (pos == 0) {
         return NULL;
     } else {
-        g_breakpointManager.allocationBitmap &= ~BIT(pos - 1);
+        g_breakpointManager.freeBitmap &= ~BIT(pos - 1);
+        g_breakpointManager.usedBitmap |= BIT(pos - 1);
         return &g_breakpointManager.breakpoints[pos - 1];
     }
 }
@@ -66,21 +68,15 @@ static DebugRegisterPair *allocateBreakpoint(void)
 static void freeBreakpoint(u32 pos)
 {
     memset(&g_breakpointManager.breakpoints[pos], 0, sizeof(DebugRegisterPair));
-    g_breakpointManager.allocationBitmap |= BIT(pos);
+    g_breakpointManager.freeBitmap |= BIT(pos);
+    g_breakpointManager.usedBitmap &= ~BIT(pos);
 }
 
 static DebugRegisterPair *findBreakpoint(uintptr_t addr)
 {
-    u16 bitmap = ~g_breakpointManager.allocationBitmap & 0xFFFF;
-    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];
-            }
+    FOREACH_BIT (tmp, i, g_breakpointManager.usedBitmap) {
+        if (g_breakpointManager.breakpoints[i - 1].vr == addr) {
+            return &g_breakpointManager.breakpoints[i - 1];
         }
     }
 
@@ -100,6 +96,11 @@ int addBreakpoint(uintptr_t addr)
         return -EINVAL;
     }
 
+    // Oops
+    if (g_breakpointManager.freeBitmap == 0) {
+        return -EBUSY;
+    }
+
     // Breakpoint already added
     if (findBreakpoint(addr) != NULL) {
         recursiveSpinlockUnlock(&g_breakpointManager.lock);
@@ -149,7 +150,8 @@ int removeBreakpoint(uintptr_t addr)
 int removeAllBreakpoints(void)
 {
     recursiveSpinlockLock(&g_breakpointManager.lock);
-    g_breakpointManager.allocationBitmap = BIT(g_breakpointManager.maxBreakpoints) - 1;
+    g_breakpointManager.freeBitmap |= g_breakpointManager.usedBitmap;
+    g_breakpointManager.usedBitmap = 0;
     memset(g_breakpointManager.breakpoints, 0, sizeof(g_breakpointManager.breakpoints));
 
     commitAndBroadcastBreakpoints();
diff --git a/thermosphere/src/breakpoints.h b/thermosphere/src/breakpoints.h
index 46a412feb..30876c77c 100644
--- a/thermosphere/src/breakpoints.h
+++ b/thermosphere/src/breakpoints.h
@@ -27,7 +27,8 @@ typedef struct BreakpointManager {
     DebugRegisterPair breakpoints[MAX_BCR];
     RecursiveSpinlock lock;
     u32 maxBreakpoints;
-    u16 allocationBitmap;
+    u16 freeBitmap;
+    u16 usedBitmap;
 } BreakpointManager;
 
 extern BreakpointManager g_breakpointManager;
diff --git a/thermosphere/src/watchpoints.c b/thermosphere/src/watchpoints.c
index a10d8ede0..6030f244a 100644
--- a/thermosphere/src/watchpoints.c
+++ b/thermosphere/src/watchpoints.c
@@ -32,7 +32,8 @@ 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 = BIT(num) - 1;
+        g_watchpointManager.freeBitmap = BIT(num) - 1;
+        g_watchpointManager.usedBitmap = 0;
     }
 
     loadWatchpointRegs(g_watchpointManager.watchpoints, g_watchpointManager.maxWatchpoints);
@@ -56,11 +57,12 @@ static inline void commitAndBroadcastWatchpoints(void)
 
 static DebugRegisterPair *allocateWatchpoint(void)
 {
-    u32 pos = __builtin_ffs(g_watchpointManager.allocationBitmap);
+    u32 pos = __builtin_ffs(g_watchpointManager.freeBitmap);
     if (pos == 0) {
         return NULL;
     } else {
-        g_watchpointManager.allocationBitmap &= ~BIT(pos - 1);
+        g_watchpointManager.freeBitmap &= ~BIT(pos - 1);
+        g_watchpointManager.usedBitmap |= BIT(pos - 1);
         return &g_watchpointManager.watchpoints[pos - 1];
     }
 }
@@ -68,7 +70,8 @@ static DebugRegisterPair *allocateWatchpoint(void)
 static void freeWatchpoint(u32 pos)
 {
     memset(&g_watchpointManager.watchpoints[pos], 0, sizeof(DebugRegisterPair));
-    g_watchpointManager.allocationBitmap |= BIT(pos);
+    g_watchpointManager.freeBitmap |= BIT(pos);
+    g_watchpointManager.usedBitmap &= ~BIT(pos);
 }
 
 static inline bool isRangeMaskWatchpoint(uintptr_t addr, size_t size)
@@ -81,8 +84,7 @@ static inline bool isRangeMaskWatchpoint(uintptr_t addr, size_t size)
 // Size = 0 means nonstrict
 static DebugRegisterPair *findWatchpoint(uintptr_t addr, size_t size, WatchpointLoadStoreControl direction)
 {
-    u64 bitmap = ~g_watchpointManager.allocationBitmap & 0xFFFF;
-    FOREACH_BIT (tmp, i, bitmap) {
+    FOREACH_BIT (tmp, i, g_watchpointManager.usedBitmap) {
         DebugRegisterPair *wp = &g_watchpointManager.watchpoints[i];
 
         size_t off;
@@ -146,7 +148,7 @@ int addWatchpoint(uintptr_t addr, size_t size, WatchpointLoadStoreControl direct
 
     recursiveSpinlockLock(&g_watchpointManager.lock);
 
-    if (g_watchpointManager.allocationBitmap == 0) {
+    if (g_watchpointManager.freeBitmap == 0) {
         recursiveSpinlockUnlock(&g_watchpointManager.lock);
         return -EBUSY;
     }
@@ -166,9 +168,8 @@ int addWatchpoint(uintptr_t addr, size_t size, WatchpointLoadStoreControl direct
         wp->cr.mask = (u32)__builtin_ffsl(size) - 1;
     } else {
         size_t off = addr & 7ull;
-        size_t sz = 8 - off;
         wp->vr = addr & ~7ul;
-        wp->cr.bas = MASK2(off + sz, off);
+        wp->cr.bas = MASK2(off + size, off);
     }
 
     wp->cr.linked = false;
@@ -213,7 +214,8 @@ int removeAllWatchpoints(void)
 
     recursiveSpinlockLock(&g_watchpointManager.lock);
 
-    g_watchpointManager.allocationBitmap = BIT(g_watchpointManager.maxWatchpoints) - 1;
+    g_watchpointManager.freeBitmap |= g_watchpointManager.usedBitmap;
+    g_watchpointManager.usedBitmap = 0;
     memset(g_watchpointManager.watchpoints, 0, sizeof(g_watchpointManager.watchpoints));
 
     commitAndBroadcastWatchpoints();
diff --git a/thermosphere/src/watchpoints.h b/thermosphere/src/watchpoints.h
index c6f4615fe..2d29cd3ae 100644
--- a/thermosphere/src/watchpoints.h
+++ b/thermosphere/src/watchpoints.h
@@ -27,7 +27,8 @@ typedef struct WatchpointManager {
     DebugRegisterPair watchpoints[MAX_WCR];
     RecursiveSpinlock lock;
     u32 maxWatchpoints;
-    u16 allocationBitmap;
+    u16 freeBitmap;
+    u16 usedBitmap;
 } WatchpointManager;
 
 void initWatchpoints(void);