1
2
3
4
5
6
7
8
9
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
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
36
37
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
53
54
55
56 if ((fpexc & FPEXC_EN) && last_VFP_context[cpu]) {
57 vfp_save_state(last_VFP_context[cpu], fpexc);
58 last_VFP_context[cpu]->hard.cpu = cpu;
59 }
60
61
62
63
64
65 if (thread->vfpstate.hard.cpu != cpu)
66 last_VFP_context[cpu] = NULL;
67#endif
68
69
70
71
72
73 fmxr(FPEXC, fpexc & ~FPEXC_EN);
74 return NOTIFY_DONE;
75 }
76
77 vfp = &thread->vfpstate;
78 if (cmd == THREAD_NOTIFY_FLUSH) {
79
80
81
82 memset(vfp, 0, sizeof(union vfp_state));
83
84 vfp->hard.fpexc = FPEXC_EN;
85 vfp->hard.fpscr = FPSCR_ROUND_NEAREST;
86
87
88
89
90 fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
91 }
92
93
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
106
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
120
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, u32 inst)
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), inst);
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
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", inst);
151 vfp_raise_sigfpe(0, regs);
152 return;
153 }
154
155
156
157
158
159
160 fpscr |= exceptions;
161
162 fmxr(FPSCR, fpscr);
163
164#define RAISE(stat,en,sig) \
165 if (exceptions & stat && fpscr & en) \
166 si_code = sig;
167
168
169
170
171 RAISE(FPSCR_DZC, FPSCR_DZE, FPE_FLTDIV);
172 RAISE(FPSCR_IXC, FPSCR_IXE, FPE_FLTRES);
173 RAISE(FPSCR_UFC, FPSCR_UFE, FPE_FLTUND);
174 RAISE(FPSCR_OFC, FPSCR_OFE, FPE_FLTOVF);
175 RAISE(FPSCR_IOC, FPSCR_IOE, FPE_FLTINV);
176
177 if (si_code)
178 vfp_raise_sigfpe(si_code, regs);
179}
180
181
182
183
184static u32 vfp_emulate_instruction(u32 inst, u32 fpscr, struct pt_regs *regs)
185{
186 u32 exceptions = VFP_EXCEPTION_ERROR;
187
188 pr_debug("VFP: emulate: INST=0x%08x SCR=0x%08x\n", inst, fpscr);
189
190 if (INST_CPRTDO(inst)) {
191 if (!INST_CPRT(inst)) {
192
193
194
195 if (vfp_single(inst)) {
196 exceptions = vfp_single_cpdo(inst, fpscr);
197 } else {
198 exceptions = vfp_double_cpdo(inst, fpscr);
199 }
200 } else {
201
202
203
204
205
206 }
207 } else {
208
209
210
211
212
213 }
214 return exceptions & ~VFP_NAN_FLAG;
215}
216
217
218
219
220void VFP_bounce(u32 trigger, u32 fpexc, struct pt_regs *regs)
221{
222 u32 fpscr, orig_fpscr, fpsid, exceptions;
223
224 pr_debug("VFP: bounce: trigger %08x fpexc %08x\n", trigger, fpexc);
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239 fmxr(FPEXC, fpexc & ~(FPEXC_EX|FPEXC_DEX|FPEXC_FP2V|FPEXC_VV|FPEXC_TRAP_MASK));
240
241 fpsid = fmrx(FPSID);
242 orig_fpscr = fpscr = fmrx(FPSCR);
243
244
245
246
247 if ((fpsid & FPSID_ARCH_MASK) == (1 << FPSID_ARCH_BIT)
248 && (fpscr & FPSCR_IXE)) {
249
250
251
252 goto emulate;
253 }
254
255 if (fpexc & FPEXC_EX) {
256#ifndef CONFIG_CPU_FEROCEON
257
258
259
260
261 trigger = fmrx(FPINST);
262 regs->ARM_pc -= 4;
263#endif
264 } else if (!(fpexc & FPEXC_DEX)) {
265
266
267
268
269
270 vfp_raise_exceptions(VFP_EXCEPTION_ERROR, trigger, fpscr, regs);
271 goto exit;
272 }
273
274
275
276
277
278
279 if (fpexc & (FPEXC_EX | FPEXC_VV)) {
280 u32 len;
281
282 len = fpexc + (1 << FPEXC_LENGTH_BIT);
283
284 fpscr &= ~FPSCR_LENGTH_MASK;
285 fpscr |= (len & FPEXC_LENGTH_MASK) << (FPSCR_LENGTH_BIT - FPEXC_LENGTH_BIT);
286 }
287
288
289
290
291
292
293 exceptions = vfp_emulate_instruction(trigger, fpscr, regs);
294 if (exceptions)
295 vfp_raise_exceptions(exceptions, trigger, orig_fpscr, regs);
296
297
298
299
300
301 if (fpexc ^ (FPEXC_EX | FPEXC_FP2V))
302 goto exit;
303
304
305
306
307
308 barrier();
309 trigger = fmrx(FPINST2);
310
311 emulate:
312 exceptions = vfp_emulate_instruction(trigger, orig_fpscr, regs);
313 if (exceptions)
314 vfp_raise_exceptions(exceptions, trigger, orig_fpscr, regs);
315 exit:
316 preempt_enable();
317}
318
319static void vfp_enable(void *unused)
320{
321 u32 access = get_copro_access();
322
323
324
325
326 set_copro_access(access | CPACC_FULL(10) | CPACC_FULL(11));
327}
328
329#ifdef CONFIG_PM
330#include <linux/sysdev.h>
331
332static int vfp_pm_suspend(struct sys_device *dev, pm_message_t state)
333{
334 struct thread_info *ti = current_thread_info();
335 u32 fpexc = fmrx(FPEXC);
336
337
338 if (fpexc & FPEXC_EN) {
339 printk(KERN_DEBUG "%s: saving vfp state\n", __func__);
340 vfp_save_state(&ti->vfpstate, fpexc);
341
342
343 fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
344 }
345
346
347 memset(last_VFP_context, 0, sizeof(last_VFP_context));
348
349 return 0;
350}
351
352static int vfp_pm_resume(struct sys_device *dev)
353{
354
355 vfp_enable(NULL);
356
357
358 fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
359
360 return 0;
361}
362
363static struct sysdev_class vfp_pm_sysclass = {
364 .name = "vfp",
365 .suspend = vfp_pm_suspend,
366 .resume = vfp_pm_resume,
367};
368
369static struct sys_device vfp_pm_sysdev = {
370 .cls = &vfp_pm_sysclass,
371};
372
373static void vfp_pm_init(void)
374{
375 sysdev_class_register(&vfp_pm_sysclass);
376 sysdev_register(&vfp_pm_sysdev);
377}
378
379
380#else
381static inline void vfp_pm_init(void) { }
382#endif
383
384
385
386
387
388#ifdef CONFIG_SMP
389void vfp_sync_state(struct thread_info *thread)
390{
391
392
393
394
395
396
397 thread->vfpstate.hard.cpu = NR_CPUS;
398}
399#else
400void vfp_sync_state(struct thread_info *thread)
401{
402 unsigned int cpu = get_cpu();
403 u32 fpexc = fmrx(FPEXC);
404
405
406
407
408
409 if (fpexc & FPEXC_EN)
410 goto out;
411
412 if (!last_VFP_context[cpu])
413 goto out;
414
415
416
417
418 fmxr(FPEXC, fpexc | FPEXC_EN);
419 vfp_save_state(last_VFP_context[cpu], fpexc);
420 fmxr(FPEXC, fpexc);
421
422
423
424
425
426 last_VFP_context[cpu] = NULL;
427
428out:
429 put_cpu();
430}
431#endif
432
433#include <linux/smp.h>
434
435
436
437
438static int __init vfp_init(void)
439{
440 unsigned int vfpsid;
441 unsigned int cpu_arch = cpu_architecture();
442
443 if (cpu_arch >= CPU_ARCH_ARMv6)
444 vfp_enable(NULL);
445
446
447
448
449
450
451 vfp_vector = vfp_testing_entry;
452 barrier();
453 vfpsid = fmrx(FPSID);
454 barrier();
455 vfp_vector = vfp_null_entry;
456
457 printk(KERN_INFO "VFP support v0.3: ");
458 if (VFP_arch)
459 printk("not present\n");
460 else if (vfpsid & FPSID_NODOUBLE) {
461 printk("no double precision support\n");
462 } else {
463 smp_call_function(vfp_enable, NULL, 1);
464
465 VFP_arch = (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT;
466 printk("implementor %02x architecture %d part %02x variant %x rev %x\n",
467 (vfpsid & FPSID_IMPLEMENTER_MASK) >> FPSID_IMPLEMENTER_BIT,
468 (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT,
469 (vfpsid & FPSID_PART_MASK) >> FPSID_PART_BIT,
470 (vfpsid & FPSID_VARIANT_MASK) >> FPSID_VARIANT_BIT,
471 (vfpsid & FPSID_REV_MASK) >> FPSID_REV_BIT);
472
473 vfp_vector = vfp_support_entry;
474
475 thread_register_notifier(&vfp_notifier_block);
476 vfp_pm_init();
477
478
479
480
481
482 elf_hwcap |= HWCAP_VFP;
483#ifdef CONFIG_VFPv3
484 if (VFP_arch >= 3) {
485 elf_hwcap |= HWCAP_VFPv3;
486
487
488
489
490
491 if (((fmrx(MVFR0) & MVFR0_A_SIMD_MASK)) == 1)
492 elf_hwcap |= HWCAP_VFPv3D16;
493 }
494#endif
495#ifdef CONFIG_NEON
496
497
498
499
500
501 if ((fmrx(MVFR1) & 0x000fff00) == 0x00011100)
502 elf_hwcap |= HWCAP_NEON;
503#endif
504 }
505 return 0;
506}
507
508late_initcall(vfp_init);
509