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.
Signed-off-by: Øyvind Rønningstad <oyvind.ronningstad@nordicsemi.no>
diff --git a/arch/arm/core/aarch32/cortex_m/CMakeLists.txt b/arch/arm/core/aarch32/cortex_m/CMakeLists.txt
index dc8fad7..f20ffb2 100644
--- a/arch/arm/core/aarch32/cortex_m/CMakeLists.txt
+++ b/arch/arm/core/aarch32/cortex_m/CMakeLists.txt
@@ -8,6 +8,7 @@
fault_s.S
fault.c
exc_exit.S
+ fpu.c
scb.c
irq_init.c
thread_abort.c
diff --git a/arch/arm/core/aarch32/cortex_m/fpu.c b/arch/arm/core/aarch32/cortex_m/fpu.c
new file mode 100644
index 0000000..3ae5857
--- /dev/null
+++ b/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
+}
diff --git a/include/arch/arm/aarch32/cortex_m/fpu.h b/include/arch/arm/aarch32/cortex_m/fpu.h
new file mode 100644
index 0000000..17f75ce
--- /dev/null
+++ b/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_ */
diff --git a/modules/trusted-firmware-m/interface/interface.c b/modules/trusted-firmware-m/interface/interface.c
index 1632d45..2672ef9 100644
--- a/modules/trusted-firmware-m/interface/interface.c
+++ b/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 @@
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 defined(CONFIG_ARM_NONSECURE_PREEMPTIBLE_SECURE_CALLS)
+ struct fpu_ctx_full context_buffer;
- 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) */
+ 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. */
