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