From 553caa18ced4906cf5060823ada7a10e73c7b535 Mon Sep 17 00:00:00 2001
From: RichardBarry <3073890+RichardBarry@users.noreply.github.com>
Date: Sat, 9 Dec 2023 01:19:39 -0800
Subject: [PATCH] Update the memory alignment within the Cortex-A9 port asm
 code (#426)

Update alignment in ARM_CA9 port.
---
 portable/GCC/ARM_CA9/portASM.S | 19 ++++++++++++++++---
 1 file changed, 16 insertions(+), 3 deletions(-)

diff --git a/portable/GCC/ARM_CA9/portASM.S b/portable/GCC/ARM_CA9/portASM.S
index 150cfabad..55baabbd3 100644
--- a/portable/GCC/ARM_CA9/portASM.S
+++ b/portable/GCC/ARM_CA9/portASM.S
@@ -75,9 +75,9 @@
 
     /* Save the floating point context, if any. */
     FMRXNE  R1,  FPSCR
+    PUSHNE  {R1}
     VPUSHNE {D0-D15}
     VPUSHNE {D16-D31}
-    PUSHNE  {R1}
 
     /* Save ulPortTaskHasFPUContext itself. */
     PUSH    {R3}
@@ -106,9 +106,9 @@
     CMP     R1, #0
 
     /* Restore the floating point context, if any. */
-    POPNE   {R0}
     VPOPNE  {D16-D31}
     VPOPNE  {D0-D15}
+    POPNE   {R0}
     VMSRNE  FPSCR, R0
 
     /* Restore the critical section nesting depth. */
@@ -145,8 +145,15 @@
 FreeRTOS_SWI_Handler:
     /* Save the context of the current task and select a new task to run. */
     portSAVE_CONTEXT
+
+    /* Ensure bit 2 of the stack pointer is clear. */
+    MOV     r2, sp
+    AND     r2, r2, #4
+    SUB     sp, sp, r2
+
     LDR R0, vTaskSwitchContextConst
     BLX R0
+
     portRESTORE_CONTEXT
 
 
@@ -256,7 +263,13 @@ switch_before_exit:
     /* Call the function that selects the new task to execute.
     vTaskSwitchContext() if vTaskSwitchContext() uses LDRD or STRD
     instructions, or 8 byte aligned stack allocated data.  LR does not need
-    saving as a new LR will be loaded by portRESTORE_CONTEXT anyway. */
+    saving as a new LR will be loaded by portRESTORE_CONTEXT anyway.
+    Ensure bit 2 of the stack pointer is clear.  r2 holds the bit 2 value for
+    future use. */
+    MOV     r2, sp
+    AND     r2, r2, #4
+    SUB     sp, sp, r2
+
     LDR     R0, vTaskSwitchContextConst
     BLX     R0