2 * Copyright (c) 2014 Ian Lepore <ian@freebsd.org>
3 * Copyright (c) 2012 Mark Tinguely
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
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.
16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
17 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
20 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
22 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
24 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
25 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
29 #include <sys/cdefs.h>
30 __FBSDID("$FreeBSD$");
33 #include <sys/param.h>
34 #include <sys/systm.h>
36 #include <sys/kernel.h>
38 #include <machine/armreg.h>
39 #include <machine/frame.h>
40 #include <machine/fp.h>
41 #include <machine/pcb.h>
42 #include <machine/undefined.h>
43 #include <machine/vfp.h>
45 /* function prototypes */
46 static int vfp_bounce(u_int, u_int, struct trapframe *, int);
47 static void vfp_restore(struct vfp_state *);
49 extern int vfp_exists;
50 static struct undefined_handler vfp10_uh, vfp11_uh;
51 /* If true the VFP unit has 32 double registers, otherwise it has 16 */
54 /* The VFMXR command using coprocessor commands */
55 #define fmxr(reg, val) \
56 __asm __volatile("mcr p10, 7, %0, " __STRING(reg) " , c0, 0" :: "r"(val));
58 /* The VFMRX command using coprocessor commands */
61 __asm __volatile("mrc p10, 7, %0, " __STRING(reg) " , c0, 0" : "=r"(val));\
66 * Work around an issue with GCC where the asm it generates is not unified
67 * syntax and fails to assemble because it expects the ldcleq instruction in the
68 * form ldc<c>l, not in the UAL form ldcl<c>, and similar for stcleq.
71 #define LDCLNE "ldclne "
72 #define STCLNE "stclne "
74 #define LDCLNE "ldcnel "
75 #define STCLNE "stcnel "
79 get_coprocessorACR(void)
82 __asm __volatile("mrc p15, 0, %0, c1, c0, 2" : "=r" (val) : : "cc");
87 set_coprocessorACR(u_int val)
89 __asm __volatile("mcr p15, 0, %0, c1, c0, 2\n\t"
90 : : "r" (val) : "cc");
95 /* called for each cpu */
99 u_int fpsid, fpexc, tmp;
100 u_int coproc, vfp_arch;
102 coproc = get_coprocessorACR();
103 coproc |= COPROC10 | COPROC11;
104 set_coprocessorACR(coproc);
106 fpsid = fmrx(VFPSID); /* read the vfp system id */
107 fpexc = fmrx(VFPEXC); /* read the vfp exception reg */
109 if (!(fpsid & VFPSID_HARDSOFT_IMP)) {
112 PCPU_SET(vfpsid, fpsid); /* save the VFPSID */
115 (fpsid & VFPSID_SUBVERSION2_MASK) >> VFPSID_SUBVERSION_OFF;
117 if (vfp_arch >= VFP_ARCH3) {
119 PCPU_SET(vfpmvfr0, tmp);
121 if ((tmp & VMVFR0_RB_MASK) == 2)
125 PCPU_SET(vfpmvfr1, tmp);
128 /* initialize the coprocess 10 and 11 calls
129 * These are called to restore the registers and enable
132 if (vfp10_uh.uh_handler == NULL) {
133 vfp10_uh.uh_handler = vfp_bounce;
134 vfp11_uh.uh_handler = vfp_bounce;
135 install_coproc_handler_static(10, &vfp10_uh);
136 install_coproc_handler_static(11, &vfp11_uh);
141 SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL);
144 /* start VFP unit, restore the vfp registers from the PCB and retry
148 vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code)
154 if ((code & FAULT_USER) == 0)
155 panic("undefined floating point instruction in supervisor mode");
160 * If the VFP is already on and we got an undefined instruction, then
161 * something tried to executate a truly invalid instruction that maps to
164 fpexc = fmrx(VFPEXC);
165 if (fpexc & VFPEXC_EN) {
166 /* Clear any exceptions */
167 fmxr(VFPEXC, fpexc & ~(VFPEXC_EX | VFPEXC_FP2V));
169 /* kill the process - we do not handle emulation */
172 if (fpexc & VFPEXC_EX) {
173 /* We have an exception, signal a SIGFPE */
174 ksiginfo_init_trap(&ksi);
175 ksi.ksi_signo = SIGFPE;
176 if (fpexc & VFPEXC_UFC)
177 ksi.ksi_code = FPE_FLTUND;
178 else if (fpexc & VFPEXC_OFC)
179 ksi.ksi_code = FPE_FLTOVF;
180 else if (fpexc & VFPEXC_IOC)
181 ksi.ksi_code = FPE_FLTINV;
182 ksi.ksi_addr = (void *)addr;
183 trapsignal(curthread, &ksi);
191 * If the last time this thread used the VFP it was on this core, and
192 * the last thread to use the VFP on this core was this thread, then the
193 * VFP state is valid, otherwise restore this thread's state to the VFP.
195 fmxr(VFPEXC, fpexc | VFPEXC_EN);
196 curpcb = curthread->td_pcb;
198 if (curpcb->pcb_vfpcpu != cpu || curthread != PCPU_GET(fpcurthread)) {
199 vfp_restore(&curpcb->pcb_vfpstate);
200 curpcb->pcb_vfpcpu = cpu;
201 PCPU_SET(fpcurthread, curthread);
209 * Restore the given state to the VFP hardware.
212 vfp_restore(struct vfp_state *vfpsave)
216 /* On VFPv2 we may need to restore FPINST and FPINST2 */
217 fpexc = vfpsave->fpexec;
218 if (fpexc & VFPEXC_EX) {
219 fmxr(VFPINST, vfpsave->fpinst);
220 if (fpexc & VFPEXC_FP2V)
221 fmxr(VFPINST2, vfpsave->fpinst2);
223 fmxr(VFPSCR, vfpsave->fpscr);
225 __asm __volatile("ldc p10, c0, [%0], #128\n" /* d0-d15 */
226 "cmp %1, #0\n" /* -D16 or -D32? */
227 LDCLNE "p11, c0, [%0], #128\n" /* d16-d31 */
228 "addeq %0, %0, #128\n" /* skip missing regs */
229 : : "r" (vfpsave), "r" (is_d32) : "cc");
235 * If the VFP is on, save its current state and turn it off if requested to do
236 * so. If the VFP is not on, does not change the values at *vfpsave. Caller is
237 * responsible for preventing a context switch while this is running.
240 vfp_store(struct vfp_state *vfpsave, boolean_t disable_vfp)
244 fpexc = fmrx(VFPEXC); /* Is the vfp enabled? */
245 if (fpexc & VFPEXC_EN) {
246 vfpsave->fpexec = fpexc;
247 vfpsave->fpscr = fmrx(VFPSCR);
249 /* On VFPv2 we may need to save FPINST and FPINST2 */
250 if (fpexc & VFPEXC_EX) {
251 vfpsave->fpinst = fmrx(VFPINST);
252 if (fpexc & VFPEXC_FP2V)
253 vfpsave->fpinst2 = fmrx(VFPINST2);
258 "stc p11, c0, [%0], #128\n" /* d0-d15 */
259 "cmp %1, #0\n" /* -D16 or -D32? */
260 STCLNE "p11, c0, [%0], #128\n" /* d16-d31 */
261 "addeq %0, %0, #128\n" /* skip missing regs */
262 : : "r" (vfpsave), "r" (is_d32) : "cc");
265 fmxr(VFPEXC , fpexc & ~VFPEXC_EN);
270 * The current thread is dying. If the state currently in the hardware belongs
271 * to the current thread, set fpcurthread to NULL to indicate that the VFP
272 * hardware state does not belong to any thread. If the VFP is on, turn it off.
273 * Called only from cpu_throw(), so we don't have to worry about a context
277 vfp_discard(struct thread *td)
281 if (PCPU_GET(fpcurthread) == td)
282 PCPU_SET(fpcurthread, NULL);
286 fmxr(VFPEXC, tmp & ~VFPEXC_EN);