3 files changed, 74 insertions, 0 deletions

diff --git a/core/cortex-m/cpu.c b/core/cortex-m/cpu.c
index ffb6b7780c..5a882bb532 100644
--- a/core/cortex-m/cpu.c
+++ b/core/cortex-m/cpu.c
@@ -9,6 +9,10 @@
 #include "cpu.h"
 #include "hooks.h"
 
+#define STACK_IDX_REG_LR 5
+#define STACK_IDX_REG_PC 6
+#define STACK_IDX_REG_PSR 7
+
 void cpu_init(void)
 {
 	/* Catch divide by 0 and unaligned access */
@@ -20,6 +24,40 @@ void cpu_init(void)
 			  CPU_NVIC_SHCSR_USGFAULTENA;
 }
 
+void cpu_return_from_exception_msp(void (*func)(void))
+{
+	uint32_t *msp;
+
+	__asm__ volatile("mrs %0, msp" : "=r"(msp));
+
+	msp[STACK_IDX_REG_LR] = 0; /* Will never return */
+	msp[STACK_IDX_REG_PC] = (uint32_t)func; /* Return to this function */
+	msp[STACK_IDX_REG_PSR] = (1 << 24); /* Just set thumb mode */
+
+	/* Return from exception using main stack */
+	__asm__ volatile("bx %0" : : "r"(0xFFFFFFF9));
+
+	/* should not reach here */
+	__builtin_unreachable();
+}
+
+void cpu_return_from_exception_psp(void (*func)(void))
+{
+	uint32_t *psp;
+
+	__asm__ volatile("mrs %0, psp" : "=r"(psp));
+
+	psp[STACK_IDX_REG_LR] = 0; /* Will never return */
+	psp[STACK_IDX_REG_PC] = (uint32_t)func; /* Return to this function */
+	psp[STACK_IDX_REG_PSR] = (1 << 24); /* Just set thumb mode */
+
+	/* Return from exception using main stack */
+	__asm__ volatile("bx %0" : : "r"(0xFFFFFFFD));
+
+	/* should not reach here */
+	__builtin_unreachable();
+}
+
 #ifdef CONFIG_ARMV7M_CACHE
 static void cpu_invalidate_icache(void)
 {
diff --git a/core/cortex-m/cpu.h b/core/cortex-m/cpu.h
index 4a36d63dda..d3144006f6 100644
--- a/core/cortex-m/cpu.h
+++ b/core/cortex-m/cpu.h
@@ -127,6 +127,11 @@ void cpu_invalidate_dcache_range(uintptr_t base, unsigned int length);
 /* Clean and Invalidate a single range of the D-cache */
 void cpu_clean_invalidate_dcache_range(uintptr_t base, unsigned int length);
 
+/* Return to specified function from exception handler using main stack. */
+void cpu_return_from_exception_msp(void (*func)(void));
+/* Return to specified function from exception handler using process stack. */
+void cpu_return_from_exception_psp(void (*func)(void));
+
 /* Set the priority of the given IRQ in the NVIC (0 is highest). */
 static inline void cpu_set_interrupt_priority(uint8_t irq, uint8_t priority)
 {
diff --git a/core/cortex-m/panic.c b/core/cortex-m/panic.c
index 1de8376cfb..eefe068931 100644
--- a/core/cortex-m/panic.c
+++ b/core/cortex-m/panic.c
@@ -11,6 +11,7 @@
 #include "panic.h"
 #include "printf.h"
 #include "system.h"
+#include "system_safe_mode.h"
 #include "task.h"
 #include "timer.h"
 #include "uart.h"
@@ -287,6 +288,16 @@ void panic_data_print(const struct panic_data *pdata)
 #endif
 }
 
+/* This is just a placeholder function for returning from exception.
+ * It's not expected to actually be executed.
+ */
+static void exception_return_placeholder(void)
+{
+	panic_printf("Unexpected return from exception\n");
+	panic_reboot();
+	__builtin_unreachable();
+}
+
 void __keep report_panic(void)
 {
 	/*
@@ -353,6 +364,26 @@ void __keep report_panic(void)
 	if (IS_ENABLED(CONFIG_ARMV7M_CACHE))
 		cpu_clean_invalidate_dcache();
 
+	/* Start safe mode if possible */
+	if (IS_ENABLED(CONFIG_SYSTEM_SAFE_MODE)) {
+		/* TODO: check for nested exceptions */
+		if (start_system_safe_mode() == EC_SUCCESS) {
+			/* Return from exception on process stack.
+			 * We should not actually land in
+			 * exception_return_placeholder function. Instead the
+			 * scheduler should interrupt and schedule
+			 * a different task since the current task has
+			 * been disabled.
+			 */
+			pdata->flags |= PANIC_DATA_FLAG_SAFE_MODE_STARTED;
+			cpu_return_from_exception_psp(
+				exception_return_placeholder);
+
+			__builtin_unreachable();
+		}
+		pdata->flags |= PANIC_DATA_FLAG_SAFE_MODE_FAIL_PRECONDITIONS;
+	}
+
 	panic_reboot();
 }