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