From 630cfb5b79dee06faaeedf2b69893b68a97c4c70 Mon Sep 17 00:00:00 2001
From: Marouen Ghodhbane <marouen.ghodhbane@nxp.com>
Date: Wed, 9 Oct 2024 18:34:55 +0200
Subject: [PATCH] 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 <marouen.ghodhbane@nxp.com>
---
 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 8cebcaf54e7..ffc34d746df 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 ed3c031d09e..8d69b2aa1a4 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. */