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("vstmia %0, {s0-s15}\n" 27 "vstmia %1, {s16-s31}\n" ::"r"(buffer->caller_saved), 28 "r"(buffer->callee_saved) 29 :); 30 31 buffer->fpscr = __get_FPSCR(); 32 buffer->ctx_saved = true; 33 34 /* Disable FPCA so no stacking of FP registers happens in TFM. */ 35 __set_CONTROL(CONTROL & ~CONTROL_FPCA_Msk); 36 37 /* ISB is recommended after setting CONTROL. It's not needed 38 * here though, since FPCA should have no impact on instruction 39 * fetching. 40 */ 41 } 42 #endif 43 } 44 z_arm_restore_fp_context(const struct fpu_ctx_full * buffer)45void z_arm_restore_fp_context(const struct fpu_ctx_full *buffer) 46 { 47 #if defined(CONFIG_FPU_SHARING) 48 if (buffer->ctx_saved) { 49 /* Set FPCA first so it is set even if an interrupt happens 50 * during restoration. 51 */ 52 __set_CONTROL(__get_CONTROL() | CONTROL_FPCA_Msk); 53 54 /* Restore FP state. */ 55 __set_FPSCR(buffer->fpscr); 56 57 __asm__ volatile("vldmia %0, {s0-s15}\n" 58 "vldmia %1, {s16-s31}\n" ::"r"(buffer->caller_saved), 59 "r"(buffer->callee_saved) 60 :); 61 } 62 #endif 63 } 64
