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