arch: arm: aarch32: cortex_a_r: Add shared FPU support

This adds lazy floating point context switching.  On svc/irq entrance,
the VFP is disabled and a pointer to the exception stack frame is saved
away.  If the esf pointer is still valid on exception exit, then no
other context used the VFP so the context is still valid and nothing
needs to be restored.  If the esf pointer is NULL on exception exit,
then some other context used the VFP and the floating point context is
restored from the esf.

The undefined instruction handler is responsible for saving away the
floating point context if needed.  If the handler is in the first
irq/svc context and the current thread uses the VFP, then the float
context needs to be saved.  Also, if the handler is in a nested context
and the previous context was using the FVP, save the float context.

Signed-off-by: Bradley Bolen <[email protected]>

Author: Bradley Bolen

arch/arm/core/aarch32/cortex_a_r/exc.S

@@ -30,6 +30,9 @@
 
 _ASM_FILE_PROLOGUE
 
+#if defined(CONFIG_FPU_SHARING)
+GTEXT(z_arm_fault_undef_instruction_fp)
+#endif
 GTEXT(z_arm_fault_undef_instruction)
 GTEXT(z_arm_fault_prefetch)
 GTEXT(z_arm_fault_data)
@@ -47,6 +50,19 @@ GTEXT(z_arm_data_abort)
 	stmfd sp, {r0-r3, r12, lr}^
 	sub sp, #24
 
+#if defined(CONFIG_FPU_SHARING)
+	sub sp, #___fpu_t_SIZEOF
+
+	vmrs r1, fpexc
+	mov r0, #FPEXC_EN
+	vmsr fpexc, r0
+	vmrs r0, fpscr
+
+	mov r2, sp
+	vstmia r2!, {s0-s15}
+	stm r2, {r0, r1}
+#endif
+
 #if defined(CONFIG_EXTRA_EXCEPTION_INFO)
 	/* Pointer to extra esf info */
 	sub sp, #___extra_esf_info_t_SIZEOF
@@ -100,7 +116,56 @@ SECTION_SUBSEC_FUNC(TEXT, __exc, z_arm_undef_instruction)
 	subne lr, #2	/* Thumb (T_BIT) */
 	pop {r0}
 
-	exception_entry MODE_UND
+	/*
+	 * Store r0-r3, r12, lr, lr_und and spsr_und into the stack to
+	 * construct an exception stack frame.
+	 */
+	srsdb sp!, #MODE_UND
+	stmfd sp, {r0-r3, r12, lr}^
+	sub sp, #24
+
+	/* Increment exception nesting count */
+	ldr r2, =_kernel
+	ldr r1, [r2, #_kernel_offset_to_nested]
+	add r1, r1, #1
+	str r1, [r2, #_kernel_offset_to_nested]
+
+#if defined(CONFIG_FPU_SHARING)
+	sub sp, #___fpu_t_SIZEOF
+
+	bl z_arm_fault_undef_instruction_fp
+	cmp r0, #0
+	beq z_arm_exc_exit
+
+	vmrs r1, fpexc
+	mov r0, #FPEXC_EN
+	vmsr fpexc, r0
+	vmrs r0, fpscr
+
+	mov r2, sp
+	vstmia r2!, {s0-s15}
+	stm r2, {r0, r1}
+#endif
+
+#if defined(CONFIG_EXTRA_EXCEPTION_INFO)
+	/* Pointer to extra esf info */
+	sub sp, #___extra_esf_info_t_SIZEOF
+	mov r0, #0
+	str r0, [sp, #4]
+	str r0, [sp, #8]
+
+	sub r1, sp, #___callee_saved_t_SIZEOF
+	str r1, [sp]
+	cps #MODE_SYS
+	stm r1, {r4-r11, sp}
+	cps #MODE_UND
+
+	mov r0, sp
+	mov sp, r1
+#else
+	mov r0, sp
+#endif
+
 	bl z_arm_fault_undef_instruction
 	exception_exit
 
@@ -125,6 +190,12 @@ SECTION_SUBSEC_FUNC(TEXT, __exc, z_arm_prefetch_abort)
 
 	b z_arm_exc_exit
 
+#if defined(CONFIG_FPU_SHARING)
+#define FPU_SF_SIZE	___fpu_t_SIZEOF
+#else
+#define FPU_SF_SIZE	0
+#endif
+
 /**
  * @brief Data abort exception handler
  *
@@ -148,10 +219,10 @@ SECTION_SUBSEC_FUNC(TEXT, __exc, z_arm_data_abort)
 	 * the true esf from the one passed to z_arm_fault_data.
 	 */
 	cmp r0, #0
-	ldreq r1, [sp, #24]
+	ldreq r1, [sp, #24 + FPU_SF_SIZE]
 
 	exception_exit
 
-	streq r1, [sp, #24]
+	streq r1, [sp, #24 + FPU_SF_SIZE]
 
 	b z_arm_exc_exit

arch/arm/core/aarch32/cortex_a_r/exc_exit.S

@@ -62,6 +62,45 @@ system_thread_exit\@:
 #endif
 .endm
 
+.macro fpu_exc_exit
+#if defined(CONFIG_FPU_SHARING)
+	/*
+	 * If the floating point context pointer is null, then a context was
+	 * saved so restore the float context from the exception stack frame.
+	 */
+	ldr r2, =_kernel
+	ldr r1, [r2, #_kernel_offset_to_fp_ctx]
+	cmp r1, #0
+	beq vfp_restore\@
+
+	/*
+	 * If leaving the last interrupt context, remove the floating point
+	 * context pointer.
+	 */
+	cmp r0, #0
+	moveq r1, #0
+	streq r1, [r2, #_kernel_offset_to_fp_ctx]
+	b vfp_exit\@
+
+vfp_restore\@:
+	add r3, sp, #___fpu_sf_t_fpscr_OFFSET
+	ldm r3, {r1, r2}
+	tst r2, #FPEXC_EN
+	beq vfp_exit\@
+
+	vmsr fpexc, r2
+	vmsr fpscr, r1
+	vldmia sp, {s0-s15}
+
+vfp_exit\@:
+	/* Leave the VFP disabled when leaving */
+	mov r1, #0
+	vmsr fpexc, r1
+
+	add sp, sp, #___fpu_t_SIZEOF
+#endif
+.endm
+
 /**
  * @brief Kernel housekeeping when exiting interrupt handler installed directly
  *        in the vector table
@@ -133,6 +172,11 @@ __EXIT_INT:
 	 * out or they are the args to _new_thread for a new thread.
 	 */
 	cps #MODE_SYS
+
+#if defined(CONFIG_FPU_SHARING)
+	fpu_exc_exit
+#endif
+
 	pop {r0-r3, r12, lr}
 	userspace_exc_exit
 	rfeia sp!
@@ -173,6 +217,9 @@ SECTION_SUBSEC_FUNC(TEXT, _HandlerModeExit, z_arm_exc_exit)
 	 */
 
 	/* Clean up exception stack frame */
+#if defined(CONFIG_FPU_SHARING)
+	add sp, sp, #___fpu_t_SIZEOF
+#endif
 	add sp, #32
 
 	/*
@@ -193,6 +240,9 @@ SECTION_SUBSEC_FUNC(TEXT, _HandlerModeExit, z_arm_exc_exit)
 
 	/* Return to the switched thread */
 	cps #MODE_SYS
+#if defined(CONFIG_FPU_SHARING)
+	fpu_exc_exit
+#endif
 	pop {r0-r3, r12, lr}
 	userspace_exc_exit
 	rfeia sp!
@@ -203,6 +253,9 @@ __EXIT_EXC:
 	sub r0, r0, #1
 	str r0, [r3, #_kernel_offset_to_nested]
 
+#if defined(CONFIG_FPU_SHARING)
+	add sp, sp, #___fpu_t_SIZEOF
+#endif
 	/*
 	 * Restore r0-r3, r12, lr, lr_und and spsr_und from the exception stack
 	 * and return to the current thread.

arch/arm/core/aarch32/cortex_a_r/fault.c

@@ -86,13 +86,101 @@ static void dump_fault(uint32_t status, uint32_t addr)
 }
 #endif
 
+#if defined(CONFIG_FPU_SHARING)
+/**
+ * @brief FPU undefined instruction fault handler
+ *
+ * @return Returns true if the FPU is already enabled
+ *           implying a true undefined instruction
+ *         Returns false if the FPU was disabled
+ */
+bool z_arm_fault_undef_instruction_fp(void)
+{
+	/*
+	 * Assume this is a floating point instruction that faulted because
+	 * the FP unit was disabled.  Enable the FP unit and try again.  If
+	 * the FP was already enabled then this was an actual undefined
+	 * instruction.
+	 */
+	if (__get_FPEXC() & FPEXC_EN)
+		return true;
+
+	__set_FPEXC(FPEXC_EN);
+
+	if (_kernel.cpus[0].nested > 1) {
+		/*
+		 * If the nested count is greater than 1, the undefined
+		 * instruction exception came from an irq/svc context.  (The
+		 * irq/svc handler would have the nested count at 1 and then
+		 * the undef exception would increment it to 2).
+		 */
+		struct __fpu_sf *spill_esf =
+			(struct __fpu_sf *)_kernel.cpus[0].fp_ctx;
+
+		if (spill_esf == NULL)
+			return false;
+
+		_kernel.cpus[0].fp_ctx = NULL;
+
+		/*
+		 * If the nested count is 2 and the current thread has used the
+		 * VFP (whether or not it was actually using the VFP before the
+		 * current exception) OR if the nested count is greater than 2
+		 * and the VFP was enabled on the irq/svc entrance for the
+		 * saved exception stack frame, then save the floating point
+		 * context because it is about to be overwritten.
+		 */
+		if (((_kernel.cpus[0].nested == 2)
+				&& (_current->base.user_options & K_FP_REGS))
+			|| ((_kernel.cpus[0].nested > 2)
+				&& (spill_esf->undefined & FPEXC_EN))) {
+			/*
+			 * Spill VFP registers to specified exception stack
+			 * frame
+			 */
+			spill_esf->undefined |= FPEXC_EN;
+			spill_esf->fpscr = __get_FPSCR();
+			__asm__ volatile (
+				"vstmia %0, {s0-s15};\n"
+				: : "r" (&spill_esf->s[0])
+				: "memory"
+				);
+		}
+	} else {
+		/*
+		 * If the nested count is one, a thread was the faulting
+		 * context.  Just flag that this thread uses the VFP.  This
+		 * means that a thread that uses the VFP does not have to,
+		 * but should, set K_FP_REGS on thread creation.
+		 */
+		_current->base.user_options |= K_FP_REGS;
+	}
+
+	return false;
+}
+#endif
+
 /**
  * @brief Undefined instruction fault handler
  *
  * @return Returns true if the fault is fatal
  */
 bool z_arm_fault_undef_instruction(z_arch_esf_t *esf)
 {
+#if defined(CONFIG_FPU_SHARING)
+	/*
+	 * This is a true undefined instruction and we will be crashing
+	 * so save away the VFP registers.
+	 */
+	esf->fpu.undefined = __get_FPEXC();
+	esf->fpu.fpscr = __get_FPSCR();
+	__asm__ volatile (
+		"vstmia %0, {s0-s15};\n"
+		: : "r" (&esf->fpu.s[0])
+		: "memory"
+		);
+#endif
+
 	/* Print fault information */
 	LOG_ERR("***** UNDEFINED INSTRUCTION ABORT *****");
 

arch/arm/core/aarch32/isr_wrapper.S

@@ -88,6 +88,31 @@ isr_system_thread:
 	cps #MODE_SYS
 	push {r0-r3, r12, lr}
 
+#if defined(CONFIG_FPU_SHARING)
+	sub sp, sp, #___fpu_t_SIZEOF
+
+	/*
+	 * Note that this handler was entered with the VFP unit enabled.
+	 * The undefined instruction handler uses this to know that it
+	 * needs to save the current floating context.
+	 */
+	vmrs r0, fpexc
+	str r0, [sp, #___fpu_t_SIZEOF - 4]
+
+	/* Disable VFP */
+	mov r0, #0
+	vmsr fpexc, r0
+
+	/*
+	 * Mark where to store the floating context for the undefined
+	 * instruction handler
+	 */
+	ldr r2, =_kernel
+	ldr r0, [r2, #_kernel_offset_to_fp_ctx]
+	cmp r0, #0
+	streq sp, [r2, #_kernel_offset_to_fp_ctx]
+#endif /* CONFIG_FPU_SHARING */
+
 	/*
 	 * Use SVC mode stack for predictable interrupt behaviour; running ISRs
 	 * in the SYS/USR mode stack (i.e. interrupted thread stack) leaves the