1/*
2 *  linux/arch/arm/vfp/vfpmodule.c
3 *
4 *  Copyright (C) 2004 ARM Limited.
5 *  Written by Deep Blue Solutions Limited.
6 *
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License version 2 as
9 * published by the Free Software Foundation.
10 */
11#include <linux/module.h>
12#include <linux/types.h>
13#include <linux/kernel.h>
14#include <linux/signal.h>
15#include <linux/sched.h>
16#include <linux/init.h>
17
18#include <asm/thread_notify.h>
19#include <asm/vfp.h>
20
21#include "vfpinstr.h"
22#include "vfp.h"
23
24/*
25 * Our undef handlers (in entry.S)
26 */
27void vfp_testing_entry(void);
28void vfp_support_entry(void);
29void vfp_null_entry(void);
30
31void (*vfp_vector)(void) = vfp_null_entry;
32union vfp_state *last_VFP_context[NR_CPUS];
33
34/*
35 * Dual-use variable.
36 * Used in startup: set to non-zero if VFP checks fail
37 * After startup, holds VFP architecture
38 */
39unsigned int VFP_arch;
40
41static int vfp_notifier(struct notifier_block *self, unsigned long cmd, void *v)
42{
43	struct thread_info *thread = v;
44	union vfp_state *vfp;
45	__u32 cpu = thread->cpu;
46
47	if (likely(cmd == THREAD_NOTIFY_SWITCH)) {
48		u32 fpexc = fmrx(FPEXC);
49
50#ifdef CONFIG_SMP
51		/*
52		 * On SMP, if VFP is enabled, save the old state in
53		 * case the thread migrates to a different CPU. The
54		 * restoring is done lazily.
55		 */
56		if ((fpexc & FPEXC_ENABLE) && last_VFP_context[cpu]) {
57			vfp_save_state(last_VFP_context[cpu], fpexc);
58			last_VFP_context[cpu]->hard.cpu = cpu;
59		}
60		/*
61		 * Thread migration, just force the reloading of the
62		 * state on the new CPU in case the VFP registers
63		 * contain stale data.
64		 */
65		if (thread->vfpstate.hard.cpu != cpu)
66			last_VFP_context[cpu] = NULL;
67#endif
68
69		/*
70		 * Always disable VFP so we can lazily save/restore the
71		 * old state.
72		 */
73		fmxr(FPEXC, fpexc & ~FPEXC_ENABLE);
74		return NOTIFY_DONE;
75	}
76
77	vfp = &thread->vfpstate;
78	if (cmd == THREAD_NOTIFY_FLUSH) {
79		/*
80		 * Per-thread VFP initialisation.
81		 */
82		memset(vfp, 0, sizeof(union vfp_state));
83
84		vfp->hard.fpexc = FPEXC_ENABLE;
85		vfp->hard.fpscr = FPSCR_ROUND_NEAREST;
86
87		/*
88		 * Disable VFP to ensure we initialise it first.
89		 */
90		fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_ENABLE);
91	}
92
93	/* flush and release case: Per-thread VFP cleanup. */
94	if (last_VFP_context[cpu] == vfp)
95		last_VFP_context[cpu] = NULL;
96
97	return NOTIFY_DONE;
98}
99
100static struct notifier_block vfp_notifier_block = {
101	.notifier_call	= vfp_notifier,
102};
103
104/*
105 * Raise a SIGFPE for the current process.
106 * sicode describes the signal being raised.
107 */
108void vfp_raise_sigfpe(unsigned int sicode, struct pt_regs *regs)
109{
110	siginfo_t info;
111
112	memset(&info, 0, sizeof(info));
113
114	info.si_signo = SIGFPE;
115	info.si_code = sicode;
116	info.si_addr = (void __user *)(instruction_pointer(regs) - 4);
117
118	/*
119	 * This is the same as NWFPE, because it's not clear what
120	 * this is used for
121	 */
122	current->thread.error_code = 0;
123	current->thread.trap_no = 6;
124
125	send_sig_info(SIGFPE, &info, current);
126}
127
128static void vfp_panic(char *reason)
129{
130	int i;
131
132	printk(KERN_ERR "VFP: Error: %s\n", reason);
133	printk(KERN_ERR "VFP: EXC 0x%08x SCR 0x%08x INST 0x%08x\n",
134		fmrx(FPEXC), fmrx(FPSCR), fmrx(FPINST));
135	for (i = 0; i < 32; i += 2)
136		printk(KERN_ERR "VFP: s%2u: 0x%08x s%2u: 0x%08x\n",
137		       i, vfp_get_float(i), i+1, vfp_get_float(i+1));
138}
139
140/*
141 * Process bitmask of exception conditions.
142 */
143static void vfp_raise_exceptions(u32 exceptions, u32 inst, u32 fpscr, struct pt_regs *regs)
144{
145	int si_code = 0;
146
147	pr_debug("VFP: raising exceptions %08x\n", exceptions);
148
149	if (exceptions == VFP_EXCEPTION_ERROR) {
150		vfp_panic("unhandled bounce");
151		vfp_raise_sigfpe(0, regs);
152		return;
153	}
154
155	/*
156	 * If any of the status flags are set, update the FPSCR.
157	 * Comparison instructions always return at least one of
158	 * these flags set.
159	 */
160	if (exceptions & (FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V))
161		fpscr &= ~(FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V);
162
163	fpscr |= exceptions;
164
165	fmxr(FPSCR, fpscr);
166
167#define RAISE(stat,en,sig)				\
168	if (exceptions & stat && fpscr & en)		\
169		si_code = sig;
170
171	/*
172	 * These are arranged in priority order, least to highest.
173	 */
174	RAISE(FPSCR_DZC, FPSCR_DZE, FPE_FLTDIV);
175	RAISE(FPSCR_IXC, FPSCR_IXE, FPE_FLTRES);
176	RAISE(FPSCR_UFC, FPSCR_UFE, FPE_FLTUND);
177	RAISE(FPSCR_OFC, FPSCR_OFE, FPE_FLTOVF);
178	RAISE(FPSCR_IOC, FPSCR_IOE, FPE_FLTINV);
179
180	if (si_code)
181		vfp_raise_sigfpe(si_code, regs);
182}
183
184/*
185 * Emulate a VFP instruction.
186 */
187static u32 vfp_emulate_instruction(u32 inst, u32 fpscr, struct pt_regs *regs)
188{
189	u32 exceptions = VFP_EXCEPTION_ERROR;
190
191	pr_debug("VFP: emulate: INST=0x%08x SCR=0x%08x\n", inst, fpscr);
192
193	if (INST_CPRTDO(inst)) {
194		if (!INST_CPRT(inst)) {
195			/*
196			 * CPDO
197			 */
198			if (vfp_single(inst)) {
199				exceptions = vfp_single_cpdo(inst, fpscr);
200			} else {
201				exceptions = vfp_double_cpdo(inst, fpscr);
202			}
203		} else {
204			/*
205			 * A CPRT instruction can not appear in FPINST2, nor
206			 * can it cause an exception.  Therefore, we do not
207			 * have to emulate it.
208			 */
209		}
210	} else {
211		/*
212		 * A CPDT instruction can not appear in FPINST2, nor can
213		 * it cause an exception.  Therefore, we do not have to
214		 * emulate it.
215		 */
216	}
217	return exceptions & ~VFP_NAN_FLAG;
218}
219
220/*
221 * Package up a bounce condition.
222 */
223void VFP9_bounce(u32 trigger, u32 fpexc, struct pt_regs *regs)
224{
225	u32 fpscr, orig_fpscr, exceptions, inst;
226
227	pr_debug("VFP: bounce: trigger %08x fpexc %08x\n", trigger, fpexc);
228
229	/*
230	 * Enable access to the VFP so we can handle the bounce.
231	 */
232	fmxr(FPEXC, fpexc & ~(FPEXC_EXCEPTION|FPEXC_INV|FPEXC_UFC|FPEXC_IOC));
233
234	orig_fpscr = fpscr = fmrx(FPSCR);
235
236	/*
237	 * If we are running with inexact exceptions enabled, we need to
238	 * emulate the trigger instruction.  Note that as we're emulating
239	 * the trigger instruction, we need to increment PC.
240	 */
241	if (fpscr & FPSCR_IXE) {
242		regs->ARM_pc += 4;
243		goto emulate;
244	}
245
246	barrier();
247
248	/*
249	 * Modify fpscr to indicate the number of iterations remaining
250	 */
251	if (fpexc & FPEXC_EXCEPTION) {
252		u32 len;
253
254		len = fpexc + (1 << FPEXC_LENGTH_BIT);
255
256		fpscr &= ~FPSCR_LENGTH_MASK;
257		fpscr |= (len & FPEXC_LENGTH_MASK) << (FPSCR_LENGTH_BIT - FPEXC_LENGTH_BIT);
258	}
259
260	/*
261	 * Handle the first FP instruction.  We used to take note of the
262	 * FPEXC bounce reason, but this appears to be unreliable.
263	 * Emulate the bounced instruction instead.
264	 */
265	inst = fmrx(FPINST);
266	exceptions = vfp_emulate_instruction(inst, fpscr, regs);
267	if (exceptions)
268		vfp_raise_exceptions(exceptions, inst, orig_fpscr, regs);
269
270	/*
271	 * If there isn't a second FP instruction, exit now.
272	 */
273	if (!(fpexc & FPEXC_FPV2))
274		return;
275
276	/*
277	 * The barrier() here prevents fpinst2 being read
278	 * before the condition above.
279	 */
280	barrier();
281	trigger = fmrx(FPINST2);
282	orig_fpscr = fpscr = fmrx(FPSCR);
283
284 emulate:
285	exceptions = vfp_emulate_instruction(trigger, fpscr, regs);
286	if (exceptions)
287		vfp_raise_exceptions(exceptions, trigger, orig_fpscr, regs);
288}
289
290static void vfp_enable(void *unused)
291{
292	u32 access = get_copro_access();
293
294	/*
295	 * Enable full access to VFP (cp10 and cp11)
296	 */
297	set_copro_access(access | CPACC_FULL(10) | CPACC_FULL(11));
298}
299
300#include <linux/smp.h>
301
302/*
303 * VFP support code initialisation.
304 */
305static int __init vfp_init(void)
306{
307	unsigned int vfpsid;
308	unsigned int cpu_arch = cpu_architecture();
309	u32 access = 0;
310
311	if (cpu_arch >= CPU_ARCH_ARMv6) {
312		access = get_copro_access();
313
314		/*
315		 * Enable full access to VFP (cp10 and cp11)
316		 */
317		set_copro_access(access | CPACC_FULL(10) | CPACC_FULL(11));
318	}
319
320	/*
321	 * First check that there is a VFP that we can use.
322	 * The handler is already setup to just log calls, so
323	 * we just need to read the VFPSID register.
324	 */
325	vfp_vector = vfp_testing_entry;
326	vfpsid = fmrx(FPSID);
327	barrier();
328	vfp_vector = vfp_null_entry;
329
330	printk(KERN_INFO "VFP support v0.3: ");
331	if (VFP_arch) {
332		printk("not present\n");
333
334		/*
335		 * Restore the copro access register.
336		 */
337		if (cpu_arch >= CPU_ARCH_ARMv6)
338			set_copro_access(access);
339	} else if (vfpsid & FPSID_NODOUBLE) {
340		printk("no double precision support\n");
341	} else {
342		smp_call_function(vfp_enable, NULL, 1, 1);
343
344		VFP_arch = (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT;  /* Extract the architecture version */
345		printk("implementor %02x architecture %d part %02x variant %x rev %x\n",
346			(vfpsid & FPSID_IMPLEMENTER_MASK) >> FPSID_IMPLEMENTER_BIT,
347			(vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT,
348			(vfpsid & FPSID_PART_MASK) >> FPSID_PART_BIT,
349			(vfpsid & FPSID_VARIANT_MASK) >> FPSID_VARIANT_BIT,
350			(vfpsid & FPSID_REV_MASK) >> FPSID_REV_BIT);
351
352		vfp_vector = vfp_support_entry;
353
354		thread_register_notifier(&vfp_notifier_block);
355
356		/*
357		 * We detected VFP, and the support code is
358		 * in place; report VFP support to userspace.
359		 */
360		elf_hwcap |= HWCAP_VFP;
361	}
362	return 0;
363}
364
365late_initcall(vfp_init);
366