AArch32: Print ASM_ASSERT and panic messages

ASM_ASSERT failure and panic messages are suppressed at present. This
patch enables printing the PC location for panic messages, and file name
and line number upon assembly assert failure.

Change-Id: I80cb715988e7ce766f64da1e1d7065a74a096a0c
Signed-off-by: Jeenu Viswambharan <jeenu.viswambharan@arm.com>

common/aarch32/debug.S

@@ -31,14 +31,48 @@
 #include <arch.h>
 #include <asm_macros.S>
 
+	.globl	asm_assert
 	.globl	do_panic
 	.globl	report_exception
 
+/* Since the max decimal input number is 65536 */
+#define MAX_DEC_DIVISOR		10000
+
+/* The offset to add to get ascii for numerals '0 - 9' */
+#define ASCII_OFFSET_NUM	'0'
+
+	.section .rodata.panic_str, "aS"
+panic_msg:
+	.asciz "PANIC at PC : 0x"
+panic_end:
+	.asciz "\r\n"
+
 	/***********************************************************
 	 * The common implementation of do_panic for all BL stages
 	 ***********************************************************/
 func do_panic
-	no_ret	plat_panic_handler
+	/* Have LR copy point to PC at the time of panic */
+	sub	r6, lr, #4
+
+	/* Initialize crash console and verify success */
+	bl	plat_crash_console_init
+	cmp	r0, #0
+	beq	1f
+
+	/* Print panic message */
+	ldr	r4, =panic_msg
+	bl	asm_print_str
+
+	/* Print LR in hex */
+	mov	r4, r6
+	bl	asm_print_hex
+
+	/* Print new line */
+	ldr	r4, =panic_end
+	bl	asm_print_str
+1:
+	mov	lr, r6
+	b	plat_panic_handler
 endfunc do_panic
 
 	/***********************************************************
@@ -52,3 +86,103 @@ func report_exception
 	bl	plat_report_exception
 	no_ret	plat_panic_handler
 endfunc report_exception
+
+#if ASM_ASSERTION
+.section .rodata.assert_str, "aS"
+assert_msg1:
+	.asciz "ASSERT: File "
+assert_msg2:
+	.asciz " Line "
+
+/* ---------------------------------------------------------------------------
+ * Assertion support in assembly.
+ * The below function helps to support assertions in assembly where we do not
+ * have a C runtime stack. Arguments to the function are :
+ * r0 - File name
+ * r1 - Line no
+ * Clobber list : lr, r0 - r6
+ * ---------------------------------------------------------------------------
+ */
+func asm_assert
+	/* Stash the parameters already in r0 and r1 */
+	mov	r5, r0
+	mov	r6, r1
+
+	/* Initialize crash console and verify success */
+	bl	plat_crash_console_init
+	cmp	r0, #0
+	beq	1f
+
+	/* Print file name */
+	ldr	r4, =assert_msg1
+	bl	asm_print_str
+	mov	r4, r5
+	bl	asm_print_str
+
+	/* Print line number string */
+	ldr	r4, =assert_msg2
+	bl	asm_print_str
+
+	/* Test for maximum supported line number */
+	ldr	r4, =~0xffff
+	tst	r6, r4
+	bne	1f
+	mov	r4, r6
+
+	/* Print line number in decimal */
+	mov	r6, #10			/* Divide by 10 after every loop iteration */
+	ldr	r5, =MAX_DEC_DIVISOR
+dec_print_loop:
+	udiv	r0, r4, r5			/* Quotient */
+	mls	r4, r0, r5, r4			/* Remainder */
+	add	r0, r0, #ASCII_OFFSET_NUM	/* Convert to ASCII */
+	bl	plat_crash_console_putc
+	udiv	r5, r5, r6			/* Reduce divisor */
+	cmp	r5, #0
+	bne	dec_print_loop
+1:
+	no_ret	plat_panic_handler
+endfunc asm_assert
+#endif
+
+/*
+ * This function prints a string from address in r4
+ * Clobber: lr, r0 - r4
+ */
+func asm_print_str
+	mov	r3, lr
+1:
+	ldrb	r0, [r4], #0x1
+	cmp	r0, #0
+	beq	2f
+	bl	plat_crash_console_putc
+	b	1b
+2:
+	bx	r3
+endfunc asm_print_str
+
+/*
+ * This function prints a hexadecimal number in r4.
+ * In: r4 = the hexadecimal to print.
+ * Clobber: lr, r0 - r3, r5
+ */
+func asm_print_hex
+	mov	r3, lr
+	mov	r5, #32  /* No of bits to convert to ascii */
+1:
+	sub	r5, r5, #4
+	lsr	r0, r4, r5
+	and	r0, r0, #0xf
+	cmp	r0, #0xa
+	blo	2f
+	/* Add by 0x27 in addition to ASCII_OFFSET_NUM
+	 * to get ascii for characters 'a - f'.
+	 */
+	add	r0, r0, #0x27
+2:
+	add	r0, r0, #ASCII_OFFSET_NUM
+	bl	plat_crash_console_putc
+	cmp	r5, #0
+	bne	1b
+	bx	r3
+endfunc asm_print_hex

include/common/aarch32/assert_macros.S

@@ -43,8 +43,8 @@
 .endif ;\
 	b##_cc	300f ;\
 	ldr	r0, =.L_assert_filename ;\
-	mov	r1, #__LINE__ ;\
-	b	. ;\
+	ldr	r1, =__LINE__ ;\
+	b	asm_assert;\
 300:
 
 #endif /* __ASSERT_MACROS_S__ */

plat/arm/common/aarch32/arm_helpers.S

@@ -31,6 +31,8 @@
 #include <platform_def.h>
 
 	.weak	plat_arm_calc_core_pos
+	.weak	plat_crash_console_init
+	.weak	plat_crash_console_putc
 	.weak	plat_my_core_pos
 
 	/* -----------------------------------------------------
@@ -57,3 +59,29 @@ func plat_arm_calc_core_pos
 	add	r0, r1, r0, LSR #6
 	bx	lr
 endfunc plat_arm_calc_core_pos
+
+	/* ---------------------------------------------
+	 * int plat_crash_console_init(void)
+	 * Function to initialize the crash console
+	 * without a C Runtime to print crash report.
+	 * Clobber list : r0 - r3
+	 * ---------------------------------------------
+	 */
+func plat_crash_console_init
+	ldr	r0, =PLAT_ARM_CRASH_UART_BASE
+	ldr	r1, =PLAT_ARM_CRASH_UART_CLK_IN_HZ
+	ldr	r2, =ARM_CONSOLE_BAUDRATE
+	b	console_core_init
+endfunc plat_crash_console_init
+
+	/* ---------------------------------------------
+	 * int plat_crash_console_putc(int c)
+	 * Function to print a character on the crash
+	 * console without a C Runtime.
+	 * Clobber list : r1 - r2
+	 * ---------------------------------------------
+	 */
+func plat_crash_console_putc
+	ldr	r1, =PLAT_ARM_CRASH_UART_BASE
+	b	console_core_putc
+endfunc plat_crash_console_putc