From 0e47f7f46bdcefaf10ed55241dbed93e394fc0e8 Mon Sep 17 00:00:00 2001
From: TuxSH <1922548+TuxSH@users.noreply.github.com>
Date: Mon, 27 Jan 2020 00:46:00 +0000
Subject: [PATCH] thermosphere: debug manager wip

---
 thermosphere/src/debug_manager.c              | 146 ++++++++++++++++++
 .../src/{debug_pause.h => debug_manager.h}    |  33 +++-
 thermosphere/src/debug_pause.c                | 104 -------------
 thermosphere/src/exceptions.c                 |   4 +-
 thermosphere/src/gdb/context.h                |   2 +-
 thermosphere/src/gdb/debug.h                  |   2 +-
 thermosphere/src/irq.c                        |   4 +-
 thermosphere/src/main.c                       |   6 +-
 8 files changed, 183 insertions(+), 118 deletions(-)
 create mode 100644 thermosphere/src/debug_manager.c
 rename thermosphere/src/{debug_pause.h => debug_manager.h} (58%)
 delete mode 100644 thermosphere/src/debug_pause.c

diff --git a/thermosphere/src/debug_manager.c b/thermosphere/src/debug_manager.c
new file mode 100644
index 000000000..0888329e5
--- /dev/null
+++ b/thermosphere/src/debug_manager.c
@@ -0,0 +1,146 @@
+/*
+ * 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 <stdatomic.h>
+#include <stdarg.h>
+
+#include "debug_manager.h"
+#include "core_ctx.h"
+#include "irq.h"
+#include "spinlock.h"
+#include "single_step.h"
+
+#include "gdb/debug.h"
+
+typedef struct DebugManager {
+    DebugEventInfo              debugEventInfos[MAX_CORE];
+
+    ALIGN(64) atomic_uint       pausedCoreList;
+    atomic_uint                 singleStepCoreList;
+    atomic_uint                 eventsSentList;
+    Barrier                     pauseBarrier;
+
+    atomic_bool                 nonStop;
+} DebugManager;
+
+static DebugManager g_debugManager = { 0 };
+
+static void debugManagerDoPauseCores(u32 coreList)
+{
+    __builtin_prefetch(&g_debugManager.pausedCoreList, 1, 0);
+
+    u32 desiredList = coreList;
+    u32 remainingList = coreList;
+    u32 readList = atomic_load(&g_debugManager.pausedCoreList);
+    do {
+        desiredList |= readList;
+        remainingList &= ~readList;
+    } while (atomic_compare_exchange_weak(&g_debugManager.pausedCoreList, &readList, desiredList));
+
+    if (remainingList != BIT(currentCoreCtx->coreId)) {
+        // We need to notify other cores...
+        u32 otherCores = remainingList & ~BIT(currentCoreCtx->coreId);
+        barrierInit(&g_debugManager.pauseBarrier, otherCores | BIT(currentCoreCtx->coreId));
+        generateSgiForList(ThermosphereSgi_DebugPause, otherCores);
+        barrierWait(&g_debugManager.pauseBarrier);
+    }
+
+    if (remainingList & BIT(currentCoreCtx->coreId)) {
+        currentCoreCtx->wasPaused = true;
+    }
+}
+
+void debugManagerPauseSgiHandler(void)
+{
+    currentCoreCtx->wasPaused = true;
+    barrierWait(&g_debugManager.pauseBarrier);
+}
+
+void debugManagerHandlePause(void)
+{
+    u32 coreId = currentCoreCtx->coreId;
+    __builtin_prefetch(&g_debugManager.pausedCoreList, 0, 3);
+
+    if (atomic_load(&g_debugManager.pausedCoreList) & BIT(coreId)) {
+        unmaskIrq();
+        do {
+            __wfe();
+        } while (atomic_load(&g_debugManager.pausedCoreList) & 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_debugManager.singleStepCoreList) & 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 debugManagerPauseCores(u32 coreList)
+{
+    u64 flags = maskIrq();
+    debugManagerDoPauseCores(coreList);
+    restoreInterruptFlags(flags);
+}
+
+void debugManagerUnpauseCores(u32 coreList, u32 singleStepList)
+{
+    singleStepList &= coreList;
+
+    __builtin_prefetch(&g_debugManager.pausedCoreList, 1, 0);
+
+    // Since we're using a debugger lock, a simple stlr should be fine...
+    atomic_store(&g_debugManager.singleStepCoreList, singleStepList);
+    atomic_fetch_and(&g_debugManager.pausedCoreList, ~coreList);
+
+    __sev();
+}
+
+void debugManagerReportEvent(DebugEventType type, ...)
+{
+    u64 flags = maskIrq();
+    u32 coreId = currentCoreCtx->coreId;
+
+    DebugEventInfo *info = &g_debugManager.debugEventInfos[coreId];
+
+    info->type = type;
+    info->coreId = coreId;
+    info->frame = currentCoreCtx->guestFrame;
+
+    va_list args;
+    va_start(args, type);
+
+    switch (type) {
+        case DBGEVENT_OUTPUT_STRING:
+            info->outputString.address = va_arg(args, uintptr_t);
+            info->outputString.size = va_arg(args, size_t);
+            break;
+        default:
+            break;
+    }
+
+    va_end(args);
+
+    // Now, pause ourselves and try to signal we have a debug event
+    debugManagerDoPauseCores(BIT(coreId));
+    // TODO gdb enter leave functions
+    restoreInterruptFlags(flags);
+}
diff --git a/thermosphere/src/debug_pause.h b/thermosphere/src/debug_manager.h
similarity index 58%
rename from thermosphere/src/debug_pause.h
rename to thermosphere/src/debug_manager.h
index edaa43487..3c5bf501e 100644
--- a/thermosphere/src/debug_pause.h
+++ b/thermosphere/src/debug_manager.h
@@ -16,15 +16,38 @@
 
 #pragma once
 
-#include "utils.h"
+#include "exceptions.h"
 
-void debugPauseSgiHandler(void);
+typedef enum DebugEventType {
+    DBGEVENT_DEBUGGER_BREAK = 0,
+    DBGEVENT_EXCEPTION,
+    DBGEVENT_CORE_ON,
+    DBGEVENT_CORE_OFF,
+    DBGEVENT_EXIT,
+    DBGEVENT_OUTPUT_STRING,
+} DebugEventType;
+
+typedef struct OutputStringDebugEventInfo {
+    uintptr_t address;
+    size_t size;
+} OutputStringDebugEventInfo;
+
+typedef struct DebugEventInfo {
+    DebugEventType type;
+    u32 coreId;
+    ExceptionStackFrame *frame;
+    union {
+        OutputStringDebugEventInfo outputString;
+    };
+} DebugEventInfo;
+
+void debugManagerPauseSgiHandler(void);
 
 // Hypervisor interrupts will be serviced during the pause-wait
-void debugPauseWaitAndUpdateSingleStep(void);
+void debugManagerHandlePause(void);
 
 // Note: these functions are not reentrant EXCEPT debugPauseCores(1 << currentCoreId)
 // "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, u32 singleStepList);
+void debugManagerPauseCores(u32 coreList);
+void debugManagerUnpauseCores(u32 coreList, u32 singleStepList);
diff --git a/thermosphere/src/debug_pause.c b/thermosphere/src/debug_pause.c
deleted file mode 100644
index e3714ba91..000000000
--- a/thermosphere/src/debug_pause.c
+++ /dev/null
@@ -1,104 +0,0 @@
-/*
- * 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 <stdatomic.h>
-
-#include "debug_pause.h"
-#include "core_ctx.h"
-#include "irq.h"
-#include "spinlock.h"
-#include "single_step.h"
-
-static Barrier g_debugPauseBarrier;
-static ALIGN(64) atomic_uint g_debugPausePausedCoreList;
-static atomic_uint g_debugPauseSingleStepCoreList; // TODO: put this variable on the same cache line as the above
-
-static inline void debugSetThisCorePaused(void)
-{
-    currentCoreCtx->wasPaused = true;
-}
-
-void debugPauseSgiHandler(void)
-{
-    debugSetThisCorePaused();
-    barrierWait(&g_debugPauseBarrier);
-}
-
-void debugPauseWaitAndUpdateSingleStep(void)
-{
-    u32 coreId = currentCoreCtx->coreId;
-    __builtin_prefetch(&g_debugPausePausedCoreList, 0, 3);
-    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)
-{
-    maskIrq();
-
-    __builtin_prefetch(&g_debugPausePausedCoreList, 1, 3);
-
-    u32 desiredList = coreList;
-    u32 remainingList = coreList;
-    u32 readList = atomic_load(&g_debugPausePausedCoreList);
-    do {
-        desiredList |= readList;
-        remainingList &= ~readList;
-    } while (atomic_compare_exchange_weak(&g_debugPausePausedCoreList, &readList, desiredList));
-
-    if (remainingList != BIT(currentCoreCtx->coreId)) {
-        // We need to notify other cores...
-        u32 otherCores = remainingList & ~BIT(currentCoreCtx->coreId);
-        barrierInit(&g_debugPauseBarrier, otherCores | BIT(currentCoreCtx->coreId));
-        generateSgiForList(ThermosphereSgi_DebugPause, otherCores);
-        barrierWait(&g_debugPauseBarrier);
-    }
-
-    if (remainingList & BIT(currentCoreCtx->coreId)) {
-        debugSetThisCorePaused();
-    }
-
-    unmaskIrq();
-}
-
-void debugUnpauseCores(u32 coreList, u32 singleStepList)
-{
-    singleStepList &= coreList;
-
-    __builtin_prefetch(&g_debugPausePausedCoreList, 1, 0);
-
-    // Since we're using a debugger lock, a simple stlr should be fine...
-    atomic_store(&g_debugPauseSingleStepCoreList, singleStepList);
-    atomic_fetch_and(&g_debugPausePausedCoreList, ~coreList);
-
-    __sev();
-}
diff --git a/thermosphere/src/exceptions.c b/thermosphere/src/exceptions.c
index 266197c1c..dcd649264 100644
--- a/thermosphere/src/exceptions.c
+++ b/thermosphere/src/exceptions.c
@@ -24,7 +24,7 @@
 #include "single_step.h"
 #include "data_abort.h"
 #include "spinlock.h"
-#include "debug_pause.h"
+#include "debug_manager.h"
 #include "timer.h"
 
 #include "fpu.h"
@@ -122,7 +122,7 @@ void exceptionReturnPreprocess(ExceptionStackFrame *frame)
     if (currentCoreCtx->wasPaused && frame == currentCoreCtx->guestFrame) {
         // Were we paused & are we about to return to the guest?
         exceptionEnterInterruptibleHypervisorCode();
-        debugPauseWaitAndUpdateSingleStep();
+        debugManagerHandlePause();
         fpuCleanInvalidateRegisterCache();
     }
 
diff --git a/thermosphere/src/gdb/context.h b/thermosphere/src/gdb/context.h
index ce64c2139..55d596983 100644
--- a/thermosphere/src/gdb/context.h
+++ b/thermosphere/src/gdb/context.h
@@ -25,7 +25,7 @@
 #pragma once
 
 #include "defines.h"
-#include "transport_interface.h"
+#include "../transport_interface.h"
 
 typedef struct PackedGdbHioRequest
 {
diff --git a/thermosphere/src/gdb/debug.h b/thermosphere/src/gdb/debug.h
index 3c5389ab4..87c09e5d2 100644
--- a/thermosphere/src/gdb/debug.h
+++ b/thermosphere/src/gdb/debug.h
@@ -21,4 +21,4 @@ GDB_DECLARE_HANDLER(GetStopReason);
 void GDB_ContinueExecution(GDBContext *ctx);
 int GDB_SendStopReply(GDBContext *ctx, const DebugEventInfo *info);
 int GDB_HandleDebugEvents(GDBContext *ctx);
-void GDB_BreakProcessAndSinkDebugEvents(GDBContext *ctx, DebugFlags flags);
+//void GDB_BreakProcessAndSinkDebugEvents(GDBContext *ctx, DebugFlags flags);
diff --git a/thermosphere/src/irq.c b/thermosphere/src/irq.c
index 62787c205..557e9136a 100644
--- a/thermosphere/src/irq.c
+++ b/thermosphere/src/irq.c
@@ -21,7 +21,7 @@
 #include "timer.h"
 #include "guest_timers.h"
 #include "transport_interface.h"
-#include "debug_pause.h"
+#include "debug_manager.h"
 
 IrqManager g_irqManager = {0};
 
@@ -230,7 +230,7 @@ void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32)
             // Nothing in particular to do here
             break;
         case ThermosphereSgi_DebugPause:
-            debugPauseSgiHandler();
+            debugManagerPauseSgiHandler();
             break;
         case GIC_IRQID_MAINTENANCE:
             isMaintenanceInterrupt = true;
diff --git a/thermosphere/src/main.c b/thermosphere/src/main.c
index 56f915299..dd8173d85 100644
--- a/thermosphere/src/main.c
+++ b/thermosphere/src/main.c
@@ -57,7 +57,7 @@ static void loadKernelViaSemihosting(void)
 
 
 #include "platform/uart.h"
-#include "debug_pause.h"
+#include "debug_manager.h"
 typedef struct TestCtx {
     char buf[512+1];
 } TestCtx;
@@ -67,7 +67,7 @@ static TestCtx g_testCtx;
 size_t testReceiveCallback(TransportInterface *iface, void *p)
 {
     TestCtx *ctx = (TestCtx *)p;
-    debugPauseCores(BIT(0));
+    debugManagerPauseCores(BIT(0));
     return transportInterfaceReadDataUntil(iface, ctx->buf, 512, '\r');
 }
 
@@ -75,7 +75,7 @@ void testProcessDataCallback(TransportInterface *iface, void *p, size_t sz)
 {
     (void)iface;
     (void)sz;
-    debugUnpauseCores(BIT(0), BIT(0));
+    debugManagerUnpauseCores(BIT(0), BIT(0));
     TestCtx *ctx = (TestCtx *)p;
     DEBUG("EL2 [core %u]: you typed: %s\n", currentCoreCtx->coreId, ctx->buf);
 }