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 struct fpu_kern_ctx { 59 struct vfp_state *prev; 60 #define FPU_KERN_CTX_DUMMY 0x01 /* avoided save for the kern thread */ 61 #define FPU_KERN_CTX_INUSE 0x02 62 uint32_t flags; 63 struct vfp_state state; 64 }; 65 66 /* 67 * About .fpu directives in this file... 68 * 69 * We should need simply .fpu vfpv3, but clang 3.5 has a quirk where setting 70 * vfpv3 doesn't imply that vfp2 features are also available -- both have to be 71 * explicitly set to get all the features of both. This is probably a bug in 72 * clang, so it may get fixed and require changes here some day. Other changes 73 * are probably coming in clang too, because there is email and open PRs 74 * indicating they want to completely disable the ability to use .fpu and 75 * similar directives in inline asm. That would be catastrophic for us, 76 * hopefully they come to their senses. There was also some discusion of a new 77 * syntax such as .push fpu=vfpv3; ...; .pop fpu; and that would be ideal for 78 * us, better than what we have now really. 79 * 80 * For gcc, each .fpu directive completely overrides the prior directive, unlike 81 * with clang, but luckily on gcc saying v3 implies all the v2 features as well. 82 */ 83 84 #define fmxr(reg, val) \ 85 __asm __volatile(" .fpu vfpv2\n .fpu vfpv3\n" \ 86 " vmsr " __STRING(reg) ", %0" :: "r"(val)); 87 88 #define fmrx(reg) \ 89 ({ u_int val = 0;\ 90 __asm __volatile(" .fpu vfpv2\n .fpu vfpv3\n" \ 91 " vmrs %0, " __STRING(reg) : "=r"(val)); \ 92 val; \ 93 }) 94 95 static u_int 96 get_coprocessorACR(void) 97 { 98 u_int val; 99 __asm __volatile("mrc p15, 0, %0, c1, c0, 2" : "=r" (val) : : "cc"); 100 return val; 101 } 102 103 static void 104 set_coprocessorACR(u_int val) 105 { 106 __asm __volatile("mcr p15, 0, %0, c1, c0, 2\n\t" 107 : : "r" (val) : "cc"); 108 isb(); 109 } 110 111 static void 112 vfp_enable(void) 113 { 114 uint32_t fpexc; 115 116 fpexc = fmrx(fpexc); 117 fmxr(fpexc, fpexc | VFPEXC_EN); 118 isb(); 119 } 120 121 static void 122 vfp_disable(void) 123 { 124 uint32_t fpexc; 125 126 fpexc = fmrx(fpexc); 127 fmxr(fpexc, fpexc & ~VFPEXC_EN); 128 isb(); 129 } 130 131 /* called for each cpu */ 132 void 133 vfp_init(void) 134 { 135 u_int fpsid, tmp; 136 u_int coproc, vfp_arch; 137 138 coproc = get_coprocessorACR(); 139 coproc |= COPROC10 | COPROC11; 140 set_coprocessorACR(coproc); 141 142 fpsid = fmrx(fpsid); /* read the vfp system id */ 143 144 if (!(fpsid & VFPSID_HARDSOFT_IMP)) { 145 vfp_exists = 1; 146 is_d32 = 0; 147 PCPU_SET(vfpsid, fpsid); /* save the fpsid */ 148 elf_hwcap |= HWCAP_VFP; 149 150 vfp_arch = 151 (fpsid & VFPSID_SUBVERSION2_MASK) >> VFPSID_SUBVERSION_OFF; 152 153 if (vfp_arch >= VFP_ARCH3) { 154 tmp = fmrx(mvfr0); 155 PCPU_SET(vfpmvfr0, tmp); 156 elf_hwcap |= HWCAP_VFPv3; 157 158 if ((tmp & VMVFR0_RB_MASK) == 2) { 159 elf_hwcap |= HWCAP_VFPD32; 160 is_d32 = 1; 161 } else 162 elf_hwcap |= HWCAP_VFPv3D16; 163 164 tmp = fmrx(mvfr1); 165 PCPU_SET(vfpmvfr1, tmp); 166 167 if (PCPU_GET(cpuid) == 0) { 168 if ((tmp & VMVFR1_FZ_MASK) == 0x1) { 169 /* Denormals arithmetic support */ 170 initial_fpscr &= ~VFPSCR_FZ; 171 thread0.td_pcb->pcb_vfpstate.fpscr = 172 initial_fpscr; 173 } 174 } 175 176 if ((tmp & VMVFR1_LS_MASK) >> VMVFR1_LS_OFF == 1 && 177 (tmp & VMVFR1_I_MASK) >> VMVFR1_I_OFF == 1 && 178 (tmp & VMVFR1_SP_MASK) >> VMVFR1_SP_OFF == 1) 179 elf_hwcap |= HWCAP_NEON; 180 if ((tmp & VMVFR1_FMAC_MASK) >> VMVFR1_FMAC_OFF == 1) 181 elf_hwcap |= HWCAP_VFPv4; 182 } 183 184 /* initialize the coprocess 10 and 11 calls 185 * These are called to restore the registers and enable 186 * the VFP hardware. 187 */ 188 if (vfp10_uh.uh_handler == NULL) { 189 vfp10_uh.uh_handler = vfp_bounce; 190 vfp11_uh.uh_handler = vfp_bounce; 191 install_coproc_handler_static(10, &vfp10_uh); 192 install_coproc_handler_static(11, &vfp11_uh); 193 } 194 } 195 } 196 197 SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL); 198 199 /* start VFP unit, restore the vfp registers from the PCB and retry 200 * the instruction 201 */ 202 static int 203 vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code) 204 { 205 u_int cpu, fpexc; 206 struct pcb *curpcb; 207 ksiginfo_t ksi; 208 209 if ((code & FAULT_USER) == 0) 210 panic("undefined floating point instruction in supervisor mode"); 211 212 critical_enter(); 213 214 /* 215 * If the VFP is already on and we got an undefined instruction, then 216 * something tried to executate a truly invalid instruction that maps to 217 * the VFP. 218 */ 219 fpexc = fmrx(fpexc); 220 if (fpexc & VFPEXC_EN) { 221 /* Clear any exceptions */ 222 fmxr(fpexc, fpexc & ~(VFPEXC_EX | VFPEXC_FP2V)); 223 224 /* kill the process - we do not handle emulation */ 225 critical_exit(); 226 227 if (fpexc & VFPEXC_EX) { 228 /* We have an exception, signal a SIGFPE */ 229 ksiginfo_init_trap(&ksi); 230 ksi.ksi_signo = SIGFPE; 231 if (fpexc & VFPEXC_UFC) 232 ksi.ksi_code = FPE_FLTUND; 233 else if (fpexc & VFPEXC_OFC) 234 ksi.ksi_code = FPE_FLTOVF; 235 else if (fpexc & VFPEXC_IOC) 236 ksi.ksi_code = FPE_FLTINV; 237 ksi.ksi_addr = (void *)addr; 238 trapsignal(curthread, &ksi); 239 return 0; 240 } 241 242 return 1; 243 } 244 245 /* 246 * If the last time this thread used the VFP it was on this core, and 247 * the last thread to use the VFP on this core was this thread, then the 248 * VFP state is valid, otherwise restore this thread's state to the VFP. 249 */ 250 fmxr(fpexc, fpexc | VFPEXC_EN); 251 curpcb = curthread->td_pcb; 252 cpu = PCPU_GET(cpuid); 253 if (curpcb->pcb_vfpcpu != cpu || curthread != PCPU_GET(fpcurthread)) { 254 if (curpcb->pcb_vfpsaved == NULL) 255 curpcb->pcb_vfpsaved = &curpcb->pcb_vfpstate; 256 vfp_restore(curpcb->pcb_vfpsaved); 257 curpcb->pcb_vfpcpu = cpu; 258 PCPU_SET(fpcurthread, curthread); 259 } 260 261 critical_exit(); 262 return (0); 263 } 264 265 /* 266 * Restore the given state to the VFP hardware. 267 */ 268 static void 269 vfp_restore(struct vfp_state *vfpsave) 270 { 271 uint32_t fpexc; 272 273 /* On vfpv3 we may need to restore FPINST and FPINST2 */ 274 fpexc = vfpsave->fpexec; 275 if (fpexc & VFPEXC_EX) { 276 fmxr(fpinst, vfpsave->fpinst); 277 if (fpexc & VFPEXC_FP2V) 278 fmxr(fpinst2, vfpsave->fpinst2); 279 } 280 fmxr(fpscr, vfpsave->fpscr); 281 282 __asm __volatile( 283 " .fpu vfpv2\n" 284 " .fpu vfpv3\n" 285 " vldmia %0!, {d0-d15}\n" /* d0-d15 */ 286 " cmp %1, #0\n" /* -D16 or -D32? */ 287 " vldmiane %0!, {d16-d31}\n" /* d16-d31 */ 288 " addeq %0, %0, #128\n" /* skip missing regs */ 289 : "+&r" (vfpsave) : "r" (is_d32) : "cc" 290 ); 291 292 fmxr(fpexc, fpexc); 293 } 294 295 /* 296 * If the VFP is on, save its current state and turn it off if requested to do 297 * so. If the VFP is not on, does not change the values at *vfpsave. Caller is 298 * responsible for preventing a context switch while this is running. 299 */ 300 void 301 vfp_store(struct vfp_state *vfpsave, boolean_t disable_vfp) 302 { 303 uint32_t fpexc; 304 305 fpexc = fmrx(fpexc); /* Is the vfp enabled? */ 306 if (fpexc & VFPEXC_EN) { 307 vfpsave->fpexec = fpexc; 308 vfpsave->fpscr = fmrx(fpscr); 309 310 /* On vfpv3 we may need to save FPINST and FPINST2 */ 311 if (fpexc & VFPEXC_EX) { 312 vfpsave->fpinst = fmrx(fpinst); 313 if (fpexc & VFPEXC_FP2V) 314 vfpsave->fpinst2 = fmrx(fpinst2); 315 fpexc &= ~VFPEXC_EX; 316 } 317 318 __asm __volatile( 319 " .fpu vfpv2\n" 320 " .fpu vfpv3\n" 321 " vstmia %0!, {d0-d15}\n" /* d0-d15 */ 322 " cmp %1, #0\n" /* -D16 or -D32? */ 323 " vstmiane %0!, {d16-d31}\n" /* d16-d31 */ 324 " addeq %0, %0, #128\n" /* skip missing regs */ 325 : "+&r" (vfpsave) : "r" (is_d32) : "cc" 326 ); 327 328 if (disable_vfp) 329 fmxr(fpexc , fpexc & ~VFPEXC_EN); 330 } 331 } 332 333 /* 334 * The current thread is dying. If the state currently in the hardware belongs 335 * to the current thread, set fpcurthread to NULL to indicate that the VFP 336 * hardware state does not belong to any thread. If the VFP is on, turn it off. 337 * Called only from cpu_throw(), so we don't have to worry about a context 338 * switch here. 339 */ 340 void 341 vfp_discard(struct thread *td) 342 { 343 u_int tmp; 344 345 if (PCPU_GET(fpcurthread) == td) 346 PCPU_SET(fpcurthread, NULL); 347 348 tmp = fmrx(fpexc); 349 if (tmp & VFPEXC_EN) 350 fmxr(fpexc, tmp & ~VFPEXC_EN); 351 } 352 353 void 354 vfp_save_state(struct thread *td, struct pcb *pcb) 355 { 356 int32_t fpexc; 357 358 KASSERT(pcb != NULL, ("NULL vfp pcb")); 359 KASSERT(td == NULL || td->td_pcb == pcb, ("Invalid vfp pcb")); 360 361 /* 362 * savectx() will be called on panic with dumppcb as an argument, 363 * dumppcb doesn't have pcb_vfpsaved set, so set it to save 364 * the VFP registers. 365 */ 366 if (pcb->pcb_vfpsaved == NULL) 367 pcb->pcb_vfpsaved = &pcb->pcb_vfpstate; 368 369 if (td == NULL) 370 td = curthread; 371 372 critical_enter(); 373 /* 374 * Only store the registers if the VFP is enabled, 375 * i.e. return if we are trapping on FP access. 376 */ 377 fpexc = fmrx(fpexc); 378 if (fpexc & VFPEXC_EN) { 379 KASSERT(PCPU_GET(fpcurthread) == td, 380 ("Storing an invalid VFP state")); 381 382 vfp_store(pcb->pcb_vfpsaved, true); 383 } 384 critical_exit(); 385 } 386 387 void 388 fpu_kern_enter(struct thread *td, struct fpu_kern_ctx *ctx, u_int flags) 389 { 390 struct pcb *pcb; 391 392 pcb = td->td_pcb; 393 KASSERT((flags & FPU_KERN_NOCTX) != 0 || ctx != NULL, 394 ("ctx is required when !FPU_KERN_NOCTX")); 395 KASSERT(ctx == NULL || (ctx->flags & FPU_KERN_CTX_INUSE) == 0, 396 ("using inuse ctx")); 397 KASSERT((pcb->pcb_fpflags & PCB_FP_NOSAVE) == 0, 398 ("recursive fpu_kern_enter while in PCB_FP_NOSAVE state")); 399 400 if ((flags & FPU_KERN_NOCTX) != 0) { 401 critical_enter(); 402 if (curthread == PCPU_GET(fpcurthread)) { 403 vfp_save_state(curthread, pcb); 404 } 405 PCPU_SET(fpcurthread, NULL); 406 407 vfp_enable(); 408 pcb->pcb_fpflags |= PCB_FP_KERN | PCB_FP_NOSAVE | 409 PCB_FP_STARTED; 410 return; 411 } 412 413 if ((flags & FPU_KERN_KTHR) != 0 && is_fpu_kern_thread(0)) { 414 ctx->flags = FPU_KERN_CTX_DUMMY | FPU_KERN_CTX_INUSE; 415 return; 416 } 417 /* 418 * Check either we are already using the VFP in the kernel, or 419 * the the saved state points to the default user space. 420 */ 421 KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0 || 422 pcb->pcb_vfpsaved == &pcb->pcb_vfpstate, 423 ("Mangled pcb_vfpsaved %x %p %p", pcb->pcb_fpflags, pcb->pcb_vfpsaved, 424 &pcb->pcb_vfpstate)); 425 ctx->flags = FPU_KERN_CTX_INUSE; 426 vfp_save_state(curthread, pcb); 427 ctx->prev = pcb->pcb_vfpsaved; 428 pcb->pcb_vfpsaved = &ctx->state; 429 pcb->pcb_fpflags |= PCB_FP_KERN; 430 pcb->pcb_fpflags &= ~PCB_FP_STARTED; 431 432 return; 433 } 434 435 int 436 fpu_kern_leave(struct thread *td, struct fpu_kern_ctx *ctx) 437 { 438 struct pcb *pcb; 439 440 pcb = td->td_pcb; 441 442 if ((pcb->pcb_fpflags & PCB_FP_NOSAVE) != 0) { 443 KASSERT(ctx == NULL, ("non-null ctx after FPU_KERN_NOCTX")); 444 KASSERT(PCPU_GET(fpcurthread) == NULL, 445 ("non-NULL fpcurthread for PCB_FP_NOSAVE")); 446 CRITICAL_ASSERT(td); 447 448 vfp_disable(); 449 pcb->pcb_fpflags &= ~(PCB_FP_NOSAVE | PCB_FP_STARTED); 450 critical_exit(); 451 } else { 452 KASSERT((ctx->flags & FPU_KERN_CTX_INUSE) != 0, 453 ("FPU context not inuse")); 454 ctx->flags &= ~FPU_KERN_CTX_INUSE; 455 456 if (is_fpu_kern_thread(0) && 457 (ctx->flags & FPU_KERN_CTX_DUMMY) != 0) 458 return (0); 459 KASSERT((ctx->flags & FPU_KERN_CTX_DUMMY) == 0, ("dummy ctx")); 460 critical_enter(); 461 vfp_discard(td); 462 critical_exit(); 463 pcb->pcb_fpflags &= ~PCB_FP_STARTED; 464 pcb->pcb_vfpsaved = ctx->prev; 465 } 466 467 if (pcb->pcb_vfpsaved == &pcb->pcb_vfpstate) { 468 pcb->pcb_fpflags &= ~PCB_FP_KERN; 469 } else { 470 KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0, 471 ("unpaired fpu_kern_leave")); 472 } 473 474 return (0); 475 } 476 477 int 478 fpu_kern_thread(u_int flags __unused) 479 { 480 struct pcb *pcb = curthread->td_pcb; 481 482 KASSERT((curthread->td_pflags & TDP_KTHREAD) != 0, 483 ("Only kthread may use fpu_kern_thread")); 484 KASSERT(pcb->pcb_vfpsaved == &pcb->pcb_vfpstate, 485 ("Mangled pcb_vfpsaved")); 486 KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) == 0, 487 ("Thread already setup for the VFP")); 488 pcb->pcb_fpflags |= PCB_FP_KERN; 489 return (0); 490 } 491 492 int 493 is_fpu_kern_thread(u_int flags __unused) 494 { 495 struct pcb *curpcb; 496 497 if ((curthread->td_pflags & TDP_KTHREAD) == 0) 498 return (0); 499 curpcb = curthread->td_pcb; 500 return ((curpcb->pcb_fpflags & PCB_FP_KERN) != 0); 501 } 502 503 #endif 504