thermosphere: rewrite debug pause & fix single step state machine

thermosphere/src/asm_macros.s

@@ -16,7 +16,7 @@
 
 #define EXCEP_STACK_FRAME_SIZE      0x140
 
-#define CORECTX_USER_FRAME_OFFSET   0x000
+#define CORECTX_GUEST_FRAME_OFFSET  0x000
 #define CORECTX_SCRATCH_OFFSET      0x008
 #define CORECTX_CRASH_STACK_OFFSET  0x010
 

thermosphere/src/barrier.c

@@ -14,7 +14,6 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <stdatomic.h>
 #include "barrier.h"
 #include "core_ctx.h"
 #include "utils.h"

thermosphere/src/barrier.h

@@ -27,4 +27,4 @@ void barrierInit(Barrier *barrier, u32 coreList);
 void barrierInitAllButSelf(Barrier *barrier);
 void barrierInitAll(Barrier *barrier);
 
-void barrierWait(Barrier *barrier);
+void barrierWait(Barrier *barrier);

thermosphere/src/core_ctx.h

@@ -15,6 +15,7 @@
  */
 
 #pragma once
+#include <stdatomic.h>
 #include <assert.h>
 #include "utils.h"
 #include "barrier.h"
@@ -22,7 +23,7 @@
 
 struct ExceptionStackFrame;
 typedef struct CoreCtx {
-    struct ExceptionStackFrame *userFrame;      // @0x00
+    struct ExceptionStackFrame *guestFrame;     // @0x00
     u64 scratch;                                // @0x08
     u8 *crashStack;                             // @0x10
     u64 kernelArgument;                         // @0x18
@@ -42,6 +43,9 @@ typedef struct CoreCtx {
     Barrier executedFunctionBarrier;            // @0x50
     bool executedFunctionSync;                  // @0x54
 
+    // Debug features
+    bool wasPaused;                             // @0x55
+
     // Cache stuff
     u32 setWayCounter;                          // @0x58
 } CoreCtx;
@@ -49,6 +53,7 @@ typedef struct 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");
 
 extern CoreCtx g_coreCtxs[4];
 register CoreCtx *currentCoreCtx asm("x18");

thermosphere/src/debug_pause.c

@@ -14,46 +14,74 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <stdatomic.h>
+
 #include "debug_pause.h"
 #include "core_ctx.h"
 #include "irq.h"
 #include "spinlock.h"
+#include "single_step.h"
 
 // Reminder: use these functions behind a lock
 
 static Barrier g_debugPauseBarrier;
-static RecursiveSpinlock g_debugPauseContinueLocks[4];
+static atomic_uint g_debugPausePausedCoreList;
+static atomic_uint g_debugPauseSingleStepCoreList;
 
-void debugPauseSgiTopHalf(void)
+void debugPauseSgiHandler(void)
 {
+    currentCoreCtx->wasPaused = true;
     barrierWait(&g_debugPauseBarrier);
 }
 
-void debugPauseSgiBottomHalf(void)
+void debugPauseWaitAndUpdateSingleStep(void)
 {
-    recursiveSpinlockLock(&g_debugPauseContinueLocks[currentCoreCtx->coreId]);
-    maskIrq(); // <- unlikely race condition here? If it happens, it shouldn't happen more than once/should be fine anyway
-    recursiveSpinlockUnlock(&g_debugPauseContinueLocks[currentCoreCtx->coreId]);
+    u32 coreId = currentCoreCtx->coreId;
+    if (atomic_load(&g_debugPausePausedCoreList) & BIT(coreId)) {
+        unmaskIrq();
+        do {
+            __wfe();
+        } while (atomic_load(&g_debugPausePausedCoreList) & BIT(coreId));
+        maskIrq();
+    }
+
+    currentCoreCtx->wasPaused = false;
+
+    // Single-step: if inactive and requested, start single step; cancel if active and not requested
+    u32 ssReqd = (atomic_load(&g_debugPauseSingleStepCoreList) & ~BIT(currentCoreCtx->coreId)) != 0;
+    SingleStepState singleStepState = singleStepGetNextState(currentCoreCtx->guestFrame);
+    if (ssReqd && singleStepState == SingleStepState_Inactive) {
+        singleStepSetNextState(currentCoreCtx->guestFrame, SingleStepState_ActiveNotPending);
+    } else if (!ssReqd && singleStepState != SingleStepState_Inactive) {
+        singleStepSetNextState(currentCoreCtx->guestFrame, SingleStepState_Inactive);
+    }
 }
 
 void debugPauseCores(u32 coreList)
 {
-    coreList &= ~BIT(currentCoreCtx->coreId);
+    // Since we're using a debugger lock, a simple stlr should be fine...
+    atomic_store(&g_debugPausePausedCoreList, coreList);
 
-    barrierInit(&g_debugPauseBarrier, coreList | BIT(currentCoreCtx->coreId));
-    FOREACH_BIT (tmp, core, coreList) {
-        recursiveSpinlockLock(&g_debugPauseContinueLocks[core]);
+    if (coreList != BIT(currentCoreCtx->coreId)) {
+        // We need to notify other cores...
+        u32 otherCores = coreList & ~BIT(currentCoreCtx->coreId);
+        barrierInit(&g_debugPauseBarrier, otherCores | BIT(currentCoreCtx->coreId));
+        generateSgiForList(ThermosphereSgi_DebugPause, otherCores);
+        barrierWait(&g_debugPauseBarrier);
     }
 
-    generateSgiForList(ThermosphereSgi_DebugPause, coreList);
-    barrierWait(&g_debugPauseBarrier);
+    if (coreList & BIT(currentCoreCtx->coreId)) {
+        currentCoreCtx->wasPaused = true;
+    }
 }
 
-void debugUnpauseCores(u32 coreList)
+void debugUnpauseCores(u32 coreList, u32 singleStepList)
 {
-    coreList &= ~BIT(currentCoreCtx->coreId);
+    singleStepList &= coreList;
 
-    FOREACH_BIT (tmp, core, coreList) {
-        recursiveSpinlockUnlock(&g_debugPauseContinueLocks[core]);
-    }
+    // Since we're using a debugger lock, a simple stlr should be fine...
+    atomic_store(&g_debugPauseSingleStepCoreList, singleStepList);
+    atomic_store(&g_debugPausePausedCoreList, 0);
+
+    __sev();
 }

thermosphere/src/debug_pause.h

@@ -18,14 +18,13 @@
 
 #include "utils.h"
 
-void debugPauseSgiTopHalf(void);
-void debugPauseSgiBottomHalf(void);
+void debugPauseSgiHandler(void);
+
+// Hypervisor interrupts will be serviced during the pause-wait
+void debugPauseWaitAndUpdateSingleStep(void);
 
 // Note: these functions are not reentrant! (need a global debug lock...)
-// These functions also run with interrupts unmasked (but we know we're in our code -- should be safe if we take care)
-// while the core is paused.
 // "Pause" makes sure all cores reaches the pause function before proceeding.
 // "Unpause" doesn't synchronize, it just makes sure the core resumes & that "pause" can be called again.
-
 void debugPauseCores(u32 coreList);
-void debugUnpauseCores(u32 coreList);
+void debugUnpauseCores(u32 coreList, u32 singleStepList);

thermosphere/src/exception_vectors.s

@@ -104,7 +104,7 @@ vector_entry \name
 
     .if \type == EXCEPTION_TYPE_GUEST
         ldp     x18, xzr, [sp, #EXCEP_STACK_FRAME_SIZE]
-        str     x0, [x18, #CORECTX_USER_FRAME_OFFSET]
+        str     x0, [x18, #CORECTX_GUEST_FRAME_OFFSET]
         mov     w1, #1
     .else
         mov     w1, #0

thermosphere/src/exceptions.c

@@ -23,7 +23,8 @@
 #include "core_ctx.h"
 #include "single_step.h"
 #include "data_abort.h"
-
+#include "spinlock.h"
+#include "debug_pause.h"
 #include "timer.h"
 
 bool spsrEvaluateConditionCode(u64 spsr, u32 conditionCode)
@@ -113,10 +114,15 @@ void exceptionEntryPostprocess(ExceptionStackFrame *frame, bool isLowerEl)
 // Called on exception return (avoids overflowing a vector section)
 void exceptionReturnPreprocess(ExceptionStackFrame *frame)
 {
+    if (currentCoreCtx->wasPaused && frame == currentCoreCtx->guestFrame) {
+        // Were we paused & are we about to return to the guest?
+        exceptionEnterInterruptibleHypervisorCode(frame);
+        debugPauseWaitAndUpdateSingleStep();
+    }
+
     // Update virtual counter
     currentCoreCtx->totalTimeInHypervisor += timerGetSystemTick() - frame->cntpct_el0;
     SET_SYSREG(cntvoff_el2, currentCoreCtx->totalTimeInHypervisor);
-    //DEBUG("pct %lu - vct %lu = voff %lu\n", timerGetSystemTick() - GET_SYSREG(cntvct_el0), GET_SYSREG(cntvoff_el2));
 
     // Restore interrupt mask
     SET_SYSREG(cntp_ctl_el0, frame->cntp_ctl_el0);

thermosphere/src/irq.c

@@ -237,8 +237,7 @@ void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32)
             // Nothing in particular to do here
             break;
         case ThermosphereSgi_DebugPause:
-            debugPauseSgiTopHalf();
-            hasBottomHalf = true;
+            debugPauseSgiHandler();
             break;
         case GIC_IRQID_MAINTENANCE:
             isMaintenanceInterrupt = true;
@@ -280,10 +279,7 @@ void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32)
     if (hasBottomHalf) {
         exceptionEnterInterruptibleHypervisorCode(frame);
         unmaskIrq();
-
-        if (irqId == ThermosphereSgi_DebugPause) {
-            debugPauseSgiBottomHalf();
-        } else if (transportIface != NULL) {
+        if (transportIface != NULL) {
             transportInterfaceIrqHandlerBottomHalf(transportIface);
         }
     }

thermosphere/src/main.c

@@ -59,7 +59,7 @@ void testProcessDataCallback(TransportInterface *iface, void *p, size_t sz)
 {
     (void)iface;
     (void)sz;
-    debugUnpauseCores(BIT(0));
+    debugUnpauseCores(BIT(0), BIT(0));
     TestCtx *ctx = (TestCtx *)p;
     DEBUG("EL2 [core %u]: you typed: %s\n", currentCoreCtx->coreId, ctx->buf);
 }

thermosphere/src/single_step.c

@@ -28,7 +28,7 @@ SingleStepState singleStepGetNextState(ExceptionStackFrame *frame)
     if (!mdscrSS) {
         return SingleStepState_Inactive;
     } else {
-        return pstateSS ? SingleStepState_ActivePending : SingleStepState_ActiveNotPending;
+        return pstateSS ? SingleStepState_ActiveNotPending : SingleStepState_ActivePending;
     }
 }
 
@@ -41,15 +41,16 @@ void singleStepSetNextState(ExceptionStackFrame *frame, SingleStepState state)
             // Unset mdscr_el1.ss
             mdscr &= ~MDSCR_SS;
             break;
-        case SingleStepState_ActivePending:
+        case SingleStepState_ActiveNotPending:
             // Set mdscr_el1.ss and pstate.ss
             mdscr |= MDSCR_SS;
             frame->spsr_el2 |= PSTATE_SS;
             break;
-        case SingleStepState_ActiveNotPending:
+        case SingleStepState_ActivePending:
+            // We never use this because pstate.ss is 0 by default...
             // Set mdscr_el1.ss and unset pstate.ss
             mdscr |= MDSCR_SS;
-            frame->spsr_el2 |= PSTATE_SS;
+            frame->spsr_el2 &= ~PSTATE_SS;
             break;
         default:
             break;
@@ -65,7 +66,4 @@ void handleSingleStep(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
     singleStepSetNextState(NULL, SingleStepState_Inactive);
 
     DEBUG("Single-step exeception ELR = 0x%016llx, ISV = %u, EX = %u\n", frame->elr_el2, (esr.iss >> 24) & 1, (esr.iss >> 6) & 1);
-
-    // Hehe boi
-    //singleStepSetNextState(frame, SingleStepState_ActivePending);
 }

thermosphere/src/single_step.h

@@ -21,8 +21,8 @@
 
 typedef enum SingleStepState {
     SingleStepState_Inactive            = 0, // Single step disabled OR in the debugger
-    SingleStepState_ActivePending       = 1, // Instruction not yet executed
-    SingleStepState_ActiveNotPending    = 2, // Instruction executed, single-step exception is going to be generated soon
+    SingleStepState_ActiveNotPending    = 1, // Instruction not yet executed
+    SingleStepState_ActivePending       = 2, // Instruction executed or return-from-trap, single-step exception is going to be generated soon
 } SingleStepState;
 
 /// Get the single-step state machine state (state after eret)

thermosphere/src/utils.h

@@ -62,6 +62,21 @@ typedef enum ReadWriteDirection {
     DIRECTION_READWRITE = DIRECTION_READ | DIRECTION_WRITE,
 } ReadWriteDirection;
 
+static inline void __wfe(void)
+{
+    __asm__ __volatile__ ("wfe" ::: "memory");
+}
+
+static inline void __sev(void)
+{
+    __asm__ __volatile__ ("sev" ::: "memory");
+}
+
+static inline void __sevl(void)
+{
+    __asm__ __volatile__ ("sevl" ::: "memory");
+}
+
 /*
     Domains:
         - Inner shareable: typically cores within a cluster (maybe more) with L1+L2 caches