1 // SPDX-License-Identifier: BSD-2-Clause 2 /* 3 * Copyright (c) 2015, Linaro Limited 4 * All rights reserved. 5 */ 6 7 #include <arm.h> 8 #include <assert.h> 9 #include <kernel/vfp.h> 10 #include "vfp_private.h" 11 12 #ifdef ARM32 13 bool vfp_is_enabled(void) 14 { 15 return !!(vfp_read_fpexc() & FPEXC_EN); 16 } 17 18 void vfp_enable(void) 19 { 20 vfp_write_fpexc(vfp_read_fpexc() | FPEXC_EN); 21 } 22 23 void vfp_disable(void) 24 { 25 vfp_write_fpexc(vfp_read_fpexc() & ~FPEXC_EN); 26 } 27 28 void vfp_lazy_save_state_init(struct vfp_state *state) 29 { 30 uint32_t fpexc = vfp_read_fpexc(); 31 32 state->fpexc = fpexc; 33 vfp_write_fpexc(fpexc & ~FPEXC_EN); 34 } 35 36 void vfp_lazy_save_state_final(struct vfp_state *state) 37 { 38 if (state->fpexc & FPEXC_EN) { 39 uint32_t fpexc = vfp_read_fpexc(); 40 41 assert(!(fpexc & FPEXC_EN)); 42 vfp_write_fpexc(fpexc | FPEXC_EN); 43 state->fpscr = vfp_read_fpscr(); 44 vfp_save_extension_regs(state->reg); 45 vfp_write_fpexc(fpexc); 46 } 47 } 48 49 void vfp_lazy_restore_state(struct vfp_state *state, bool full_state) 50 { 51 52 if (full_state) { 53 /* 54 * Only restore VFP registers if they have been touched as they 55 * otherwise are intact. 56 */ 57 58 /* FPEXC is restored to what's in state->fpexc below */ 59 vfp_write_fpexc(vfp_read_fpexc() | FPEXC_EN); 60 61 vfp_write_fpscr(state->fpscr); 62 vfp_restore_extension_regs(state->reg); 63 } 64 vfp_write_fpexc(state->fpexc); 65 } 66 #endif /* ARM32 */ 67 68 #ifdef ARM64 69 bool vfp_is_enabled(void) 70 { 71 return (CPACR_EL1_FPEN(read_cpacr_el1()) & CPACR_EL1_FPEN_EL0EL1); 72 } 73 74 void vfp_enable(void) 75 { 76 uint32_t val = read_cpacr_el1(); 77 78 val |= (CPACR_EL1_FPEN_EL0EL1 << CPACR_EL1_FPEN_SHIFT); 79 write_cpacr_el1(val); 80 isb(); 81 } 82 83 void vfp_disable(void) 84 { 85 uint32_t val = read_cpacr_el1(); 86 87 val &= ~(CPACR_EL1_FPEN_MASK << CPACR_EL1_FPEN_SHIFT); 88 write_cpacr_el1(val); 89 isb(); 90 } 91 92 void vfp_lazy_save_state_init(struct vfp_state *state) 93 { 94 state->cpacr_el1 = read_cpacr_el1(); 95 vfp_disable(); 96 } 97 98 void vfp_lazy_save_state_final(struct vfp_state *state) 99 { 100 if ((CPACR_EL1_FPEN(state->cpacr_el1) & CPACR_EL1_FPEN_EL0EL1) || 101 state->force_save) { 102 assert(!vfp_is_enabled()); 103 vfp_enable(); 104 state->fpcr = read_fpcr(); 105 state->fpsr = read_fpsr(); 106 vfp_save_extension_regs(state->reg); 107 vfp_disable(); 108 } 109 } 110 111 void vfp_lazy_restore_state(struct vfp_state *state, bool full_state) 112 { 113 if (full_state) { 114 /* 115 * Only restore VFP registers if they have been touched as they 116 * otherwise are intact. 117 */ 118 119 /* CPACR_EL1 is restored to what's in state->cpacr_el1 below */ 120 vfp_enable(); 121 write_fpcr(state->fpcr); 122 write_fpsr(state->fpsr); 123 vfp_restore_extension_regs(state->reg); 124 } 125 write_cpacr_el1(state->cpacr_el1); 126 isb(); 127 } 128 #endif /* ARM64 */ 129