1 files changed, 29 insertions, 53 deletions

diff --git a/core/cortex-m/panic.c b/core/cortex-m/panic.c
index 1b28aa00bf..122b609ec0 100644
--- a/core/cortex-m/panic.c
+++ b/core/cortex-m/panic.c
@@ -125,7 +125,7 @@ static void print_reg(int regnum, const uint32_t *regs, int index)
  *
  * See B1.5.8 "Exception return behavior" of ARM DDI 0403D for details.
  */
-static int is_exception_in_handler_context(const uint32_t exc_return)
+static int32_t is_frame_in_handler_stack(const uint32_t exc_return)
 {
 	return (exc_return & 0xf) == 1 || (exc_return & 0xf) == 9;
 }
@@ -259,7 +259,7 @@ static uint32_t get_exception_frame_size(const struct panic_data *pdata)
 #ifdef CONFIG_FPU
 	/* CPU uses EXC_RETURN[4] to indicate whether it stored extended
 	 * frame for FPU or not. */
-	if (!(pdata->regs[2] & (1 << 4)))
+	if (!(pdata->regs[11] & (1 << 4)))
 		frame_size += 18 * sizeof(uint32_t);
 #endif
 
@@ -275,7 +275,7 @@ static uint32_t get_process_stack_position(const struct panic_data *pdata)
 {
 	uint32_t psp = pdata->regs[0];
 
-	if (!is_exception_in_handler_context(pdata->regs[2]))
+	if (!is_frame_in_handler_stack(pdata->regs[11]))
 		psp += get_exception_frame_size(pdata);
 
 	return psp;
@@ -298,7 +298,6 @@ static void panic_show_extra(const struct panic_data *pdata)
 	panic_printf("shcsr = %x, ", pdata->shcsr);
 	panic_printf("hfsr = %x, ", pdata->hfsr);
 	panic_printf("dfsr = %x\n", pdata->dfsr);
-	panic_printf("exc_return = %x\n", pdata->regs[2]);
 }
 
 /*
@@ -334,19 +333,21 @@ static void panic_reboot(void)
 	system_reset(0);
 }
 
-/**
+/*
  * Print panic data
  */
 static void panic_print(const struct panic_data *pdata)
 {
 	const uint32_t *lregs = pdata->regs;
 	const uint32_t *sregs = NULL;
+	const int32_t in_handler = is_frame_in_handler_stack(pdata->regs[11]);
 	int i;
 
 	if (pdata->flags & PANIC_DATA_FLAG_FRAME_VALID)
 		sregs = pdata->frame;
 
-	panic_printf("\n=== EXCEPTION: %02x ====== xPSR: %08x ===========\n",
+	panic_printf("\n=== %s EXCEPTION: %02x ====== xPSR: %08x ===\n",
+		     in_handler ? "HANDLER" : "PROCESS",
 		     lregs[1] & 0xff, sregs ? sregs[7] : -1);
 	for (i = 0; i < 4; i++)
 		print_reg(i, sregs, i);
@@ -355,7 +356,7 @@ static void panic_print(const struct panic_data *pdata)
 	print_reg(10, lregs, 9);
 	print_reg(11, lregs, 10);
 	print_reg(12, sregs, 4);
-	print_reg(13, lregs, 0);
+	print_reg(13, lregs, in_handler ? 2 : 0);
 	print_reg(14, sregs, 5);
 	print_reg(15, sregs, 6);
 
@@ -367,7 +368,7 @@ static void panic_print(const struct panic_data *pdata)
 void report_panic(void)
 {
 	struct panic_data *pdata = pdata_ptr;
-	const uint32_t psp = pdata->regs[0];
+	uint32_t sp;
 
 	pdata->magic = PANIC_DATA_MAGIC;
 	pdata->struct_size = sizeof(*pdata);
@@ -376,12 +377,14 @@ void report_panic(void)
 	pdata->flags = 0;
 	pdata->reserved = 0;
 
-	/* If stack is valid, save exception frame */
-	if (!is_exception_in_handler_context(pdata->regs[2]) &&
-	    (psp & 3) == 0 &&
-	    psp >= CONFIG_RAM_BASE &&
-	    psp <= CONFIG_RAM_BASE + CONFIG_RAM_SIZE - 8 * sizeof(uint32_t)) {
-		const uint32_t *sregs = (const uint32_t *)psp;
+	/* Choose the right sp (psp or msp) based on EXC_RETURN value */
+	sp = is_frame_in_handler_stack(pdata->regs[11])
+		? pdata->regs[2] : pdata->regs[0];
+	/* If stack is valid, copy exception frame to pdata */
+	if ((sp & 3) == 0 &&
+	    sp >= CONFIG_RAM_BASE &&
+	    sp <= CONFIG_RAM_BASE + CONFIG_RAM_SIZE - 8 * sizeof(uint32_t)) {
+		const uint32_t *sregs = (const uint32_t *)sp;
 		int i;
 		for (i = 0; i < 8; i++)
 			pdata->frame[i] = sregs[i];
@@ -405,53 +408,26 @@ void report_panic(void)
 	panic_reboot();
 }
 
-/* Default exception handler, which reports a panic */
+/* Default exception handler, which reports a panic.
+ * Naked call so we can extract raw LR and IPSR. */
 void exception_panic(void) __attribute__((naked));
 void exception_panic(void)
 {
-	/* Naked call so we can extract raw LR and IPSR */
-
-	/*
-	 * Set a new stack pointer at the end of RAM, before the saved
-	 * exception data.
-	 */
-	asm volatile(
-		/*
-		 * This instruction will generate ldr rx, [pc, #offset]
-		 * followed by a mov sp, rx.  See below for more explanation.
-		 *
-		 * Oddly, gcc is able to add 4 to the value loaded here to
-		 * compute [pregs] below if the asm blocks are separate, but if
-		 * they are merged it uses two temporary registers and two
-		 * immediate values.
-		 *
-		 * TODO: Save sp somewhere so that we can access exception frame
-		 * when exception happens in handler's context.
-		 */
-		"mov sp, %[pstack]\n" : :
-			[pstack] "r" (pstack_addr)
-		);
-
 	/* Save registers and branch directly to panic handler */
 	asm volatile(
-		/*
-		 * This instruction will generate ldr rx, [pc, #offset]
-		 * followed by a mov r0, rx. It would clearly be better if
-		 * we could get ldr r0, [pc, #offset] but that doesn't seem
-		 * to be supported. Nor does gcc seem to define which
-		 * temporary register it uses. Therefore we put this
-		 * instruction first so that it matters less.
-		 *
-		 * If you see a failure in the panic handler, please check
-		 * the final assembler output here.
-		 */
 		"mov r0, %[pregs]\n"
 		"mrs r1, psp\n"
 		"mrs r2, ipsr\n"
-		"mov r3, lr\n"
-		"stmia r0, {r1-r11}\n"
-		"b report_panic" : :
-			[pregs] "r" (pdata_ptr->regs)
+		"mov r3, sp\n"
+		"stmia r0, {r1-r11, lr}\n"
+		"mov sp, %[pstack]\n"
+		"b report_panic\n" : :
+			[pregs] "r" (pdata_ptr->regs),
+			[pstack] "r" (pstack_addr) :
+			/* Constraints protecting these from being clobbered.
+			 * Gcc should be using r0 & r12 for pregs and pstack. */
+			"r1", "r2", "r3", "r4", "r5", "r6", "r7", "r8", "r9",
+			"r10", "r11", "cc", "memory"
 		);
 }