trusted-firmware-m: Store FP context before entering secure calls

When ARM_NONSECURE_PREEMPTIBLE_SECURE_CALLS is enabled, if FPU is
being used (CONTROL.FPCA == 1), store all FP registers before
entering the secure function, and restore them afterwards.

This is needed if any NS thread or ISR that interrupts the secure
function uses FP registers. If they do, a secure UsageFault occurs
unless this change is applied.

This allows k_sched_lock() and k_sched_unlock() to be dropped when
ARM_NONSECURE_PREEMPTIBLE_SECURE_CALLS is enabled.

Enable ARM_NONSECURE_PREEMPTIBLE_SECURE_CALLS by default when
building TF-M.

Signed-off-by: Øyvind Rønningstad <[email protected]>

modules/trusted-firmware-m/Kconfig

@@ -28,6 +28,7 @@ menuconfig BUILD_WITH_TFM
 	depends on ARM_TRUSTZONE_M
 	select BUILD_OUTPUT_HEX
 	imply INIT_ARCH_HW_AT_BOOT
+	imply ARM_NONSECURE_PREEMPTIBLE_SECURE_CALLS
 	help
 	  When enabled, this option instructs the Zephyr build process to
 	  additionaly generate a TF-M image for the Secure Execution
@@ -38,11 +39,14 @@ menuconfig BUILD_WITH_TFM
 	  TF-M and Zephyr images, as well as the veneer object file that links
 	  them, are generated during the normal Zephyr build process.
 
-	  Note:
+	  Notes:
 	    Building with the "_nonsecure" BOARD variant (e.g.
 	    "mps2_an521_nonsecure") ensures that
 	    CONFIG_TRUSTED_EXECUTION_NONSECURE ie enabled.
 
+	    By default we allow Zephyr preemptible threads be preempted
+	    while performing a secure function call.
+
 if BUILD_WITH_TFM
 
 config TFM_KEY_FILE_S

modules/trusted-firmware-m/interface/interface.c

@@ -8,6 +8,7 @@
 #include <device.h>
 #include <init.h>
 #include <kernel.h>
+#include <arch/arm/aarch32/cortex_m/cmsis.h>
 
 #include <tfm_ns_interface.h>
 
@@ -34,18 +35,68 @@ int32_t tfm_ns_interface_dispatch(veneer_fn fn,
 		return (int32_t)TFM_ERROR_GENERIC;
 	}
 
+#if !defined(CONFIG_ARM_NONSECURE_PREEMPTIBLE_SECURE_CALLS)
 	/*
 	 * Prevent the thread from being preempted, while executing a Secure
 	 * function. This is required to prevent system crashes that could
 	 * occur, if a thead context switch is triggered in the middle of a
 	 * Secure call.
 	 */
 	k_sched_lock();
+#endif
+
+#if defined(CONFIG_ARM_NONSECURE_PREEMPTIBLE_SECURE_CALLS) && defined(CONFIG_FPU_SHARING)
+	uint32_t fp_ctx_caller_saved[16];
+	uint32_t fp_ctx_callee_saved[16];
+	uint32_t fp_ctx_FPSCR;
+	bool context_saved = false;
+	uint32_t CONTROL = __get_CONTROL();
+
+	if (CONTROL & CONTROL_FPCA_Msk) {
+		/* Store caller-saved and callee-saved FP registers. */
+		__asm__ volatile(
+			"vstmia %0, {s0-s15}\n"
+			"vstmia %1, {s16-s31}\n"
+			:: "r" (fp_ctx_caller_saved), "r" (fp_ctx_callee_saved) :
+		);
+
+		fp_ctx_FPSCR = __get_FPSCR();
+		context_saved = true;
+
+		/* Disable FPCA so no stacking of FP registers happens in TFM. */
+		__set_CONTROL(CONTROL & ~CONTROL_FPCA_Msk);
+
+		/* ISB is recommended after setting CONTROL. It's not needed
+		 * here though, since FPCA should have no impact on instruction
+		 * fetching.
+		 */
+	}
+#endif /* defined(CONFIG_ARM_NONSECURE_PREEMPTIBLE_SECURE_CALLS) && defined(CONFIG_FPU_SHARING) */
 
 	result = fn(arg0, arg1, arg2, arg3);
 
+#if defined(CONFIG_ARM_NONSECURE_PREEMPTIBLE_SECURE_CALLS) && defined(CONFIG_FPU_SHARING)
+	if (context_saved) {
+		/* Set FPCA first so it is set even if an interrupt happens
+		 * during restoration.
+		 */
+		__set_CONTROL(__get_CONTROL() | CONTROL_FPCA_Msk);
+
+		/* Restore FP state. */
+		__set_FPSCR(fp_ctx_FPSCR);
+
+		__asm__ volatile(
+			"vldmia %0, {s0-s15}\n"
+			"vldmia %0, {s16-s31}\n"
+			:: "r" (fp_ctx_caller_saved), "r" (fp_ctx_callee_saved) :
+		);
+	}
+#endif /* defined(CONFIG_ARM_NONSECURE_PREEMPTIBLE_SECURE_CALLS) && defined(CONFIG_FPU_SHARING) */
+
+#if !defined(CONFIG_ARM_NONSECURE_PREEMPTIBLE_SECURE_CALLS)
 	/* Unlock the scheduler, to allow the thread to be preempted. */
 	k_sched_unlock();
+#endif
 
 	k_mutex_unlock(&tfm_mutex);