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