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>
diff --git a/portable/GCC/ARM_AARCH64_SRE/port.c b/portable/GCC/ARM_AARCH64_SRE/port.c index 8cebcaf..ffc34d7 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 ed3c031..8d69b2a 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. */