From a9eda77c22d045dc7f09272b3cba76225177f9f5 Mon Sep 17 00:00:00 2001
From: Yann Gautier <yann.gautier@st.com>
Date: Tue, 15 Sep 2020 12:24:46 +0200
Subject: [PATCH] stm32mp1: update plat_report_exception

In case DEBUG is enabled, plat_report_exception will now display extra
information of the cause of the exception.

Change-Id: I72cc9d180959cbf31c13821dd051eaf4462b733e
Signed-off-by: Yann Gautier <yann.gautier@st.com>
---
 plat/st/stm32mp1/stm32mp1_helper.S | 57 +++++++++++++++++++++++++++++-
 1 file changed, 56 insertions(+), 1 deletion(-)

diff --git a/plat/st/stm32mp1/stm32mp1_helper.S b/plat/st/stm32mp1/stm32mp1_helper.S
index bfcd991a3..b4b699ea4 100644
--- a/plat/st/stm32mp1/stm32mp1_helper.S
+++ b/plat/st/stm32mp1/stm32mp1_helper.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -32,7 +32,48 @@ func platform_mem_init
 endfunc platform_mem_init
 
 func plat_report_exception
+#if DEBUG
+	mov	r8, lr
+
+	/* Test if an abort occurred */
+	cmp	r0, #MODE32_abt
+	bne	undef_inst_lbl
+	ldr	r4, =abort_str
+	bl	asm_print_str
+	mrs	r4, lr_abt
+	sub	r4, r4, #4
+	b	print_exception_info
+
+undef_inst_lbl:
+	/* Test for an undefined instruction */
+	cmp	r0, #MODE32_und
+	bne	other_exception_lbl
+	ldr	r4, =undefined_str
+	bl	asm_print_str
+	mrs	r4, lr_und
+	b	print_exception_info
+
+other_exception_lbl:
+	/* Other exceptions */
+	mov	r9, r0
+	ldr	r4, =exception_start_str
+	bl	asm_print_str
+	mov	r4, r9
+	bl	asm_print_hex
+	ldr	r4, =exception_end_str
+	bl	asm_print_str
+	mov	r4, r6
+
+print_exception_info:
+	bl	asm_print_hex
+
+	ldr	r4, =end_error_str
+	bl	asm_print_str
+
+	bx	r8
+#else
 	bx	lr
+#endif
 endfunc plat_report_exception
 
 func plat_reset_handler
@@ -174,3 +215,17 @@ func plat_crash_console_putc
 	ldr	r1, =STM32MP_DEBUG_USART_BASE
 	b	console_stm32_core_putc
 endfunc plat_crash_console_putc
+
+#if DEBUG
+.section .rodata.rev_err_str, "aS"
+abort_str:
+	.asciz "\nAbort at: 0x"
+undefined_str:
+	.asciz "\nUndefined instruction at: 0x"
+exception_start_str:
+	.asciz "\nException mode=0x"
+exception_end_str:
+	.asciz " at: 0x"
+end_error_str:
+	.asciz "\n\r"
+#endif