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

@@ -122,8 +122,8 @@
     }
 
 /* The space on the stack required to hold the FPU registers.
- * There are 32 128-bit registers.*/
+ * There are 32 128-bit plus 2 64-bit status registers.*/
-#define portFPU_REGISTER_WORDS     ( 32 * 2 )
+#define portFPU_REGISTER_WORDS     ( (32 * 2) + 2 )
 
 /*-----------------------------------------------------------*/
 

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. */