1/*-
2 * Copyright (c) 2014 Ian Lepore <ian@freebsd.org>
3 * Copyright (c) 2012 Mark Tinguely
4 *
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 *
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
26 * SUCH DAMAGE.
27 */
28
29#include <sys/cdefs.h>
30__FBSDID("$FreeBSD$");
31
32#ifdef VFP
33#include <sys/param.h>
34#include <sys/systm.h>
35#include <sys/proc.h>
36#include <sys/kernel.h>
37
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>
44
45/* function prototypes */
46static int vfp_bounce(u_int, u_int, struct trapframe *, int);
47static void vfp_restore(struct vfp_state *);
48
49extern int vfp_exists;
50static struct undefined_handler vfp10_uh, vfp11_uh;
51/* If true the VFP unit has 32 double registers, otherwise it has 16 */
52static int is_d32;
53
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));
57
58/* The VFMRX command using coprocessor commands */
59#define fmrx(reg) \
60({ u_int val = 0;\
61    __asm __volatile("mrc p10, 7, %0, " __STRING(reg) " , c0, 0" : "=r"(val));\
62    val; \
63})
64
65/*
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.
69 */
70#ifdef __clang__
71#define	LDCLNE  "ldclne "
72#define	STCLNE  "stclne "
73#else
74#define	LDCLNE  "ldcnel "
75#define	STCLNE  "stcnel "
76#endif
77
78static u_int
79get_coprocessorACR(void)
80{
81	u_int val;
82	__asm __volatile("mrc p15, 0, %0, c1, c0, 2" : "=r" (val) : : "cc");
83	return val;
84}
85
86static void
87set_coprocessorACR(u_int val)
88{
89	__asm __volatile("mcr p15, 0, %0, c1, c0, 2\n\t"
90	 : : "r" (val) : "cc");
91	isb();
92}
93
94
95	/* called for each cpu */
96void
97vfp_init(void)
98{
99	u_int fpsid, fpexc, tmp;
100	u_int coproc, vfp_arch;
101
102	coproc = get_coprocessorACR();
103	coproc |= COPROC10 | COPROC11;
104	set_coprocessorACR(coproc);
105
106	fpsid = fmrx(VFPSID);		/* read the vfp system id */
107	fpexc = fmrx(VFPEXC);		/* read the vfp exception reg */
108
109	if (!(fpsid & VFPSID_HARDSOFT_IMP)) {
110		vfp_exists = 1;
111		is_d32 = 0;
112		PCPU_SET(vfpsid, fpsid);	/* save the VFPSID */
113
114		vfp_arch =
115		    (fpsid & VFPSID_SUBVERSION2_MASK) >> VFPSID_SUBVERSION_OFF;
116
117		if (vfp_arch >= VFP_ARCH3) {
118			tmp = fmrx(VMVFR0);
119			PCPU_SET(vfpmvfr0, tmp);
120
121			if ((tmp & VMVFR0_RB_MASK) == 2)
122				is_d32 = 1;
123
124			tmp = fmrx(VMVFR1);
125			PCPU_SET(vfpmvfr1, tmp);
126		}
127
128		/* initialize the coprocess 10 and 11 calls
129		 * These are called to restore the registers and enable
130		 * the VFP hardware.
131		 */
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);
137		}
138	}
139}
140
141SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL);
142
143
144/* start VFP unit, restore the vfp registers from the PCB  and retry
145 * the instruction
146 */
147static int
148vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code)
149{
150	u_int cpu, fpexc;
151	struct pcb *curpcb;
152	ksiginfo_t ksi;
153
154	if ((code & FAULT_USER) == 0)
155		panic("undefined floating point instruction in supervisor mode");
156
157	critical_enter();
158
159	/*
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
162	 * the VFP.
163	 */
164	fpexc = fmrx(VFPEXC);
165	if (fpexc & VFPEXC_EN) {
166		/* Clear any exceptions */
167		fmxr(VFPEXC, fpexc & ~(VFPEXC_EX | VFPEXC_FP2V));
168
169		/* kill the process - we do not handle emulation */
170		critical_exit();
171
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);
184			return 0;
185		}
186
187		return 1;
188	}
189
190	/*
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.
194	 */
195	fmxr(VFPEXC, fpexc | VFPEXC_EN);
196	curpcb = curthread->td_pcb;
197	cpu = PCPU_GET(cpu);
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);
202	}
203
204	critical_exit();
205	return (0);
206}
207
208/*
209 * Restore the given state to the VFP hardware.
210 */
211static void
212vfp_restore(struct vfp_state *vfpsave)
213{
214	uint32_t fpexc;
215
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);
222	}
223	fmxr(VFPSCR, vfpsave->fpscr);
224
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");
230
231	fmxr(VFPEXC, fpexc);
232}
233
234/*
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.
238 */
239void
240vfp_store(struct vfp_state *vfpsave, boolean_t disable_vfp)
241{
242	uint32_t fpexc;
243
244	fpexc = fmrx(VFPEXC);		/* Is the vfp enabled? */
245	if (fpexc & VFPEXC_EN) {
246		vfpsave->fpexec = fpexc;
247		vfpsave->fpscr = fmrx(VFPSCR);
248
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);
254			fpexc &= ~VFPEXC_EX;
255		}
256
257		__asm __volatile(
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");
263
264		if (disable_vfp)
265			fmxr(VFPEXC , fpexc & ~VFPEXC_EN);
266	}
267}
268
269/*
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
274 * switch here.
275 */
276void
277vfp_discard(struct thread *td)
278{
279	u_int tmp;
280
281	if (PCPU_GET(fpcurthread) == td)
282		PCPU_SET(fpcurthread, NULL);
283
284	tmp = fmrx(VFPEXC);
285	if (tmp & VFPEXC_EN)
286		fmxr(VFPEXC, tmp & ~VFPEXC_EN);
287}
288
289#endif
290
291