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