1 /*- 2 * SPDX-License-Identifier: BSD-2-Clause-FreeBSD 3 * 4 * Copyright (c) 2014 Ian Lepore <ian@freebsd.org> 5 * Copyright (c) 2012 Mark Tinguely 6 * 7 * All rights reserved. 8 * 9 * Redistribution and use in source and binary forms, with or without 10 * modification, are permitted provided that the following conditions 11 * are met: 12 * 1. Redistributions of source code must retain the above copyright 13 * notice, this list of conditions and the following disclaimer. 14 * 2. Redistributions in binary form must reproduce the above copyright 15 * notice, this list of conditions and the following disclaimer in the 16 * documentation and/or other materials provided with the distribution. 17 * 18 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 19 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 22 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 24 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 25 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 26 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 27 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 28 * SUCH DAMAGE. 29 */ 30 31 #include <sys/cdefs.h> 32 __FBSDID("$FreeBSD$"); 33 34 #ifdef VFP 35 #include <sys/param.h> 36 #include <sys/systm.h> 37 #include <sys/proc.h> 38 #include <sys/imgact_elf.h> 39 #include <sys/kernel.h> 40 41 #include <machine/armreg.h> 42 #include <machine/elf.h> 43 #include <machine/frame.h> 44 #include <machine/md_var.h> 45 #include <machine/pcb.h> 46 #include <machine/undefined.h> 47 #include <machine/vfp.h> 48 49 /* function prototypes */ 50 static int vfp_bounce(u_int, u_int, struct trapframe *, int); 51 static void vfp_restore(struct vfp_state *); 52 53 extern int vfp_exists; 54 static struct undefined_handler vfp10_uh, vfp11_uh; 55 /* If true the VFP unit has 32 double registers, otherwise it has 16 */ 56 static int is_d32; 57 58 /* 59 * About .fpu directives in this file... 60 * 61 * We should need simply .fpu vfpv3, but clang 3.5 has a quirk where setting 62 * vfpv3 doesn't imply that vfp2 features are also available -- both have to be 63 * explicitly set to get all the features of both. This is probably a bug in 64 * clang, so it may get fixed and require changes here some day. Other changes 65 * are probably coming in clang too, because there is email and open PRs 66 * indicating they want to completely disable the ability to use .fpu and 67 * similar directives in inline asm. That would be catastrophic for us, 68 * hopefully they come to their senses. There was also some discusion of a new 69 * syntax such as .push fpu=vfpv3; ...; .pop fpu; and that would be ideal for 70 * us, better than what we have now really. 71 * 72 * For gcc, each .fpu directive completely overrides the prior directive, unlike 73 * with clang, but luckily on gcc saying v3 implies all the v2 features as well. 74 */ 75 76 #define fmxr(reg, val) \ 77 __asm __volatile(" .fpu vfpv2\n .fpu vfpv3\n" \ 78 " vmsr " __STRING(reg) ", %0" :: "r"(val)); 79 80 #define fmrx(reg) \ 81 ({ u_int val = 0;\ 82 __asm __volatile(" .fpu vfpv2\n .fpu vfpv3\n" \ 83 " vmrs %0, " __STRING(reg) : "=r"(val)); \ 84 val; \ 85 }) 86 87 static u_int 88 get_coprocessorACR(void) 89 { 90 u_int val; 91 __asm __volatile("mrc p15, 0, %0, c1, c0, 2" : "=r" (val) : : "cc"); 92 return val; 93 } 94 95 static void 96 set_coprocessorACR(u_int val) 97 { 98 __asm __volatile("mcr p15, 0, %0, c1, c0, 2\n\t" 99 : : "r" (val) : "cc"); 100 isb(); 101 } 102 103 104 /* called for each cpu */ 105 void 106 vfp_init(void) 107 { 108 u_int fpsid, fpexc, tmp; 109 u_int coproc, vfp_arch; 110 111 coproc = get_coprocessorACR(); 112 coproc |= COPROC10 | COPROC11; 113 set_coprocessorACR(coproc); 114 115 fpsid = fmrx(fpsid); /* read the vfp system id */ 116 fpexc = fmrx(fpexc); /* read the vfp exception reg */ 117 118 if (!(fpsid & VFPSID_HARDSOFT_IMP)) { 119 vfp_exists = 1; 120 is_d32 = 0; 121 PCPU_SET(vfpsid, fpsid); /* save the fpsid */ 122 elf_hwcap |= HWCAP_VFP; 123 124 vfp_arch = 125 (fpsid & VFPSID_SUBVERSION2_MASK) >> VFPSID_SUBVERSION_OFF; 126 127 if (vfp_arch >= VFP_ARCH3) { 128 tmp = fmrx(mvfr0); 129 PCPU_SET(vfpmvfr0, tmp); 130 elf_hwcap |= HWCAP_VFPv3; 131 132 if ((tmp & VMVFR0_RB_MASK) == 2) { 133 elf_hwcap |= HWCAP_VFPD32; 134 is_d32 = 1; 135 } else 136 elf_hwcap |= HWCAP_VFPv3D16; 137 138 tmp = fmrx(mvfr1); 139 PCPU_SET(vfpmvfr1, tmp); 140 141 if (PCPU_GET(cpuid) == 0) { 142 if ((tmp & VMVFR1_FZ_MASK) == 0x1) { 143 /* Denormals arithmetic support */ 144 initial_fpscr &= ~VFPSCR_FZ; 145 thread0.td_pcb->pcb_vfpstate.fpscr = 146 initial_fpscr; 147 } 148 } 149 150 if ((tmp & VMVFR1_LS_MASK) >> VMVFR1_LS_OFF == 1 && 151 (tmp & VMVFR1_I_MASK) >> VMVFR1_I_OFF == 1 && 152 (tmp & VMVFR1_SP_MASK) >> VMVFR1_SP_OFF == 1) 153 elf_hwcap |= HWCAP_NEON; 154 if ((tmp & VMVFR1_FMAC_MASK) >> VMVFR1_FMAC_OFF == 1) 155 elf_hwcap |= HWCAP_VFPv4; 156 } 157 158 /* initialize the coprocess 10 and 11 calls 159 * These are called to restore the registers and enable 160 * the VFP hardware. 161 */ 162 if (vfp10_uh.uh_handler == NULL) { 163 vfp10_uh.uh_handler = vfp_bounce; 164 vfp11_uh.uh_handler = vfp_bounce; 165 install_coproc_handler_static(10, &vfp10_uh); 166 install_coproc_handler_static(11, &vfp11_uh); 167 } 168 } 169 } 170 171 SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL); 172 173 174 /* start VFP unit, restore the vfp registers from the PCB and retry 175 * the instruction 176 */ 177 static int 178 vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code) 179 { 180 u_int cpu, fpexc; 181 struct pcb *curpcb; 182 ksiginfo_t ksi; 183 184 if ((code & FAULT_USER) == 0) 185 panic("undefined floating point instruction in supervisor mode"); 186 187 critical_enter(); 188 189 /* 190 * If the VFP is already on and we got an undefined instruction, then 191 * something tried to executate a truly invalid instruction that maps to 192 * the VFP. 193 */ 194 fpexc = fmrx(fpexc); 195 if (fpexc & VFPEXC_EN) { 196 /* Clear any exceptions */ 197 fmxr(fpexc, fpexc & ~(VFPEXC_EX | VFPEXC_FP2V)); 198 199 /* kill the process - we do not handle emulation */ 200 critical_exit(); 201 202 if (fpexc & VFPEXC_EX) { 203 /* We have an exception, signal a SIGFPE */ 204 ksiginfo_init_trap(&ksi); 205 ksi.ksi_signo = SIGFPE; 206 if (fpexc & VFPEXC_UFC) 207 ksi.ksi_code = FPE_FLTUND; 208 else if (fpexc & VFPEXC_OFC) 209 ksi.ksi_code = FPE_FLTOVF; 210 else if (fpexc & VFPEXC_IOC) 211 ksi.ksi_code = FPE_FLTINV; 212 ksi.ksi_addr = (void *)addr; 213 trapsignal(curthread, &ksi); 214 return 0; 215 } 216 217 return 1; 218 } 219 220 /* 221 * If the last time this thread used the VFP it was on this core, and 222 * the last thread to use the VFP on this core was this thread, then the 223 * VFP state is valid, otherwise restore this thread's state to the VFP. 224 */ 225 fmxr(fpexc, fpexc | VFPEXC_EN); 226 curpcb = curthread->td_pcb; 227 cpu = PCPU_GET(cpuid); 228 if (curpcb->pcb_vfpcpu != cpu || curthread != PCPU_GET(fpcurthread)) { 229 vfp_restore(&curpcb->pcb_vfpstate); 230 curpcb->pcb_vfpcpu = cpu; 231 PCPU_SET(fpcurthread, curthread); 232 } 233 234 critical_exit(); 235 return (0); 236 } 237 238 /* 239 * Restore the given state to the VFP hardware. 240 */ 241 static void 242 vfp_restore(struct vfp_state *vfpsave) 243 { 244 uint32_t fpexc; 245 246 /* On vfpv3 we may need to restore FPINST and FPINST2 */ 247 fpexc = vfpsave->fpexec; 248 if (fpexc & VFPEXC_EX) { 249 fmxr(fpinst, vfpsave->fpinst); 250 if (fpexc & VFPEXC_FP2V) 251 fmxr(fpinst2, vfpsave->fpinst2); 252 } 253 fmxr(fpscr, vfpsave->fpscr); 254 255 __asm __volatile( 256 " .fpu vfpv2\n" 257 " .fpu vfpv3\n" 258 " vldmia %0!, {d0-d15}\n" /* d0-d15 */ 259 " cmp %1, #0\n" /* -D16 or -D32? */ 260 " vldmiane %0!, {d16-d31}\n" /* d16-d31 */ 261 " addeq %0, %0, #128\n" /* skip missing regs */ 262 : "+&r" (vfpsave) : "r" (is_d32) : "cc" 263 ); 264 265 fmxr(fpexc, fpexc); 266 } 267 268 /* 269 * If the VFP is on, save its current state and turn it off if requested to do 270 * so. If the VFP is not on, does not change the values at *vfpsave. Caller is 271 * responsible for preventing a context switch while this is running. 272 */ 273 void 274 vfp_store(struct vfp_state *vfpsave, boolean_t disable_vfp) 275 { 276 uint32_t fpexc; 277 278 fpexc = fmrx(fpexc); /* Is the vfp enabled? */ 279 if (fpexc & VFPEXC_EN) { 280 vfpsave->fpexec = fpexc; 281 vfpsave->fpscr = fmrx(fpscr); 282 283 /* On vfpv3 we may need to save FPINST and FPINST2 */ 284 if (fpexc & VFPEXC_EX) { 285 vfpsave->fpinst = fmrx(fpinst); 286 if (fpexc & VFPEXC_FP2V) 287 vfpsave->fpinst2 = fmrx(fpinst2); 288 fpexc &= ~VFPEXC_EX; 289 } 290 291 __asm __volatile( 292 " .fpu vfpv2\n" 293 " .fpu vfpv3\n" 294 " vstmia %0!, {d0-d15}\n" /* d0-d15 */ 295 " cmp %1, #0\n" /* -D16 or -D32? */ 296 " vstmiane %0!, {d16-d31}\n" /* d16-d31 */ 297 " addeq %0, %0, #128\n" /* skip missing regs */ 298 : "+&r" (vfpsave) : "r" (is_d32) : "cc" 299 ); 300 301 if (disable_vfp) 302 fmxr(fpexc , fpexc & ~VFPEXC_EN); 303 } 304 } 305 306 /* 307 * The current thread is dying. If the state currently in the hardware belongs 308 * to the current thread, set fpcurthread to NULL to indicate that the VFP 309 * hardware state does not belong to any thread. If the VFP is on, turn it off. 310 * Called only from cpu_throw(), so we don't have to worry about a context 311 * switch here. 312 */ 313 void 314 vfp_discard(struct thread *td) 315 { 316 u_int tmp; 317 318 if (PCPU_GET(fpcurthread) == td) 319 PCPU_SET(fpcurthread, NULL); 320 321 tmp = fmrx(fpexc); 322 if (tmp & VFPEXC_EN) 323 fmxr(fpexc, tmp & ~VFPEXC_EN); 324 } 325 326 #endif 327 328