1 /* $NetBSD: vfp_init.c,v 1.64 2019/10/29 16:18:23 joerg Exp $ */ 2 3 /* 4 * Copyright (c) 2008 ARM Ltd 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 * 3. The name of the company may not be used to endorse or promote 16 * products derived from this software without specific prior written 17 * permission. 18 * 19 * THIS SOFTWARE IS PROVIDED BY ARM LTD ``AS IS'' AND ANY EXPRESS OR 20 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 * ARE DISCLAIMED. IN NO EVENT SHALL ARM LTD BE LIABLE FOR ANY 23 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 25 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 27 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 28 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 */ 31 32 #include "opt_cputypes.h" 33 34 #include <sys/cdefs.h> 35 __KERNEL_RCSID(0, "$NetBSD: vfp_init.c,v 1.64 2019/10/29 16:18:23 joerg Exp $"); 36 37 #include <sys/param.h> 38 #include <sys/types.h> 39 #include <sys/systm.h> 40 #include <sys/device.h> 41 #include <sys/proc.h> 42 #include <sys/cpu.h> 43 44 #include <arm/locore.h> 45 #include <arm/pcb.h> 46 #include <arm/undefined.h> 47 #include <arm/vfpreg.h> 48 #include <arm/mcontext.h> 49 50 #include <uvm/uvm_extern.h> /* for pmap.h */ 51 52 #ifdef FPU_VFP 53 54 #ifdef CPU_CORTEX 55 #define SETFPU __asm(".fpu\tvfpv4") 56 #else 57 #define SETFPU __asm(".fpu\tvfp") 58 #endif 59 SETFPU; 60 61 /* FLDMD <X>, {d0-d15} */ 62 static inline void 63 load_vfpregs_lo(const uint64_t *p) 64 { 65 SETFPU; 66 __asm __volatile(".fpu vfp\n vldmia\t%0, {d0-d15}" :: "r" (p) : "memory"); 67 } 68 69 /* FSTMD <X>, {d0-d15} */ 70 static inline void 71 save_vfpregs_lo(uint64_t *p) 72 { 73 SETFPU; 74 __asm __volatile(".fpu vfp\n vstmia\t%0, {d0-d15}" :: "r" (p) : "memory"); 75 } 76 77 #ifdef CPU_CORTEX 78 /* FLDMD <X>, {d16-d31} */ 79 static inline void 80 load_vfpregs_hi(const uint64_t *p) 81 { 82 SETFPU; 83 __asm __volatile(".fpu neon-vfpv4\n vldmia\t%0, {d16-d31}" :: "r" (&p[16]) : "memory"); 84 } 85 86 /* FLDMD <X>, {d16-d31} */ 87 static inline void 88 save_vfpregs_hi(uint64_t *p) 89 { 90 SETFPU; 91 __asm __volatile(".fpu neon-vfpv4\nvstmia\t%0, {d16-d31}" :: "r" (&p[16]) : "memory"); 92 } 93 #endif 94 95 static inline void 96 load_vfpregs(const struct vfpreg *fregs) 97 { 98 load_vfpregs_lo(fregs->vfp_regs); 99 #ifdef CPU_CORTEX 100 #ifdef CPU_ARM11 101 switch (curcpu()->ci_vfp_id) { 102 case FPU_VFP_CORTEXA5: 103 case FPU_VFP_CORTEXA7: 104 case FPU_VFP_CORTEXA8: 105 case FPU_VFP_CORTEXA9: 106 case FPU_VFP_CORTEXA15: 107 case FPU_VFP_CORTEXA15_QEMU: 108 case FPU_VFP_CORTEXA53: 109 case FPU_VFP_CORTEXA57: 110 #endif 111 load_vfpregs_hi(fregs->vfp_regs); 112 #ifdef CPU_ARM11 113 break; 114 } 115 #endif 116 #endif 117 } 118 119 static inline void 120 save_vfpregs(struct vfpreg *fregs) 121 { 122 save_vfpregs_lo(fregs->vfp_regs); 123 #ifdef CPU_CORTEX 124 #ifdef CPU_ARM11 125 switch (curcpu()->ci_vfp_id) { 126 case FPU_VFP_CORTEXA5: 127 case FPU_VFP_CORTEXA7: 128 case FPU_VFP_CORTEXA8: 129 case FPU_VFP_CORTEXA9: 130 case FPU_VFP_CORTEXA15: 131 case FPU_VFP_CORTEXA15_QEMU: 132 case FPU_VFP_CORTEXA53: 133 case FPU_VFP_CORTEXA57: 134 #endif 135 save_vfpregs_hi(fregs->vfp_regs); 136 #ifdef CPU_ARM11 137 break; 138 } 139 #endif 140 #endif 141 } 142 143 /* The real handler for VFP bounces. */ 144 static int vfp_handler(u_int, u_int, trapframe_t *, int); 145 #ifdef CPU_CORTEX 146 static int neon_handler(u_int, u_int, trapframe_t *, int); 147 #endif 148 149 static void vfp_state_load(lwp_t *, u_int); 150 static void vfp_state_save(lwp_t *); 151 static void vfp_state_release(lwp_t *); 152 153 const pcu_ops_t arm_vfp_ops = { 154 .pcu_id = PCU_FPU, 155 .pcu_state_save = vfp_state_save, 156 .pcu_state_load = vfp_state_load, 157 .pcu_state_release = vfp_state_release, 158 }; 159 160 /* determine what bits can be changed */ 161 uint32_t vfp_fpscr_changable = VFP_FPSCR_CSUM; 162 /* default to run fast */ 163 uint32_t vfp_fpscr_default = (VFP_FPSCR_DN | VFP_FPSCR_FZ | VFP_FPSCR_RN); 164 165 /* 166 * Used to test for a VFP. The following function is installed as a coproc10 167 * handler on the undefined instruction vector and then we issue a VFP 168 * instruction. If undefined_test is non zero then the VFP did not handle 169 * the instruction so must be absent, or disabled. 170 */ 171 172 static int undefined_test; 173 174 static int 175 vfp_test(u_int address, u_int insn, trapframe_t *frame, int fault_code) 176 { 177 178 frame->tf_pc += INSN_SIZE; 179 ++undefined_test; 180 return 0; 181 } 182 183 #else 184 /* determine what bits can be changed */ 185 uint32_t vfp_fpscr_changable = VFP_FPSCR_CSUM|VFP_FPSCR_ESUM|VFP_FPSCR_RMODE; 186 #endif /* FPU_VFP */ 187 188 static int 189 vfp_fpscr_handler(u_int address, u_int insn, trapframe_t *frame, int fault_code) 190 { 191 struct lwp * const l = curlwp; 192 const u_int regno = (insn >> 12) & 0xf; 193 /* 194 * Only match move to/from the FPSCR register and we 195 * can't be using the SP,LR,PC as a source. 196 */ 197 if ((insn & 0xffef0fff) != 0xeee10a10 || regno > 12) 198 return 1; 199 200 struct pcb * const pcb = lwp_getpcb(l); 201 202 #ifdef FPU_VFP 203 /* 204 * If FPU is valid somewhere, let's just reenable VFP and 205 * retry the instruction (only safe thing to do since the 206 * pcb has a stale copy). 207 */ 208 if (pcb->pcb_vfp.vfp_fpexc & VFP_FPEXC_EN) 209 return 1; 210 211 if (__predict_false(!vfp_used_p(l))) { 212 pcb->pcb_vfp.vfp_fpscr = vfp_fpscr_default; 213 } 214 #endif 215 216 /* 217 * We now know the pcb has the saved copy. 218 */ 219 register_t * const regp = &frame->tf_r0 + regno; 220 if (insn & 0x00100000) { 221 *regp = pcb->pcb_vfp.vfp_fpscr; 222 } else { 223 pcb->pcb_vfp.vfp_fpscr &= ~vfp_fpscr_changable; 224 pcb->pcb_vfp.vfp_fpscr |= *regp & vfp_fpscr_changable; 225 } 226 227 curcpu()->ci_vfp_evs[0].ev_count++; 228 229 frame->tf_pc += INSN_SIZE; 230 return 0; 231 } 232 233 #ifndef FPU_VFP 234 /* 235 * If we don't want VFP support, we still need to handle emulating VFP FPSCR 236 * instructions. 237 */ 238 void 239 vfp_attach(struct cpu_info *ci) 240 { 241 if (CPU_IS_PRIMARY(ci)) { 242 install_coproc_handler(VFP_COPROC, vfp_fpscr_handler); 243 } 244 evcnt_attach_dynamic(&ci->ci_vfp_evs[0], EVCNT_TYPE_TRAP, NULL, 245 ci->ci_cpuname, "vfp fpscr traps"); 246 } 247 248 #else 249 void 250 vfp_attach(struct cpu_info *ci) 251 { 252 const char *model = NULL; 253 254 if (CPU_ID_ARM11_P(ci->ci_arm_cpuid) 255 || CPU_ID_MV88SV58XX_P(ci->ci_arm_cpuid) 256 || CPU_ID_CORTEX_P(ci->ci_arm_cpuid)) { 257 #if 0 258 const uint32_t nsacr = armreg_nsacr_read(); 259 const uint32_t nsacr_vfp = __BITS(VFP_COPROC,VFP_COPROC2); 260 if ((nsacr & nsacr_vfp) != nsacr_vfp) { 261 aprint_normal_dev(ci->ci_dev, 262 "VFP access denied (NSACR=%#x)\n", nsacr); 263 if (CPU_IS_PRIMARY(ci)) 264 install_coproc_handler(VFP_COPROC, vfp_fpscr_handler); 265 ci->ci_vfp_id = 0; 266 evcnt_attach_dynamic(&ci->ci_vfp_evs[0], 267 EVCNT_TYPE_TRAP, NULL, ci->ci_cpuname, 268 "vfp fpscr traps"); 269 return; 270 } 271 #endif 272 const uint32_t cpacr_vfp = CPACR_CPn(VFP_COPROC); 273 const uint32_t cpacr_vfp2 = CPACR_CPn(VFP_COPROC2); 274 275 /* 276 * We first need to enable access to the coprocessors. 277 */ 278 uint32_t cpacr = armreg_cpacr_read(); 279 cpacr |= __SHIFTIN(CPACR_ALL, cpacr_vfp); 280 cpacr |= __SHIFTIN(CPACR_ALL, cpacr_vfp2); 281 armreg_cpacr_write(cpacr); 282 283 arm_isb(); 284 285 /* 286 * If we could enable them, then they exist. 287 */ 288 cpacr = armreg_cpacr_read(); 289 bool vfp_p = __SHIFTOUT(cpacr, cpacr_vfp2) == CPACR_ALL 290 && __SHIFTOUT(cpacr, cpacr_vfp) == CPACR_ALL; 291 if (!vfp_p) { 292 aprint_normal_dev(ci->ci_dev, 293 "VFP access denied (CPACR=%#x)\n", cpacr); 294 if (CPU_IS_PRIMARY(ci)) 295 install_coproc_handler(VFP_COPROC, vfp_fpscr_handler); 296 ci->ci_vfp_id = 0; 297 evcnt_attach_dynamic(&ci->ci_vfp_evs[0], 298 EVCNT_TYPE_TRAP, NULL, ci->ci_cpuname, 299 "vfp fpscr traps"); 300 return; 301 } 302 } 303 304 void *uh = install_coproc_handler(VFP_COPROC, vfp_test); 305 306 undefined_test = 0; 307 308 const uint32_t fpsid = armreg_fpsid_read(); 309 310 remove_coproc_handler(uh); 311 312 if (undefined_test != 0) { 313 aprint_normal_dev(ci->ci_dev, "No VFP detected\n"); 314 if (CPU_IS_PRIMARY(ci)) 315 install_coproc_handler(VFP_COPROC, vfp_fpscr_handler); 316 ci->ci_vfp_id = 0; 317 return; 318 } 319 320 ci->ci_vfp_id = fpsid; 321 switch (fpsid & ~ VFP_FPSID_REV_MSK) { 322 case FPU_VFP10_ARM10E: 323 model = "VFP10 R1"; 324 break; 325 case FPU_VFP11_ARM11: 326 model = "VFP11"; 327 break; 328 case FPU_VFP_MV88SV58XX: 329 model = "VFP3"; 330 break; 331 case FPU_VFP_CORTEXA5: 332 case FPU_VFP_CORTEXA7: 333 case FPU_VFP_CORTEXA8: 334 case FPU_VFP_CORTEXA9: 335 case FPU_VFP_CORTEXA12: 336 case FPU_VFP_CORTEXA15: 337 case FPU_VFP_CORTEXA15_QEMU: 338 case FPU_VFP_CORTEXA17: 339 case FPU_VFP_CORTEXA53: 340 case FPU_VFP_CORTEXA57: 341 if (armreg_cpacr_read() & CPACR_V7_ASEDIS) { 342 model = "VFP 4.0+"; 343 } else { 344 model = "NEON MPE (VFP 3.0+)"; 345 cpu_neon_present = 1; 346 } 347 break; 348 default: 349 aprint_normal_dev(ci->ci_dev, "unrecognized VFP version %#x\n", 350 fpsid); 351 if (CPU_IS_PRIMARY(ci)) 352 install_coproc_handler(VFP_COPROC, vfp_fpscr_handler); 353 vfp_fpscr_changable = VFP_FPSCR_CSUM|VFP_FPSCR_ESUM 354 |VFP_FPSCR_RMODE; 355 vfp_fpscr_default = 0; 356 return; 357 } 358 359 cpu_fpu_present = 1; 360 cpu_media_and_vfp_features[0] = armreg_mvfr0_read(); 361 cpu_media_and_vfp_features[1] = armreg_mvfr1_read(); 362 if (fpsid != 0) { 363 uint32_t f0 = armreg_mvfr0_read(); 364 uint32_t f1 = armreg_mvfr1_read(); 365 aprint_normal("vfp%d at %s: %s%s%s%s%s\n", 366 device_unit(ci->ci_dev), 367 device_xname(ci->ci_dev), 368 model, 369 ((f0 & ARM_MVFR0_ROUNDING_MASK) ? ", rounding" : ""), 370 ((f0 & ARM_MVFR0_EXCEPT_MASK) ? ", exceptions" : ""), 371 ((f1 & ARM_MVFR1_D_NAN_MASK) ? ", NaN propagation" : ""), 372 ((f1 & ARM_MVFR1_FTZ_MASK) ? ", denormals" : "")); 373 aprint_debug("vfp%d: mvfr: [0]=%#x [1]=%#x\n", 374 device_unit(ci->ci_dev), f0, f1); 375 if (CPU_IS_PRIMARY(ci)) { 376 if (f0 & ARM_MVFR0_ROUNDING_MASK) { 377 vfp_fpscr_changable |= VFP_FPSCR_RMODE; 378 } 379 if (f1 & ARM_MVFR0_EXCEPT_MASK) { 380 vfp_fpscr_changable |= VFP_FPSCR_ESUM; 381 } 382 // If hardware supports propagation of NaNs, select it. 383 if (f1 & ARM_MVFR1_D_NAN_MASK) { 384 vfp_fpscr_default &= ~VFP_FPSCR_DN; 385 vfp_fpscr_changable |= VFP_FPSCR_DN; 386 } 387 // If hardware supports denormalized numbers, use it. 388 if (cpu_media_and_vfp_features[1] & ARM_MVFR1_FTZ_MASK) { 389 vfp_fpscr_default &= ~VFP_FPSCR_FZ; 390 vfp_fpscr_changable |= VFP_FPSCR_FZ; 391 } 392 } 393 } 394 evcnt_attach_dynamic(&ci->ci_vfp_evs[0], EVCNT_TYPE_MISC, NULL, 395 ci->ci_cpuname, "vfp coproc use"); 396 evcnt_attach_dynamic(&ci->ci_vfp_evs[1], EVCNT_TYPE_MISC, NULL, 397 ci->ci_cpuname, "vfp coproc re-use"); 398 evcnt_attach_dynamic(&ci->ci_vfp_evs[2], EVCNT_TYPE_TRAP, NULL, 399 ci->ci_cpuname, "vfp coproc fault"); 400 if (CPU_IS_PRIMARY(ci)) { 401 install_coproc_handler(VFP_COPROC, vfp_handler); 402 install_coproc_handler(VFP_COPROC2, vfp_handler); 403 #ifdef CPU_CORTEX 404 if (cpu_neon_present) 405 install_coproc_handler(CORE_UNKNOWN_HANDLER, neon_handler); 406 #endif 407 } 408 } 409 410 /* The real handler for VFP bounces. */ 411 static int 412 vfp_handler(u_int address, u_int insn, trapframe_t *frame, int fault_code) 413 { 414 struct cpu_info * const ci = curcpu(); 415 416 /* This shouldn't ever happen. */ 417 if (fault_code != FAULT_USER) 418 panic("VFP fault at %#x in non-user mode", frame->tf_pc); 419 420 if (ci->ci_vfp_id == 0) { 421 /* No VFP detected, just fault. */ 422 return 1; 423 } 424 425 /* 426 * If we already own the FPU and it's enabled (and no exception), raise 427 * SIGILL. If there is an exception, drop through to raise a SIGFPE. 428 */ 429 if (curcpu()->ci_pcu_curlwp[PCU_FPU] == curlwp 430 && (armreg_fpexc_read() & (VFP_FPEXC_EX|VFP_FPEXC_EN)) == VFP_FPEXC_EN) 431 return 1; 432 433 /* 434 * Make sure we own the FP. 435 */ 436 pcu_load(&arm_vfp_ops); 437 438 uint32_t fpexc = armreg_fpexc_read(); 439 if (fpexc & VFP_FPEXC_EX) { 440 ksiginfo_t ksi; 441 KASSERT(fpexc & VFP_FPEXC_EN); 442 443 curcpu()->ci_vfp_evs[2].ev_count++; 444 445 /* 446 * Need the clear the exception condition so any signal 447 * and future use can proceed. 448 */ 449 armreg_fpexc_write(fpexc & ~(VFP_FPEXC_EX|VFP_FPEXC_FSUM)); 450 451 pcu_save(&arm_vfp_ops, curlwp); 452 453 /* 454 * XXX Need to emulate bounce instructions here to get correct 455 * XXX exception codes, etc. 456 */ 457 KSI_INIT_TRAP(&ksi); 458 ksi.ksi_signo = SIGFPE; 459 if (fpexc & VFP_FPEXC_IXF) 460 ksi.ksi_code = FPE_FLTRES; 461 else if (fpexc & VFP_FPEXC_UFF) 462 ksi.ksi_code = FPE_FLTUND; 463 else if (fpexc & VFP_FPEXC_OFF) 464 ksi.ksi_code = FPE_FLTOVF; 465 else if (fpexc & VFP_FPEXC_DZF) 466 ksi.ksi_code = FPE_FLTDIV; 467 else if (fpexc & VFP_FPEXC_IOF) 468 ksi.ksi_code = FPE_FLTINV; 469 ksi.ksi_addr = (uint32_t *)address; 470 ksi.ksi_trap = 0; 471 trapsignal(curlwp, &ksi); 472 return 0; 473 } 474 475 /* Need to restart the faulted instruction. */ 476 // frame->tf_pc -= INSN_SIZE; 477 return 0; 478 } 479 480 #ifdef CPU_CORTEX 481 /* The real handler for NEON bounces. */ 482 static int 483 neon_handler(u_int address, u_int insn, trapframe_t *frame, int fault_code) 484 { 485 struct cpu_info * const ci = curcpu(); 486 487 if (ci->ci_vfp_id == 0) 488 /* No VFP detected, just fault. */ 489 return 1; 490 491 if ((insn & 0xfe000000) != 0xf2000000 492 && (insn & 0xfe000000) != 0xf4000000) 493 /* Not NEON instruction, just fault. */ 494 return 1; 495 496 /* This shouldn't ever happen. */ 497 if (fault_code != FAULT_USER) 498 panic("NEON fault in non-user mode"); 499 500 /* if we already own the FPU and it's enabled, raise SIGILL */ 501 if (curcpu()->ci_pcu_curlwp[PCU_FPU] == curlwp 502 && (armreg_fpexc_read() & VFP_FPEXC_EN) != 0) 503 return 1; 504 505 pcu_load(&arm_vfp_ops); 506 507 /* Need to restart the faulted instruction. */ 508 // frame->tf_pc -= INSN_SIZE; 509 return 0; 510 } 511 #endif 512 513 static void 514 vfp_state_load(lwp_t *l, u_int flags) 515 { 516 struct pcb * const pcb = lwp_getpcb(l); 517 struct vfpreg * const fregs = &pcb->pcb_vfp; 518 519 /* 520 * Instrument VFP usage -- if a process has not previously 521 * used the VFP, mark it as having used VFP for the first time, 522 * and count this event. 523 * 524 * If a process has used the VFP, count a "used VFP, and took 525 * a trap to use it again" event. 526 */ 527 if (__predict_false((flags & PCU_VALID) == 0)) { 528 curcpu()->ci_vfp_evs[0].ev_count++; 529 pcb->pcb_vfp.vfp_fpscr = vfp_fpscr_default; 530 } else { 531 curcpu()->ci_vfp_evs[1].ev_count++; 532 } 533 534 KASSERT((armreg_fpexc_read() & VFP_FPEXC_EN) == 0); 535 /* 536 * If the VFP is already enabled we must be bouncing an instruction. 537 */ 538 if (flags & PCU_REENABLE) { 539 uint32_t fpexc = armreg_fpexc_read(); 540 armreg_fpexc_write(fpexc | VFP_FPEXC_EN); 541 fregs->vfp_fpexc |= VFP_FPEXC_EN; 542 return; 543 } 544 KASSERT((fregs->vfp_fpexc & VFP_FPEXC_EN) == 0); 545 546 /* 547 * Load and Enable the VFP (so that we can write the registers). 548 */ 549 fregs->vfp_fpexc |= VFP_FPEXC_EN; 550 armreg_fpexc_write(fregs->vfp_fpexc); 551 KASSERT(curcpu()->ci_pcu_curlwp[PCU_FPU] == NULL); 552 KASSERT(l->l_pcu_cpu[PCU_FPU] == NULL); 553 554 load_vfpregs(fregs); 555 armreg_fpscr_write(fregs->vfp_fpscr); 556 557 if (fregs->vfp_fpexc & VFP_FPEXC_EX) { 558 /* Need to restore the exception handling state. */ 559 armreg_fpinst_write(fregs->vfp_fpinst); 560 if (fregs->vfp_fpexc & VFP_FPEXC_FP2V) 561 armreg_fpinst2_write(fregs->vfp_fpinst2); 562 } 563 } 564 565 void 566 vfp_state_save(lwp_t *l) 567 { 568 struct pcb * const pcb = lwp_getpcb(l); 569 struct vfpreg * const fregs = &pcb->pcb_vfp; 570 uint32_t fpexc = armreg_fpexc_read(); 571 572 KASSERT(curcpu()->ci_pcu_curlwp[PCU_FPU] == l); 573 KASSERT(curcpu() == l->l_pcu_cpu[PCU_FPU]); 574 KASSERT(curlwp == l || curlwp->l_pcu_cpu[PCU_FPU] != curcpu()); 575 /* 576 * Enable the VFP (so we can read the registers). 577 * Make sure the exception bit is cleared so that we can 578 * safely dump the registers. 579 */ 580 armreg_fpexc_write((fpexc | VFP_FPEXC_EN) & ~VFP_FPEXC_EX); 581 582 fregs->vfp_fpexc = fpexc; 583 if (fpexc & VFP_FPEXC_EX) { 584 /* Need to save the exception handling state */ 585 fregs->vfp_fpinst = armreg_fpinst_read(); 586 if (fpexc & VFP_FPEXC_FP2V) 587 fregs->vfp_fpinst2 = armreg_fpinst2_read(); 588 } 589 fregs->vfp_fpscr = armreg_fpscr_read(); 590 save_vfpregs(fregs); 591 592 /* Disable the VFP. */ 593 armreg_fpexc_write(fpexc & ~VFP_FPEXC_EN); 594 } 595 596 void 597 vfp_state_release(lwp_t *l) 598 { 599 struct pcb * const pcb = lwp_getpcb(l); 600 601 /* 602 * Now mark the VFP as disabled (and our state 603 * has been already saved or is being discarded). 604 */ 605 pcb->pcb_vfp.vfp_fpexc &= ~VFP_FPEXC_EN; 606 607 /* 608 * Turn off the FPU so the next time a VFP instruction is issued 609 * an exception happens. We don't know if this LWP's state was 610 * loaded but if we turned off the FPU for some other LWP, when 611 * pcu_load invokes vfp_state_load it will see that VFP_FPEXC_EN 612 * is still set so it just restore fpexc and return since its 613 * contents are still sitting in the VFP. 614 */ 615 armreg_fpexc_write(armreg_fpexc_read() & ~VFP_FPEXC_EN); 616 } 617 618 void 619 vfp_savecontext(lwp_t *l) 620 { 621 pcu_save(&arm_vfp_ops, l); 622 } 623 624 void 625 vfp_discardcontext(lwp_t *l, bool used_p) 626 { 627 pcu_discard(&arm_vfp_ops, l, used_p); 628 } 629 630 bool 631 vfp_used_p(const lwp_t *l) 632 { 633 return pcu_valid_p(&arm_vfp_ops, l); 634 } 635 636 void 637 vfp_getcontext(struct lwp *l, mcontext_t *mcp, int *flagsp) 638 { 639 if (vfp_used_p(l)) { 640 const struct pcb * const pcb = lwp_getpcb(l); 641 642 pcu_save(&arm_vfp_ops, l); 643 mcp->__fpu.__vfpregs.__vfp_fpscr = pcb->pcb_vfp.vfp_fpscr; 644 memcpy(mcp->__fpu.__vfpregs.__vfp_fstmx, pcb->pcb_vfp.vfp_regs, 645 sizeof(mcp->__fpu.__vfpregs.__vfp_fstmx)); 646 *flagsp |= _UC_FPU|_UC_ARM_VFP; 647 } 648 } 649 650 void 651 vfp_setcontext(struct lwp *l, const mcontext_t *mcp) 652 { 653 struct pcb * const pcb = lwp_getpcb(l); 654 655 pcu_discard(&arm_vfp_ops, l, true); 656 pcb->pcb_vfp.vfp_fpscr = mcp->__fpu.__vfpregs.__vfp_fpscr; 657 memcpy(pcb->pcb_vfp.vfp_regs, mcp->__fpu.__vfpregs.__vfp_fstmx, 658 sizeof(mcp->__fpu.__vfpregs.__vfp_fstmx)); 659 } 660 661 #endif /* FPU_VFP */ 662