From 1b8f5965d360953ebf5559c97418f5f5264bdbee Mon Sep 17 00:00:00 2001 From: kzorer <98871026+kzorer@users.noreply.github.com> Date: Wed, 22 Jan 2025 12:23:35 +0300 Subject: [PATCH 1/4] Add xQueueCreateSetStatic method for static allocation of Queue Sets (#1228) Add xQueueCreateSetStatic method for static allocation of Queue Sets This commit introduces the xQueueCreateSetStatic function, which allows for the static allocation of Queue Sets in FreeRTOS when both configUSE_QUEUE_SETS and configSUPPORT_STATIC_ALLOCATION are enabled. --- include/FreeRTOS.h | 8 ++++ include/mpu_prototypes.h | 6 +++ include/mpu_wrappers.h | 1 + include/queue.h | 74 ++++++++++++++++++++++++++++--- portable/Common/mpu_wrappers.c | 28 ++++++++++++ portable/Common/mpu_wrappers_v2.c | 33 ++++++++++++++ queue.c | 22 ++++++++- 7 files changed, 164 insertions(+), 8 deletions(-) diff --git a/include/FreeRTOS.h b/include/FreeRTOS.h index dfccccb40..ce8530e7d 100644 --- a/include/FreeRTOS.h +++ b/include/FreeRTOS.h @@ -1484,6 +1484,14 @@ #define traceRETURN_xQueueCreateSet( pxQueue ) #endif +#ifndef traceENTER_xQueueCreateSetStatic + #define traceENTER_xQueueCreateSetStatic( uxEventQueueLength ) +#endif + +#ifndef traceRETURN_xQueueCreateSetStatic + #define traceRETURN_xQueueCreateSetStatic( pxQueue ) +#endif + #ifndef traceENTER_xQueueAddToSet #define traceENTER_xQueueAddToSet( xQueueOrSemaphore, xQueueSet ) #endif diff --git a/include/mpu_prototypes.h b/include/mpu_prototypes.h index 1efd13440..9df2a5762 100644 --- a/include/mpu_prototypes.h +++ b/include/mpu_prototypes.h @@ -269,6 +269,9 @@ uint8_t MPU_ucQueueGetQueueType( QueueHandle_t xQueue ) FREERTOS_SYSTEM_CALL; StaticQueue_t * pxStaticQueue, const uint8_t ucQueueType ) FREERTOS_SYSTEM_CALL; QueueSetHandle_t MPU_xQueueCreateSet( const UBaseType_t uxEventQueueLength ) FREERTOS_SYSTEM_CALL; + QueueSetHandle_t MPU_xQueueCreateSetStatic( const UBaseType_t uxEventQueueLength, + uint8_t * pucQueueStorage, + StaticQueue_t * pxStaticQueue ) FREERTOS_SYSTEM_CALL; BaseType_t MPU_xQueueRemoveFromSet( QueueSetMemberHandle_t xQueueOrSemaphore, QueueSetHandle_t xQueueSet ) FREERTOS_SYSTEM_CALL; BaseType_t MPU_xQueueGenericReset( QueueHandle_t xQueue, @@ -294,6 +297,9 @@ uint8_t MPU_ucQueueGetQueueType( QueueHandle_t xQueue ) FREERTOS_SYSTEM_CALL; StaticQueue_t * pxStaticQueue, const uint8_t ucQueueType ) PRIVILEGED_FUNCTION; QueueSetHandle_t MPU_xQueueCreateSet( const UBaseType_t uxEventQueueLength ) PRIVILEGED_FUNCTION; + QueueSetHandle_t MPU_xQueueCreateSetStatic( const UBaseType_t uxEventQueueLength, + uint8_t * pucQueueStorage, + StaticQueue_t * pxStaticQueue ) PRIVILEGED_FUNCTION; BaseType_t MPU_xQueueRemoveFromSet( QueueSetMemberHandle_t xQueueOrSemaphore, QueueSetHandle_t xQueueSet ) PRIVILEGED_FUNCTION; BaseType_t MPU_xQueueGenericReset( QueueHandle_t xQueue, diff --git a/include/mpu_wrappers.h b/include/mpu_wrappers.h index 4513ca996..3b4738e96 100644 --- a/include/mpu_wrappers.h +++ b/include/mpu_wrappers.h @@ -150,6 +150,7 @@ #define xQueueGenericCreateStatic MPU_xQueueGenericCreateStatic #define xQueueGenericReset MPU_xQueueGenericReset #define xQueueCreateSet MPU_xQueueCreateSet + #define xQueueCreateSetStatic MPU_xQueueCreateSetStatic #define xQueueRemoveFromSet MPU_xQueueRemoveFromSet #if ( configUSE_MPU_WRAPPERS_V1 == 0 ) diff --git a/include/queue.h b/include/queue.h index 963904bb3..56753d300 100644 --- a/include/queue.h +++ b/include/queue.h @@ -1638,12 +1638,12 @@ BaseType_t xQueueGiveMutexRecursive( QueueHandle_t xMutex ) PRIVILEGED_FUNCTION; * See FreeRTOS/Source/Demo/Common/Minimal/QueueSet.c for an example using this * function. * - * A queue set must be explicitly created using a call to xQueueCreateSet() - * before it can be used. Once created, standard FreeRTOS queues and semaphores - * can be added to the set using calls to xQueueAddToSet(). - * xQueueSelectFromSet() is then used to determine which, if any, of the queues - * or semaphores contained in the set is in a state where a queue read or - * semaphore take operation would be successful. + * A queue set must be explicitly created using a call to xQueueCreateSet() or + * xQueueCreateSetStatic() before it can be used. Once created, standard + * FreeRTOS queues and semaphores can be added to the set using calls to + * xQueueAddToSet(). xQueueSelectFromSet() is then used to determine which, if + * any, of the queues or semaphores contained in the set is in a state where a + * queue read or semaphore take operation would be successful. * * Note 1: See the documentation on https://www.freertos.org/Documentation/02-Kernel/04-API-references/07-Queue-sets/00-RTOS-queue-sets * for reasons why queue sets are very rarely needed in practice as there are @@ -1683,9 +1683,69 @@ BaseType_t xQueueGiveMutexRecursive( QueueHandle_t xMutex ) PRIVILEGED_FUNCTION; QueueSetHandle_t xQueueCreateSet( const UBaseType_t uxEventQueueLength ) PRIVILEGED_FUNCTION; #endif +/* + * Queue sets provide a mechanism to allow a task to block (pend) on a read + * operation from multiple queues or semaphores simultaneously. + * + * See FreeRTOS/Source/Demo/Common/Minimal/QueueSet.c for an example using this + * function. + * + * A queue set must be explicitly created using a call to xQueueCreateSet() + * or xQueueCreateSetStatic() before it can be used. Once created, standard + * FreeRTOS queues and semaphores can be added to the set using calls to + * xQueueAddToSet(). xQueueSelectFromSet() is then used to determine which, if + * any, of the queues or semaphores contained in the set is in a state where a + * queue read or semaphore take operation would be successful. + * + * Note 1: See the documentation on https://www.freertos.org/Documentation/02-Kernel/04-API-references/07-Queue-sets/00-RTOS-queue-sets + * for reasons why queue sets are very rarely needed in practice as there are + * simpler methods of blocking on multiple objects. + * + * Note 2: Blocking on a queue set that contains a mutex will not cause the + * mutex holder to inherit the priority of the blocked task. + * + * Note 3: An additional 4 bytes of RAM is required for each space in a every + * queue added to a queue set. Therefore counting semaphores that have a high + * maximum count value should not be added to a queue set. + * + * Note 4: A receive (in the case of a queue) or take (in the case of a + * semaphore) operation must not be performed on a member of a queue set unless + * a call to xQueueSelectFromSet() has first returned a handle to that set member. + * + * @param uxEventQueueLength Queue sets store events that occur on + * the queues and semaphores contained in the set. uxEventQueueLength specifies + * the maximum number of events that can be queued at once. To be absolutely + * certain that events are not lost uxEventQueueLength should be set to the + * total sum of the length of the queues added to the set, where binary + * semaphores and mutexes have a length of 1, and counting semaphores have a + * length set by their maximum count value. Examples: + * + If a queue set is to hold a queue of length 5, another queue of length 12, + * and a binary semaphore, then uxEventQueueLength should be set to + * (5 + 12 + 1), or 18. + * + If a queue set is to hold three binary semaphores then uxEventQueueLength + * should be set to (1 + 1 + 1 ), or 3. + * + If a queue set is to hold a counting semaphore that has a maximum count of + * 5, and a counting semaphore that has a maximum count of 3, then + * uxEventQueueLength should be set to (5 + 3), or 8. + * + * @param pucQueueStorage pucQueueStorage must point to a uint8_t array that is + * at least large enough to hold uxEventQueueLength events. + * + * @param pxQueueBuffer Must point to a variable of type StaticQueue_t, which + * will be used to hold the queue's data structure. + * + * @return If the queue set is created successfully then a handle to the created + * queue set is returned. If pxQueueBuffer is NULL then NULL is returned. + */ +#if ( ( configUSE_QUEUE_SETS == 1 ) && ( configSUPPORT_STATIC_ALLOCATION == 1 ) ) + QueueSetHandle_t xQueueCreateSetStatic( const UBaseType_t uxEventQueueLength, + uint8_t * pucQueueStorage, + StaticQueue_t * pxStaticQueue ) PRIVILEGED_FUNCTION; +#endif + /* * Adds a queue or semaphore to a queue set that was previously created by a - * call to xQueueCreateSet(). + * call to xQueueCreateSet() or xQueueCreateSetStatic(). * * See FreeRTOS/Source/Demo/Common/Minimal/QueueSet.c for an example using this * function. diff --git a/portable/Common/mpu_wrappers.c b/portable/Common/mpu_wrappers.c index 4c5731665..5bc4181e2 100644 --- a/portable/Common/mpu_wrappers.c +++ b/portable/Common/mpu_wrappers.c @@ -1524,6 +1524,34 @@ #endif /* if ( ( configUSE_QUEUE_SETS == 1 ) && ( configSUPPORT_DYNAMIC_ALLOCATION == 1 ) ) */ /*-----------------------------------------------------------*/ + #if ( ( configUSE_QUEUE_SETS == 1 ) && ( configSUPPORT_STATIC_ALLOCATION == 1 ) ) + QueueSetHandle_t MPU_xQueueCreateSetStatic( const UBaseType_t uxEventQueueLength, + uint8_t * pucQueueStorage, + StaticQueue_t * pxStaticQueue ) /* FREERTOS_SYSTEM_CALL */ + { + QueueSetHandle_t xReturn; + + if( portIS_PRIVILEGED() == pdFALSE ) + { + portRAISE_PRIVILEGE(); + portMEMORY_BARRIER(); + + xReturn = xQueueCreateSetStatic( uxEventQueueLength, pucQueueStorage, pxStaticQueue ); + portMEMORY_BARRIER(); + + portRESET_PRIVILEGE(); + portMEMORY_BARRIER(); + } + else + { + xReturn = xQueueCreateSetStatic( uxEventQueueLength, pucQueueStorage, pxStaticQueue ); + } + + return xReturn; + } + #endif /* if ( ( configUSE_QUEUE_SETS == 1 ) && ( configSUPPORT_STATIC_ALLOCATION == 1 ) ) */ +/*-----------------------------------------------------------*/ + #if ( configUSE_QUEUE_SETS == 1 ) QueueSetMemberHandle_t MPU_xQueueSelectFromSet( QueueSetHandle_t xQueueSet, TickType_t xBlockTimeTicks ) /* FREERTOS_SYSTEM_CALL */ diff --git a/portable/Common/mpu_wrappers_v2.c b/portable/Common/mpu_wrappers_v2.c index eb9e6f056..e92aca0f5 100644 --- a/portable/Common/mpu_wrappers_v2.c +++ b/portable/Common/mpu_wrappers_v2.c @@ -3016,6 +3016,39 @@ #endif /* if ( ( configUSE_QUEUE_SETS == 1 ) && ( configSUPPORT_DYNAMIC_ALLOCATION == 1 ) ) */ /*-----------------------------------------------------------*/ + #if ( ( configUSE_QUEUE_SETS == 1 ) && ( configSUPPORT_STATIC_ALLOCATION == 1 ) ) + + QueueSetHandle_t MPU_xQueueCreateSetStatic( const UBaseType_t uxEventQueueLength, + uint8_t * pucQueueStorage, + StaticQueue_t * pxStaticQueue ) /* PRIVILEGED_FUNCTION */ + { + QueueSetHandle_t xInternalQueueSetHandle = NULL; + QueueSetHandle_t xExternalQueueSetHandle = NULL; + int32_t lIndex; + + lIndex = MPU_GetFreeIndexInKernelObjectPool(); + + if( lIndex != -1 ) + { + xInternalQueueSetHandle = xQueueCreateSetStatic( uxEventQueueLength, pucQueueStorage, pxStaticQueue ); + + if( xInternalQueueSetHandle != NULL ) + { + MPU_StoreQueueSetHandleAtIndex( lIndex, xInternalQueueSetHandle ); + xExternalQueueSetHandle = ( QueueSetHandle_t ) CONVERT_TO_EXTERNAL_INDEX( lIndex ); + } + else + { + MPU_SetIndexFreeInKernelObjectPool( lIndex ); + } + } + + return xExternalQueueSetHandle; + } + + #endif /* if ( ( configUSE_QUEUE_SETS == 1 ) && ( configSUPPORT_STATIC_ALLOCATION == 1 ) ) */ +/*-----------------------------------------------------------*/ + #if ( configUSE_QUEUE_SETS == 1 ) BaseType_t MPU_xQueueRemoveFromSet( QueueSetMemberHandle_t xQueueOrSemaphore, diff --git a/queue.c b/queue.c index 4759b439b..688fb3113 100644 --- a/queue.c +++ b/queue.c @@ -3186,7 +3186,27 @@ BaseType_t xQueueIsQueueFullFromISR( const QueueHandle_t xQueue ) return pxQueue; } -#endif /* configUSE_QUEUE_SETS */ +#endif /* #if ( ( configUSE_QUEUE_SETS == 1 ) && ( configSUPPORT_DYNAMIC_ALLOCATION == 1 ) ) */ +/*-----------------------------------------------------------*/ + +#if ( ( configUSE_QUEUE_SETS == 1 ) && ( configSUPPORT_STATIC_ALLOCATION == 1 ) ) + + QueueSetHandle_t xQueueCreateSetStatic( const UBaseType_t uxEventQueueLength, + uint8_t * pucQueueStorage, + StaticQueue_t * pxStaticQueue ) + { + QueueSetHandle_t pxQueue; + + traceENTER_xQueueCreateSetStatic( uxEventQueueLength ); + + pxQueue = xQueueGenericCreateStatic( uxEventQueueLength, ( UBaseType_t ) sizeof( Queue_t * ), pucQueueStorage, pxStaticQueue, queueQUEUE_TYPE_SET ); + + traceRETURN_xQueueCreateSetStatic( pxQueue ); + + return pxQueue; + } + +#endif /* #if ( ( configUSE_QUEUE_SETS == 1 ) && ( configSUPPORT_STATIC_ALLOCATION == 1 ) ) */ /*-----------------------------------------------------------*/ #if ( configUSE_QUEUE_SETS == 1 ) From ff0989e46b15dca255b57cecaff22795bff9df85 Mon Sep 17 00:00:00 2001 From: Marouen Ghodhbane Date: Wed, 9 Oct 2024 17:31:45 +0200 Subject: [PATCH 2/4] portable: aarch64_sre: add configUSE_TASK_FPU_SUPPORT support This is a direct backport of upstream commit [1] for aarch64 (legacy operation port) done under [2] The same code can be applied on the aarch SRE port to be able to enable FPU context saving on all tasks context switch to mitigate GCC optimization to use SIMD registers for copy. [1] "55eceb22: Add configUSE_TASK_FPU_SUPPORT to AARCH64 port (#1048)" [2] https://github.com/FreeRTOS/FreeRTOS-Kernel/pull/1048 Signed-off-by: Marouen Ghodhbane --- portable/GCC/ARM_AARCH64_SRE/port.c | 54 +++++++++++++++++++----- portable/GCC/ARM_AARCH64_SRE/portmacro.h | 15 +++++-- 2 files changed, 55 insertions(+), 14 deletions(-) diff --git a/portable/GCC/ARM_AARCH64_SRE/port.c b/portable/GCC/ARM_AARCH64_SRE/port.c index 1c96c0efd..8cebcaf54 100644 --- a/portable/GCC/ARM_AARCH64_SRE/port.c +++ b/portable/GCC/ARM_AARCH64_SRE/port.c @@ -121,6 +121,10 @@ ::"r" ( portUNMASK_VALUE ) ); \ } +/* The space on the stack required to hold the FPU registers. + * There are 32 128-bit registers.*/ +#define portFPU_REGISTER_WORDS ( 32 * 2 ) + /*-----------------------------------------------------------*/ /* @@ -229,23 +233,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; } @@ -384,6 +412,8 @@ void FreeRTOS_Tick_Handler( void ) } /*-----------------------------------------------------------*/ +#if ( configUSE_TASK_FPU_SUPPORT != 2 ) + void vPortTaskUsesFPU( void ) { /* A task is registering the fact that it needs an FPU context. Set the @@ -393,6 +423,8 @@ void vPortTaskUsesFPU( void ) /* Consider initialising the FPSR here - but probably not necessary in * AArch64. */ } + +#endif /* configUSE_TASK_FPU_SUPPORT */ /*-----------------------------------------------------------*/ void vPortClearInterruptMask( UBaseType_t uxNewMaskValue ) diff --git a/portable/GCC/ARM_AARCH64_SRE/portmacro.h b/portable/GCC/ARM_AARCH64_SRE/portmacro.h index 296984d5b..5810741d2 100644 --- a/portable/GCC/ARM_AARCH64_SRE/portmacro.h +++ b/portable/GCC/ARM_AARCH64_SRE/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 ) From 630cfb5b79dee06faaeedf2b69893b68a97c4c70 Mon Sep 17 00:00:00 2001 From: Marouen Ghodhbane Date: Wed, 9 Oct 2024 18:34:55 +0200 Subject: [PATCH 3/4] portable: aarch64_sre: add the configuration and status registers to the fpu saved context FPSR and FPCR are two 64-bits registers where only the lower 32 bits are defined. Save them when doing context switch with FPU context saving enabled. Signed-off-by: Marouen Ghodhbane --- portable/GCC/ARM_AARCH64_SRE/port.c | 4 ++-- portable/GCC/ARM_AARCH64_SRE/portASM.S | 10 +++++++++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/portable/GCC/ARM_AARCH64_SRE/port.c b/portable/GCC/ARM_AARCH64_SRE/port.c index 8cebcaf54..ffc34d746 100644 --- a/portable/GCC/ARM_AARCH64_SRE/port.c +++ b/portable/GCC/ARM_AARCH64_SRE/port.c @@ -122,8 +122,8 @@ } /* The space on the stack required to hold the FPU registers. - * There are 32 128-bit registers.*/ -#define portFPU_REGISTER_WORDS ( 32 * 2 ) + * There are 32 128-bit plus 2 64-bit status registers.*/ +#define portFPU_REGISTER_WORDS ( (32 * 2) + 2 ) /*-----------------------------------------------------------*/ diff --git a/portable/GCC/ARM_AARCH64_SRE/portASM.S b/portable/GCC/ARM_AARCH64_SRE/portASM.S index ed3c031d0..8d69b2aa1 100644 --- a/portable/GCC/ARM_AARCH64_SRE/portASM.S +++ b/portable/GCC/ARM_AARCH64_SRE/portASM.S @@ -87,7 +87,7 @@ LDR X0, ullPortTaskHasFPUContextConst LDR X2, [X0] - /* Save the FPU context, if any (32 128-bit registers). */ + /* Save the FPU context, if any (32 128-bit plus two 64-bit status registers). */ CMP X2, #0 B.EQ 1f STP Q0, Q1, [SP,#-0x20]! @@ -107,6 +107,11 @@ STP Q28, Q29, [SP,#-0x20]! STP Q30, Q31, [SP,#-0x20]! + /* Even though upper 32 bits of FPSR and FPCR are reserved, save and restore the whole 64 bits to keep 16-byte SP alignement. */ + MRS X9, FPSR + MRS X10, FPCR + STP X9, X10, [SP, #-0x10]! + 1: /* Store the critical nesting count and FPU context indicator. */ STP X2, X3, [SP, #-0x10]! @@ -157,6 +162,7 @@ /* Restore the FPU context, if any. */ CMP X2, #0 B.EQ 1f + LDP X9, X10, [SP], #0x10 LDP Q30, Q31, [SP], #0x20 LDP Q28, Q29, [SP], #0x20 LDP Q26, Q27, [SP], #0x20 @@ -173,6 +179,8 @@ LDP Q4, Q5, [SP], #0x20 LDP Q2, Q3, [SP], #0x20 LDP Q0, Q1, [SP], #0x20 + MSR FPSR, X9 + MSR FPCR, X10 1: LDP X2, X3, [SP], #0x10 /* SPSR and ELR. */ From 72bb476bf3d20c65d054557884f624fde9eec704 Mon Sep 17 00:00:00 2001 From: Marouen Ghodhbane Date: Fri, 11 Oct 2024 15:36:42 +0200 Subject: [PATCH 4/4] portable: aarch64_sre: Add support for vApplicationFPUSafeIRQHandler The application writer needs to name their IRQ handler as: 1. vApplicationIRQHandler if the IRQ handler does not use FPU registers. 2. vApplicationFPUSafeIRQHandler is the IRQ handler uses FPU registers. When the application uses vApplicationFPUSafeIRQHandler, a default implementation of vApplicationIRQHandler is used which stores FPU registers and then calls vApplicationFPUSafeIRQHandler. Note that recent versions of GCC may use FP/SIMD registers to optimize 16-bytes copy and especially when using va_start()/va_arg() functions (e.g printing some thing in IRQ handlers may trigger usage of FPU registers) This implementation is heavily inspired by both the ARM_CA9 port and the ARM_CRx_No_GIC port done in [1] [1] https://github.com/FreeRTOS/FreeRTOS-Kernel/pull/1113 Signed-off-by: Marouen Ghodhbane --- portable/GCC/ARM_AARCH64_SRE/port.c | 27 ++++++++++ portable/GCC/ARM_AARCH64_SRE/portASM.S | 74 ++++++++++++++++++++++++++ 2 files changed, 101 insertions(+) diff --git a/portable/GCC/ARM_AARCH64_SRE/port.c b/portable/GCC/ARM_AARCH64_SRE/port.c index ffc34d746..ab9290d43 100644 --- a/portable/GCC/ARM_AARCH64_SRE/port.c +++ b/portable/GCC/ARM_AARCH64_SRE/port.c @@ -133,6 +133,27 @@ */ extern void vPortRestoreTaskContext( void ); +/* + * If the application provides an implementation of vApplicationIRQHandler(), + * then it will get called directly without saving the FPU registers on + * interrupt entry, and this weak implementation of + * vApplicationFPUSafeIRQHandler() is just provided to remove linkage errors - + * it should never actually get called so its implementation contains a + * call to configASSERT() that will always fail. + * + * If the application provides its own implementation of + * vApplicationFPUSafeIRQHandler() then the implementation of + * vApplicationIRQHandler() provided in portASM.S will save the FPU registers + * before calling it. + * + * Therefore, if the application writer wants FPU registers to be saved on + * interrupt entry their IRQ handler must be called + * vApplicationFPUSafeIRQHandler(), and if the application writer does not want + * FPU registers to be saved on interrupt entry their IRQ handler must be + * called vApplicationIRQHandler(). + */ +void vApplicationFPUSafeIRQHandler( uint32_t ulICCIAR ) __attribute__((weak) ); + /*-----------------------------------------------------------*/ /* A variable is used to keep track of the critical section nesting. This @@ -495,3 +516,9 @@ UBaseType_t uxPortSetInterruptMask( void ) #endif /* configASSERT_DEFINED */ /*-----------------------------------------------------------*/ + +void vApplicationFPUSafeIRQHandler( uint32_t ulICCIAR ) +{ + ( void ) ulICCIAR; + configASSERT( ( volatile void * ) NULL ); +} diff --git a/portable/GCC/ARM_AARCH64_SRE/portASM.S b/portable/GCC/ARM_AARCH64_SRE/portASM.S index 8d69b2aa1..f1f59cd33 100644 --- a/portable/GCC/ARM_AARCH64_SRE/portASM.S +++ b/portable/GCC/ARM_AARCH64_SRE/portASM.S @@ -414,8 +414,82 @@ Exit_IRQ_No_Context_Switch: ERET +/****************************************************************************** + * If the application provides an implementation of vApplicationIRQHandler(), + * then it will get called directly without saving the FPU registers on + * interrupt entry, and this weak implementation of + * vApplicationIRQHandler() will not get called. + * + * If the application provides its own implementation of + * vApplicationFPUSafeIRQHandler() then this implementation of + * vApplicationIRQHandler() will be called, save the FPU registers, and then + * call vApplicationFPUSafeIRQHandler(). + * + * Therefore, if the application writer wants FPU registers to be saved on + * interrupt entry their IRQ handler must be called + * vApplicationFPUSafeIRQHandler(), and if the application writer does not want + * FPU registers to be saved on interrupt entry their IRQ handler must be + * called vApplicationIRQHandler(). + *****************************************************************************/ +.align 8 +.weak vApplicationIRQHandler +.type vApplicationIRQHandler, %function +vApplicationIRQHandler: + /* Save LR and FP on the stack */ + STP X29, X30, [SP, #-0x10]! + /* Save FPU registers (32 128-bits + 2 64-bits configuration and status registers) */ + STP Q0, Q1, [SP,#-0x20]! + STP Q2, Q3, [SP,#-0x20]! + STP Q4, Q5, [SP,#-0x20]! + STP Q6, Q7, [SP,#-0x20]! + STP Q8, Q9, [SP,#-0x20]! + STP Q10, Q11, [SP,#-0x20]! + STP Q12, Q13, [SP,#-0x20]! + STP Q14, Q15, [SP,#-0x20]! + STP Q16, Q17, [SP,#-0x20]! + STP Q18, Q19, [SP,#-0x20]! + STP Q20, Q21, [SP,#-0x20]! + STP Q22, Q23, [SP,#-0x20]! + STP Q24, Q25, [SP,#-0x20]! + STP Q26, Q27, [SP,#-0x20]! + STP Q28, Q29, [SP,#-0x20]! + STP Q30, Q31, [SP,#-0x20]! + + /* Even though upper 32 bits of FPSR and FPCR are reserved, save and restore the whole 64 bits to keep 16-byte SP alignement. */ + MRS X9, FPSR + MRS X10, FPCR + STP X9, X10, [SP, #-0x10]! + + /* Call the C handler. */ + BL vApplicationFPUSafeIRQHandler + + /* Restore FPU registers */ + + LDP X9, X10, [SP], #0x10 + LDP Q30, Q31, [SP], #0x20 + LDP Q28, Q29, [SP], #0x20 + LDP Q26, Q27, [SP], #0x20 + LDP Q24, Q25, [SP], #0x20 + LDP Q22, Q23, [SP], #0x20 + LDP Q20, Q21, [SP], #0x20 + LDP Q18, Q19, [SP], #0x20 + LDP Q16, Q17, [SP], #0x20 + LDP Q14, Q15, [SP], #0x20 + LDP Q12, Q13, [SP], #0x20 + LDP Q10, Q11, [SP], #0x20 + LDP Q8, Q9, [SP], #0x20 + LDP Q6, Q7, [SP], #0x20 + LDP Q4, Q5, [SP], #0x20 + LDP Q2, Q3, [SP], #0x20 + LDP Q0, Q1, [SP], #0x20 + MSR FPSR, X9 + MSR FPCR, X10 + + /* Restore FP and LR */ + LDP X29, X30, [SP], #0x10 + RET .align 8 pxCurrentTCBConst: .dword pxCurrentTCB