[PATCH v1 5/7] bsps/aarch64: RTEMS_DEBUG stack alignment faults

[Kinsey Moore]

Run with stack alignment faults enabled under RTEMS_DEBUG to catch any
stack misalignments early. This makes it easier to track them down
should they ever occur.
---
 bsps/aarch64/shared/start/start.S                    |  8 ++++++++
 cpukit/score/cpu/aarch64/aarch64-exception-default.S | 10 ++++++++--
 2 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/bsps/aarch64/shared/start/start.S b/bsps/aarch64/shared/start/start.S
index f4c62b2b6c..e4dd3c2cee 100644
--- a/bsps/aarch64/shared/start/start.S
+++ b/bsps/aarch64/shared/start/start.S
@@ -90,6 +90,14 @@ _start:
 #endif
 #endif
 
+	/* Initialize SCTLR_EL1 */
+	mov x0, XZR
+#if defined(RTEMS_DEBUG)
+	/* Enable Stack alignment checking */
+	orr x0, x0, #(1<<3)
+#endif
+	msr SCTLR_EL1, x0
+
 #ifdef RTEMS_SMP
 	/* Read MPIDR and get current processor index */
         mrs	x7, mpidr_el1
diff --git a/cpukit/score/cpu/aarch64/aarch64-exception-default.S b/cpukit/score/cpu/aarch64/aarch64-exception-default.S
index 970ccf735f..d139fdc6a4 100644
--- a/cpukit/score/cpu/aarch64/aarch64-exception-default.S
+++ b/cpukit/score/cpu/aarch64/aarch64-exception-default.S
@@ -76,7 +76,7 @@
  * TODO(kmoore) The current implementation here assumes that SP is not
  * misaligned.
  */
-	.macro	JUMP_HANDLER
+	.macro	JUMP_HANDLER_SHORT
 /* Mask to use in BIC, lower 7 bits */
 	mov x0, #0x7f
 /* LR contains PC, mask off to the base of the current vector */
@@ -113,6 +113,10 @@
 	nop
 	nop
 	nop
+	.endm
+
+	.macro	JUMP_HANDLER
+	JUMP_HANDLER_SHORT
 	nop
 	.endm
 
@@ -182,10 +186,12 @@ curr_el_sp0_serror_get_pc:		/* The current PC is now in LR */
  * the current SP.
  */
 curr_el_spx_sync:
+	msr SCTLR_EL1, XZR
 	stp x0,	lr,	[sp, #-0x10]!	/* Push x0,lr on to the stack */
 	bl curr_el_spx_sync_get_pc	/* Get current execution address */
 curr_el_spx_sync_get_pc:		/* The current PC is now in LR */
-	JUMP_HANDLER
+/* Use short jump handler since this has an extra instruction to clear SCTLR */
+	JUMP_HANDLER_SHORT
 	JUMP_TARGET_SPx
 .balign 0x80
 /*
-- 
2.20.1