From 71e2bc9f742e6e914dabbffdc2c4e8fef9c8ac30 Mon Sep 17 00:00:00 2001
From: Andrey Gusakov <dron0gus@gmail.com>
Date: Wed, 22 Jan 2025 18:09:02 +0300
Subject: [PATCH] error_handling: save SP and some values from stack

---
 firmware/controllers/core/error_handling.cpp | 25 ++++++++++++++++++++
 firmware/controllers/core/error_handling.h   |  5 ++++
 2 files changed, 30 insertions(+)

diff --git a/firmware/controllers/core/error_handling.cpp b/firmware/controllers/core/error_handling.cpp
index 87dc6d28d8..50050354c0 100644
--- a/firmware/controllers/core/error_handling.cpp
+++ b/firmware/controllers/core/error_handling.cpp
@@ -197,6 +197,14 @@ do {																\
 	}																\
 } while(0)
 
+#define printErrorStack()											\
+do {																\
+	PRINT("SP 0x%08lx", err->sp);									\
+	for (size_t i = 0; i < ERROR_STACK_DEPTH; i++) {				\
+		PRINT(" 0x%08lx: 0x%08lx", err->sp - i * 4, err->stack[i]);	\
+	}																\
+} while(0)
+
 // TODO: reuse this code for writing crash report file
 void errorHandlerShowBootReasonAndErrors() {
 	//this is console print
@@ -209,6 +217,7 @@ void errorHandlerShowBootReasonAndErrors() {
 	ErrorCookie cookie = err->Cookie;
 
 	printErrorState();
+	printErrorStack();
 #endif // EFI_BACKUP_SRAM
 	#undef PRINT
 }
@@ -276,6 +285,7 @@ void errorHandlerWriteReportFile(FIL *fd, int index) {
 			printResetReason();
 #if EFI_BACKUP_SRAM
 			printErrorState();
+			printErrorStack();
 #endif // EFI_BACKUP_SRAM
 			// additional board-specific data
 			onBoardWriteErrorFile(fd);
@@ -295,6 +305,15 @@ backupErrorState *errorHandlerGetLastErrorDescriptor(void)
 #endif
 }
 
+static void errorHandlerSaveStack(backupErrorState *err, uint32_t *sp)
+{
+	err->sp = (uint32_t)sp;
+	for (size_t i = 0; i < ERROR_STACK_DEPTH; i++) {
+		err->stack[i] = *sp;
+		sp++;
+	}
+}
+
 void logHardFault(uint32_t type, uintptr_t faultAddress, port_extctx* ctx, uint32_t csfr) {
     criticalShutdown();
 #if EFI_BACKUP_SRAM
@@ -315,6 +334,9 @@ void logHardFault(uint32_t type, uintptr_t faultAddress, port_extctx* ctx, uint3
 #if EFI_SIMULATOR || EFI_PROD_CODE
 
 void chDbgPanic3(const char *msg, const char * file, int line) {
+	// following is allocated on stack
+	// add some marker
+	uint32_t tmp = 0xfffffa11;
 #if EFI_PROD_CODE
 	// Attempt to break in to the debugger, if attached
 	if (CoreDebug->DHCSR & CoreDebug_DHCSR_C_DEBUGEN_Msk)
@@ -331,6 +353,9 @@ void chDbgPanic3(const char *msg, const char * file, int line) {
 		err->line = line;
 		strlncpy(err->msg, msg, efi::size(err->msg));
 		err->Cookie = ErrorCookie::ChibiOsPanic;
+		// copy stack last as it can be corrupted and cause another exeption
+		uint32_t *sp = &tmp;
+		errorHandlerSaveStack(err, sp);
 	}
 #endif // EFI_BACKUP_SRAM
 
diff --git a/firmware/controllers/core/error_handling.h b/firmware/controllers/core/error_handling.h
index f4a1f2b638..a97d66f9f7 100644
--- a/firmware/controllers/core/error_handling.h
+++ b/firmware/controllers/core/error_handling.h
@@ -89,6 +89,9 @@ enum class ErrorCookie : uint32_t {
 const char *errorCookieToName(ErrorCookie cookie);
 
 // Error handling/recovery/reporting information
+
+#define ERROR_STACK_DEPTH   32
+
 typedef struct {
     ErrorCookie Cookie;
 
@@ -99,6 +102,8 @@ typedef struct {
     uint32_t FaultType;
     uint32_t FaultAddress;
     uint32_t Csfr;
+    uint32_t sp;
+    uint32_t stack[ERROR_STACK_DEPTH];
 } backupErrorState;
 
 // reads backup ram and checks for any error report