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 <kernel/vfp.h> 30 #include "vfp_private.h" 31 #include <assert.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_EL1 << CPACR_EL1_FPEN_SHIFT); 100 write_cpacr_el1(val); 101 } 102 103 void vfp_disable(void) 104 { 105 uint32_t val = read_cpacr_el1(); 106 107 val &= ~(CPACR_EL1_FPEN_MASK << CPACR_EL1_FPEN_SHIFT); 108 write_cpacr_el1(val); 109 } 110 111 void vfp_lazy_save_state_init(struct vfp_state *state) 112 { 113 state->cpacr_el1 = read_cpacr_el1(); 114 vfp_disable(); 115 } 116 117 void vfp_lazy_save_state_final(struct vfp_state *state) 118 { 119 if ((CPACR_EL1_FPEN(state->cpacr_el1) & CPACR_EL1_FPEN_EL0EL1) || 120 state->force_save) { 121 assert(!vfp_is_enabled()); 122 vfp_enable(); 123 state->fpcr = read_fpcr(); 124 state->fpsr = read_fpsr(); 125 vfp_save_extension_regs(state->reg); 126 vfp_disable(); 127 } 128 } 129 130 void vfp_lazy_restore_state(struct vfp_state *state, bool full_state) 131 { 132 if (full_state) { 133 /* 134 * Only restore VFP registers if they have been touched as they 135 * otherwise are intact. 136 */ 137 138 /* CPACR_EL1 is restored to what's in state->cpacr_el1 below */ 139 vfp_enable(); 140 write_fpcr(state->fpcr); 141 write_fpsr(state->fpsr); 142 vfp_restore_extension_regs(state->reg); 143 } 144 write_cpacr_el1(state->cpacr_el1); 145 } 146 #endif /* ARM64 */ 147