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