Add configUSE_TASK_FPU_SUPPORT to AARCH64 port (#1048)

* 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

@@ -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 = portINITIAL_PSTATE;
-    pxTopOfStack--;
 
-    *pxTopOfStack = ( StackType_t ) pxCode; /* Exception return address. */
     pxTopOfStack--;
+    *pxTopOfStack = portINITIAL_PSTATE;
 
-    /* The task will start with a critical nesting count of 0 as interrupts are
-     * enabled. */
-    *pxTopOfStack = portNO_CRITICAL_NESTING;
     pxTopOfStack--;
+    *pxTopOfStack = ( StackType_t ) pxCode; /* Exception return address. */
 
-    /* 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;
+    #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--;
+        *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;
 }
@@ -440,15 +468,19 @@ void FreeRTOS_Tick_Handler( void )
 }
 /*-----------------------------------------------------------*/
 
-void vPortTaskUsesFPU( void )
-{
-    /* A task is registering the fact that it needs an FPU context.  Set the
-     * FPU flag (which is saved as part of the task context). */
-    ullPortTaskHasFPUContext = pdTRUE;
+#if ( configUSE_TASK_FPU_SUPPORT != 2 )
 
-    /* Consider initialising the FPSR here - but probably not necessary in
-     * AArch64. */
-}
+    void vPortTaskUsesFPU( void )
+    {
+        /* A task is registering the fact that it needs an FPU context.  Set the
+         * FPU flag (which is saved as part of the task context). */
+        ullPortTaskHasFPUContext = pdTRUE;
+
+        /* Consider initialising the FPSR here - but probably not necessary in
+         * AArch64. */
+    }
+
+#endif /* configUSE_TASK_FPU_SUPPORT */
 /*-----------------------------------------------------------*/
 
 void vPortClearInterruptMask( UBaseType_t uxNewMaskValue )

portable/GCC/ARM_AARCH64/portmacro.h

@@ -135,9 +135,18 @@ extern void vPortInstallFreeRTOSVectorTable( void );
  * handler for whichever peripheral is used to generate the RTOS tick. */
 void FreeRTOS_Tick_Handler( void );
 
-/* Any task that uses the floating point unit MUST call vPortTaskUsesFPU()
- * before any floating point instructions are executed. */
-void vPortTaskUsesFPU( void );
+/* If configUSE_TASK_FPU_SUPPORT is set to 1 (or left undefined) then tasks are
+ * created without an FPU context and must call vPortTaskUsesFPU() to give
+ * themselves an FPU context before using any FPU instructions.  If
+ * configUSE_TASK_FPU_SUPPORT is set to 2 then all tasks will have an FPU context
+ * by default. */
+#if ( configUSE_TASK_FPU_SUPPORT != 2 )
+    void vPortTaskUsesFPU( void );
+#else
+    /* Each task has an FPU context already, so define this function away to
+     * nothing to prevent it from being called accidentally. */
+    #define vPortTaskUsesFPU()
+#endif
 #define portTASK_USES_FLOATING_POINT()    vPortTaskUsesFPU()
 
 #define portLOWEST_INTERRUPT_PRIORITY           ( ( ( uint32_t ) configUNIQUE_INTERRUPT_PRIORITIES ) - 1UL )