[nrf fromtree] tfm: Put saving of FPU context into its own file so ...

... it can be reused

Also, this eases readability.

The new API can be used any time all FP registers must be manually
saved and restored for an operation.

This also fixes a bug where the FPU context was not restored correctly.
The first 16 register values were restored into the last 16 registers
as well.

Signed-off-by: Øyvind Rønningstad <[email protected]>
(cherry picked from commit b75c7d4)

Author: oyvindronningstad

arch/arm/core/aarch32/cortex_m/CMakeLists.txt

@@ -8,6 +8,7 @@ zephyr_library_sources(
   fault_s.S
   fault.c
   exc_exit.S
+  fpu.c
   scb.c
   irq_init.c
   thread_abort.c

arch/arm/core/aarch32/cortex_m/fpu.c

@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2019,2020 Linaro Limited
+ * Copyright (c) 2021 Nordic Semiconductor ASA
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <kernel.h>
+#include <arch/arm/aarch32/cortex_m/cmsis.h>
+#include <arch/arm/aarch32/cortex_m/fpu.h>
+
+/**
+ * @file @brief Helper functions for saving and restoring the FP context.
+ *
+ */
+
+void z_arm_save_fp_context(struct fpu_ctx_full *buffer)
+{
+#if defined(CONFIG_FPU_SHARING)
+	__ASSERT_NO_MSG(buffer != NULL);
+
+	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" (buffer->caller_saved), "r" (buffer->callee_saved) :
+		);
+
+		buffer->fpscr = __get_FPSCR();
+		buffer->ctx_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
+}
+
+void z_arm_restore_fp_context(const struct fpu_ctx_full *buffer)
+{
+#if defined(CONFIG_FPU_SHARING)
+	if (buffer->ctx_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(buffer->fpscr);
+
+		__asm__ volatile(
+			"vldmia %0, {s0-s15}\n"
+			"vldmia %1, {s16-s31}\n"
+			:: "r" (buffer->caller_saved), "r" (buffer->callee_saved) :
+		);
+	}
+#endif
+}

include/arch/arm/aarch32/cortex_m/fpu.h

@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) 2021, Nordic Semiconductor ASA
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef ZEPHYR_INCLUDE_ARCH_ARM_AARCH32_CORTEX_M_FPU_H_
+#define ZEPHYR_INCLUDE_ARCH_ARM_AARCH32_CORTEX_M_FPU_H_
+
+struct fpu_ctx_full {
+	uint32_t caller_saved[16];
+	uint32_t callee_saved[16];
+	uint32_t fpscr;
+	bool ctx_saved;
+};
+
+void z_arm_save_fp_context(struct fpu_ctx_full *buffer);
+void z_arm_restore_fp_context(const struct fpu_ctx_full *buffer);
+
+#endif /* ZEPHYR_INCLUDE_ARCH_ARM_AARCH32_CORTEX_M_FPU_H_ */

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

@@ -9,6 +9,7 @@
 #include <init.h>
 #include <kernel.h>
 #include <arch/arm/aarch32/cortex_m/cmsis.h>
+#include <arch/arm/aarch32/cortex_m/fpu.h>
 
 #include <tfm_ns_interface.h>
 
@@ -45,53 +46,17 @@ int32_t tfm_ns_interface_dispatch(veneer_fn fn,
 	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) */
+#if defined(CONFIG_ARM_NONSECURE_PREEMPTIBLE_SECURE_CALLS)
+	struct fpu_ctx_full context_buffer;
+
+	z_arm_save_fp_context(&context_buffer);
+#endif
 
 	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 %1, {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)
+	z_arm_restore_fp_context(&context_buffer);
+#endif
 
 #if !defined(CONFIG_ARM_NONSECURE_PREEMPTIBLE_SECURE_CALLS)
 	/* Unlock the scheduler, to allow the thread to be preempted. */