From b0ae19a6f931827b50a44f68d0e6cb2ec0f4a3f3 Mon Sep 17 00:00:00 2001
From: TuxSH <1922548+TuxSH@users.noreply.github.com>
Date: Sat, 1 Feb 2020 23:42:08 +0000
Subject: [PATCH] thermosphere: better self-debug fault reporting

---
 thermosphere/src/exceptions.c | 18 ++++++++++++------
 thermosphere/src/memory_map.h |  2 ++
 2 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/thermosphere/src/exceptions.c b/thermosphere/src/exceptions.c
index 8d107ff21..5b17a3d69 100644
--- a/thermosphere/src/exceptions.c
+++ b/thermosphere/src/exceptions.c
@@ -26,6 +26,7 @@
 #include "spinlock.h"
 #include "debug_manager.h"
 #include "timer.h"
+#include "memory_map.h"
 
 #include "fpu.h"
 
@@ -56,6 +57,8 @@ bool spsrEvaluateConditionCode(u64 spsr, u32 conditionCode)
 void dumpStackFrame(const ExceptionStackFrame *frame, bool sameEl)
 {
 #ifndef NDEBUG
+    uintptr_t stackTop = memoryMapGetStackTop(currentCoreCtx->coreId);
+
     for (u32 i = 0; i < 30; i += 2) {
         DEBUG("x%u\t\t%016llx\t\tx%u\t\t%016llx\n", i, frame->x[i], i + 1, frame->x[i + 1]);
     }
@@ -74,15 +77,18 @@ 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 {
+    } else if ((frame->sp_el2 & ~0xFFFul) + 0x1000 == stackTop) {
         // Try to dump the stack (comment if this crashes)
-        u64 *sp = (u64 *)(frame->sp_el2 - 8);
-        u64 *spEnd = (u64 *)((frame->sp_el2 & ~0xFFF) + 0x1000);
+        u64 *sp = (u64 *)frame->sp_el2;
+        u64 *spEnd = sp + 0x20;
+        u64 *spMax = (u64 *)((frame->sp_el2 + 0xFFF) & ~0xFFFul);
         DEBUG("Stack trace:\n");
-        while (sp < spEnd) {
-            DEBUG("\t%016lx\n", *--sp);
+        while (sp < spEnd && sp < spMax) {
+            DEBUG("\t%016lx\n", *sp++);
         }
-    }*/
+    } else {
+        DEBUG("Stack overflow/double fault detected!\n");
+    }
 #else
     (void)frame;
     (void)sameEl;
diff --git a/thermosphere/src/memory_map.h b/thermosphere/src/memory_map.h
index 74d8f3a12..75e32ae07 100644
--- a/thermosphere/src/memory_map.h
+++ b/thermosphere/src/memory_map.h
@@ -60,5 +60,7 @@ typedef struct LoadImageLayout {
 
 extern LoadImageLayout g_loadImageLayout;
 
+uintptr_t memoryMapGetStackTop(u32 coreId);
+
 // Non-reentrant
 uintptr_t memoryMapPlatformMmio(uintptr_t pa, size_t size);