thermosphere: unfuck sw breakpoint logic

thermosphere/src/core_ctx.h

@@ -41,19 +41,20 @@ typedef struct CoreCtx {
     ExecutedFunction executedFunction;          // @0x40
     void *executedFunctionArgs;                 // @0x48
     Barrier executedFunctionBarrier;            // @0x50
-    bool executedFunctionSync;                  // @0x54
+    u32 executedFunctionSrcCore;                // @0x54
+    bool executedFunctionSync;                  // @0x58. Receiver fills it
 
     // Debug features
-    bool wasPaused;                             // @0x55
+    bool wasPaused;                             // @0x59
 
     // Cache stuff
-    u32 setWayCounter;                          // @0x58
+    u32 setWayCounter;                          // @0x5C
 } CoreCtx;
 
 static_assert(offsetof(CoreCtx, warmboot) == 0x2E, "Wrong definition for CoreCtx");
 static_assert(offsetof(CoreCtx, emulPtimerCval) == 0x38, "Wrong definition for CoreCtx");
-static_assert(offsetof(CoreCtx, executedFunctionSync) == 0x54, "Wrong definition for CoreCtx");
-static_assert(offsetof(CoreCtx, setWayCounter) == 0x58, "Wrong definition for CoreCtx");
+static_assert(offsetof(CoreCtx, executedFunctionSync) == 0x58, "Wrong definition for CoreCtx");
+static_assert(offsetof(CoreCtx, setWayCounter) == 0x5C, "Wrong definition for CoreCtx");
 
 extern CoreCtx g_coreCtxs[4];
 register CoreCtx *currentCoreCtx asm("x18");

thermosphere/src/execute_function.c

@@ -42,6 +42,7 @@ void executeFunctionOnAllCoresButSelf(ExecutedFunction fun, void *args, bool syn
 void executeFunctionInterruptHandler(u32 srcCore)
 {
     CoreCtx *ctx = &g_coreCtxs[srcCore];
+    currentCoreCtx->executedFunctionSrcCore = srcCore;
     ctx->executedFunction(ctx->executedFunctionArgs);
     if (ctx->executedFunctionSync) {
         barrierWait(&ctx->executedFunctionBarrier);

thermosphere/src/software_breakpoints.c

@@ -17,6 +17,8 @@
 #include <string.h>
 #include "software_breakpoints.h"
 #include "utils.h"
+#include "guest_memory.h"
+#include "core_ctx.h"
 
 SoftwareBreakpointManager g_softwareBreakpointManager = {0};
 
@@ -56,61 +58,74 @@ static size_t findClosestSoftwareBreakpointSlot(u64 address)
 static inline 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;
+    size_t sz = guestReadWriteMemory(bp->address, 4, &bp->savedInstruction, &brkInst);
+    bp->applied = sz == 4;
+    atomic_store(&bp->triedToApplyOrRevert, true);
+    return sz == 4;
 }
 
 static void applySoftwareBreakpointHandler(void *p)
 {
-    u64 flags = maskIrq();
-    __dmb();
-    doApplySoftwareBreakpoint(*(size_t *)p);
-    restoreInterruptFlags(flags);
+    size_t id = *(size_t *)p;
+    if (currentCoreCtx->coreId == currentCoreCtx->executedFunctionSrcCore) {
+        doApplySoftwareBreakpoint(id);
+        __sev();
+    } else {
+        SoftwareBreakpoint *bp = &g_softwareBreakpointManager.breakpoints[id];
+        while (!atomic_load(&bp->triedToApplyOrRevert)) {
+            __wfe();
+        }
+    }
 }
 
-static void applySoftwareBreakpoint(size_t id)
+static bool applySoftwareBreakpoint(size_t id)
 {
-    __dmb();
+    if (g_softwareBreakpointManager.breakpoints[id].applied) {
+        return true;
+    }
+
+    atomic_store(&g_softwareBreakpointManager.breakpoints[id].triedToApplyOrRevert, false);
     executeFunctionOnAllCores(applySoftwareBreakpointHandler, &id, true);
+    atomic_signal_fence(memory_order_seq_cst);
+    return g_softwareBreakpointManager.breakpoints[id].applied;
 }
 
 static inline 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;
+    size_t sz = guestWriteMemory(bp->address, 4, &bp->savedInstruction);
+    bp->applied = sz != 4;
+    atomic_store(&bp->triedToApplyOrRevert, true);
+    return sz == 4;
 }
 
 static void revertSoftwareBreakpointHandler(void *p)
 {
-    u64 flags = maskIrq();
-    __dmb();
-    doRevertSoftwareBreakpoint(*(size_t *)p);
-    restoreInterruptFlags(flags);
+    size_t id = *(size_t *)p;
+    if (currentCoreCtx->coreId == currentCoreCtx->executedFunctionSrcCore) {
+        doRevertSoftwareBreakpoint(id);
+        __sev();
+    } else {
+        SoftwareBreakpoint *bp = &g_softwareBreakpointManager.breakpoints[id];
+        while (!atomic_load(&bp->triedToApplyOrRevert)) {
+            __wfe();
+        }
+    }
 }
 
-static void revertSoftwareBreakpoint(size_t id)
+static bool revertSoftwareBreakpoint(size_t id)
 {
-    __dmb();
+    if (!g_softwareBreakpointManager.breakpoints[id].applied) {
+        return true;
+    }
+
+    atomic_store(&g_softwareBreakpointManager.breakpoints[id].triedToApplyOrRevert, false);
     executeFunctionOnAllCores(revertSoftwareBreakpointHandler, &id, true);
+    atomic_signal_fence(memory_order_seq_cst);
+    return !g_softwareBreakpointManager.breakpoints[id].applied;
 }
 
 bool applyAllSoftwareBreakpoints(void)
@@ -167,11 +182,10 @@ int addSoftwareBreakpoint(u64 addr, bool persistent)
     bp->applied = false;
     bp->uid = 0x2000 + g_softwareBreakpointManager.bpUniqueCounter++;
 
-    applySoftwareBreakpoint(id);
-    // Note: no way to handle breakpoint failing to apply on 1+ core but not all, we need to assume operation succeeds
+    int rc = applySoftwareBreakpoint(id) ? 0 : -EFAULT;
     recursiveSpinlockUnlock(&g_softwareBreakpointManager.lock);
 
-    return 0;
+    return rc;
 }
 
 int removeSoftwareBreakpoint(u64 addr, bool keepPersistent)
@@ -183,6 +197,7 @@ int removeSoftwareBreakpoint(u64 addr, bool keepPersistent)
     recursiveSpinlockLock(&g_softwareBreakpointManager.lock);
 
     size_t id = findClosestSoftwareBreakpointSlot(addr);
+    bool ok = true;
 
     if(id == g_softwareBreakpointManager.numBreakpoints || g_softwareBreakpointManager.breakpoints[id].address != addr) {
         recursiveSpinlockUnlock(&g_softwareBreakpointManager.lock);
@@ -191,8 +206,7 @@ int removeSoftwareBreakpoint(u64 addr, bool keepPersistent)
 
     SoftwareBreakpoint *bp = &g_softwareBreakpointManager.breakpoints[id];
     if (!keepPersistent || !bp->persistent) {
-        revertSoftwareBreakpoint(id);
-        // Note: no way to handle breakpoint failing to revert on 1+ core but not all, we need to assume operation succeeds
+        ok = revertSoftwareBreakpoint(id);
     }
 
     for(size_t i = id; i < g_softwareBreakpointManager.numBreakpoints - 1; i++) {
@@ -203,19 +217,18 @@ int removeSoftwareBreakpoint(u64 addr, bool keepPersistent)
 
     recursiveSpinlockUnlock(&g_softwareBreakpointManager.lock);
 
-    return 0;
+    return ok ? 0 : -EFAULT;
 }
 
 int removeAllSoftwareBreakpoints(bool keepPersistent)
 {
-    int ret = 0;
+    bool ok = true;
     recursiveSpinlockLock(&g_softwareBreakpointManager.lock);
 
     for (size_t id = 0; id < g_softwareBreakpointManager.numBreakpoints; id++) {
         SoftwareBreakpoint *bp = &g_softwareBreakpointManager.breakpoints[id];
         if (!keepPersistent || !bp->persistent) {
-            revertSoftwareBreakpoint(id);
-            // Note: no way to handle breakpoint failing to revert on 1+ core but not all, we need to assume operation succeeds
+            ok = ok && revertSoftwareBreakpoint(id);
         }
     }
 
@@ -225,5 +238,5 @@ int removeAllSoftwareBreakpoints(bool keepPersistent)
 
     recursiveSpinlockUnlock(&g_softwareBreakpointManager.lock);
 
-    return ret;
+    return ok ? 0 : -EFAULT;
 }

thermosphere/src/software_breakpoints.h

@@ -19,16 +19,18 @@
 #define _REENT_ONLY
 #include <errno.h>
 
+#include <stdatomic.h>
 #include "spinlock.h"
 
 #define MAX_SW_BREAKPOINTS  32
 
 typedef struct SoftwareBreakpoint {
-    u64 address; // VA
+    uintptr_t address; // VA
     u32 savedInstruction;
     u32 uid;
     bool persistent;
     bool applied;
+    atomic_bool triedToApplyOrRevert;
 } SoftwareBreakpoint;
 
 typedef struct SoftwareBreakpointManager {