Annotation of src/sys/arch/arm/vfp/vfp_init.c, Revision 1.53
1.53 ! jmcneill 1: /* $NetBSD: vfp_init.c,v 1.52 2017/03/22 23:36:02 chs Exp $ */
1.1 rearnsha 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>
1.4 matt 37: #include <sys/cpu.h>
1.1 rearnsha 38:
1.23 matt 39: #include <arm/locore.h>
1.5 matt 40: #include <arm/pcb.h>
1.1 rearnsha 41: #include <arm/undefined.h>
42: #include <arm/vfpreg.h>
1.8 matt 43: #include <arm/mcontext.h>
1.1 rearnsha 44:
1.12 matt 45: #include <uvm/uvm_extern.h> /* for pmap.h */
46:
1.11 matt 47: #ifdef FPU_VFP
48:
1.29 matt 49: #ifdef CPU_CORTEX
50: __asm(".fpu\tvfpv4");
51: #else
52: __asm(".fpu\tvfp");
53: #endif
54:
1.1 rearnsha 55: /* FLDMD <X>, {d0-d15} */
1.11 matt 56: static inline void
1.13 matt 57: load_vfpregs_lo(const uint64_t *p)
1.10 matt 58: {
1.29 matt 59: __asm __volatile("vldmia %0, {d0-d15}" :: "r" (p) : "memory");
1.10 matt 60: }
61:
62: /* FSTMD <X>, {d0-d15} */
1.11 matt 63: static inline void
1.10 matt 64: save_vfpregs_lo(uint64_t *p)
65: {
1.29 matt 66: __asm __volatile("vstmia %0, {d0-d15}" :: "r" (p) : "memory");
1.10 matt 67: }
68:
69: #ifdef CPU_CORTEX
70: /* FLDMD <X>, {d16-d31} */
1.11 matt 71: static inline void
1.13 matt 72: load_vfpregs_hi(const uint64_t *p)
1.10 matt 73: {
1.29 matt 74: __asm __volatile("vldmia\t%0, {d16-d31}" :: "r" (&p[16]) : "memory");
1.10 matt 75: }
76:
77: /* FLDMD <X>, {d16-d31} */
1.11 matt 78: static inline void
1.10 matt 79: save_vfpregs_hi(uint64_t *p)
80: {
1.29 matt 81: __asm __volatile("vstmia\t%0, {d16-d31}" :: "r" (&p[16]) : "memory");
1.10 matt 82: }
83: #endif
1.1 rearnsha 84:
1.13 matt 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:
1.20 matt 96: case FPU_VFP_CORTEXA15:
1.42 slp 97: case FPU_VFP_CORTEXA15_QEMU:
1.50 skrll 98: case FPU_VFP_CORTEXA53:
1.53 ! jmcneill 99: case FPU_VFP_CORTEXA57:
1.13 matt 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:
1.20 matt 120: case FPU_VFP_CORTEXA15:
1.42 slp 121: case FPU_VFP_CORTEXA15_QEMU:
1.50 skrll 122: case FPU_VFP_CORTEXA53:
1.53 ! jmcneill 123: case FPU_VFP_CORTEXA57:
1.13 matt 124: #endif
125: save_vfpregs_hi(fregs->vfp_regs);
126: #ifdef CPU_ARM11
127: break;
128: }
129: #endif
130: #endif
131: }
132:
1.1 rearnsha 133: /* The real handler for VFP bounces. */
134: static int vfp_handler(u_int, u_int, trapframe_t *, int);
1.13 matt 135: #ifdef CPU_CORTEX
136: static int neon_handler(u_int, u_int, trapframe_t *, int);
137: #endif
1.1 rearnsha 138:
1.13 matt 139: static void vfp_state_load(lwp_t *, u_int);
1.39 rmind 140: static void vfp_state_save(lwp_t *);
141: static void vfp_state_release(lwp_t *);
1.4 matt 142:
143: const pcu_ops_t arm_vfp_ops = {
144: .pcu_id = PCU_FPU,
1.13 matt 145: .pcu_state_save = vfp_state_save,
1.4 matt 146: .pcu_state_load = vfp_state_load,
147: .pcu_state_release = vfp_state_release,
148: };
1.1 rearnsha 149:
1.34 matt 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:
1.1 rearnsha 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
1.4 matt 165: vfp_test(u_int address, u_int insn, trapframe_t *frame, int fault_code)
1.1 rearnsha 166: {
167:
168: frame->tf_pc += INSN_SIZE;
169: ++undefined_test;
1.4 matt 170: return 0;
171: }
172:
1.35 matt 173: #else
174: /* determine what bits can be changed */
175: uint32_t vfp_fpscr_changable = VFP_FPSCR_CSUM|VFP_FPSCR_ESUM|VFP_FPSCR_RMODE;
1.4 matt 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:
1.51 chs 201: if (__predict_false(!vfp_used_p(l))) {
1.35 matt 202: pcb->pcb_vfp.vfp_fpscr = vfp_fpscr_default;
1.4 matt 203: }
1.26 matt 204: #endif
1.4 matt 205:
206: /*
1.30 skrll 207: * We now know the pcb has the saved copy.
1.4 matt 208: */
209: register_t * const regp = &frame->tf_r0 + regno;
210: if (insn & 0x00100000) {
211: *regp = pcb->pcb_vfp.vfp_fpscr;
212: } else {
1.34 matt 213: pcb->pcb_vfp.vfp_fpscr &= ~vfp_fpscr_changable;
214: pcb->pcb_vfp.vfp_fpscr |= *regp & vfp_fpscr_changable;
1.4 matt 215: }
216:
1.37 matt 217: curcpu()->ci_vfp_evs[0].ev_count++;
1.4 matt 218:
219: frame->tf_pc += INSN_SIZE;
220: return 0;
1.1 rearnsha 221: }
222:
1.4 matt 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
1.37 matt 229: vfp_attach(struct cpu_info *ci)
1.4 matt 230: {
1.37 matt 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");
1.4 matt 236: }
237:
238: #else
1.1 rearnsha 239: void
1.37 matt 240: vfp_attach(struct cpu_info *ci)
1.1 rearnsha 241: {
1.4 matt 242: const char *model = NULL;
1.1 rearnsha 243:
1.37 matt 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) {
1.40 matt 251: aprint_normal_dev(ci->ci_dev,
252: "VFP access denied (NSACR=%#x)\n", nsacr);
1.37 matt 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
1.7 matt 261: const uint32_t cpacr_vfp = CPACR_CPn(VFP_COPROC);
262: const uint32_t cpacr_vfp2 = CPACR_CPn(VFP_COPROC2);
1.1 rearnsha 263:
1.7 matt 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);
1.1 rearnsha 271:
1.48 jmcneill 272: arm_isb();
273:
1.7 matt 274: /*
275: * If we could enable them, then they exist.
276: */
277: cpacr = armreg_cpacr_read();
1.40 matt 278: bool vfp_p = __SHIFTOUT(cpacr, cpacr_vfp2) == CPACR_ALL
279: && __SHIFTOUT(cpacr, cpacr_vfp) == CPACR_ALL;
1.28 matt 280: if (!vfp_p) {
1.40 matt 281: aprint_normal_dev(ci->ci_dev,
282: "VFP access denied (CPACR=%#x)\n", cpacr);
1.28 matt 283: install_coproc_handler(VFP_COPROC, vfp_fpscr_handler);
284: ci->ci_vfp_id = 0;
1.37 matt 285: evcnt_attach_dynamic(&ci->ci_vfp_evs[0],
286: EVCNT_TYPE_TRAP, NULL, ci->ci_cpuname,
287: "vfp fpscr traps");
1.28 matt 288: return;
289: }
1.6 matt 290: }
291:
1.7 matt 292: void *uh = install_coproc_handler(VFP_COPROC, vfp_test);
293:
294: undefined_test = 0;
295:
1.21 matt 296: const uint32_t fpsid = armreg_fpsid_read();
1.1 rearnsha 297:
298: remove_coproc_handler(uh);
299:
300: if (undefined_test != 0) {
1.4 matt 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;
1.1 rearnsha 304: return;
305: }
306:
1.4 matt 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;
1.36 matt 315: case FPU_VFP_MV88SV58XX:
316: model = "VFP3";
317: break;
1.7 matt 318: case FPU_VFP_CORTEXA5:
319: case FPU_VFP_CORTEXA7:
320: case FPU_VFP_CORTEXA8:
321: case FPU_VFP_CORTEXA9:
1.20 matt 322: case FPU_VFP_CORTEXA15:
1.42 slp 323: case FPU_VFP_CORTEXA15_QEMU:
1.50 skrll 324: case FPU_VFP_CORTEXA53:
1.53 ! jmcneill 325: case FPU_VFP_CORTEXA57:
1.37 matt 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: }
1.6 matt 332: break;
1.4 matt 333: default:
1.36 matt 334: aprint_normal_dev(ci->ci_dev, "unrecognized VFP version %#x\n",
1.4 matt 335: fpsid);
336: install_coproc_handler(VFP_COPROC, vfp_fpscr_handler);
1.35 matt 337: vfp_fpscr_changable = VFP_FPSCR_CSUM|VFP_FPSCR_ESUM
338: |VFP_FPSCR_RMODE;
339: vfp_fpscr_default = 0;
1.4 matt 340: return;
341: }
1.1 rearnsha 342:
1.17 matt 343: cpu_fpu_present = 1;
1.21 matt 344: cpu_media_and_vfp_features[0] = armreg_mvfr0_read();
345: cpu_media_and_vfp_features[1] = armreg_mvfr1_read();
1.1 rearnsha 346: if (fpsid != 0) {
1.34 matt 347: uint32_t f0 = armreg_mvfr0_read();
1.41 matt 348: uint32_t f1 = armreg_mvfr1_read();
1.34 matt 349: aprint_normal("vfp%d at %s: %s%s%s%s%s\n",
1.37 matt 350: device_unit(ci->ci_dev),
351: device_xname(ci->ci_dev),
1.34 matt 352: model,
353: ((f0 & ARM_MVFR0_ROUNDING_MASK) ? ", rounding" : ""),
354: ((f0 & ARM_MVFR0_EXCEPT_MASK) ? ", exceptions" : ""),
1.38 matt 355: ((f1 & ARM_MVFR1_D_NAN_MASK) ? ", NaN propagation" : ""),
1.34 matt 356: ((f1 & ARM_MVFR1_FTZ_MASK) ? ", denormals" : ""));
1.49 jmcneill 357: aprint_debug("vfp%d: mvfr: [0]=%#x [1]=%#x\n",
1.37 matt 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: }
1.38 matt 366: // If hardware supports propagation of NaNs, select it.
1.37 matt 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");
1.1 rearnsha 384: install_coproc_handler(VFP_COPROC, vfp_handler);
385: install_coproc_handler(VFP_COPROC2, vfp_handler);
1.13 matt 386: #ifdef CPU_CORTEX
1.43 matt 387: if (cpu_neon_present)
388: install_coproc_handler(CORE_UNKNOWN_HANDLER, neon_handler);
1.13 matt 389: #endif
1.1 rearnsha 390: }
391:
392: /* The real handler for VFP bounces. */
1.4 matt 393: static int
1.21 matt 394: vfp_handler(u_int address, u_int insn, trapframe_t *frame, int fault_code)
1.1 rearnsha 395: {
1.4 matt 396: struct cpu_info * const ci = curcpu();
1.1 rearnsha 397:
398: /* This shouldn't ever happen. */
399: if (fault_code != FAULT_USER)
1.14 matt 400: panic("VFP fault at %#x in non-user mode", frame->tf_pc);
1.1 rearnsha 401:
1.27 matt 402: if (ci->ci_vfp_id == 0) {
1.1 rearnsha 403: /* No VFP detected, just fault. */
404: return 1;
1.27 matt 405: }
406:
407: /*
1.47 matt 408: * If we are just changing/fetching FPSCR, don't bother loading it
409: * just emulate the instruction.
1.27 matt 410: */
411: if (!vfp_fpscr_handler(address, insn, frame, fault_code))
1.47 matt 412: return 0;
1.27 matt 413:
1.47 matt 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: */
1.46 matt 418: if (curcpu()->ci_pcu_curlwp[PCU_FPU] == curlwp
1.47 matt 419: && (armreg_fpexc_read() & (VFP_FPEXC_EX|VFP_FPEXC_EN)) == VFP_FPEXC_EN)
420: return 1;
1.44 matt 421:
1.27 matt 422: /*
423: * Make sure we own the FP.
424: */
425: pcu_load(&arm_vfp_ops);
1.1 rearnsha 426:
1.21 matt 427: uint32_t fpexc = armreg_fpexc_read();
428: if (fpexc & VFP_FPEXC_EX) {
429: ksiginfo_t ksi;
430: KASSERT(fpexc & VFP_FPEXC_EN);
431:
1.37 matt 432: curcpu()->ci_vfp_evs[2].ev_count++;
1.21 matt 433:
434: /*
435: * Need the clear the exception condition so any signal
1.33 skrll 436: * and future use can proceed.
1.21 matt 437: */
1.31 skrll 438: armreg_fpexc_write(fpexc & ~(VFP_FPEXC_EX|VFP_FPEXC_FSUM));
1.21 matt 439:
1.51 chs 440: pcu_save(&arm_vfp_ops, curlwp);
1.33 skrll 441:
442: /*
443: * XXX Need to emulate bounce instructions here to get correct
444: * XXX exception codes, etc.
445: */
1.21 matt 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:
1.4 matt 464: /* Need to restart the faulted instruction. */
465: // frame->tf_pc -= INSN_SIZE;
466: return 0;
467: }
1.1 rearnsha 468:
1.13 matt 469: #ifdef CPU_CORTEX
470: /* The real handler for NEON bounces. */
471: static int
1.21 matt 472: neon_handler(u_int address, u_int insn, trapframe_t *frame, int fault_code)
1.13 matt 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:
1.45 matt 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)
1.47 matt 492: return 1;
1.43 matt 493:
1.13 matt 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:
1.4 matt 502: static void
1.13 matt 503: vfp_state_load(lwp_t *l, u_int flags)
1.4 matt 504: {
505: struct pcb * const pcb = lwp_getpcb(l);
506: struct vfpreg * const fregs = &pcb->pcb_vfp;
1.1 rearnsha 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: */
1.39 rmind 516: if (__predict_false((flags & PCU_VALID) == 0)) {
1.37 matt 517: curcpu()->ci_vfp_evs[0].ev_count++;
1.34 matt 518: pcb->pcb_vfp.vfp_fpscr = vfp_fpscr_default;
1.4 matt 519: } else {
1.37 matt 520: curcpu()->ci_vfp_evs[1].ev_count++;
1.4 matt 521: }
1.1 rearnsha 522:
1.39 rmind 523: /*
524: * If the VFP is already enabled we must be bouncing an instruction.
525: */
526: if (flags & PCU_REENABLE) {
527: uint32_t fpexc = armreg_fpexc_read();
528: armreg_fpexc_write(fpexc | VFP_FPEXC_EN);
529: return;
530: }
1.33 skrll 531:
1.39 rmind 532: /*
533: * Load and Enable the VFP (so that we can write the registers).
534: */
535: bool enabled = fregs->vfp_fpexc & VFP_FPEXC_EN;
536: fregs->vfp_fpexc |= VFP_FPEXC_EN;
537: armreg_fpexc_write(fregs->vfp_fpexc);
538: if (enabled) {
1.4 matt 539: /*
1.39 rmind 540: * If we think the VFP is enabled, it must have be
541: * disabled by vfp_state_release for another LWP so
542: * we can now just return.
1.4 matt 543: */
1.39 rmind 544: return;
545: }
1.13 matt 546:
1.39 rmind 547: load_vfpregs(fregs);
548: armreg_fpscr_write(fregs->vfp_fpscr);
1.13 matt 549:
1.39 rmind 550: if (fregs->vfp_fpexc & VFP_FPEXC_EX) {
551: /* Need to restore the exception handling state. */
1.52 chs 552: armreg_fpinst_write(fregs->vfp_fpinst);
1.39 rmind 553: if (fregs->vfp_fpexc & VFP_FPEXC_FP2V)
1.52 chs 554: armreg_fpinst2_write(fregs->vfp_fpinst2);
1.1 rearnsha 555: }
556: }
557:
558: void
1.39 rmind 559: vfp_state_save(lwp_t *l)
1.1 rearnsha 560: {
1.4 matt 561: struct pcb * const pcb = lwp_getpcb(l);
1.39 rmind 562: struct vfpreg * const fregs = &pcb->pcb_vfp;
1.21 matt 563: uint32_t fpexc = armreg_fpexc_read();
1.33 skrll 564:
565: /*
566: * Enable the VFP (so we can read the registers).
567: * Make sure the exception bit is cleared so that we can
568: * safely dump the registers.
569: */
1.21 matt 570: armreg_fpexc_write((fpexc | VFP_FPEXC_EN) & ~VFP_FPEXC_EX);
1.1 rearnsha 571:
1.4 matt 572: fregs->vfp_fpexc = fpexc;
573: if (fpexc & VFP_FPEXC_EX) {
574: /* Need to save the exception handling state */
1.21 matt 575: fregs->vfp_fpinst = armreg_fpinst_read();
576: if (fpexc & VFP_FPEXC_FP2V)
577: fregs->vfp_fpinst2 = armreg_fpinst2_read();
1.1 rearnsha 578: }
1.21 matt 579: fregs->vfp_fpscr = armreg_fpscr_read();
1.13 matt 580: save_vfpregs(fregs);
1.4 matt 581:
1.1 rearnsha 582: /* Disable the VFP. */
1.33 skrll 583: armreg_fpexc_write(fpexc & ~VFP_FPEXC_EN);
1.1 rearnsha 584: }
585:
586: void
1.39 rmind 587: vfp_state_release(lwp_t *l)
1.1 rearnsha 588: {
1.4 matt 589: struct pcb * const pcb = lwp_getpcb(l);
1.1 rearnsha 590:
1.39 rmind 591: /*
592: * Now mark the VFP as disabled (and our state
593: * has been already saved or is being discarded).
594: */
595: pcb->pcb_vfp.vfp_fpexc &= ~VFP_FPEXC_EN;
1.1 rearnsha 596:
597: /*
1.4 matt 598: * Turn off the FPU so the next time a VFP instruction is issued
599: * an exception happens. We don't know if this LWP's state was
600: * loaded but if we turned off the FPU for some other LWP, when
601: * pcu_load invokes vfp_state_load it will see that VFP_FPEXC_EN
1.13 matt 602: * is still set so it just restore fpexc and return since its
1.4 matt 603: * contents are still sitting in the VFP.
1.1 rearnsha 604: */
1.21 matt 605: armreg_fpexc_write(armreg_fpexc_read() & ~VFP_FPEXC_EN);
1.1 rearnsha 606: }
607:
608: void
1.51 chs 609: vfp_savecontext(lwp_t *l)
1.1 rearnsha 610: {
1.51 chs 611: pcu_save(&arm_vfp_ops, l);
1.1 rearnsha 612: }
613:
614: void
1.51 chs 615: vfp_discardcontext(lwp_t *l, bool used_p)
1.1 rearnsha 616: {
1.51 chs 617: pcu_discard(&arm_vfp_ops, l, used_p);
1.25 matt 618: }
619:
620: bool
1.51 chs 621: vfp_used_p(const lwp_t *l)
1.25 matt 622: {
1.51 chs 623: return pcu_valid_p(&arm_vfp_ops, l);
1.13 matt 624: }
625:
626: void
1.8 matt 627: vfp_getcontext(struct lwp *l, mcontext_t *mcp, int *flagsp)
628: {
1.51 chs 629: if (vfp_used_p(l)) {
1.8 matt 630: const struct pcb * const pcb = lwp_getpcb(l);
1.51 chs 631:
632: pcu_save(&arm_vfp_ops, l);
1.8 matt 633: mcp->__fpu.__vfpregs.__vfp_fpscr = pcb->pcb_vfp.vfp_fpscr;
634: memcpy(mcp->__fpu.__vfpregs.__vfp_fstmx, pcb->pcb_vfp.vfp_regs,
635: sizeof(mcp->__fpu.__vfpregs.__vfp_fstmx));
1.10 matt 636: *flagsp |= _UC_FPU|_UC_ARM_VFP;
1.8 matt 637: }
638: }
639:
640: void
641: vfp_setcontext(struct lwp *l, const mcontext_t *mcp)
642: {
643: struct pcb * const pcb = lwp_getpcb(l);
1.51 chs 644:
645: pcu_discard(&arm_vfp_ops, l, true);
1.8 matt 646: pcb->pcb_vfp.vfp_fpscr = mcp->__fpu.__vfpregs.__vfp_fpscr;
647: memcpy(pcb->pcb_vfp.vfp_regs, mcp->__fpu.__vfpregs.__vfp_fstmx,
648: sizeof(mcp->__fpu.__vfpregs.__vfp_fstmx));
649: }
650:
1.4 matt 651: #endif /* FPU_VFP */
CVSweb <webmaster@jp.NetBSD.org>