1 // SPDX-License-Identifier: BSD-2-Clause 2 /* 3 * Copyright (c) 2015, Linaro Limited 4 * All rights reserved. 5 * 6 * Redistribution and use in source and binary forms, with or without 7 * modification, are permitted provided that the following conditions are met: 8 * 9 * 1. Redistributions of source code must retain the above copyright notice, 10 * this list of conditions and the following disclaimer. 11 * 12 * 2. Redistributions in binary form must reproduce the above copyright notice, 13 * this list of conditions and the following disclaimer in the documentation 14 * and/or other materials provided with the distribution. 15 * 16 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 * POSSIBILITY OF SUCH DAMAGE. 27 */ 28 29 #include <arm.h> 30 #include <assert.h> 31 #include <kernel/vfp.h> 32 #include "vfp_private.h" 33 34 #ifdef ARM32 35 bool vfp_is_enabled(void) 36 { 37 return !!(vfp_read_fpexc() & FPEXC_EN); 38 } 39 40 void vfp_enable(void) 41 { 42 vfp_write_fpexc(vfp_read_fpexc() | FPEXC_EN); 43 } 44 45 void vfp_disable(void) 46 { 47 vfp_write_fpexc(vfp_read_fpexc() & ~FPEXC_EN); 48 } 49 50 void vfp_lazy_save_state_init(struct vfp_state *state) 51 { 52 uint32_t fpexc = vfp_read_fpexc(); 53 54 state->fpexc = fpexc; 55 vfp_write_fpexc(fpexc & ~FPEXC_EN); 56 } 57 58 void vfp_lazy_save_state_final(struct vfp_state *state) 59 { 60 if (state->fpexc & FPEXC_EN) { 61 uint32_t fpexc = vfp_read_fpexc(); 62 63 assert(!(fpexc & FPEXC_EN)); 64 vfp_write_fpexc(fpexc | FPEXC_EN); 65 state->fpscr = vfp_read_fpscr(); 66 vfp_save_extension_regs(state->reg); 67 vfp_write_fpexc(fpexc); 68 } 69 } 70 71 void vfp_lazy_restore_state(struct vfp_state *state, bool full_state) 72 { 73 74 if (full_state) { 75 /* 76 * Only restore VFP registers if they have been touched as they 77 * otherwise are intact. 78 */ 79 80 /* FPEXC is restored to what's in state->fpexc below */ 81 vfp_write_fpexc(vfp_read_fpexc() | FPEXC_EN); 82 83 vfp_write_fpscr(state->fpscr); 84 vfp_restore_extension_regs(state->reg); 85 } 86 vfp_write_fpexc(state->fpexc); 87 } 88 #endif /* ARM32 */ 89 90 #ifdef ARM64 91 bool vfp_is_enabled(void) 92 { 93 return (CPACR_EL1_FPEN(read_cpacr_el1()) & CPACR_EL1_FPEN_EL0EL1); 94 } 95 96 void vfp_enable(void) 97 { 98 uint32_t val = read_cpacr_el1(); 99 100 val |= (CPACR_EL1_FPEN_EL0EL1 << CPACR_EL1_FPEN_SHIFT); 101 write_cpacr_el1(val); 102 isb(); 103 } 104 105 void vfp_disable(void) 106 { 107 uint32_t val = read_cpacr_el1(); 108 109 val &= ~(CPACR_EL1_FPEN_MASK << CPACR_EL1_FPEN_SHIFT); 110 write_cpacr_el1(val); 111 isb(); 112 } 113 114 void vfp_lazy_save_state_init(struct vfp_state *state) 115 { 116 state->cpacr_el1 = read_cpacr_el1(); 117 vfp_disable(); 118 } 119 120 void vfp_lazy_save_state_final(struct vfp_state *state) 121 { 122 if ((CPACR_EL1_FPEN(state->cpacr_el1) & CPACR_EL1_FPEN_EL0EL1) || 123 state->force_save) { 124 assert(!vfp_is_enabled()); 125 vfp_enable(); 126 state->fpcr = read_fpcr(); 127 state->fpsr = read_fpsr(); 128 vfp_save_extension_regs(state->reg); 129 vfp_disable(); 130 } 131 } 132 133 void vfp_lazy_restore_state(struct vfp_state *state, bool full_state) 134 { 135 if (full_state) { 136 /* 137 * Only restore VFP registers if they have been touched as they 138 * otherwise are intact. 139 */ 140 141 /* CPACR_EL1 is restored to what's in state->cpacr_el1 below */ 142 vfp_enable(); 143 write_fpcr(state->fpcr); 144 write_fpsr(state->fpsr); 145 vfp_restore_extension_regs(state->reg); 146 } 147 write_cpacr_el1(state->cpacr_el1); 148 isb(); 149 } 150 #endif /* ARM64 */ 151