michaelh/zephyr
1
0
Fork 0
Code Releases Activity

tfm: Put saving of FPU context into its own file so it can be reused

Browse source 
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>
This commit is contained in:
Øyvind Rønningstad 2021-06-01 15:23:51 +02:00 • committed by Ioannis Glaropoulos
commit 382bbacb0a
4 changed files with 94 additions and 43 deletions
Download patch file Download diff file Expand all files Collapse all files

1
arch/arm/core/aarch32/cortex_m/CMakeLists.txt
View file

@ -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

65
arch/arm/core/aarch32/cortex_m/fpu.c Normal file
View file

@ -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
}

20
include/arch/arm/aarch32/cortex_m/fpu.h Normal file
View file

@ -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_ */

51
modules/trusted-firmware-m/interface/interface.c
View file

@ -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 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. */