From 6457dce5b1dc37a8e3cd8dfd1163c2ebdd85e5b8 Mon Sep 17 00:00:00 2001
From: TuxSH <tuxsh@sfr.fr>
Date: Sat, 1 Feb 2020 15:11:59 +0000
Subject: [PATCH] thermosphere: add more debugging strings

---
 thermosphere/src/debug_log.c   | 18 ++++++++++++------
 thermosphere/src/debug_log.h   |  5 ++++-
 thermosphere/src/exceptions.c  | 10 +++++++++-
 thermosphere/src/gdb/context.c |  2 +-
 thermosphere/src/gdb/net.c     | 12 +++++++++++-
 5 files changed, 37 insertions(+), 10 deletions(-)

diff --git a/thermosphere/src/debug_log.c b/thermosphere/src/debug_log.c
index e8eb6e497..1627624fe 100644
--- a/thermosphere/src/debug_log.c
+++ b/thermosphere/src/debug_log.c
@@ -15,6 +15,7 @@
  */
 
 #include <stdio.h>
+#include <string.h>
 #include "debug_log.h"
 #include "platform/uart.h"
 #include "semihosting.h"
@@ -35,6 +36,16 @@ void debugLogInit(void)
     }
 }
 
+void debugLogRaw(const char *str)
+{
+    // Use semihosting if available (we assume qemu was launched with -semihosting), otherwise UART
+    if (DLOG_USE_SEMIHOSTING_WRITE0 && semihosting_connection_supported()) {
+        semihosting_write_string(str);
+    } else {
+        transportInterfaceWriteData(g_debugLogTransportInterface, str, strlen(str));
+    }
+}
+
 // NOTE: UNSAFE!
 int debugLog(const char *fmt, ...)
 {
@@ -44,12 +55,7 @@ int debugLog(const char *fmt, ...)
     int res = vsprintf(buf, fmt, args);
     va_end(args);
 
-    // Use semihosting if available (we assume qemu was launched with -semihosting), otherwise UART
-    if (DLOG_USE_SEMIHOSTING_WRITE0 && semihosting_connection_supported()) {
-        semihosting_write_string(buf);
-    } else {
-        transportInterfaceWriteData(g_debugLogTransportInterface, buf, res);
-    }
+    debugLogRaw(buf);
 
     return res;
 }
diff --git a/thermosphere/src/debug_log.h b/thermosphere/src/debug_log.h
index 8076d4f16..0b25a4581 100644
--- a/thermosphere/src/debug_log.h
+++ b/thermosphere/src/debug_log.h
@@ -18,10 +18,13 @@
 #include <stdarg.h>
 
 #ifndef NDEBUG
-#define DEBUG(...)  debugLog(__VA_ARGS__)
+#define DEBUG(...)      debugLog(__VA_ARGS__)
+#define DEBUGRAW(str)   debugLogRaw(str)
 #else
 #define DEBUG(...)
+#define DEBUGRAW(str)
 #endif
 
 void debugLogInit(void);
+void debugLogRaw(const char *str);
 int debugLog(const char *fmt, ...);
diff --git a/thermosphere/src/exceptions.c b/thermosphere/src/exceptions.c
index f5e1ff628..c92194f29 100644
--- a/thermosphere/src/exceptions.c
+++ b/thermosphere/src/exceptions.c
@@ -74,7 +74,15 @@ void dumpStackFrame(const ExceptionStackFrame *frame, bool sameEl)
     if (frame == currentCoreCtx->guestFrame) {
         DEBUG("cntp_ctl_el0\t%016llx\n", frame->cntp_ctl_el0);
         DEBUG("cntv_ctl_el0\t%016llx\n", frame->cntv_ctl_el0);
-    }
+    }/* else {
+        // Try to dump the stack (comment if this crashes)
+        u64 *sp = (u64 *)(frame->sp_el2 - 8);
+        u64 *spEnd = (u64 *)((frame->sp_el2 & ~0xFFF) + 0x1000);
+        DEBUG("Stack trace:\n");
+        while (sp < spEnd) {
+            DEBUG("\t%016lx\n", *--sp);
+        }
+    }*/
 #else
     (void)frame;
     (void)sameEl;
diff --git a/thermosphere/src/gdb/context.c b/thermosphere/src/gdb/context.c
index e4918ecee..7a0b6440f 100644
--- a/thermosphere/src/gdb/context.c
+++ b/thermosphere/src/gdb/context.c
@@ -43,7 +43,7 @@
 #include "../watchpoints.h"
 
 static TEMPORARY char g_gdbWorkBuffer[GDB_WORK_BUF_LEN];
-static TEMPORARY char g_gdbBuffer[GDB_BUF_LEN + 4];
+static TEMPORARY char g_gdbBuffer[GDB_BUF_LEN + 4 + 1];
 
 static const struct{
     char command;
diff --git a/thermosphere/src/gdb/net.c b/thermosphere/src/gdb/net.c
index 2a3bd1aee..d02c9ec47 100644
--- a/thermosphere/src/gdb/net.c
+++ b/thermosphere/src/gdb/net.c
@@ -236,11 +236,14 @@ int GDB_ReceivePacket(GDBContext *ctx)
         ctx->state = GDB_STATE_ATTACHED;
     }
 
+    DEBUGRAW("->");
+    DEBUGRAW(ctx->buffer);
+    DEBUGRAW("\n");
+
     // Set helper attributes, change '#' to NUL
     ctx->commandData = ctx->buffer + 2;
     ctx->commandEnd = ctx->buffer + delimPos;
     ctx->buffer[delimPos] = '\0';
-    DEBUG("Packet: %s\n", ctx->buffer + 1);
 
     return (int)(delimPos + 2);
 }
@@ -249,6 +252,13 @@ static int GDB_DoSendPacket(GDBContext *ctx, size_t len)
 {
     transportInterfaceWriteData(ctx->transportInterface, ctx->buffer, len);
     ctx->lastSentPacketSize = len;
+
+    // Debugging:
+    ctx->buffer[len] = 0;
+    DEBUGRAW("<-");
+    DEBUGRAW(ctx->buffer);
+    DEBUGRAW("\n");
+
     return (int)len;
 }