diff --git a/thermosphere/src/debug_pause.c b/thermosphere/src/debug_pause.c
new file mode 100644
index 000000000..ccef5e6a1
--- /dev/null
+++ b/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 .
+ */
+
+#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]);
+ }
+}
diff --git a/thermosphere/src/debug_pause.h b/thermosphere/src/debug_pause.h
new file mode 100644
index 000000000..21ec9d9ee
--- /dev/null
+++ b/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 .
+ */
+
+#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);
diff --git a/thermosphere/src/irq.c b/thermosphere/src/irq.c
index 8539b003b..d1df11cc4 100644
--- a/thermosphere/src/irq.c
+++ b/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();
- transportInterfaceIrqHandlerBottomHalf(transportIface);
+
+ if (irqId == ThermosphereSgi_DebugPause) {
+ debugPauseSgiBottomHalf();
+ } else if (transportIface != NULL) {
+ transportInterfaceIrqHandlerBottomHalf(transportIface);
+ }
}
+
}
diff --git a/thermosphere/src/irq.h b/thermosphere/src/irq.h
index 6ca212a9e..0bc318c21 100644
--- a/thermosphere/src/irq.h
+++ b/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;
diff --git a/thermosphere/src/main.c b/thermosphere/src/main.c
index 2b5b22838..ddd4edda3 100644
--- a/thermosphere/src/main.c
+++ b/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)
diff --git a/thermosphere/src/spinlock.h b/thermosphere/src/spinlock.h
index bb9c5ab41..1f71cc52f 100644
--- a/thermosphere/src/spinlock.h
+++ b/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;