Branch data Line data Source code
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 <kernel.h> 9 : : #include <arch/arm/aarch32/cortex_m/cmsis.h> 10 : : #include <arch/arm/aarch32/cortex_m/fpu.h> 11 : : 12 : : /** 13 : : * @file @brief Helper functions for saving and restoring the FP context. 14 : : * 15 : : */ 16 : : 17 : 0 : void 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 : 0 : } 45 : : 46 : 0 : void 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 : 0 : }