From 382bbacb0a911d7701cf9212d2c992d3f5dd1cee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=98yvind=20R=C3=B8nningstad?= Date: Tue, 1 Jun 2021 15:23:51 +0200 Subject: [PATCH] tfm: Put saving of FPU context into its own file so it can be reused MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- arch/arm/core/aarch32/cortex_m/CMakeLists.txt | 1 + arch/arm/core/aarch32/cortex_m/fpu.c | 65 +++++++++++++++++++ include/arch/arm/aarch32/cortex_m/fpu.h | 20 ++++++ .../trusted-firmware-m/interface/interface.c | 51 +++------------ 4 files changed, 94 insertions(+), 43 deletions(-) create mode 100644 arch/arm/core/aarch32/cortex_m/fpu.c create mode 100644 include/arch/arm/aarch32/cortex_m/fpu.h diff --git a/arch/arm/core/aarch32/cortex_m/CMakeLists.txt b/arch/arm/core/aarch32/cortex_m/CMakeLists.txt index dc8fad7e6bf..f20ffb2d97d 100644 --- a/arch/arm/core/aarch32/cortex_m/CMakeLists.txt +++ b/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 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 00000000000..3ae5857626e --- /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 +#include +#include + +/** + * @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 00000000000..17f75cec50e --- /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 1632d4549d0..2672ef986d2 100644 --- a/modules/trusted-firmware-m/interface/interface.c +++ b/modules/trusted-firmware-m/interface/interface.c @@ -9,6 +9,7 @@ #include #include #include +#include #include @@ -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 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. */