Annotation of src/sys/arch/arm/vfp/vfp_init.c, Revision 1.9
1.9 ! matt 1: /* $NetBSD: vfp_init.c,v 1.8 2012/12/05 19:05:46 matt 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.5 matt 39: #include <arm/pcb.h>
1.1 rearnsha 40: #include <arm/undefined.h>
41: #include <arm/vfpreg.h>
1.8 matt 42: #include <arm/mcontext.h>
1.1 rearnsha 43:
44: /*
45: * Use generic co-processor instructions to avoid assembly problems.
46: */
47:
48: /* FMRX <X>, fpsid */
1.4 matt 49: static inline uint32_t
50: read_fpsid(void)
51: {
52: uint32_t rv;
53: __asm __volatile("mrc p10, 7, %0, c0, c0, 0" : "=r" (rv));
54: return rv;
55: }
56:
57: /* FMRX <X>, fpexc */
58: static inline uint32_t
59: read_fpscr(void)
60: {
61: uint32_t rv;
62: __asm __volatile("mrc p10, 7, %0, c1, c0, 0" : "=r" (rv));
63: return rv;
64: }
65:
1.1 rearnsha 66: /* FMRX <X>, fpexc */
1.4 matt 67: static inline uint32_t
68: read_fpexc(void)
69: {
70: uint32_t rv;
71: __asm __volatile("mrc p10, 7, %0, c8, c0, 0" : "=r" (rv));
72: return rv;
73: }
74:
1.1 rearnsha 75: /* FMRX <X>, fpinst */
1.4 matt 76: static inline uint32_t
77: read_fpinst(void)
78: {
79: uint32_t rv;
80: __asm __volatile("mrc p10, 7, %0, c9, c0, 0" : "=r" (rv));
81: return rv;
82: }
83:
1.1 rearnsha 84: /* FMRX <X>, fpinst2 */
1.4 matt 85: static inline uint32_t
86: read_fpinst2(void)
87: {
88: uint32_t rv;
89: __asm __volatile("mrc p10, 7, %0, c10, c0, 0" : "=r" (rv));
90: return rv;
91: }
92:
1.1 rearnsha 93: /* FSTMD <X>, {d0-d15} */
94: #define save_vfpregs(X) __asm __volatile("stc p11, c0, [%0], {32}" : \
95: : "r" (X) : "memory")
96:
97: /* FMXR <X>, fpscr */
98: #define write_fpscr(X) __asm __volatile("mcr p10, 7, %0, c1, c0, 0" : \
99: : "r" (X))
100: /* FMXR <X>, fpexc */
101: #define write_fpexc(X) __asm __volatile("mcr p10, 7, %0, c8, c0, 0" : \
102: : "r" (X))
103: /* FMXR <X>, fpinst */
104: #define write_fpinst(X) __asm __volatile("mcr p10, 7, %0, c9, c0, 0" : \
105: : "r" (X))
106: /* FMXR <X>, fpinst2 */
107: #define write_fpinst2(X) __asm __volatile("mcr p10, 7, %0, c10, c0, 0" : \
108: : "r" (X))
109: /* FLDMD <X>, {d0-d15} */
110: #define load_vfpregs(X) __asm __volatile("ldc p11, c0, [%0], {32}" : \
111: : "r" (X) : "memory");
112:
1.4 matt 113: #ifdef FPU_VFP
114:
1.1 rearnsha 115: /* The real handler for VFP bounces. */
116: static int vfp_handler(u_int, u_int, trapframe_t *, int);
1.4 matt 117: static int vfp_handler(u_int, u_int, trapframe_t *, int);
1.1 rearnsha 118:
1.4 matt 119: static void vfp_state_load(lwp_t *, bool);
120: static void vfp_state_save(lwp_t *);
121: static void vfp_state_release(lwp_t *);
122:
123: const pcu_ops_t arm_vfp_ops = {
124: .pcu_id = PCU_FPU,
125: .pcu_state_load = vfp_state_load,
126: .pcu_state_save = vfp_state_save,
127: .pcu_state_release = vfp_state_release,
128: };
1.1 rearnsha 129:
130: struct evcnt vfpevent_use;
131: struct evcnt vfpevent_reuse;
132:
133: /*
134: * Used to test for a VFP. The following function is installed as a coproc10
135: * handler on the undefined instruction vector and then we issue a VFP
136: * instruction. If undefined_test is non zero then the VFP did not handle
137: * the instruction so must be absent, or disabled.
138: */
139:
140: static int undefined_test;
141:
142: static int
1.4 matt 143: vfp_test(u_int address, u_int insn, trapframe_t *frame, int fault_code)
1.1 rearnsha 144: {
145:
146: frame->tf_pc += INSN_SIZE;
147: ++undefined_test;
1.4 matt 148: return 0;
149: }
150:
151: #endif /* FPU_VFP */
152:
153: struct evcnt vfp_fpscr_ev =
154: EVCNT_INITIALIZER(EVCNT_TYPE_TRAP, NULL, "VFP", "FPSCR traps");
155: EVCNT_ATTACH_STATIC(vfp_fpscr_ev);
156:
157: static int
158: vfp_fpscr_handler(u_int address, u_int insn, trapframe_t *frame, int fault_code)
159: {
160: struct lwp * const l = curlwp;
161: const u_int regno = (insn >> 12) & 0xf;
162: /*
163: * Only match move to/from the FPSCR register and we
164: * can't be using the SP,LR,PC as a source.
165: */
166: if ((insn & 0xffef0fff) != 0xeee10a10 || regno > 12)
167: return 1;
168:
169: struct pcb * const pcb = lwp_getpcb(l);
170:
171: #ifdef FPU_VFP
172: /*
173: * If FPU is valid somewhere, let's just reenable VFP and
174: * retry the instruction (only safe thing to do since the
175: * pcb has a stale copy).
176: */
177: if (pcb->pcb_vfp.vfp_fpexc & VFP_FPEXC_EN)
178: return 1;
179: #endif
180:
181: if (__predict_false((l->l_md.md_flags & MDLWP_VFPUSED) == 0)) {
182: l->l_md.md_flags |= MDLWP_VFPUSED;
183: pcb->pcb_vfp.vfp_fpscr =
184: (VFP_FPSCR_DN | VFP_FPSCR_FZ); /* Runfast */
185: }
186:
187: /*
188: * We know know the pcb has the saved copy.
189: */
190: register_t * const regp = &frame->tf_r0 + regno;
191: if (insn & 0x00100000) {
192: *regp = pcb->pcb_vfp.vfp_fpscr;
193: } else {
194: pcb->pcb_vfp.vfp_fpscr = *regp;
195: }
196:
197: vfp_fpscr_ev.ev_count++;
198:
199: frame->tf_pc += INSN_SIZE;
200: return 0;
1.1 rearnsha 201: }
202:
1.4 matt 203: #ifndef FPU_VFP
204: /*
205: * If we don't want VFP support, we still need to handle emulating VFP FPSCR
206: * instructions.
207: */
208: void
209: vfp_attach(void)
210: {
211: install_coproc_handler(VFP_COPROC, vfp_fpscr_handler);
212: }
213:
214: #else
1.1 rearnsha 215: void
1.2 cegger 216: vfp_attach(void)
1.1 rearnsha 217: {
1.4 matt 218: struct cpu_info * const ci = curcpu();
219: const char *model = NULL;
1.7 matt 220: bool vfp_p = false;
1.1 rearnsha 221:
1.7 matt 222: #ifdef FPU_VFP
223: if (CPU_ID_ARM11_P(curcpu()->ci_arm_cpuid)
224: || CPU_ID_CORTEX_P(curcpu()->ci_arm_cpuid)) {
225: const uint32_t cpacr_vfp = CPACR_CPn(VFP_COPROC);
226: const uint32_t cpacr_vfp2 = CPACR_CPn(VFP_COPROC2);
1.1 rearnsha 227:
1.7 matt 228: /*
229: * We first need to enable access to the coprocessors.
230: */
231: uint32_t cpacr = armreg_cpacr_read();
232: cpacr |= __SHIFTIN(CPACR_ALL, cpacr_vfp);
233: cpacr |= __SHIFTIN(CPACR_ALL, cpacr_vfp2);
1.9 ! matt 234: if (CPU_ID_CORTEX_P(curcpu()->ci_arm_cpuid)) {
! 235: /*
! 236: * Disable access to the upper 16 FP registers.
! 237: */
! 238: cpacr |= CPACR_V7_D32DIS;
! 239: }
1.7 matt 240: armreg_cpacr_write(cpacr);
1.1 rearnsha 241:
1.7 matt 242: /*
243: * If we could enable them, then they exist.
244: */
245: cpacr = armreg_cpacr_read();
246: vfp_p = __SHIFTOUT(cpacr, cpacr_vfp2) != CPACR_NOACCESS
247: || __SHIFTOUT(cpacr, cpacr_vfp) != CPACR_NOACCESS;
1.6 matt 248: }
249: #endif
250:
1.7 matt 251: void *uh = install_coproc_handler(VFP_COPROC, vfp_test);
252:
253: undefined_test = 0;
254:
1.4 matt 255: const uint32_t fpsid = read_fpsid();
1.1 rearnsha 256:
257: remove_coproc_handler(uh);
258:
259: if (undefined_test != 0) {
1.4 matt 260: aprint_normal_dev(ci->ci_dev, "No VFP detected\n");
261: install_coproc_handler(VFP_COPROC, vfp_fpscr_handler);
262: ci->ci_vfp_id = 0;
1.1 rearnsha 263: return;
264: }
265:
1.4 matt 266: ci->ci_vfp_id = fpsid;
267: switch (fpsid & ~ VFP_FPSID_REV_MSK) {
268: case FPU_VFP10_ARM10E:
269: model = "VFP10 R1";
270: break;
271: case FPU_VFP11_ARM11:
272: model = "VFP11";
273: break;
1.7 matt 274: case FPU_VFP_CORTEXA5:
275: case FPU_VFP_CORTEXA7:
276: case FPU_VFP_CORTEXA8:
277: case FPU_VFP_CORTEXA9:
278: model = "NEON MPE (VFP 3.0+)";
1.6 matt 279: break;
1.4 matt 280: default:
281: aprint_normal_dev(ci->ci_dev, "unrecognized VFP version %x\n",
282: fpsid);
283: install_coproc_handler(VFP_COPROC, vfp_fpscr_handler);
284: return;
285: }
1.1 rearnsha 286:
287: if (fpsid != 0) {
288: aprint_normal("vfp%d at %s: %s\n",
1.6 matt 289: device_unit(curcpu()->ci_dev), device_xname(curcpu()->ci_dev),
1.1 rearnsha 290: model);
291: }
292: evcnt_attach_dynamic(&vfpevent_use, EVCNT_TYPE_MISC, NULL,
293: "VFP", "proc use");
294: evcnt_attach_dynamic(&vfpevent_reuse, EVCNT_TYPE_MISC, NULL,
295: "VFP", "proc re-use");
296: install_coproc_handler(VFP_COPROC, vfp_handler);
297: install_coproc_handler(VFP_COPROC2, vfp_handler);
298: }
299:
300: /* The real handler for VFP bounces. */
1.4 matt 301: static int
302: vfp_handler(u_int address, u_int insn, trapframe_t *frame,
1.1 rearnsha 303: int fault_code)
304: {
1.4 matt 305: struct cpu_info * const ci = curcpu();
1.1 rearnsha 306:
307: /* This shouldn't ever happen. */
308: if (fault_code != FAULT_USER)
309: panic("VFP fault in non-user mode");
310:
1.4 matt 311: if (ci->ci_vfp_id == 0)
1.1 rearnsha 312: /* No VFP detected, just fault. */
313: return 1;
314:
1.4 matt 315: /*
316: * If we are just changing/fetching FPSCR, don't bother loading it.
317: */
318: if (!vfp_fpscr_handler(address, insn, frame, fault_code))
319: return 0;
1.1 rearnsha 320:
1.4 matt 321: pcu_load(&arm_vfp_ops);
1.3 rmind 322:
1.4 matt 323: /* Need to restart the faulted instruction. */
324: // frame->tf_pc -= INSN_SIZE;
325: return 0;
326: }
1.1 rearnsha 327:
1.4 matt 328: static void
329: vfp_state_load(lwp_t *l, bool used)
330: {
331: struct pcb * const pcb = lwp_getpcb(l);
332: struct vfpreg * const fregs = &pcb->pcb_vfp;
1.1 rearnsha 333:
334: /*
335: * Instrument VFP usage -- if a process has not previously
336: * used the VFP, mark it as having used VFP for the first time,
337: * and count this event.
338: *
339: * If a process has used the VFP, count a "used VFP, and took
340: * a trap to use it again" event.
341: */
1.4 matt 342: if (__predict_false((l->l_md.md_flags & MDLWP_VFPUSED) == 0)) {
1.1 rearnsha 343: vfpevent_use.ev_count++;
1.4 matt 344: l->l_md.md_flags |= MDLWP_VFPUSED;
1.3 rmind 345: pcb->pcb_vfp.vfp_fpscr =
1.1 rearnsha 346: (VFP_FPSCR_DN | VFP_FPSCR_FZ); /* Runfast */
1.4 matt 347: } else {
1.1 rearnsha 348: vfpevent_reuse.ev_count++;
1.4 matt 349: }
1.1 rearnsha 350:
1.4 matt 351: if (fregs->vfp_fpexc & VFP_FPEXC_EN) {
352: /*
353: * If we think the VFP is enabled, it must have be disabled by
354: * vfp_state_release for another LWP so we can just restore
355: * FPEXC and return since our VFP state is still loaded.
356: */
357: write_fpexc(fregs->vfp_fpexc);
358: return;
359: }
1.1 rearnsha 360:
361: /* Enable the VFP (so that we can write the registers). */
1.4 matt 362: uint32_t fpexc = read_fpexc();
1.1 rearnsha 363: KDASSERT((fpexc & VFP_FPEXC_EX) == 0);
364: write_fpexc(fpexc | VFP_FPEXC_EN);
365:
366: load_vfpregs(fregs->vfp_regs);
367: write_fpscr(fregs->vfp_fpscr);
1.4 matt 368:
1.1 rearnsha 369: if (fregs->vfp_fpexc & VFP_FPEXC_EX) {
1.4 matt 370: struct cpu_info * const ci = curcpu();
1.1 rearnsha 371: /* Need to restore the exception handling state. */
1.4 matt 372: switch (ci->ci_vfp_id) {
1.1 rearnsha 373: case FPU_VFP10_ARM10E:
374: case FPU_VFP11_ARM11:
1.8 matt 375: case FPU_VFP_CORTEXA5:
376: case FPU_VFP_CORTEXA7:
377: case FPU_VFP_CORTEXA8:
378: case FPU_VFP_CORTEXA9:
1.1 rearnsha 379: write_fpinst2(fregs->vfp_fpinst2);
380: write_fpinst(fregs->vfp_fpinst);
381: break;
382: default:
1.4 matt 383: panic("%s: Unsupported VFP %#x",
384: __func__, ci->ci_vfp_id);
1.1 rearnsha 385: }
386: }
1.4 matt 387:
388: /* Finally, restore the FPEXC but don't enable the VFP. */
389: fregs->vfp_fpexc |= VFP_FPEXC_EN;
390: write_fpexc(fregs->vfp_fpexc);
1.1 rearnsha 391: }
392:
393: void
1.4 matt 394: vfp_state_save(lwp_t *l)
1.1 rearnsha 395: {
1.4 matt 396: struct pcb * const pcb = lwp_getpcb(l);
397: struct vfpreg * const fregs = &pcb->pcb_vfp;
1.1 rearnsha 398:
1.4 matt 399: /*
400: * If it's already disabled, then the state has been saved
401: * (or discarded).
402: */
403: if ((fregs->vfp_fpexc & VFP_FPEXC_EN) == 0)
1.1 rearnsha 404: return;
405:
1.4 matt 406: /*
407: * Enable the VFP (so we can read the registers).
408: * Make sure the exception bit is cleared so that we can
409: * safely dump the registers.
410: */
411: uint32_t fpexc = read_fpexc();
412: write_fpexc((fpexc | VFP_FPEXC_EN) & ~VFP_FPEXC_EX);
1.1 rearnsha 413:
1.4 matt 414: fregs->vfp_fpexc = fpexc;
415: if (fpexc & VFP_FPEXC_EX) {
416: struct cpu_info * const ci = curcpu();
417: /* Need to save the exception handling state */
418: switch (ci->ci_vfp_id) {
419: case FPU_VFP10_ARM10E:
420: case FPU_VFP11_ARM11:
1.8 matt 421: case FPU_VFP_CORTEXA5:
422: case FPU_VFP_CORTEXA7:
423: case FPU_VFP_CORTEXA8:
424: case FPU_VFP_CORTEXA9:
1.4 matt 425: fregs->vfp_fpinst = read_fpinst();
426: fregs->vfp_fpinst2 = read_fpinst2();
427: break;
428: default:
429: panic("%s: Unsupported VFP %#x",
430: __func__, ci->ci_vfp_id);
1.1 rearnsha 431: }
432: }
1.4 matt 433: fregs->vfp_fpscr = read_fpscr();
434: save_vfpregs(fregs->vfp_regs);
435:
1.1 rearnsha 436: /* Disable the VFP. */
1.4 matt 437: write_fpexc(fpexc);
1.1 rearnsha 438: }
439:
440: void
1.4 matt 441: vfp_state_release(lwp_t *l)
1.1 rearnsha 442: {
1.4 matt 443: struct pcb * const pcb = lwp_getpcb(l);
1.1 rearnsha 444:
1.4 matt 445: /*
446: * Now mark the VFP as disabled (and our state has been already
447: * saved or is being discarded).
448: */
449: pcb->pcb_vfp.vfp_fpexc &= ~VFP_FPEXC_EN;
1.1 rearnsha 450:
451: /*
1.4 matt 452: * Turn off the FPU so the next time a VFP instruction is issued
453: * an exception happens. We don't know if this LWP's state was
454: * loaded but if we turned off the FPU for some other LWP, when
455: * pcu_load invokes vfp_state_load it will see that VFP_FPEXC_EN
456: * is still set so it just restroe fpexc and return since its
457: * contents are still sitting in the VFP.
1.1 rearnsha 458: */
1.4 matt 459: write_fpexc(read_fpexc() & ~VFP_FPEXC_EN);
1.1 rearnsha 460: }
461:
462: void
1.2 cegger 463: vfp_savecontext(void)
1.1 rearnsha 464: {
1.4 matt 465: pcu_save(&arm_vfp_ops);
1.1 rearnsha 466: }
467:
468: void
1.4 matt 469: vfp_discardcontext(void)
1.1 rearnsha 470: {
1.4 matt 471: pcu_discard(&arm_vfp_ops);
472: }
1.1 rearnsha 473:
1.8 matt 474: void
475: vfp_getcontext(struct lwp *l, mcontext_t *mcp, int *flagsp)
476: {
477: if (l->l_md.md_flags & MDLWP_VFPUSED) {
478: const struct pcb * const pcb = lwp_getpcb(l);
479: pcu_save(&arm_vfp_ops);
480: mcp->__fpu.__vfpregs.__vfp_fpscr = pcb->pcb_vfp.vfp_fpscr;
481: memcpy(mcp->__fpu.__vfpregs.__vfp_fstmx, pcb->pcb_vfp.vfp_regs,
482: sizeof(mcp->__fpu.__vfpregs.__vfp_fstmx));
483: *flagsp |= _UC_FPU;
484: }
485: }
486:
487: void
488: vfp_setcontext(struct lwp *l, const mcontext_t *mcp)
489: {
490: pcu_discard(&arm_vfp_ops);
491: struct pcb * const pcb = lwp_getpcb(l);
492: l->l_md.md_flags |= MDLWP_VFPUSED;
493: pcb->pcb_vfp.vfp_fpscr = mcp->__fpu.__vfpregs.__vfp_fpscr;
494: memcpy(pcb->pcb_vfp.vfp_regs, mcp->__fpu.__vfpregs.__vfp_fstmx,
495: sizeof(mcp->__fpu.__vfpregs.__vfp_fstmx));
496: }
497:
1.4 matt 498: #endif /* FPU_VFP */
CVSweb <webmaster@jp.NetBSD.org>
![[BACK]](/icons/back.gif){kind=icon}
Return to vfp_init.c CVS log ![[TXT]](/icons/text.gif){kind=icon}
![[DIR]](/icons/dir.gif){kind=icon}
Up to [cvs.NetBSD.org] / src / sys / arch / arm / vfp