thermosphere: add debug pause logic

thermosphere/src/debug_pause.c

@@ -0,0 +1,59 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "debug_pause.h"
+#include "core_ctx.h"
+#include "irq.h"
+#include "spinlock.h"
+
+// Reminder: use these functions behind a lock
+
+static Barrier g_debugPauseBarrier;
+static RecursiveSpinlock g_debugPauseContinueLocks[4];
+
+void debugPauseSgiTopHalf(void)
+{
+    barrierWait(&g_debugPauseBarrier);
+}
+
+void debugPauseSgiBottomHalf(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]);
+}
+
+void debugPauseCores(u32 coreList)
+{
+    coreList &= ~BIT(currentCoreCtx->coreId);
+
+    barrierInit(&g_debugPauseBarrier, coreList | BIT(currentCoreCtx->coreId));
+    FOREACH_BIT (tmp, core, coreList) {
+        recursiveSpinlockLock(&g_debugPauseContinueLocks[core]);
+    }
+
+    generateSgiForList(ThermosphereSgi_DebugPause, coreList);
+    barrierWait(&g_debugPauseBarrier);
+}
+
+void debugUnpauseCores(u32 coreList)
+{
+    coreList &= ~BIT(currentCoreCtx->coreId);
+
+    FOREACH_BIT (tmp, core, coreList) {
+        recursiveSpinlockUnlock(&g_debugPauseContinueLocks[core]);
+    }
+}

thermosphere/src/debug_pause.h

@@ -0,0 +1,31 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "utils.h"
+
+void debugPauseSgiTopHalf(void);
+void debugPauseSgiBottomHalf(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);

thermosphere/src/irq.c

@@ -21,6 +21,7 @@
 #include "timer.h"
 #include "guest_timers.h"
 #include "transport_interface.h"
+#include "debug_pause.h"
 
 IrqManager g_irqManager = {0};
 
@@ -212,7 +213,7 @@ void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32)
     u32 irqId = iar & 0x3FF;
     u32 srcCore = (iar >> 10) & 7;
 
-    //DEBUG("EL2 [core %d]: Received irq %x\n", (int)currentCoreCtx->coreId, irqId);
+    DEBUG("EL2 [core %d]: Received irq %x\n", (int)currentCoreCtx->coreId, irqId);
 
     if (irqId == GIC_IRQID_SPURIOUS) {
         // Spurious interrupt received
@@ -226,6 +227,7 @@ void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32)
 
     bool isGuestInterrupt = false;
     bool isMaintenanceInterrupt = false;
+    bool hasBottomHalf = false;
 
     switch (irqId) {
         case ThermosphereSgi_ExecuteFunction:
@@ -234,6 +236,10 @@ void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32)
         case ThermosphereSgi_VgicUpdate:
             // Nothing in particular to do here
             break;
+        case ThermosphereSgi_DebugPause:
+            debugPauseSgiTopHalf();
+            hasBottomHalf = true;
+            break;
         case GIC_IRQID_MAINTENANCE:
             isMaintenanceInterrupt = true;
             break;
@@ -246,6 +252,7 @@ void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32)
     }
 
     TransportInterface *transportIface = irqId >= 32 ? transportInterfaceIrqHandlerTopHalf(irqId) : NULL;
+    hasBottomHalf = hasBottomHalf || transportIface != NULL;
 
     // Priority drop
     gicc->eoir = iar;
@@ -270,9 +277,15 @@ void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32)
     recursiveSpinlockUnlock(&g_irqManager.lock);
 
     // Bottom half part
-    if (transportIface != NULL) {
+    if (hasBottomHalf) {
         exceptionEnterInterruptibleHypervisorCode(frame);
         unmaskIrq();
+
+        if (irqId == ThermosphereSgi_DebugPause) {
+            debugPauseSgiBottomHalf();
+        } else if (transportIface != NULL) {
             transportInterfaceIrqHandlerBottomHalf(transportIface);
         }
+    }
+
 }

thermosphere/src/irq.h

@@ -41,8 +41,9 @@ typedef struct IrqManager {
 typedef enum ThermosphereSgi {
     ThermosphereSgi_ExecuteFunction = 0,
     ThermosphereSgi_VgicUpdate = 1,
+    ThermosphereSgi_DebugPause = 2,
 
-    ThermosphereSgi_Max = 2,
+    ThermosphereSgi_Max,
 } ThermosphereSgi;
 
 extern IrqManager g_irqManager;

thermosphere/src/main.c

@@ -41,6 +41,7 @@ static void loadKernelViaSemihosting(void)
 
 
 #include "platform/uart.h"
+#include "debug_pause.h"
 typedef struct TestCtx {
     char buf[512+1];
 } TestCtx;
@@ -50,6 +51,7 @@ static TestCtx g_testCtx;
 size_t testReceiveCallback(TransportInterface *iface, void *p)
 {
     TestCtx *ctx = (TestCtx *)p;
+    debugPauseCores(BIT(0));
     return transportInterfaceReadDataUntil(iface, ctx->buf, 512, '\r');
 }
 
@@ -57,6 +59,7 @@ void testProcessDataCallback(TransportInterface *iface, void *p, size_t sz)
 {
     (void)iface;
     (void)sz;
+    debugUnpauseCores(BIT(0));
     TestCtx *ctx = (TestCtx *)p;
     DEBUG("EL2 [core %u]: you typed: %s\n", currentCoreCtx->coreId, ctx->buf);
 }
@@ -71,7 +74,7 @@ void test(void)
         testProcessDataCallback,
         &g_testCtx
     );
-    transportInterfaceSetInterruptAffinity(iface, BIT(0));
+    transportInterfaceSetInterruptAffinity(iface, BIT(1));
 }
 
 void thermosphereMain(ExceptionStackFrame *frame, u64 pct)

thermosphere/src/spinlock.h

@@ -16,9 +16,9 @@
 
 #pragma once
 
+#include "core_ctx.h"
 #include "utils.h"
 #include "sysreg.h"
-#include "core_ctx.h"
 
 typedef struct Spinlock {
     u32 lock;