From 1fe4d4537e51f69014be057c39a1b5145e9c5930 Mon Sep 17 00:00:00 2001
From: Sandrine Bailleux <sandrine.bailleux@arm.com>
Date: Tue, 29 Sep 2015 13:38:20 +0100
Subject: [PATCH] Break down BL1 AArch64 synchronous exception handler

The AArch64 synchronous exception vector code in BL1 is almost
reaching its architectural limit of 32 instructions. This means
there is very little space for this code to grow.

This patch reduces the size of the exception vector code by
moving most of its code in a function to which we branch from
SynchronousExceptionA64.

Change-Id: Ib35351767a685fb2c2398029d32e54026194f7ed
---
 bl1/aarch64/bl1_exceptions.S | 93 +++++++++++++++++++-----------------
 1 file changed, 49 insertions(+), 44 deletions(-)

diff --git a/bl1/aarch64/bl1_exceptions.S b/bl1/aarch64/bl1_exceptions.S
index 1ca3a6c61..ca86107c5 100644
--- a/bl1/aarch64/bl1_exceptions.S
+++ b/bl1/aarch64/bl1_exceptions.S
@@ -115,51 +115,13 @@ SynchronousExceptionA64:
 	/* Enable the SError interrupt */
 	msr	daifclr, #DAIF_ABT_BIT
 
-	/* ------------------------------------------------
-	 * Only a single SMC exception from BL2 to ask
-	 * BL1 to pass EL3 control to BL31 is expected
-	 * here.
-	 * It expects X0 with RUN_IMAGE SMC function id
-	 * X1 with address of a entry_point_info_t structure
-	 * describing the BL3-1 entrypoint
-	 * ------------------------------------------------
-	 */
-	mov	x19, x0
-	mov	x20, x1
+	/* Expect only SMC exceptions */
+	mrs	x19, esr_el3
+	ubfx	x20, x19, #ESR_EC_SHIFT, #ESR_EC_LENGTH
+	cmp	x20, #EC_AARCH64_SMC
+	b.ne	unexpected_sync_exception
 
-	mrs	x0, esr_el3
-	ubfx	x1, x0, #ESR_EC_SHIFT, #ESR_EC_LENGTH
-	cmp	x1, #EC_AARCH64_SMC
-	b.ne	panic
-
-	mov	x0, #RUN_IMAGE
-	cmp	x19, x0
-	b.ne	panic
-
-	mov	x0, x20
-	bl	display_boot_progress
-
-	ldp	x0, x1, [x20, #ENTRY_POINT_INFO_PC_OFFSET]
-	msr	elr_el3, x0
-	msr	spsr_el3, x1
-	ubfx	x0, x1, #MODE_EL_SHIFT, #2
-	cmp	x0, #MODE_EL3
-	b.ne	panic
-
-	bl	disable_mmu_icache_el3
-	tlbi	alle3
-
-	ldp	x6, x7, [x20, #(ENTRY_POINT_INFO_ARGS_OFFSET + 0x30)]
-	ldp	x4, x5, [x20, #(ENTRY_POINT_INFO_ARGS_OFFSET + 0x20)]
-	ldp	x2, x3, [x20, #(ENTRY_POINT_INFO_ARGS_OFFSET + 0x10)]
-	ldp	x0, x1, [x20, #(ENTRY_POINT_INFO_ARGS_OFFSET + 0x0)]
-	eret
-panic:
-	mov	x0, #SYNC_EXCEPTION_AARCH64
-	bl	plat_report_exception
-
-	wfi
-	b	panic
+	b	smc_handler64
 	check_vector_size SynchronousExceptionA64
 
 	.align	7
@@ -214,3 +176,46 @@ SErrorA32:
 	bl	plat_report_exception
 	b	SErrorA32
 	check_vector_size SErrorA32
+
+
+func smc_handler64
+	/* ---------------------------------------------------------------------
+	 * Only a single SMC exception from BL2 to ask BL1 to pass EL3 control
+	 * to BL31 is expected here. It expects:
+	 *   - X0 with RUN_IMAGE SMC function ID;
+	 *   - X1 with the address of a entry_point_info_t structure describing
+	 *     the BL31 entrypoint.
+	 * ---------------------------------------------------------------------
+	 */
+	mov	x19, x0
+	mov	x20, x1
+
+	mov	x0, #RUN_IMAGE
+	cmp	x19, x0
+	b.ne	unexpected_sync_exception
+
+	mov	x0, x20
+	bl	display_boot_progress
+
+	ldp	x0, x1, [x20, #ENTRY_POINT_INFO_PC_OFFSET]
+	msr	elr_el3, x0
+	msr	spsr_el3, x1
+	ubfx	x0, x1, #MODE_EL_SHIFT, #2
+	cmp	x0, #MODE_EL3
+	b.ne	unexpected_sync_exception
+
+	bl	disable_mmu_icache_el3
+	tlbi	alle3
+
+	ldp	x6, x7, [x20, #(ENTRY_POINT_INFO_ARGS_OFFSET + 0x30)]
+	ldp	x4, x5, [x20, #(ENTRY_POINT_INFO_ARGS_OFFSET + 0x20)]
+	ldp	x2, x3, [x20, #(ENTRY_POINT_INFO_ARGS_OFFSET + 0x10)]
+	ldp	x0, x1, [x20, #(ENTRY_POINT_INFO_ARGS_OFFSET + 0x0)]
+	eret
+endfunc smc_handler64
+
+unexpected_sync_exception:
+	mov	x0, #SYNC_EXCEPTION_AARCH64
+	bl	plat_report_exception
+	wfi
+	b	unexpected_sync_exception