1 /* 2 * Copyright (c) 2019,2020 Linaro Limited 3 * Copyright (c) 2021 Nordic Semiconductor ASA 4 * 5 * SPDX-License-Identifier: Apache-2.0 6 */ 7 8 #include <zephyr/kernel.h> 9 #include <cmsis_core.h> 10 #include <zephyr/arch/arm/cortex_m/fpu.h> 11 12 /** 13 * @file @brief Helper functions for saving and restoring the FP context. 14 * 15 */ 16 z_arm_save_fp_context(struct fpu_ctx_full * buffer)17void z_arm_save_fp_context(struct fpu_ctx_full *buffer) 18 { 19 #if defined(CONFIG_FPU_SHARING) 20 __ASSERT_NO_MSG(buffer != NULL); 21 22 uint32_t CONTROL = __get_CONTROL(); 23 24 if (CONTROL & CONTROL_FPCA_Msk) { 25 /* Store caller-saved and callee-saved FP registers. */ 26 __asm__ volatile( 27 "vstmia %0, {s0-s15}\n" 28 "vstmia %1, {s16-s31}\n" 29 :: "r" (buffer->caller_saved), "r" (buffer->callee_saved) : 30 ); 31 32 buffer->fpscr = __get_FPSCR(); 33 buffer->ctx_saved = true; 34 35 /* Disable FPCA so no stacking of FP registers happens in TFM. */ 36 __set_CONTROL(CONTROL & ~CONTROL_FPCA_Msk); 37 38 /* ISB is recommended after setting CONTROL. It's not needed 39 * here though, since FPCA should have no impact on instruction 40 * fetching. 41 */ 42 } 43 #endif 44 } 45 z_arm_restore_fp_context(const struct fpu_ctx_full * buffer)46void z_arm_restore_fp_context(const struct fpu_ctx_full *buffer) 47 { 48 #if defined(CONFIG_FPU_SHARING) 49 if (buffer->ctx_saved) { 50 /* Set FPCA first so it is set even if an interrupt happens 51 * during restoration. 52 */ 53 __set_CONTROL(__get_CONTROL() | CONTROL_FPCA_Msk); 54 55 /* Restore FP state. */ 56 __set_FPSCR(buffer->fpscr); 57 58 __asm__ volatile( 59 "vldmia %0, {s0-s15}\n" 60 "vldmia %1, {s16-s31}\n" 61 :: "r" (buffer->caller_saved), "r" (buffer->callee_saved) : 62 ); 63 } 64 #endif 65 } 66
