[PATCH] arm: Fix stack alignment during interrupt handling

[Sebastian Huber]

On a public interface, the stack pointer must be aligned on an 8-byte
boundary.  However, it may temporarily be only aligned on a 4-byte
boundary.  The interrupt handling code must ensure that the stack
pointer is properly aligned before it calls a function.  See also:

https://developer.arm.com/documentation/den0013/d/Interrupt-Handling/External-interrupt-requests/Nested-interrupt-handling

Close #4579.
---
 cpukit/score/cpu/arm/arm_exc_interrupt.S | 34 ++++++++++++------------
 1 file changed, 17 insertions(+), 17 deletions(-)

diff --git a/cpukit/score/cpu/arm/arm_exc_interrupt.S b/cpukit/score/cpu/arm/arm_exc_interrupt.S
index ddcaf945b5..43568747b1 100644
--- a/cpukit/score/cpu/arm/arm_exc_interrupt.S
+++ b/cpukit/score/cpu/arm/arm_exc_interrupt.S
@@ -7,7 +7,7 @@
  */
 
 /*
- * Copyright (c) 2009, 2016 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2009, 2022 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -68,9 +68,8 @@ _ARMV4_Exception_interrupt:
 	/*
 	 * Save context.  We save the link register separately because it has
 	 * to be restored in SVC mode.  The other registers can be restored in
-	 * INT mode.  Ensure that stack remains 8 byte aligned.  Use register
-	 * necessary for the stack alignment for the stack pointer of the
-	 * interrupted context.
+	 * INT mode.  Provide a non-volatile scratch register which is used
+	 * accross function calls.
 	 */
 	stmdb	sp!, CONTEXT_LIST
 	stmdb	sp!, {NON_VOLATILE_SCRATCH, lr}
@@ -102,9 +101,6 @@ _ARMV4_Exception_interrupt:
 	cmp	r2, #0
 	moveq	sp, r1
 
-	/* Switch to Thumb-2 instructions if necessary */
-	SWITCH_FROM_ARM_TO_THUMB_2	r1
-
 	/* Increment interrupt nest and thread dispatch disable level */
 	ldr	r3, [SELF_CPU_CONTROL, #PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL]
 	add	r2, #1
@@ -139,9 +135,6 @@ _ARMV4_Exception_interrupt:
 	/* Restore stack pointer */
 	mov	sp, NON_VOLATILE_SCRATCH
 
-	/* Save CPSR in non-volatile register */
-	mrs	NON_VOLATILE_SCRATCH, CPSR
-
 	/* Decrement levels and determine thread dispatch state */
 	eor	r1, r0
 	sub	r0, #1
@@ -160,9 +153,11 @@ _ARMV4_Exception_interrupt:
 	cmp	r1, #0
 	bne	.Lthread_dispatch_done
 
-	/* Thread dispatch */
+	/* Save CPSR in non-volatile register */
 	mrs	NON_VOLATILE_SCRATCH, CPSR
 
+	/* Thread dispatch */
+
 .Ldo_thread_dispatch:
 
 	/* Set ISR dispatch disable and thread dispatch disable level to one */
@@ -170,19 +165,27 @@ _ARMV4_Exception_interrupt:
 	str	r0, [SELF_CPU_CONTROL, #PER_CPU_ISR_DISPATCH_DISABLE]
 	str	r0, [SELF_CPU_CONTROL, #PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL]
 
-	/* Call _Thread_Do_dispatch(), this function will enable interrupts */
+	/*
+	 * Call _Thread_Do_dispatch(), this function will enable interrupts.
+	 * Make sure that the stack pointer is aligned on an 8-byte boundary.
+	 */
 	mov	r0, SELF_CPU_CONTROL
 	mov	r1, NON_VOLATILE_SCRATCH
 	mov	r2, #0x80
 	bic	r1, r2
+	and	SELF_CPU_CONTROL, sp, #0x4
+	sub	sp, sp, SELF_CPU_CONTROL
 	BLX_TO_THUMB_1	_Thread_Do_dispatch
 
 	/* Disable interrupts */
 	msr	CPSR, NON_VOLATILE_SCRATCH
 
-#ifdef RTEMS_SMP
+	/*
+	 * Restore stack pointer and reinitialize SELF_CPU_CONTROL.  In SMP
+	 * configurations, we may run on a different processor.
+	 */
+	add	sp, sp, SELF_CPU_CONTROL
 	GET_SELF_CPU_CONTROL	SELF_CPU_CONTROL
-#endif
 
 	/* Check if we have to do the thread dispatch again */
 	ldrb	r0, [SELF_CPU_CONTROL, #PER_CPU_DISPATCH_NEEDED]
@@ -195,9 +198,6 @@ _ARMV4_Exception_interrupt:
 
 .Lthread_dispatch_done:
 
-	/* Switch to ARM instructions if necessary */
-	SWITCH_FROM_THUMB_2_TO_ARM
-
 #ifdef ARM_MULTILIB_VFP
 	/* Restore VFP context */
 	ldmia	sp!, {r0, r1}
-- 
2.31.1