thermosphere: gdb: add core_on and core_off handling

thermosphere/src/debug_manager.c

@@ -36,6 +36,7 @@ typedef struct DebugManager {
     atomic_uint                 singleStepCoreList;
     atomic_uint                 eventsSentList;
     Barrier                     pauseBarrier;
+    atomic_bool                 reportingEnabled;
 } DebugManager;
 
 static DebugManager g_debugManager = { 0 };
@@ -109,6 +110,11 @@ bool debugManagerHandlePause(void)
     return true;
 }
 
+void debugManagerSetReportingEnabled(bool enabled)
+{
+    atomic_store(&g_debugManager.reportingEnabled, enabled);
+}
+
 void debugManagerPauseCores(u32 coreList)
 {
     u64 flags = maskIrq();
@@ -146,13 +152,7 @@ u32 debugManagerGetPausedCoreList(void)
     return atomic_load(&g_debugManager.pausedCoreList);
 }
 
-const DebugEventInfo *debugManagerMarkAndGetCoreDebugEvent(u32 coreId)
+DebugEventInfo *debugManagerGetCoreDebugEvent(u32 coreId)
-{
-    g_debugManager.debugEventInfos[coreId].handled = true;
-    return &g_debugManager.debugEventInfos[coreId];
-}
-
-const DebugEventInfo *debugManagerGetCoreDebugEvent(u32 coreId)
 {
     return &g_debugManager.debugEventInfos[coreId];
 }
@@ -160,6 +160,11 @@ const DebugEventInfo *debugManagerGetCoreDebugEvent(u32 coreId)
 void debugManagerReportEvent(DebugEventType type, ...)
 {
     u64 flags = maskIrq();
+    if (!atomic_load(&g_debugManager.reportingEnabled)) {
+        restoreInterruptFlags(flags);
+        return;
+    }
+
     u32 coreId = currentCoreCtx->coreId;
 
     DebugEventInfo *info = &g_debugManager.debugEventInfos[coreId];

thermosphere/src/debug_manager.h

@@ -48,6 +48,8 @@ typedef struct DebugEventInfo {
 
 void debugManagerPauseSgiHandler(void);
 
+void debugManagerSetReportingEnabled(bool enabled);
+
 // Hypervisor interrupts will be serviced during the pause-wait
 bool debugManagerHandlePause(void);
 
@@ -62,8 +64,7 @@ void debugManagerSetSteppingRange(u32 coreId, uintptr_t startAddr, uintptr_t end
 
 u32 debugManagerGetPausedCoreList(void);
 
-const DebugEventInfo *debugManagerMarkAndGetCoreDebugEvent(u32 coreId);
+DebugEventInfo *debugManagerGetCoreDebugEvent(u32 coreId);
-const DebugEventInfo *debugManagerGetCoreDebugEvent(u32 coreId);
 
 void debugManagerReportEvent(DebugEventType type, ...);
 

thermosphere/src/gdb/debug.c

@@ -23,6 +23,49 @@
 #include <stdlib.h>
 #include <signal.h>
 
+static bool GDB_PreprocessDebugEvent(GDBContext *ctx, DebugEventInfo *info)
+{
+    u64 irqFlags = maskIrq();
+    bool shouldSignal;
+
+    switch (info->type) {
+        case DBGEVENT_CORE_ON: {
+            shouldSignal = ctx->catchThreadEvents;
+            if (!info->handled) {
+                ctx->attachedCoreList |= BIT(info->coreId);
+            }
+            break;
+        }
+        case DBGEVENT_CORE_OFF: {
+            if (!info->handled) {
+                u32 newLst = ctx->attachedCoreList & ~BIT(info->coreId);
+                if (ctx->selectedThreadId == info->coreId && newLst != 0) {
+                    ctx->selectedThreadId = __builtin_ctz(newLst);
+                    GDB_MigrateRxIrq(ctx, BIT(ctx->selectedThreadId));
+                }
+                ctx->attachedCoreList = newLst;
+                shouldSignal = ctx->catchThreadEvents || newLst == 0;
+            } else {
+                shouldSignal = ctx->catchThreadEvents;
+            }
+            break;
+        }
+
+        default:
+            shouldSignal = true;
+            break;
+    }
+
+    info->handled = true;
+    restoreInterruptFlags(irqFlags);
+    return shouldSignal; 
+}
+
+static inline void GDB_MarkDebugEventAcked(GDBContext *ctx, const DebugEventInfo *info)
+{
+    ctx->acknowledgedDebugEventCoreList |= BIT(info->coreId);
+}
+
 static int GDB_ParseExceptionFrame(char *out, const DebugEventInfo *info, int sig)
 {
     u32 coreId = info->coreId;
@@ -46,7 +89,7 @@ static int GDB_ParseExceptionFrame(char *out, const DebugEventInfo *info, int si
     return n;
 }
 
-int GDB_SendStopReply(GDBContext *ctx, const DebugEventInfo *info, bool asNotification)
+int GDB_SendStopReply(GDBContext *ctx, DebugEventInfo *info, bool asNotification)
 {
     char *buf = ctx->buffer + 1;
     int n;
@@ -80,7 +123,12 @@ int GDB_SendStopReply(GDBContext *ctx, const DebugEventInfo *info, bool asNotifi
         }
 
         case DBGEVENT_CORE_OFF: {
-            if(ctx->catchThreadEvents) {
+            if (ctx->attachedCoreList == 0) {
+                // All cores have exited, must report an exit
+                ctx->processExited = true;
+                ctx->processEnded = true;
+                strcat(buf, "W00");
+            } else if(ctx->catchThreadEvents) {
                 sprintf(buf, "w0;%x", info->coreId + 1);
             } else {
                 invalid = true;
@@ -178,7 +226,7 @@ int GDB_SendStopReply(GDBContext *ctx, const DebugEventInfo *info, bool asNotifi
         return GDB_SendNotificationPacket(ctx, buf, strlen(buf));
     } else {
         if (!(ctx->flags & GDB_FLAG_NONSTOP)) {
-            ctx->acknowledgedDebugEventCoreList |= BIT(info->coreId);
+            GDB_MarkDebugEventAcked(ctx, info);
         }
         return GDB_SendPacket(ctx, buf, strlen(buf));
     }
@@ -209,18 +257,12 @@ int GDB_TrySignalDebugEvent(GDBContext *ctx, DebugEventInfo *info)
     // Acquire the gdb lock/disable rx irq. We most likely block here.
     GDB_AcquireContext(ctx);
 
-    // Is the context not attached?
+    // Need to put it here otherwise core on/off would never be seen
-    if (!GDB_IsAttached(ctx)) {
+    bool shouldSignal = GDB_PreprocessDebugEvent(ctx, info);
-        // Not attached, mark the event as handled, unpause
-        debugManagerMarkAndGetCoreDebugEvent(info->coreId);
-        debugManagerUnpauseCores(BIT(info->coreId));
-        GDB_ReleaseContext(ctx);
-        return -1;
-    }
 
     // Are we still paused & has the packet not been handled & are we allowed to send on our own?
 
-    if (!ctx->sendOwnDebugEventDisallowed && !info->handled && debugManagerIsCorePaused(info->coreId)) {
+    if (shouldSignal && !ctx->sendOwnDebugEventDisallowed && !info->handled && debugManagerIsCorePaused(info->coreId)) {
         bool nonStop = (ctx->flags & GDB_FLAG_NONSTOP) != 0;
         info->handled = true;
 
@@ -233,6 +275,10 @@ int GDB_TrySignalDebugEvent(GDBContext *ctx, DebugEventInfo *info)
         ret = GDB_SendStopReply(ctx, info, nonStop);
     }
 
+    if (!shouldSignal) {
+        debugManagerContinueCores(BIT(currentCoreCtx->coreId));
+    }
+
     GDB_ReleaseContext(ctx);
 
     return ret;
@@ -254,20 +300,26 @@ GDB_DECLARE_VERBOSE_HANDLER(Stopped)
 
     // Ack
     if (ctx->lastDebugEvent != NULL) {
-        ctx->acknowledgedDebugEventCoreList |= BIT(ctx->lastDebugEvent->coreId);
+        GDB_MarkDebugEventAcked(ctx, ctx->lastDebugEvent);
     }
 
-    if (remaining != 0) {
+    for (;;) {
-        // Send one more debug event (marking it as handled)
+        if (remaining != 0) {
-        u32 coreId = __builtin_ctz(remaining);
+            // Send one more debug event (marking it as handled)
-        DebugEventInfo *info = debugManagerMarkAndGetCoreDebugEvent(coreId);
+            u32 coreId = __builtin_ctz(remaining);
+            DebugEventInfo *info = debugManagerGetCoreDebugEvent(coreId);
 
-        ctx->sendOwnDebugEventDisallowed = true;
+            if (GDB_PreprocessDebugEvent(ctx, info)) {
-        return GDB_SendStopReply(ctx, info, false);
+                ctx->sendOwnDebugEventDisallowed = true;
-    } else {
+                return GDB_SendStopReply(ctx, info, false);
-        // vStopped sequenced finished
+            } else {
-        ctx->sendOwnDebugEventDisallowed = false;
+                remaining &= ~BIT(coreId);
-        return GDB_ReplyOk(ctx);
+            }
+        } else {
+            // vStopped sequenced finished
+            ctx->sendOwnDebugEventDisallowed = false;
+            return GDB_ReplyOk(ctx);
+        }
     }
 }
 

thermosphere/src/gdb/debug.h

@@ -12,7 +12,7 @@
 #include "../debug_manager.h"
 
 void GDB_ContinueExecution(GDBContext *ctx);
-int GDB_SendStopReply(GDBContext *ctx, const DebugEventInfo *info, bool asNotification);
+int GDB_SendStopReply(GDBContext *ctx, DebugEventInfo *info, bool asNotification);
 int GDB_TrySignalDebugEvent(GDBContext *ctx, DebugEventInfo *info);
 
 GDB_DECLARE_VERBOSE_HANDLER(Stopped);

thermosphere/src/gdb/thread.h

@@ -9,9 +9,6 @@
 
 #include "context.h"
 
-u32 GDB_GetCurrentThreadFromList(GDBContext *ctx, u32 *threadIds, u32 nbThreads);
-u32 GDB_GetCurrentThread(GDBContext *ctx);
-
 GDB_DECLARE_HANDLER(SetThreadId);
 GDB_DECLARE_HANDLER(IsThreadAlive);
 

thermosphere/src/main.c

@@ -133,4 +133,8 @@ void thermosphereMain(ExceptionStackFrame *frame, u64 pct)
     // Initialize FPU registers -- no need to memset, the regcaches are in .tempbss
     fpuCommitRegisters();
     fpuCleanInvalidateRegisterCache();
+
+    if (!currentCoreCtx->isBootCore) {
+        debugManagerReportEvent(DBGEVENT_CORE_ON);
+    }
 }

thermosphere/src/smc.c

@@ -3,6 +3,7 @@
 #include "core_ctx.h"
 #include "caches.h"
 #include "memory_map.h"
+#include "debug_manager.h"
 
 // Currently in exception_vectors.s:
 extern const u32 doSmcIndirectCallImpl[];
@@ -16,7 +17,7 @@ void doSmcIndirectCall(ExceptionStackFrame *frame, u32 smcId)
 
     cacheHandleSelfModifyingCodePoU(codebuf, doSmcIndirectCallImplSize/4);
 
-    __dsb_sy();
+    __dsb();
     __isb();
     ((void (*)(ExceptionStackFrame *))codebuf)(frame);
 }
@@ -51,6 +52,7 @@ void handleSmcTrap(ExceptionStackFrame *frame, ExceptionSyndromeRegister esr)
         case 0x84000002:
             // CPU_OFF
             // TODO
+            debugManagerReportEvent(DBGEVENT_CORE_OFF);
             break;
         case 0xC4000003:
             doCpuOnHook(frame, smcId);