vfpmodule.c 20.5 KB
Newer Older
Linus Torvalds's avatar
Linus Torvalds committed
1
2
3
4
5
6
7
8
9
10
11
/*
 *  linux/arch/arm/vfp/vfpmodule.c
 *
 *  Copyright (C) 2004 ARM Limited.
 *  Written by Deep Blue Solutions Limited.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 */
#include <linux/types.h>
12
#include <linux/cpu.h>
13
#include <linux/cpu_pm.h>
14
#include <linux/hardirq.h>
Linus Torvalds's avatar
Linus Torvalds committed
15
#include <linux/kernel.h>
16
#include <linux/notifier.h>
Linus Torvalds's avatar
Linus Torvalds committed
17
#include <linux/signal.h>
18
#include <linux/sched/signal.h>
19
#include <linux/smp.h>
Linus Torvalds's avatar
Linus Torvalds committed
20
#include <linux/init.h>
21
22
#include <linux/uaccess.h>
#include <linux/user.h>
23
#include <linux/export.h>
24

25
#include <asm/cp15.h>
26
#include <asm/cputype.h>
27
#include <asm/system_info.h>
28
#include <asm/thread_notify.h>
Linus Torvalds's avatar
Linus Torvalds committed
29
30
31
32
33
34
35
36
#include <asm/vfp.h>

#include "vfpinstr.h"
#include "vfp.h"

/*
 * Our undef handlers (in entry.S)
 */
37
38
39
asmlinkage void vfp_testing_entry(void);
asmlinkage void vfp_support_entry(void);
asmlinkage void vfp_null_entry(void);
Linus Torvalds's avatar
Linus Torvalds committed
40

41
asmlinkage void (*vfp_vector)(void) = vfp_null_entry;
42

43
44
45
46
47
48
49
/*
 * Dual-use variable.
 * Used in startup: set to non-zero if VFP checks fail
 * After startup, holds VFP architecture
 */
unsigned int VFP_arch;

50
51
52
53
/*
 * The pointer to the vfpstate structure of the thread which currently
 * owns the context held in the VFP hardware, or NULL if the hardware
 * context is invalid.
54
55
56
57
 *
 * For UP, this is sufficient to tell which thread owns the VFP context.
 * However, for SMP, we also need to check the CPU number stored in the
 * saved state too to catch migrations.
58
59
 */
union vfp_state *vfp_current_hw_state[NR_CPUS];
Linus Torvalds's avatar
Linus Torvalds committed
60
61

/*
62
63
 * Is 'thread's most up to date state stored in this CPUs hardware?
 * Must be called from non-preemptible context.
Linus Torvalds's avatar
Linus Torvalds committed
64
 */
65
66
67
68
69
70
71
72
73
74
75
76
static bool vfp_state_in_hw(unsigned int cpu, struct thread_info *thread)
{
#ifdef CONFIG_SMP
	if (thread->vfpstate.hard.cpu != cpu)
		return false;
#endif
	return vfp_current_hw_state[cpu] == &thread->vfpstate;
}

/*
 * Force a reload of the VFP context from the thread structure.  We do
 * this by ensuring that access to the VFP hardware is disabled, and
77
 * clear vfp_current_hw_state.  Must be called from non-preemptible context.
78
79
80
81
82
83
84
85
86
87
88
 */
static void vfp_force_reload(unsigned int cpu, struct thread_info *thread)
{
	if (vfp_state_in_hw(cpu, thread)) {
		fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
		vfp_current_hw_state[cpu] = NULL;
	}
#ifdef CONFIG_SMP
	thread->vfpstate.hard.cpu = NR_CPUS;
#endif
}
Linus Torvalds's avatar
Linus Torvalds committed
89

90
91
92
93
94
95
96
97
98
99
/*
 * Per-thread VFP initialization.
 */
static void vfp_thread_flush(struct thread_info *thread)
{
	union vfp_state *vfp = &thread->vfpstate;
	unsigned int cpu;

	/*
	 * Disable VFP to ensure we initialize it first.  We must ensure
100
101
102
103
104
	 * that the modification of vfp_current_hw_state[] and hardware
	 * disable are done for the same CPU and without preemption.
	 *
	 * Do this first to ensure that preemption won't overwrite our
	 * state saving should access to the VFP be enabled at this point.
105
106
	 */
	cpu = get_cpu();
107
108
	if (vfp_current_hw_state[cpu] == vfp)
		vfp_current_hw_state[cpu] = NULL;
109
110
	fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
	put_cpu();
111
112
113
114
115
116
117
118

	memset(vfp, 0, sizeof(union vfp_state));

	vfp->hard.fpexc = FPEXC_EN;
	vfp->hard.fpscr = FPSCR_ROUND_NEAREST;
#ifdef CONFIG_SMP
	vfp->hard.cpu = NR_CPUS;
#endif
119
120
}

121
static void vfp_thread_exit(struct thread_info *thread)
122
123
124
{
	/* release case: Per-thread VFP cleanup. */
	union vfp_state *vfp = &thread->vfpstate;
125
	unsigned int cpu = get_cpu();
126

127
128
	if (vfp_current_hw_state[cpu] == vfp)
		vfp_current_hw_state[cpu] = NULL;
129
	put_cpu();
130
131
}

132
133
134
135
136
137
static void vfp_thread_copy(struct thread_info *thread)
{
	struct thread_info *parent = current_thread_info();

	vfp_sync_hwstate(parent);
	thread->vfpstate = parent->vfpstate;
138
139
140
#ifdef CONFIG_SMP
	thread->vfpstate.hard.cpu = NR_CPUS;
#endif
141
142
}

143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
/*
 * When this function is called with the following 'cmd's, the following
 * is true while this function is being run:
 *  THREAD_NOFTIFY_SWTICH:
 *   - the previously running thread will not be scheduled onto another CPU.
 *   - the next thread to be run (v) will not be running on another CPU.
 *   - thread->cpu is the local CPU number
 *   - not preemptible as we're called in the middle of a thread switch
 *  THREAD_NOTIFY_FLUSH:
 *   - the thread (v) will be running on the local CPU, so
 *	v === current_thread_info()
 *   - thread->cpu is the local CPU number at the time it is accessed,
 *	but may change at any time.
 *   - we could be preempted if tree preempt rcu is enabled, so
 *	it is unsafe to use thread->cpu.
158
159
160
 *  THREAD_NOTIFY_EXIT
 *   - we could be preempted if tree preempt rcu is enabled, so
 *	it is unsafe to use thread->cpu.
161
 */
162
static int vfp_notifier(struct notifier_block *self, unsigned long cmd, void *v)
Linus Torvalds's avatar
Linus Torvalds committed
163
{
164
	struct thread_info *thread = v;
165
166
167
168
	u32 fpexc;
#ifdef CONFIG_SMP
	unsigned int cpu;
#endif
Linus Torvalds's avatar
Linus Torvalds committed
169

170
171
172
	switch (cmd) {
	case THREAD_NOTIFY_SWITCH:
		fpexc = fmrx(FPEXC);
173
174

#ifdef CONFIG_SMP
175
		cpu = thread->cpu;
176

177
178
179
180
181
		/*
		 * On SMP, if VFP is enabled, save the old state in
		 * case the thread migrates to a different CPU. The
		 * restoring is done lazily.
		 */
182
		if ((fpexc & FPEXC_EN) && vfp_current_hw_state[cpu])
183
			vfp_save_state(vfp_current_hw_state[cpu], fpexc);
184
185
#endif

186
187
188
189
		/*
		 * Always disable VFP so we can lazily save/restore the
		 * old state.
		 */
190
		fmxr(FPEXC, fpexc & ~FPEXC_EN);
191
		break;
192

193
	case THREAD_NOTIFY_FLUSH:
194
		vfp_thread_flush(thread);
195
196
197
		break;

	case THREAD_NOTIFY_EXIT:
198
		vfp_thread_exit(thread);
199
200
201
202
		break;

	case THREAD_NOTIFY_COPY:
		vfp_thread_copy(thread);
203
204
		break;
	}
205

206
	return NOTIFY_DONE;
Linus Torvalds's avatar
Linus Torvalds committed
207
208
}

209
210
211
212
static struct notifier_block vfp_notifier_block = {
	.notifier_call	= vfp_notifier,
};

Linus Torvalds's avatar
Linus Torvalds committed
213
214
215
216
/*
 * Raise a SIGFPE for the current process.
 * sicode describes the signal being raised.
 */
217
static void vfp_raise_sigfpe(unsigned int sicode, struct pt_regs *regs)
Linus Torvalds's avatar
Linus Torvalds committed
218
219
220
221
222
223
224
{
	siginfo_t info;

	memset(&info, 0, sizeof(info));

	info.si_signo = SIGFPE;
	info.si_code = sicode;
Al Viro's avatar
Al Viro committed
225
	info.si_addr = (void __user *)(instruction_pointer(regs) - 4);
Linus Torvalds's avatar
Linus Torvalds committed
226
227
228
229
230
231
232
233

	/*
	 * This is the same as NWFPE, because it's not clear what
	 * this is used for
	 */
	current->thread.error_code = 0;
	current->thread.trap_no = 6;

Russell King's avatar
Russell King committed
234
	send_sig_info(SIGFPE, &info, current);
Linus Torvalds's avatar
Linus Torvalds committed
235
236
}

237
static void vfp_panic(char *reason, u32 inst)
Linus Torvalds's avatar
Linus Torvalds committed
238
239
240
{
	int i;

241
242
	pr_err("VFP: Error: %s\n", reason);
	pr_err("VFP: EXC 0x%08x SCR 0x%08x INST 0x%08x\n",
243
		fmrx(FPEXC), fmrx(FPSCR), inst);
Linus Torvalds's avatar
Linus Torvalds committed
244
	for (i = 0; i < 32; i += 2)
245
		pr_err("VFP: s%2u: 0x%08x s%2u: 0x%08x\n",
Linus Torvalds's avatar
Linus Torvalds committed
246
247
248
249
250
251
252
253
254
255
256
257
		       i, vfp_get_float(i), i+1, vfp_get_float(i+1));
}

/*
 * Process bitmask of exception conditions.
 */
static void vfp_raise_exceptions(u32 exceptions, u32 inst, u32 fpscr, struct pt_regs *regs)
{
	int si_code = 0;

	pr_debug("VFP: raising exceptions %08x\n", exceptions);

258
	if (exceptions == VFP_EXCEPTION_ERROR) {
259
		vfp_panic("unhandled bounce", inst);
260
		vfp_raise_sigfpe(FPE_FIXME, regs);
Linus Torvalds's avatar
Linus Torvalds committed
261
262
263
264
		return;
	}

	/*
265
	 * If any of the status flags are set, update the FPSCR.
Linus Torvalds's avatar
Linus Torvalds committed
266
267
268
	 * Comparison instructions always return at least one of
	 * these flags set.
	 */
269
270
271
	if (exceptions & (FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V))
		fpscr &= ~(FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V);

Linus Torvalds's avatar
Linus Torvalds committed
272
273
274
275
276
277
278
279
280
281
282
	fpscr |= exceptions;

	fmxr(FPSCR, fpscr);

#define RAISE(stat,en,sig)				\
	if (exceptions & stat && fpscr & en)		\
		si_code = sig;

	/*
	 * These are arranged in priority order, least to highest.
	 */
283
	RAISE(FPSCR_DZC, FPSCR_DZE, FPE_FLTDIV);
Linus Torvalds's avatar
Linus Torvalds committed
284
285
286
287
288
289
290
291
292
293
294
295
296
297
	RAISE(FPSCR_IXC, FPSCR_IXE, FPE_FLTRES);
	RAISE(FPSCR_UFC, FPSCR_UFE, FPE_FLTUND);
	RAISE(FPSCR_OFC, FPSCR_OFE, FPE_FLTOVF);
	RAISE(FPSCR_IOC, FPSCR_IOE, FPE_FLTINV);

	if (si_code)
		vfp_raise_sigfpe(si_code, regs);
}

/*
 * Emulate a VFP instruction.
 */
static u32 vfp_emulate_instruction(u32 inst, u32 fpscr, struct pt_regs *regs)
{
298
	u32 exceptions = VFP_EXCEPTION_ERROR;
Linus Torvalds's avatar
Linus Torvalds committed
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325

	pr_debug("VFP: emulate: INST=0x%08x SCR=0x%08x\n", inst, fpscr);

	if (INST_CPRTDO(inst)) {
		if (!INST_CPRT(inst)) {
			/*
			 * CPDO
			 */
			if (vfp_single(inst)) {
				exceptions = vfp_single_cpdo(inst, fpscr);
			} else {
				exceptions = vfp_double_cpdo(inst, fpscr);
			}
		} else {
			/*
			 * A CPRT instruction can not appear in FPINST2, nor
			 * can it cause an exception.  Therefore, we do not
			 * have to emulate it.
			 */
		}
	} else {
		/*
		 * A CPDT instruction can not appear in FPINST2, nor can
		 * it cause an exception.  Therefore, we do not have to
		 * emulate it.
		 */
	}
326
	return exceptions & ~VFP_NAN_FLAG;
Linus Torvalds's avatar
Linus Torvalds committed
327
328
329
330
331
}

/*
 * Package up a bounce condition.
 */
332
void VFP_bounce(u32 trigger, u32 fpexc, struct pt_regs *regs)
Linus Torvalds's avatar
Linus Torvalds committed
333
{
334
	u32 fpscr, orig_fpscr, fpsid, exceptions;
Linus Torvalds's avatar
Linus Torvalds committed
335
336
337
338

	pr_debug("VFP: bounce: trigger %08x fpexc %08x\n", trigger, fpexc);

	/*
339
340
341
342
343
344
345
346
347
348
349
	 * At this point, FPEXC can have the following configuration:
	 *
	 *  EX DEX IXE
	 *  0   1   x   - synchronous exception
	 *  1   x   0   - asynchronous exception
	 *  1   x   1   - sychronous on VFP subarch 1 and asynchronous on later
	 *  0   0   1   - synchronous on VFP9 (non-standard subarch 1
	 *                implementation), undefined otherwise
	 *
	 * Clear various bits and enable access to the VFP so we can
	 * handle the bounce.
Linus Torvalds's avatar
Linus Torvalds committed
350
	 */
351
	fmxr(FPEXC, fpexc & ~(FPEXC_EX|FPEXC_DEX|FPEXC_FP2V|FPEXC_VV|FPEXC_TRAP_MASK));
Linus Torvalds's avatar
Linus Torvalds committed
352

353
	fpsid = fmrx(FPSID);
Linus Torvalds's avatar
Linus Torvalds committed
354
355
356
	orig_fpscr = fpscr = fmrx(FPSCR);

	/*
357
	 * Check for the special VFP subarch 1 and FPSCR.IXE bit case
Linus Torvalds's avatar
Linus Torvalds committed
358
	 */
359
360
361
362
363
	if ((fpsid & FPSID_ARCH_MASK) == (1 << FPSID_ARCH_BIT)
	    && (fpscr & FPSCR_IXE)) {
		/*
		 * Synchronous exception, emulate the trigger instruction
		 */
Linus Torvalds's avatar
Linus Torvalds committed
364
365
366
		goto emulate;
	}

367
	if (fpexc & FPEXC_EX) {
368
#ifndef CONFIG_CPU_FEROCEON
369
370
371
372
373
374
		/*
		 * Asynchronous exception. The instruction is read from FPINST
		 * and the interrupted instruction has to be restarted.
		 */
		trigger = fmrx(FPINST);
		regs->ARM_pc -= 4;
375
#endif
376
377
378
379
380
381
382
	} else if (!(fpexc & FPEXC_DEX)) {
		/*
		 * Illegal combination of bits. It can be caused by an
		 * unallocated VFP instruction but with FPSCR.IXE set and not
		 * on VFP subarch 1.
		 */
		 vfp_raise_exceptions(VFP_EXCEPTION_ERROR, trigger, fpscr, regs);
383
		goto exit;
384
	}
Linus Torvalds's avatar
Linus Torvalds committed
385
386

	/*
387
388
389
	 * Modify fpscr to indicate the number of iterations remaining.
	 * If FPEXC.EX is 0, FPEXC.DEX is 1 and the FPEXC.VV bit indicates
	 * whether FPEXC.VECITR or FPSCR.LEN is used.
Linus Torvalds's avatar
Linus Torvalds committed
390
	 */
391
	if (fpexc & (FPEXC_EX | FPEXC_VV)) {
Linus Torvalds's avatar
Linus Torvalds committed
392
393
394
395
396
397
398
399
400
401
402
403
404
		u32 len;

		len = fpexc + (1 << FPEXC_LENGTH_BIT);

		fpscr &= ~FPSCR_LENGTH_MASK;
		fpscr |= (len & FPEXC_LENGTH_MASK) << (FPSCR_LENGTH_BIT - FPEXC_LENGTH_BIT);
	}

	/*
	 * Handle the first FP instruction.  We used to take note of the
	 * FPEXC bounce reason, but this appears to be unreliable.
	 * Emulate the bounced instruction instead.
	 */
405
	exceptions = vfp_emulate_instruction(trigger, fpscr, regs);
Linus Torvalds's avatar
Linus Torvalds committed
406
	if (exceptions)
407
		vfp_raise_exceptions(exceptions, trigger, orig_fpscr, regs);
Linus Torvalds's avatar
Linus Torvalds committed
408
409

	/*
410
411
	 * If there isn't a second FP instruction, exit now. Note that
	 * the FPEXC.FP2V bit is valid only if FPEXC.EX is 1.
Linus Torvalds's avatar
Linus Torvalds committed
412
	 */
413
	if ((fpexc & (FPEXC_EX | FPEXC_FP2V)) != (FPEXC_EX | FPEXC_FP2V))
414
		goto exit;
Linus Torvalds's avatar
Linus Torvalds committed
415
416
417
418
419
420
421
422
423

	/*
	 * The barrier() here prevents fpinst2 being read
	 * before the condition above.
	 */
	barrier();
	trigger = fmrx(FPINST2);

 emulate:
424
	exceptions = vfp_emulate_instruction(trigger, orig_fpscr, regs);
Linus Torvalds's avatar
Linus Torvalds committed
425
426
	if (exceptions)
		vfp_raise_exceptions(exceptions, trigger, orig_fpscr, regs);
427
428
 exit:
	preempt_enable();
Linus Torvalds's avatar
Linus Torvalds committed
429
}
430

431
432
static void vfp_enable(void *unused)
{
433
434
435
436
	u32 access;

	BUG_ON(preemptible());
	access = get_copro_access();
437
438
439
440
441
442
443

	/*
	 * Enable full access to VFP (cp10 and cp11)
	 */
	set_copro_access(access | CPACC_FULL(10) | CPACC_FULL(11));
}

444
445
446
447
448
449
450
451
452
453
454
455
456
/* Called by platforms on which we want to disable VFP because it may not be
 * present on all CPUs within a SMP complex. Needs to be called prior to
 * vfp_init().
 */
void vfp_disable(void)
{
	if (VFP_arch) {
		pr_debug("%s: should be called prior to vfp_init\n", __func__);
		return;
	}
	VFP_arch = 1;
}

457
#ifdef CONFIG_CPU_PM
458
static int vfp_pm_suspend(void)
459
460
461
462
463
464
{
	struct thread_info *ti = current_thread_info();
	u32 fpexc = fmrx(FPEXC);

	/* if vfp is on, then save state for resumption */
	if (fpexc & FPEXC_EN) {
465
		pr_debug("%s: saving vfp state\n", __func__);
466
467
468
469
		vfp_save_state(&ti->vfpstate, fpexc);

		/* disable, just in case */
		fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
470
471
472
473
474
475
	} else if (vfp_current_hw_state[ti->cpu]) {
#ifndef CONFIG_SMP
		fmxr(FPEXC, fpexc | FPEXC_EN);
		vfp_save_state(vfp_current_hw_state[ti->cpu], fpexc);
		fmxr(FPEXC, fpexc);
#endif
476
477
478
	}

	/* clear any information we had about last context state */
479
	vfp_current_hw_state[ti->cpu] = NULL;
480
481
482
483

	return 0;
}

484
static void vfp_pm_resume(void)
485
486
487
488
489
490
491
492
{
	/* ensure we have access to the vfp */
	vfp_enable(NULL);

	/* and disable it to ensure the next usage restores the state */
	fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
}

493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
static int vfp_cpu_pm_notifier(struct notifier_block *self, unsigned long cmd,
	void *v)
{
	switch (cmd) {
	case CPU_PM_ENTER:
		vfp_pm_suspend();
		break;
	case CPU_PM_ENTER_FAILED:
	case CPU_PM_EXIT:
		vfp_pm_resume();
		break;
	}
	return NOTIFY_OK;
}

static struct notifier_block vfp_cpu_pm_notifier_block = {
	.notifier_call = vfp_cpu_pm_notifier,
510
511
512
513
};

static void vfp_pm_init(void)
{
514
	cpu_pm_register_notifier(&vfp_cpu_pm_notifier_block);
515
516
517
518
}

#else
static inline void vfp_pm_init(void) { }
519
#endif /* CONFIG_CPU_PM */
520

521
522
523
524
/*
 * Ensure that the VFP state stored in 'thread->vfpstate' is up to date
 * with the hardware state.
 */
525
void vfp_sync_hwstate(struct thread_info *thread)
526
527
528
{
	unsigned int cpu = get_cpu();

529
	if (vfp_state_in_hw(cpu, thread)) {
Russell King's avatar
Russell King committed
530
		u32 fpexc = fmrx(FPEXC);
531

Russell King's avatar
Russell King committed
532
533
534
535
536
		/*
		 * Save the last VFP state on this CPU.
		 */
		fmxr(FPEXC, fpexc | FPEXC_EN);
		vfp_save_state(&thread->vfpstate, fpexc | FPEXC_EN);
537
538
		fmxr(FPEXC, fpexc);
	}
539

540
541
542
	put_cpu();
}

543
/* Ensure that the thread reloads the hardware VFP state on the next use. */
544
545
546
void vfp_flush_hwstate(struct thread_info *thread)
{
	unsigned int cpu = get_cpu();
547

548
	vfp_force_reload(cpu, thread);
549

550
551
552
	put_cpu();
}

553
554
555
556
/*
 * Save the current VFP state into the provided structures and prepare
 * for entry into a new function (signal handler).
 */
557
558
int vfp_preserve_user_clear_hwstate(struct user_vfp *ufp,
				    struct user_vfp_exc *ufp_exc)
559
560
561
562
563
564
565
566
567
568
569
{
	struct thread_info *thread = current_thread_info();
	struct vfp_hard_struct *hwstate = &thread->vfpstate.hard;

	/* Ensure that the saved hwstate is up-to-date. */
	vfp_sync_hwstate(thread);

	/*
	 * Copy the floating point registers. There can be unused
	 * registers see asm/hwcap.h for details.
	 */
570
571
	memcpy(&ufp->fpregs, &hwstate->fpregs, sizeof(hwstate->fpregs));

572
573
574
	/*
	 * Copy the status and control register.
	 */
575
	ufp->fpscr = hwstate->fpscr;
576
577
578
579

	/*
	 * Copy the exception registers.
	 */
580
581
582
	ufp_exc->fpexc = hwstate->fpexc;
	ufp_exc->fpinst = hwstate->fpinst;
	ufp_exc->fpinst2 = ufp_exc->fpinst2;
583
584
585
586
587
588
589
590
591

	/* Ensure that VFP is disabled. */
	vfp_flush_hwstate(thread);

	/*
	 * As per the PCS, clear the length and stride bits for function
	 * entry.
	 */
	hwstate->fpscr &= ~(FPSCR_LENGTH_MASK | FPSCR_STRIDE_MASK);
592
593
594
595
	return 0;
}

/* Sanitise and restore the current VFP state from the provided structures. */
596
int vfp_restore_user_hwstate(struct user_vfp *ufp, struct user_vfp_exc *ufp_exc)
597
598
599
600
601
{
	struct thread_info *thread = current_thread_info();
	struct vfp_hard_struct *hwstate = &thread->vfpstate.hard;
	unsigned long fpexc;

602
603
	/* Disable VFP to avoid corrupting the new thread state. */
	vfp_flush_hwstate(thread);
604
605
606
607
608

	/*
	 * Copy the floating point registers. There can be unused
	 * registers see asm/hwcap.h for details.
	 */
609
	memcpy(&hwstate->fpregs, &ufp->fpregs, sizeof(hwstate->fpregs));
610
611
612
	/*
	 * Copy the status and control register.
	 */
613
	hwstate->fpscr = ufp->fpscr;
614
615
616
617

	/*
	 * Sanitise and restore the exception registers.
	 */
618
	fpexc = ufp_exc->fpexc;
619
620
621
622
623
624
625
626

	/* Ensure the VFP is enabled. */
	fpexc |= FPEXC_EN;

	/* Ensure FPINST2 is invalid and the exception flag is cleared. */
	fpexc &= ~(FPEXC_EX | FPEXC_FP2V);
	hwstate->fpexc = fpexc;

627
628
	hwstate->fpinst = ufp_exc->fpinst;
	hwstate->fpinst2 = ufp_exc->fpinst2;
629

630
	return 0;
631
632
}

633
634
/*
 * VFP hardware can lose all context when a CPU goes offline.
635
636
637
638
 * As we will be running in SMP mode with CPU hotplug, we will save the
 * hardware state at every thread switch.  We clear our held state when
 * a CPU has been killed, indicating that the VFP hardware doesn't contain
 * a threads VFP state.  When a CPU starts up, we re-enable access to the
639
 * VFP hardware. The callbacks below are called on the CPU which
640
641
 * is being offlined/onlined.
 */
642
static int vfp_dying_cpu(unsigned int cpu)
643
{
644
	vfp_current_hw_state[cpu] = NULL;
645
646
647
648
649
650
651
	return 0;
}

static int vfp_starting_cpu(unsigned int unused)
{
	vfp_enable(NULL);
	return 0;
652
}
653

654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
void vfp_kmode_exception(void)
{
	/*
	 * If we reach this point, a floating point exception has been raised
	 * while running in kernel mode. If the NEON/VFP unit was enabled at the
	 * time, it means a VFP instruction has been issued that requires
	 * software assistance to complete, something which is not currently
	 * supported in kernel mode.
	 * If the NEON/VFP unit was disabled, and the location pointed to below
	 * is properly preceded by a call to kernel_neon_begin(), something has
	 * caused the task to be scheduled out and back in again. In this case,
	 * rebuilding and running with CONFIG_DEBUG_ATOMIC_SLEEP enabled should
	 * be helpful in localizing the problem.
	 */
	if (fmrx(FPEXC) & FPEXC_EN)
		pr_crit("BUG: unsupported FP instruction in kernel mode\n");
	else
		pr_crit("BUG: FP instruction issued in kernel mode with FP unit disabled\n");
}

674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
#ifdef CONFIG_KERNEL_MODE_NEON

/*
 * Kernel-side NEON support functions
 */
void kernel_neon_begin(void)
{
	struct thread_info *thread = current_thread_info();
	unsigned int cpu;
	u32 fpexc;

	/*
	 * Kernel mode NEON is only allowed outside of interrupt context
	 * with preemption disabled. This will make sure that the kernel
	 * mode NEON register contents never need to be preserved.
	 */
	BUG_ON(in_interrupt());
	cpu = get_cpu();

	fpexc = fmrx(FPEXC) | FPEXC_EN;
	fmxr(FPEXC, fpexc);

	/*
	 * Save the userland NEON/VFP state. Under UP,
	 * the owner could be a task other than 'current'
	 */
	if (vfp_state_in_hw(cpu, thread))
		vfp_save_state(&thread->vfpstate, fpexc);
#ifndef CONFIG_SMP
	else if (vfp_current_hw_state[cpu] != NULL)
		vfp_save_state(vfp_current_hw_state[cpu], fpexc);
#endif
	vfp_current_hw_state[cpu] = NULL;
}
EXPORT_SYMBOL(kernel_neon_begin);

void kernel_neon_end(void)
{
	/* Disable the NEON/VFP unit. */
	fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
	put_cpu();
}
EXPORT_SYMBOL(kernel_neon_end);

#endif /* CONFIG_KERNEL_MODE_NEON */

Linus Torvalds's avatar
Linus Torvalds committed
720
721
722
723
724
725
/*
 * VFP support code initialisation.
 */
static int __init vfp_init(void)
{
	unsigned int vfpsid;
726
727
	unsigned int cpu_arch = cpu_architecture();

728
729
730
731
	/*
	 * Enable the access to the VFP on all online CPUs so the
	 * following test on FPSID will succeed.
	 */
732
	if (cpu_arch >= CPU_ARCH_ARMv6)
733
		on_each_cpu(vfp_enable, NULL, 1);
Linus Torvalds's avatar
Linus Torvalds committed
734
735
736
737
738
739

	/*
	 * First check that there is a VFP that we can use.
	 * The handler is already setup to just log calls, so
	 * we just need to read the VFPSID register.
	 */
740
	vfp_vector = vfp_testing_entry;
741
	barrier();
Linus Torvalds's avatar
Linus Torvalds committed
742
	vfpsid = fmrx(FPSID);
743
	barrier();
744
	vfp_vector = vfp_null_entry;
Linus Torvalds's avatar
Linus Torvalds committed
745

746
	pr_info("VFP support v0.3: ");
747
	if (VFP_arch) {
748
		pr_cont("not present\n");
749
750
751
752
753
		return 0;
	/* Extract the architecture on CPUID scheme */
	} else if ((read_cpuid_id() & 0x000f0000) == 0x000f0000) {
		VFP_arch = vfpsid & FPSID_CPUID_ARCH_MASK;
		VFP_arch >>= FPSID_ARCH_BIT;
754
		/*
755
756
757
758
		 * Check for the presence of the Advanced SIMD
		 * load/store instructions, integer and single
		 * precision floating point operations. Only check
		 * for NEON if the hardware has the MVFR registers.
759
		 */
760
761
		if (IS_ENABLED(CONFIG_NEON) &&
		   (fmrx(MVFR1) & 0x000fff00) == 0x00011100)
762
763
			elf_hwcap |= HWCAP_NEON;

764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
		if (IS_ENABLED(CONFIG_VFPv3)) {
			u32 mvfr0 = fmrx(MVFR0);
			if (((mvfr0 & MVFR0_DP_MASK) >> MVFR0_DP_BIT) == 0x2 ||
			    ((mvfr0 & MVFR0_SP_MASK) >> MVFR0_SP_BIT) == 0x2) {
				elf_hwcap |= HWCAP_VFPv3;
				/*
				 * Check for VFPv3 D16 and VFPv4 D16.  CPUs in
				 * this configuration only have 16 x 64bit
				 * registers.
				 */
				if ((mvfr0 & MVFR0_A_SIMD_MASK) == 1)
					/* also v4-D16 */
					elf_hwcap |= HWCAP_VFPv3D16;
				else
					elf_hwcap |= HWCAP_VFPD32;
			}

			if ((fmrx(MVFR1) & 0xf0000000) == 0x10000000)
				elf_hwcap |= HWCAP_VFPv4;
		}
784
785
786
787
788
	/* Extract the architecture version on pre-cpuid scheme */
	} else {
		if (vfpsid & FPSID_NODOUBLE) {
			pr_cont("no double precision support\n");
			return 0;
789
		}
790
791

		VFP_arch = (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT;
Linus Torvalds's avatar
Linus Torvalds committed
792
	}
793

794
	cpuhp_setup_state_nocalls(CPUHP_AP_ARM_VFP_STARTING,
795
				  "arm/vfp:starting", vfp_starting_cpu,
796
				  vfp_dying_cpu);
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815

	vfp_vector = vfp_support_entry;

	thread_register_notifier(&vfp_notifier_block);
	vfp_pm_init();

	/*
	 * We detected VFP, and the support code is
	 * in place; report VFP support to userspace.
	 */
	elf_hwcap |= HWCAP_VFP;

	pr_cont("implementor %02x architecture %d part %02x variant %x rev %x\n",
		(vfpsid & FPSID_IMPLEMENTER_MASK) >> FPSID_IMPLEMENTER_BIT,
		VFP_arch,
		(vfpsid & FPSID_PART_MASK) >> FPSID_PART_BIT,
		(vfpsid & FPSID_VARIANT_MASK) >> FPSID_VARIANT_BIT,
		(vfpsid & FPSID_REV_MASK) >> FPSID_REV_BIT);

Linus Torvalds's avatar
Linus Torvalds committed
816
817
818
	return 0;
}

819
core_initcall(vfp_init);