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