thermosphere: rewrite gdb/reg

thermosphere/src/exceptions.h

@@ -135,7 +135,7 @@ static inline void writeFrameRegisterZ(ExceptionStackFrame *frame, u32 id, u64 v
     }
 }
 
-static inline uintptr_t exceptionGetSp(const ExceptionStackFrame *frame)
+static inline u64 *exceptionGetSpPtr(ExceptionStackFrame *frame)
 {
     // Note: the return value is more or less meaningless if we took an exception from A32...
     // We try our best to reflect which privilege level the exception was took from, nonetheless
@@ -149,7 +149,7 @@ static inline uintptr_t exceptionGetSp(const ExceptionStackFrame *frame)
         spEl0 = el == 2 || el == 0 || (m & 1) == 0; // note: frame->sp_el2 is aliased to frame->sp_el0
     }
 
-    return spEl0 ? frame->sp_el0 : frame->sp_el1;
+    return spEl0 ? &frame->sp_el0 : &frame->sp_el1;
 }
 
 bool spsrEvaluateConditionCode(u64 spsr, u32 conditionCode);

thermosphere/src/gdb/debug.c

@@ -173,7 +173,7 @@ static int GDB_ParseExceptionFrame(char *out, const DebugEventInfo *info, int si
     n += sprintf(
         out + n,
         "1f:%016lx;20:%016lx;21:%08x",
-        __builtin_bswap64(exceptionGetSp(frame)),
+        __builtin_bswap64(*exceptionGetSpPtr(frame)),
         __builitin_bswap32((u32)frame->spsr_el2)
     );
 

thermosphere/src/gdb/net.c

@@ -23,7 +23,7 @@ u8 GDB_ComputeChecksum(const char *packetData, size_t len)
     return (u8)cksum;
 }
 
-void GDB_EncodeHex(char *dst, const void *src, size_t len)
+size_t GDB_EncodeHex(char *dst, const void *src, size_t len)
 {
     static const char *alphabet = "0123456789abcdef";
     const u8 *src8 = (const u8 *)src;
@@ -32,6 +32,8 @@ void GDB_EncodeHex(char *dst, const void *src, size_t len)
         dst[2 * i] = alphabet[(src8[i] & 0xf0) >> 4];
         dst[2 * i + 1] = alphabet[src8[i] & 0x0f];
     }
+
+    return 2 * len;
 }
 
 static inline u32 GDB_DecodeHexDigit(char src, bool *ok)

thermosphere/src/gdb/net.h

@@ -12,7 +12,7 @@
 #include <errno.h>
 
 u8 GDB_ComputeChecksum(const char *packetData, size_t len);
-void GDB_EncodeHex(char *dst, const void *src, size_t len);
+size_t GDB_EncodeHex(char *dst, const void *src, size_t len);
 size_t GDB_DecodeHex(void *dst, const char *src, size_t len);
 size_t GDB_EscapeBinaryData(size_t *encodedCount, void *dst, const void *src, size_t len, size_t maxLen);
 size_t GDB_UnescapeBinaryData(void *dst, const void *src, size_t len);

thermosphere/src/gdb/regs.c

@@ -5,149 +5,212 @@
 *   SPDX-License-Identifier: (MIT OR GPL-2.0-or-later)
 */
 
-#include "gdb/regs.h"
+#include <string.h>
-#include "gdb/net.h"
+#include <assert.h>
+
+#include "../exceptions.h"
+#include "../fpu.h"
+
+#include "regs.h"
+#include "net.h"
+
+// GDB treats cpsr, fpsr, fpcr as 32-bit integers...
 
 GDB_DECLARE_HANDLER(ReadRegisters)
 {
-    if(ctx->selectedThreadId == 0)
+    ENSURE(ctx->selectedThreadId == currentCoreCtx->coreId);
-        ctx->selectedThreadId = ctx->currentThreadId;
 
-    ThreadContext regs;
+    const ExceptionStackFrame *frame = currentCoreCtx->guestFrame;
-    Result r = svcGetDebugThreadContext(&regs, ctx->debug, ctx->selectedThreadId, THREADCONTEXT_CONTROL_ALL);
+    const FpuRegisterCache *fpuRegCache = fpuReadRegisters();
 
-    if(R_FAILED(r))
+    char *buf = ctx->buffer + 1;
-        return GDB_ReplyErrno(ctx, EPERM);
+    size_t n = 0;
 
-    return GDB_SendHexPacket(ctx, &regs, sizeof(ThreadContext));
+    struct PACKED ALIGN(4) {
+        u64 sp;
+        u64 pc;
+        u32 cpsr;
+    } cpuSprs = {
+        .sp = *exceptionGetSpPtr(frame),
+        .pc = frame->elr_el2,
+        .cpsr = (u32)frame->spsr_el2,
+    };
+    static_assert(sizeof(cpuSprs) == 12, "sizeof(cpuSprs) != 12");
+
+    u32 fpuSprs[2] = {
+        (u32)fpuRegCache->fpsr,
+        (u32)fpuRegCache->fpcr,
+    };
+
+
+    n += GDB_EncodeHex(buf + n, frame->x, sizeof(frame->x));
+    n += GDB_EncodeHex(buf + n, &cpuSprs, sizeof(cpuSprs));
+    n += GDB_EncodeHex(buf + n, fpuRegCache->q, sizeof(fpuRegCache->q));
+    n += GDB_EncodeHex(buf + n, fpuSprs, sizeof(fpuSprs));
+
+    return GDB_SendPacket(ctx, buf, n);
 }
 
 GDB_DECLARE_HANDLER(WriteRegisters)
 {
-    if(ctx->selectedThreadId == 0)
+    ENSURE(ctx->selectedThreadId == currentCoreCtx->coreId);
-        ctx->selectedThreadId = ctx->currentThreadId;
 
-    ThreadContext regs;
+    ExceptionStackFrame *frame = currentCoreCtx->guestFrame;
+    FpuRegisterCache *fpuRegCache = fpuGetRegisterCache();
 
-    if(GDB_DecodeHex(&regs, ctx->commandData, sizeof(ThreadContext)) != sizeof(ThreadContext))
+    char *buf = ctx->commandData;
+    char *tmp = ctx->workBuffer;
+
+    size_t n = 0;
+    size_t m = 0;
+
+    struct PACKED ALIGN(4) {
+        u64 sp;
+        u64 pc;
+        u32 cpsr;
+    } cpuSprs;
+    static_assert(sizeof(cpuSprs) == 12, "sizeof(cpuSprs) != 12");
+
+    u32 fpuSprs[2];
+
+    struct {
+        void *dst;
+        size_t sz;
+    } info[4] = {
+        { frame->x,         sizeof(frame->x)        },
+        { &cpuSprs,         sizeof(cpuSprs)         },
+        { fpuRegCache->q,   sizeof(fpuRegCache->q)  },
+        { fpuSprs,          sizeof(fpuSprs)         },
+    };
+
+    // Parse & return on error
+    for (u32 i = 0; i < 4; i++) {
+        if (GDB_DecodeHex(tmp + m, buf + n, info[i].sz) != info[i].sz) {
             return GDB_ReplyErrno(ctx, EPERM);
+        }
+        n += 2 * info[i].sz;
+        m += info[i].sz;
+    }
+
+    // Copy. Note: we don't check if cpsr (spsr_el2) was modified to return to EL2...
+    m = 0;
+    for (u32 i = 0; i < 4; i++) {
+        memcpy(info[i].dst, tmp + m, info[i].sz);
+        m += info[i].sz;
+    }
+    *exceptionGetSpPtr(frame) = cpuSprs.sp;
+    frame->elr_el2 = cpuSprs.pc;
+    frame->spsr_el2 = cpuSprs.cpsr;
+    fpuRegCache->fpsr = fpuSprs[0];
+    fpuRegCache->fpcr = fpuSprs[1];
+    fpuCommitRegisters();
 
-    Result r = svcSetDebugThreadContext(ctx->debug, ctx->selectedThreadId, &regs, THREADCONTEXT_CONTROL_ALL);
-    if(R_FAILED(r))
-        return GDB_ReplyErrno(ctx, EPERM);
-    else
     return GDB_ReplyOk(ctx);
 }
 
-static u32 GDB_ConvertRegisterNumber(ThreadContextControlFlags *flags, u32 gdbNum)
+static void GDB_GetRegisterPointerAndSize(size_t *outSz, void **outPtr, unsigned long id, ExceptionStackFrame *frame, FpuRegisterCache *fpuRegCache)
 {
-    if(gdbNum <= 15)
+    switch (id) {
-    {
+        case 0 ... 30:
-        *flags = (gdbNum >= 13) ? THREADCONTEXT_CONTROL_CPU_SPRS : THREADCONTEXT_CONTROL_CPU_GPRS;
+            *outPtr = &frame->x[id];
-        return gdbNum;
+            *outSz = 8;
-    }
+            break;
-    else if(gdbNum == 25)
+        case 31:
-    {
+            *outPtr = exceptionGetSpPtr(frame);
-        *flags = THREADCONTEXT_CONTROL_CPU_SPRS;
+            *outSz = 8;
-        return 16;
+            break;
-    }
+        case 32:
-    else if(gdbNum >= 26 && gdbNum <= 41)
+            *outPtr = &frame->spsr_el2;
-    {
+            *outSz = 4;
-        *flags = THREADCONTEXT_CONTROL_FPU_GPRS;
+            break;
-        return gdbNum - 26;
+        case 33 ... 64:
-    }
+            *outPtr = &fpuRegCache->q[id - 33];
-    else if(gdbNum == 42 || gdbNum == 43)
+            *outSz = 16;
-    {
+        case 65:
-        *flags = THREADCONTEXT_CONTROL_FPU_SPRS;
+            *outPtr = &fpuRegCache->fpsr;
-        return gdbNum - 42;
+            *outSz = 4;
-    }
+        case 66:
-    else
+            *outPtr = &fpuRegCache->fpcr;
-    {
+            *outSz = 4;
-        *flags = (ThreadContextControlFlags)0;
+        default:
-        return 0;
+            __builtin_unreachable();
+            return;
     }
 }
 
 GDB_DECLARE_HANDLER(ReadRegister)
 {
-    if(ctx->selectedThreadId == 0)
+    ENSURE(ctx->selectedThreadId == currentCoreCtx->coreId);
-        ctx->selectedThreadId = ctx->currentThreadId;
 
-    ThreadContext regs;
+    const ExceptionStackFrame *frame = currentCoreCtx->guestFrame;
-    ThreadContextControlFlags flags;
+    const FpuRegisterCache *fpuRegCache = NULL;
-    u32 gdbRegNum;
 
-    if(GDB_ParseHexIntegerList(&gdbRegNum, ctx->commandData, 1, 0) == NULL)
+    char *buf = ctx->buffer + 1;
+    unsigned long gdbRegNum;
+
+    if (GDB_ParseHexIntegerList(&gdbRegNum, ctx->commandData, 1, 0) == NULL) {
         return GDB_ReplyErrno(ctx, EILSEQ);
+    }
 
-    u32 n = GDB_ConvertRegisterNumber(&flags, gdbRegNum);
+    // Check the register number
-    if(!flags)
+    if (gdbRegNum >= 31 + 3 + 32 + 2) {
         return GDB_ReplyErrno(ctx, EINVAL);
+    }
 
-    Result r = svcGetDebugThreadContext(&regs, ctx->debug, ctx->selectedThreadId, flags);
+    if (gdbRegNum > 31 + 3) {
+        // FPU register -- must read the FPU registers first
+        fpuRegCache = fpuReadRegisters();
+    }
 
-    if(R_FAILED(r))
+    size_t sz;
-        return GDB_ReplyErrno(ctx, EPERM);
+    void *regPtr;
+    GDB_GetRegisterPointerAndSize(&sz, &regPtr, gdbRegNum, frame, fpuRegCache);
 
-    if(flags & THREADCONTEXT_CONTROL_CPU_GPRS)
+    return GDB_SendHexPacket(ctx, regPtr, sz);
-        return GDB_SendHexPacket(ctx, &regs.cpu_registers.r[n], 4);
-    else if(flags & THREADCONTEXT_CONTROL_CPU_SPRS)
-        return GDB_SendHexPacket(ctx, &regs.cpu_registers.sp + (n - 13), 4); // hacky
-    else if(flags & THREADCONTEXT_CONTROL_FPU_GPRS)
-        return GDB_SendHexPacket(ctx, &regs.fpu_registers.d[n], 8);
-    else
-        return GDB_SendHexPacket(ctx, &regs.fpu_registers.fpscr + n, 4); // hacky
 }
 
 GDB_DECLARE_HANDLER(WriteRegister)
 {
-    if(ctx->selectedThreadId == 0)
+    ENSURE(ctx->selectedThreadId == currentCoreCtx->coreId);
-        ctx->selectedThreadId = ctx->currentThreadId;
 
-    ThreadContext regs;
+    ExceptionStackFrame *frame = currentCoreCtx->guestFrame;
-    ThreadContextControlFlags flags;
+    FpuRegisterCache *fpuRegCache = fpuGetRegisterCache();
-    u32 gdbRegNum;
+
+    char *tmp = ctx->workBuffer;
+    unsigned long gdbRegNum;
 
     const char *valueStart = GDB_ParseHexIntegerList(&gdbRegNum, ctx->commandData, 1, '=');
-    if(valueStart == NULL || *valueStart != '=')
+    if(valueStart == NULL || *valueStart != '=') {
         return GDB_ReplyErrno(ctx, EILSEQ);
-
+    }
     valueStart++;
 
-    u32 n = GDB_ConvertRegisterNumber(&flags, gdbRegNum);
+    // Check the register number
-    u32 value;
+    if (gdbRegNum >= 31 + 3 + 32 + 2) {
-    u64 value64;
-
-    if(flags & THREADCONTEXT_CONTROL_FPU_GPRS)
-    {
-        if(GDB_DecodeHex(&value64, valueStart, 8) != 8 || valueStart[16] != 0)
         return GDB_ReplyErrno(ctx, EINVAL);
     }
-    else if(flags)
+
-    {
+    size_t sz;
-       if(GDB_DecodeHex(&value, valueStart, 4) != 4 || valueStart[8] != 0)
+    void *regPtr;
-            return GDB_ReplyErrno(ctx, EINVAL);
+    GDB_GetRegisterPointerAndSize(&sz, &regPtr, gdbRegNum, frame, fpuRegCache);
+
+    // Check if we got 2 hex digits per byte
+    if (strlen(valueStart) != 2 * sz) {
+        return GDB_ReplyErrno(ctx, EILSEQ);
     }
-    else
-        return GDB_ReplyErrno(ctx, EINVAL);
 
-    Result r = svcGetDebugThreadContext(&regs, ctx->debug, ctx->selectedThreadId, flags);
+    // Decode, check for errors
+    if (GDB_DecodeHex(tmp, valueStart, sz) != sz) {
+        return GDB_ReplyErrno(ctx, EILSEQ);
+    }
 
-    if(R_FAILED(r))
+    memcpy(regPtr, tmp, sz);
-        return GDB_ReplyErrno(ctx, EPERM);
 
-    if(flags & THREADCONTEXT_CONTROL_CPU_GPRS)
+    if (gdbRegNum > 31 + 3) {
-        regs.cpu_registers.r[n] = value;
+        // FPU register -- must commit the FPU registers
-    else if(flags & THREADCONTEXT_CONTROL_CPU_SPRS)
+        fpuCommitRegisters();
-        *(&regs.cpu_registers.sp + (n - 13)) = value; // hacky
+    }
-    else if(flags & THREADCONTEXT_CONTROL_FPU_GPRS)
-        memcpy(&regs.fpu_registers.d[n], &value64, 8);
-    else
-        *(&regs.fpu_registers.fpscr + n) = value; // hacky
 
-    r = svcSetDebugThreadContext(ctx->debug, ctx->selectedThreadId, &regs, flags);
-    if(R_FAILED(r))
-        return GDB_ReplyErrno(ctx, EPERM);
     else
         return GDB_ReplyOk(ctx);
 }

thermosphere/src/gdb/regs.h

@@ -7,7 +7,7 @@
 
 #pragma once
 
-#include "gdb.h"
+#include "../gdb.h"
 
 GDB_DECLARE_HANDLER(ReadRegisters);
 GDB_DECLARE_HANDLER(WriteRegisters);