Convert panic() to C code

Move the implementation of panic into C code. Only a very small part
needs to be in assembler, and the reset is easier to maintain as C.

As part of this, define panic_putc() and panic_puts() which directly
wite to the UART.

To make things more convenience for the future, add a simple printf()
implementation in the panic path. This is not reliant on the uart
buffering system being in a happy state. However, we do call the
emergency flush so that our panic message will appear after previous
output rather that surpressing it (which would be extremely confusing).

Code/data size for panic.o grows by about 200 bytes, but this is mostly
due to the increased flexibility.

   text	   data	    bss	    dec	    hex	filename
    292	    272	      0	    564	    234	old panic.S
    692	      3	     48	    743	    2e7	new panic.c

BUG=chrome-os-partner:10146
TEST=manual:
build and boot on snow:
> rw 0x06000000

=== EXCEPTION: 03 ====== xPSR: 01000000 ===========
r0 :0000000b r1 :00000047 r2 :06000000 r3 :200013dd
r4 :00000000 r5 :080052cc r6 :200013d0 r7 :00000002
r8 :00000000 r9 :200013de r10:00000000 r11:00000000
r12:00000000 sp :200009a0 lr :08002a5d pc :08003962

Rebooting...

Change-Id: If3e3f572e0f32af780b6ebda235b1b3cde4de5e4
Signed-off-by: Simon Glass <sjg@chromium.org>
Reviewed-on: https://gerrit.chromium.org/gerrit/24503
Reviewed-by: Vincent Palatin <vpalatin@chromium.org>

3 files changed, 216 insertions, 110 deletions

diff --git a/core/cortex-m/init.S b/core/cortex-m/init.S
index bef720d46c..22cece17cb 100644
--- a/core/cortex-m/init.S
+++ b/core/cortex-m/init.S
@@ -368,7 +368,7 @@ fini_loop:
 /* default exception handler */
 .thumb_func
 default_handler:
-    b panic
+    b exception_panic
 
 _bss_start:
     .long __bss_start
diff --git a/core/cortex-m/panic.S b/core/cortex-m/panic.S
deleted file mode 100644
index f9ea888713..0000000000
--- a/core/cortex-m/panic.S
+++ /dev/null
@@ -1,109 +0,0 @@
-/* Copyright (c) 2012 The Chromium OS Authors. All rights reserved.
- * Use of this source code is governed by a BSD-style license that can be
- * found in the LICENSE file.
- *
- * Fatal exception handling and debug tracing
- */
-
-#include "config.h"
-
-.text
-
-.syntax unified
-.code 16
-
-.macro hex_reg r, offset                      @ prepare to build
-    add r1, r3, #\offset                      @ .. hexadecimal string
-    mov r0, \r                                @ .. from the reg value
-    bl buildhex
-.endm
-
-/* fatal exception handler */
-.global panic
-.thumb_func
-panic:
-#ifndef CONFIG_DEBUG
-    b EcSystemReset                           @ Reboot the system
-#else /* CONFIG_DEBUG */
-    /* check that the exception stack pointer is valid */
-    ldr r0, =CONFIG_RAM_BASE                  @ start of RAM
-    ldr r1, =CONFIG_RAM_BASE+CONFIG_RAM_SIZE  @ end of RAM
-    mrs r12, psp                              @ process stack pointer
-    cmp r12, r0                               @ is sp >= RAM start ?
-    it  ge
-    cmpge r1, r12                             @ is sp < RAM end ?
-    blt panic_print                           @ no => no values to read
-    /* output registers value */
-    ldr r3, =msg_excep                        @ pointer to the text buffer
-    mrs r2, ipsr                              @ get exception num from IPSR
-    bfc r2, #9, #23                           @ the exception is the 3 LSB
-    hex_reg r2, 18                            @ prepare hexa for excep number
-    hex_reg r4, 119                           @ prepare hexa for R4
-    hex_reg r5, 132                           @ prepare hexa for R5
-    hex_reg r6, 145                           @ prepare hexa for R6
-    hex_reg r7, 156                           @ prepare hexa for R7
-    hex_reg r8, 171                           @ prepare hexa for R8
-    hex_reg r9, 184                           @ prepare hexa for R9
-    hex_reg r10, 197                          @ prepare hexa for R10
-    hex_reg r11, 210                          @ prepare hexa for R11
-    ldmia r12!, {r4-r11}                      @ load saved r0-r3,r12,lr,pc,psr
-    hex_reg r4, 66                            @ prepare hexa for R0
-    hex_reg r5, 79                            @ prepare hexa for R1
-    hex_reg r6, 92                            @ prepare hexa for R2
-    hex_reg r7, 105                           @ prepare hexa for R3
-    hex_reg r8, 225                           @ prepare hexa for R12
-    hex_reg r12, 238                          @ prepare hexa for SP
-    hex_reg r9, 251                           @ prepare hexa for LR
-    hex_reg r10, 264                          @ prepare hexa for PC
-    hex_reg r11, 40                           @ prepare hexa for xPSR
-    /* print exception trace */
-panic_print:
-    ldr r0, =msg_excep                        @ pointer to the text buffer
-    bl emergency_puts                         @ print the banner
-    mov r0, #0                                @ Soft boot
-    b system_reset                            @ Reboot the system
-
-/* Helpers for exception trace */
-/* print a string on the UART
- * r0: asciiZ string pointer
- */
-emergency_puts:
-    ldr r1, =CONFIG_UART_ADDRESS              @ UART base address
-1:
-    ldrb r3, [r0], #1                         @ read one character
-    cmp r3, #0                                @ end of the string ?
-    beq 3f                                    @ if yes, return
-2:  /* putchar */
-    ldr r2, [r1, #CONFIG_UART_SR_OFFSET]      @ read UART status
-    tst r2, #CONFIG_UART_SR_TXEMPTY           @ free space on TX ?
-    beq 2b                                    @ if no, wait
-    str r3, [r1, #CONFIG_UART_DR_OFFSET]      @ send character to UART DR
-    b 1b                                      @ goto next character
-3:  /* return */
-    bx lr
-
-/* write a number in hexadecimal in a text buffer
- * r0: number to print
- * r1: pointer to *end* of the buffer (filled with '0')
- */
-buildhex:
-    cmp r0, #0
-    it  eq
-    bxeq lr
-    and r2, r0, #0xf
-    cmp r2, #10
-    ite  lt
-    addlt r2, #'0'
-    addge r2, #'A'-10
-    strb r2, [r1],#-1
-    lsr r0, #4
-    b buildhex
-
-.data
-msg_excep: .ascii "\r\n=== EXCEPTION: 00 ====== xPSR: 00000000 ===========\r\n"
-msg_reg0:  .ascii "R0 :00000000 R1 :00000000 R2 :00000000 R3 :00000000\r\n"
-msg_reg1:  .ascii "R4 :00000000 R5 :00000000 R6 :00000000 R7 :00000000\r\n"
-msg_reg2:  .ascii "R8 :00000000 R9 :00000000 R10:00000000 R11:00000000\r\n"
-msg_reg3:  .ascii "R12:00000000 SP :00000000 LR :00000000 PC :00000000\r\n\0"
-.align 4
-#endif /* CONFIG_DEBUG */
diff --git a/core/cortex-m/panic.c b/core/cortex-m/panic.c
new file mode 100644
index 0000000000..cbdef6a96f
--- /dev/null
+++ b/core/cortex-m/panic.c
@@ -0,0 +1,215 @@
+/* Copyright (c) 2012 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include <stdarg.h>
+
+#include "config.h"
+#include "cpu.h"
+#include "panic.h"
+#include "system.h"
+#include "task.h"
+#include "timer.h"
+#include "uart.h"
+#include "util.h"
+#include "watchdog.h"
+
+
+/* We save registers here for display by report_panic() */
+static struct save_area
+{
+	uint32_t saved_regs[11];	/* psp, ipsr, lr, r4-r11 */
+} save_area __attribute__((aligned(8)));
+
+
+void panic_putc(int ch)
+{
+	uart_emergency_flush();
+	if (ch == '\n')
+		panic_putc('\r');
+	uart_write_char(ch);
+	while (uart_tx_ready())
+		;
+}
+
+
+void panic_puts(const char *s)
+{
+	while (*s)
+		panic_putc(*s++);
+}
+
+
+void panic_vprintf(const char *format, va_list args)
+{
+	int pad_width;
+
+	while (*format) {
+		int c = *format++;
+
+		/* Copy normal characters */
+		if (c != '%') {
+			panic_putc(c);
+			continue;
+		}
+
+		/* Get first format character */
+		c = *format++;
+
+		/* Handle %c */
+		if (c == 'c') {
+			c = va_arg(args, int);
+			panic_putc(c);
+			continue;
+		}
+
+		/* Count padding length (only supported for hex) */
+		pad_width = 0;
+		while (c >= '0' && c <= '9') {
+			pad_width = (10 * pad_width) + c - '0';
+			c = *format++;
+		}
+
+		if (c == 's') {
+			char *vstr;
+
+			vstr = va_arg(args, char *);
+			panic_puts(vstr ? vstr : "(null)");
+		} else { /* assume 'x' */
+			uint32_t v, shift;
+			int i;
+
+			v = va_arg(args, uint32_t);
+			if (!pad_width)
+				pad_width = 8;
+			shift = pad_width * 4 - 4;
+			for (i = 0; i < pad_width; i++) {
+				int ch = '0' + ((v >> shift) & 0xf);
+
+				if (ch > '9')
+					ch += 'a' - '9' - 1;
+				panic_putc(ch);
+				shift -= 4;
+			}
+		}
+	}
+}
+
+
+void panic_printf(const char *format, ...)
+{
+	va_list args;
+
+	va_start(args, format);
+	panic_vprintf(format, args);
+	va_end(args);
+}
+
+
+/**
+ * Print the name and value of a register
+ *
+ * This is a convenient helper function for displaying a register value.
+ * It shows the register name in a 3 character field, followed by a colon.
+ * The register value is regs[index], and this is shown in hex. If regs is
+ * NULL, then we display spaces instead.
+ *
+ * After displaying the value, either a space or \n is displayed depending
+ * on the register number, so that (assuming the caller passes all 16
+ * registers in sequence) we put 4 values per line like this
+ *
+ * r0 :0000000b r1 :00000047 r2 :60000000 r3 :200012b5
+ * r4 :00000000 r5 :08004e64 r6 :08004e1c r7 :200012a8
+ * r8 :08004e64 r9 :00000002 r10:00000000 r11:00000000
+ * r12:0000003f sp :200009a0 lr :0800270d pc :0800351a
+ *
+ * @param regnum	Register number to display (0-15)
+ * @param regs		Pointer to array holding the registers, or NULL
+ * @param index		Index into array where the register value is present
+ */
+static void print_reg(int regnum, uint32_t *regs, int index)
+{
+	static const char regname[] = "r10r11r12sp lr pc ";
+	static char rname[3] = "r  ";
+	const char *name;
+
+	rname[1] = '0' + regnum;
+	name = regnum < 10 ? rname : &regname[(regnum - 10) * 3];
+	panic_printf("%c%c%c:", name[0], name[1], name[2]);
+	if (regs)
+		panic_printf("%8x", regs[index]);
+	else
+		panic_puts("        ");
+	panic_putc((regnum & 3) == 3 ? '\n' : ' ');
+}
+
+
+/**
+ * Display a message and reboot
+ */
+static void panic_reboot(void)
+{
+	panic_puts("\n\nRebooting...\n");
+	system_reset(0);
+}
+
+
+void report_panic(const char *msg, uint32_t *lregs)
+{
+	if (msg) {
+		panic_printf("\n** PANIC: %s\n", msg);
+	} else if (lregs) {
+		uint32_t *sregs = NULL;
+		uint32_t psp;
+		int i;
+
+		psp = lregs[0];
+		if (psp >= CONFIG_RAM_BASE
+				&& psp < CONFIG_RAM_BASE + CONFIG_RAM_SIZE)
+			sregs = (uint32_t *)psp;
+		panic_printf("\n=== EXCEPTION: %2x ====== xPSR: %8x "
+			"===========\n", lregs[1] & 7, sregs ? sregs[7] : -1);
+		for (i = 0; i < 4; i++)
+			print_reg(i, sregs, i);
+		for (i = 4; i < 10; i++)
+			print_reg(i, lregs, i - 1);
+		print_reg(10, lregs, 9);
+		print_reg(11, lregs, 10);
+		print_reg(12, sregs, 4);
+		print_reg(13, &psp, 0);
+		print_reg(14, sregs, 5);
+		print_reg(15, sregs, 6);
+	}
+
+	panic_reboot();
+}
+
+/* Default exception handler, which reports a panic */
+void exception_panic(void) __attribute__((naked));
+void exception_panic(void)
+{
+	/* Naked call so we can extract raw LR and IPSR */
+	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, %[save_area]\n"
+		"mrs r1, psp\n"
+		"mrs r2, ipsr\n"
+		"mov r3, lr\n"
+		"stmia r0, {r1-r11}\n"
+		"mov r1, r0\n"
+		"mov r0, #0\n"
+		"b report_panic" : :
+			[save_area] "r" (save_area.saved_regs)
+		);
+}