From d57cbc07d03f343e01386c6f5acbbab2a7c0ecc6 Mon Sep 17 00:00:00 2001
From: Stefan Baltruweit <Stefan.Baltruweit@ifta.com>
Date: Thu, 2 May 2024 18:09:29 +0200
Subject: [PATCH] Add configUSE_TASK_FPU_SUPPORT to AARCH64 port

NEON SIMD is required by standard AARCH64 and its registers are
frequently utilized by standard functions such as memcpy().
This means that even simple tasks that do not use any floating point
arithmetics may still alter the contents of the FPU registers.

For this reason it makes sense to add support for
configUSE_TASK_FPU_SUPPORT to be able to enforce FPU register saving and
restoring globally.

The implementation was largely adopted from the ARM_CA9 port. However,
the FPU registers must be placed on the stack before the critical
nesting count to match the AARCH64 portASM.S.
---
 portable/GCC/ARM_AARCH64/port.c | 50 +++++++++++++++++++++++++--------
 1 file changed, 39 insertions(+), 11 deletions(-)

diff --git a/portable/GCC/ARM_AARCH64/port.c b/portable/GCC/ARM_AARCH64/port.c
index 5b52dadda..259098e68 100644
--- a/portable/GCC/ARM_AARCH64/port.c
+++ b/portable/GCC/ARM_AARCH64/port.c
@@ -133,6 +133,10 @@
 #define portMAX_8_BIT_VALUE                       ( ( uint8_t ) 0xff )
 #define portBIT_0_SET                             ( ( uint8_t ) 0x01 )
 
+/* The space on the stack required to hold the FPU registers.
+ * There are 32 128-bit registers.*/
+#define portFPU_REGISTER_WORDS     ( 32 * 2 )
+
 /*-----------------------------------------------------------*/
 
 /*
@@ -244,23 +248,47 @@ StackType_t * pxPortInitialiseStack( StackType_t * pxTopOfStack,
     *pxTopOfStack = ( StackType_t ) 0x00;         /* XZR - has no effect, used so there are an even number of registers. */
     pxTopOfStack--;
     *pxTopOfStack = ( StackType_t ) 0x00;         /* R30 - procedure call link register. */
-    pxTopOfStack--;
 
+    pxTopOfStack--;
     *pxTopOfStack = portINITIAL_PSTATE;
-    pxTopOfStack--;
 
+    pxTopOfStack--;
     *pxTopOfStack = ( StackType_t ) pxCode; /* Exception return address. */
-    pxTopOfStack--;
 
-    /* The task will start with a critical nesting count of 0 as interrupts are
-     * enabled. */
-    *pxTopOfStack = portNO_CRITICAL_NESTING;
-    pxTopOfStack--;
+    #if ( configUSE_TASK_FPU_SUPPORT == 1 )
+    {
+        /* The task will start with a critical nesting count of 0 as interrupts are
+        * enabled. */
+        pxTopOfStack--;
+        *pxTopOfStack = portNO_CRITICAL_NESTING;
 
-    /* The task will start without a floating point context.  A task that uses
-     * the floating point hardware must call vPortTaskUsesFPU() before executing
-     * any floating point instructions. */
-    *pxTopOfStack = portNO_FLOATING_POINT_CONTEXT;
+        /* The task will start without a floating point context.  A task that
+         * uses the floating point hardware must call vPortTaskUsesFPU() before
+         * executing any floating point instructions. */
+        pxTopOfStack--;
+        *pxTopOfStack = portNO_FLOATING_POINT_CONTEXT;
+    }
+    #elif ( configUSE_TASK_FPU_SUPPORT == 2 )
+    {
+        /* The task will start with a floating point context.  Leave enough
+         * space for the registers - and ensure they are initialised to 0. */
+        pxTopOfStack -= portFPU_REGISTER_WORDS;
+        memset( pxTopOfStack, 0x00, portFPU_REGISTER_WORDS * sizeof( StackType_t ) );
+
+        /* The task will start with a critical nesting count of 0 as interrupts are
+        * enabled. */
+        pxTopOfStack--;
+        *pxTopOfStack = portNO_CRITICAL_NESTING;
+
+        pxTopOfStack--;
+        *pxTopOfStack = pdTRUE;
+        ullPortTaskHasFPUContext = pdTRUE;
+    }
+    #else /* if ( configUSE_TASK_FPU_SUPPORT == 1 ) */
+    {
+        #error "Invalid configUSE_TASK_FPU_SUPPORT setting - configUSE_TASK_FPU_SUPPORT must be set to 1, 2, or left undefined."
+    }
+    #endif /* if ( configUSE_TASK_FPU_SUPPORT == 1 ) */
 
     return pxTopOfStack;
 }