arch: xtensa: save FPU register in context switching

Save FP user register and FP register file during context switch.

This change enables shared FP registers mode using CONFIG_FPU_SHARING.

Since there is no lazy stacking, the FPU registers will be saved regardless
of whether floating point calculations are performed in the threads when
CONFIG_FPU_SHARING is enabled. This require 72 additional bytes in the
stack memory.

Signed-off-by: Lucas Tamborrino <lucas.tamborrino@espressif.com>

arch/xtensa/core/xtensa-asm2-util.S

@@ -152,6 +152,10 @@ _restore_context:
 	l32i a0, a1, BSA_PS_OFF
 	wsr a0, ZSR_EPS
 
+#if XCHAL_HAVE_FP && defined(CONFIG_CPU_HAS_FPU) && defined(CONFIG_FPU_SHARING)
+	FPU_REG_RESTORE
+#endif
+
 	l32i a0, a1, BSA_SAR_OFF
 	wsr a0, SAR
 #if XCHAL_HAVE_LOOPS

arch/xtensa/include/xtensa-asm2-context.h

@@ -85,12 +85,19 @@
 #define BASE_SAVE_AREA_SIZE_THREADPTR	0
 #endif
 
+#if XCHAL_HAVE_FP && defined(CONFIG_CPU_HAS_FPU) && defined(CONFIG_FPU_SHARING)
+#define BASE_SAVE_AREA_SIZE_FPU		(18 * 4)
+#else
+#define BASE_SAVE_AREA_SIZE_FPU		0
+#endif
+
 #define BASE_SAVE_AREA_SIZE \
 	(BASE_SAVE_AREA_SIZE_COMMON + \
 	 BASE_SAVE_AREA_SIZE_LOOPS + \
 	 BASE_SAVE_AREA_SIZE_EXCCAUSE + \
 	 BASE_SAVE_AREA_SIZE_SCOMPARE + \
-	 BASE_SAVE_AREA_SIZE_THREADPTR)
+	 BASE_SAVE_AREA_SIZE_THREADPTR + \
+	 BASE_SAVE_AREA_SIZE_FPU)
 
 #define BSA_A3_OFF	(BASE_SAVE_AREA_SIZE - 20)
 #define BSA_A2_OFF	(BASE_SAVE_AREA_SIZE - 24)
@@ -127,4 +134,15 @@
 			  BASE_SAVE_AREA_SIZE_THREADPTR))
 #endif
 
+#if XCHAL_HAVE_FP && defined(CONFIG_CPU_HAS_FPU) && defined(CONFIG_FPU_SHARING)
+#define BSA_FPU_OFF \
+			(BASE_SAVE_AREA_SIZE - \
+			 (BASE_SAVE_AREA_SIZE_COMMON + \
+			  BASE_SAVE_AREA_SIZE_LOOPS + \
+			  BASE_SAVE_AREA_SIZE_EXCCAUSE + \
+			  BASE_SAVE_AREA_SIZE_SCOMPARE + \
+			  BASE_SAVE_AREA_SIZE_THREADPTR + \
+			  BASE_SAVE_AREA_SIZE_FPU))
+#endif
+
 #endif /* ZEPHYR_ARCH_XTENSA_INCLUDE_XTENSA_ASM2_CONTEXT_H_ */

arch/xtensa/include/xtensa-asm2-s.h

@@ -88,6 +88,63 @@
 #endif
 .endm
 
+#if XCHAL_HAVE_FP && defined(CONFIG_CPU_HAS_FPU) && defined(CONFIG_FPU_SHARING)
+/*
+ * FPU_REG_SAVE
+ *
+ * Saves the Float Point Unit context registers in the base save
+ * area pointed to by the current stack pointer A1. The Floating-Point
+ * Coprocessor Option adds the FR register file and two User Registers
+ * called FCR and FSR.The FR register file consists of 16 registers of
+ * 32 bits each and is used for all data computation.
+ */
+.macro FPU_REG_SAVE
+	rur.fcr	a0
+	s32i	a0, a1, BSA_FPU_OFF
+	rur.fsr	a0
+	s32i	a0, a1, 4+BSA_FPU_OFF
+	ssi	f0, a1, 8+BSA_FPU_OFF
+	ssi	f1, a1, 12+BSA_FPU_OFF
+	ssi	f2, a1, 16+BSA_FPU_OFF
+	ssi	f3, a1, 20+BSA_FPU_OFF
+	ssi	f4, a1, 24+BSA_FPU_OFF
+	ssi	f5, a1, 28+BSA_FPU_OFF
+	ssi	f6, a1, 32+BSA_FPU_OFF
+	ssi	f7, a1, 36+BSA_FPU_OFF
+	ssi	f8, a1, 40+BSA_FPU_OFF
+	ssi	f9, a1, 44+BSA_FPU_OFF
+	ssi	f10, a1, 48+BSA_FPU_OFF
+	ssi	f11, a1, 52+BSA_FPU_OFF
+	ssi	f12, a1, 56+BSA_FPU_OFF
+	ssi	f13, a1, 60+BSA_FPU_OFF
+	ssi	f14, a1, 64+BSA_FPU_OFF
+	ssi	f15, a1, 68+BSA_FPU_OFF
+.endm
+
+.macro FPU_REG_RESTORE
+	l32i.n	a0, a1, BSA_FPU_OFF
+	wur.fcr	a0
+	l32i.n	a0, a1, 4+BSA_FPU_OFF
+	wur.fsr	a0
+	lsi	f0, a1, 8+BSA_FPU_OFF
+	lsi	f1, a1, 12+BSA_FPU_OFF
+	lsi	f2, a1, 16+BSA_FPU_OFF
+	lsi	f3, a1, 20+BSA_FPU_OFF
+	lsi	f4, a1, 24+BSA_FPU_OFF
+	lsi	f5, a1, 28+BSA_FPU_OFF
+	lsi	f6, a1, 32+BSA_FPU_OFF
+	lsi	f7, a1, 36+BSA_FPU_OFF
+	lsi	f8, a1, 40+BSA_FPU_OFF
+	lsi	f9, a1, 44+BSA_FPU_OFF
+	lsi	f10, a1, 48+BSA_FPU_OFF
+	lsi	f11, a1, 52+BSA_FPU_OFF
+	lsi	f12, a1, 56+BSA_FPU_OFF
+	lsi	f13, a1, 60+BSA_FPU_OFF
+	lsi	f14, a1, 64+BSA_FPU_OFF
+	lsi	f15, a1, 68+BSA_FPU_OFF
+.endm
+#endif
+
 /*
  * ODD_REG_SAVE
  *
@@ -120,6 +177,9 @@
 	rur.THREADPTR a0
 	s32i a0, a1, BSA_THREADPTR_OFF
 #endif
+#if XCHAL_HAVE_FP && defined(CONFIG_CPU_HAS_FPU) && defined(CONFIG_FPU_SHARING)
+	FPU_REG_SAVE
+#endif
 .endm
 
 /*

soc/xtensa/esp32/Kconfig.soc

@@ -11,6 +11,7 @@ config SOC_ESP32
 	select PINCTRL
 	select XIP
 	select HAS_ESPRESSIF_HAL
+	select CPU_HAS_FPU
 
 if SOC_ESP32