From a84f51003ab31a05665865347d464b29011e85ad Mon Sep 17 00:00:00 2001
From: Richard Elberger <elberger@amazon.com>
Date: Thu, 18 Jun 2026 13:21:18 -0700
Subject: [PATCH] armv9: reconcile critical section, add stack overflow MTE
 fix, per-task VL

- Restore portENABLE_INTERRUPTS() in uxPortSetInterruptMask to match
  upstream ARM_AARCH64_SRE port behavior (PMR masking is sufficient).

- Fix stack_macros.h method 2 overflow check: strip MTE address tags
  from pxStack before dereferencing watermark bytes (prevents tag
  mismatch fault) and before pointer comparison (prevents false trigger).

- Add configARMV9_TASK_VL support: save/restore ZCR_EL1 per-task,
  vPortTaskSetVL() API, initial frame includes ZCR=0xF (max VL).
  Validated on FVP at sve.veclen=4; veclen=2 has a known RDVL
  coherency issue under investigation with Arm.
---
 include/stack_macros.h                     |  4 +--
 portable/GCC/ARM_AARCH64_ARMV9/port.c      | 31 +++++++++++++++++++---
 portable/GCC/ARM_AARCH64_ARMV9/portmacro.h |  5 ++++
 3 files changed, 35 insertions(+), 5 deletions(-)

diff --git a/include/stack_macros.h b/include/stack_macros.h
index 3d66f6f91..7e7c92f7a 100644
--- a/include/stack_macros.h
+++ b/include/stack_macros.h
@@ -103,10 +103,10 @@
     #define taskCHECK_FOR_STACK_OVERFLOW()                                                       \
     do                                                                                           \
     {                                                                                            \
-        const uint32_t * const pulStack = ( uint32_t * ) pxCurrentTCB->pxStack;                  \
+        const uint32_t * const pulStack = ( uint32_t * ) portSTRIP_ADDRESS_TAG( pxCurrentTCB->pxStack ); \
         const uint32_t ulCheckValue = ( uint32_t ) 0xa5a5a5a5U;                                  \
                                                                                                  \
-        if( ( pxCurrentTCB->pxTopOfStack <= pxCurrentTCB->pxStack + portSTACK_LIMIT_PADDING ) || \
+        if( ( portSTRIP_ADDRESS_TAG( pxCurrentTCB->pxTopOfStack ) <= ( StackType_t * ) portSTRIP_ADDRESS_TAG( pxCurrentTCB->pxStack ) + portSTACK_LIMIT_PADDING ) || \
             ( pulStack[ 0 ] != ulCheckValue ) ||                                                 \
             ( pulStack[ 1 ] != ulCheckValue ) ||                                                 \
             ( pulStack[ 2 ] != ulCheckValue ) ||                                                 \
diff --git a/portable/GCC/ARM_AARCH64_ARMV9/port.c b/portable/GCC/ARM_AARCH64_ARMV9/port.c
index 4d31c5cd6..b85ab46ee 100644
--- a/portable/GCC/ARM_AARCH64_ARMV9/port.c
+++ b/portable/GCC/ARM_AARCH64_ARMV9/port.c
@@ -339,6 +339,16 @@ StackType_t * pxPortInitialiseStack( StackType_t * pxTopOfStack,
         * executing any floating point instructions. */
         pxTopOfStack--;
         *pxTopOfStack = portNO_FLOATING_POINT_CONTEXT;
+
+        #if ( configARMV9_TASK_VL == 1 )
+        /* Initial ZCR_EL1: max VL (0xF = implementation maximum).
+         * LDP X9, XZR reads X9 from [SP] (low addr), XZR from [SP+8] (high addr).
+         * So ZCR value must be at the lower address. */
+        pxTopOfStack--;
+        *pxTopOfStack = 0;    /* pad (XZR slot, higher address) */
+        pxTopOfStack--;
+        *pxTopOfStack = 0xF;  /* ZCR_EL1 (X9 slot, lower address = SP) */
+        #endif
     }
     #elif ( configUSE_TASK_FPU_SUPPORT == 2 )
     {
@@ -556,9 +566,10 @@ UBaseType_t uxPortSetInterruptMask( void )
                          ::"r" ( ( uint64_t ) configMAX_API_CALL_INTERRUPT_PRIORITY << portPRIORITY_SHIFT ) : "memory" );
     }
 
-    /* Do NOT call portENABLE_INTERRUPTS() here. On FVP the timer PPI has
-     * priority 0 which bypasses PMR masking. Keep DAIF.I=1 for the entire
-     * critical section; vPortExitCritical will clear it. */
+    /* Re-enable DAIF.I now that PMR is set. PMR-based priority masking
+     * prevents interrupts below configMAX_API_CALL_INTERRUPT_PRIORITY
+     * from being taken. This matches the upstream ARM_AARCH64_SRE port. */
+    portENABLE_INTERRUPTS();
 
     return ulReturn;
 }
@@ -650,3 +661,17 @@ void vPortTaskRegeneratePACKeys( void )
 }
 
 #endif /* configARMV9_PAC */
+
+#if ( configARMV9_TASK_VL == 1 )
+
+void vPortTaskSetVL( uint32_t ulVL )
+{
+    /* Set the SVE vector length for the current task.
+     * ulVL is the ZCR_EL1.LEN field value: VL = (LEN+1) * 128 bits.
+     * E.g., ulVL=0 → 128-bit, ulVL=1 → 256-bit, ulVL=3 → 512-bit.
+     * The actual VL is clamped by the implementation's maximum.
+     * ZCR_EL1 = S3_0_C1_C2_0 */
+    __asm volatile( "MSR S3_0_C1_C2_0, %0\n ISB\n" :: "r"( (uint64_t)ulVL ) );
+}
+
+#endif /* configARMV9_TASK_VL */
\ No newline at end of file
diff --git a/portable/GCC/ARM_AARCH64_ARMV9/portmacro.h b/portable/GCC/ARM_AARCH64_ARMV9/portmacro.h
index 0245fc005..11daad1d0 100644
--- a/portable/GCC/ARM_AARCH64_ARMV9/portmacro.h
+++ b/portable/GCC/ARM_AARCH64_ARMV9/portmacro.h
@@ -226,6 +226,11 @@ void FreeRTOS_Tick_Handler( void );
     void vPortTaskRegeneratePACKeys( void );
 #endif
 
+/* ---- Armv9 per-task SVE vector length ---- */
+#if ( configARMV9_TASK_VL == 1 )
+    void vPortTaskSetVL( uint32_t ulVL );
+#endif
+
 /* *INDENT-OFF* */
 #ifdef __cplusplus
     }