Rework the VFP code that handles demand-based save and restore of state.

The old code was full of complexity that would only matter if the
kernel itself used the VFP hardware.  Now that's reduced to either killing
the userland process or panicking the kernel on an illegal VFP instruction.

This removes most of the complexity from the assembler code, reducing it
to just calling the save code if the outgoing thread used the VFP.

The routine that stores the VFP state now takes a flag that indicates
whether the hardware should be disabled after saving state.  Right now it
always is, but this makes the code ready to be used by get/set_mcontext()
(doing so will be addressed in a future commit).

Remove the arm-specific pc_vfpcthread from struct pcpu and use the MI
field pc_fpcurthread instead.

Reviewed by:	cognet

5 files changed, 102 insertions, 164 deletions

diff --git a/sys/arm/arm/genassym.c b/sys/arm/arm/genassym.c
index e38733b..6f94032 100644
--- a/sys/arm/arm/genassym.c
+++ b/sys/arm/arm/genassym.c
@@ -119,9 +119,7 @@ ASSYM(ARM_RAS_END, ARM_RAS_END);
 
 #ifdef VFP
 ASSYM(PCB_VFPSTATE, offsetof(struct pcb, pcb_vfpstate));
-ASSYM(PCB_VFPCPU, offsetof(struct pcb, pcb_vfpcpu));
 
-ASSYM(PC_VFPCTHREAD, offsetof(struct pcpu, pc_vfpcthread));
 ASSYM(PC_CPU, offsetof(struct pcpu, pc_cpu));
 
 ASSYM(PC_CURPMAP, offsetof(struct pcpu, pc_curpmap));
diff --git a/sys/arm/arm/swtch.S b/sys/arm/arm/swtch.S
index 133b0d9..743d4c3 100644
--- a/sys/arm/arm/swtch.S
+++ b/sys/arm/arm/swtch.S
@@ -84,6 +84,8 @@
 #include <machine/asm.h>
 #include <machine/asmacros.h>
 #include <machine/armreg.h>
+#include <machine/vfp.h>
+
 __FBSDID("$FreeBSD$");
 
 #define DOMAIN_CLIENT	0x01
@@ -102,6 +104,10 @@ __FBSDID("$FreeBSD$");
 	ldr	tmp, .Lcurpcpu
 #endif
 
+#ifdef VFP
+	.fpu vfp	/* allow VFP instructions */
+#endif
+
 .Lcurpcpu:
         .word   _C_LABEL(__pcpu)
 	.word	PCPU_SIZE
@@ -121,16 +127,10 @@ ENTRY(cpu_throw)
 	GET_PCPU(r7, r9)
 
 #ifdef VFP
-	/*
-	 * vfp_discard will clear pcpu->pc_vfpcthread, and modify
-	 * and modify the control as needed.
-	 */
-	ldr     r4, [r7, #(PC_VFPCTHREAD)]      /* this thread using vfp? */
-	cmp     r0, r4
-	bne     3f
-	bl      _C_LABEL(vfp_discard)           /* yes, shut down vfp */
-3:
-#endif		/* VFP */
+	fmrx	r0, fpexc		/* This thread is dying, if the VFP */
+	tst	r0, #(VFPEXC_EN)	/* is enabled, go shut it down */
+	blne	_C_LABEL(vfp_discard)	/* without preserving its state. */
+#endif
 
 	ldr	r7, [r5, #(TD_PCB)]		/* r7 = new thread's PCB */
   
@@ -319,30 +319,14 @@ ENTRY(cpu_switch)
 	/* rem: interrupts are enabled */
 
 #ifdef VFP
-	/*
-	 * vfp_store will clear pcpu->pc_vfpcthread, save 
-	 * registers and state, and modify the control as needed.
-	 * a future exception will bounce the backup settings in the fp unit.
-	 * XXX vfp_store can't change r4
-	 */
-	GET_PCPU(r7, r8)
-	ldr	r8, [r7, #(PC_VFPCTHREAD)]
-	cmp	r4, r8				/* old thread used vfp? */
-	bne	1f				/* no, don't save */
-	cmp	r1, r4				/* same thread ? */
-	beq	1f				/* yes, skip vfp store */
-#ifdef SMP
-	ldr	r8, [r7, #(PC_CPU)]		/* last used on this cpu? */
-	ldr	r3, [r2, #(PCB_VFPCPU)]
-	cmp	r8, r3		/* last cpu to use these registers? */
-	bne	1f		/* no. these values are stale */
+	fmrx	r0, fpexc		/* If the VFP is enabled */
+	tst	r0, #(VFPEXC_EN)	/* the current thread has */
+	movne	r1, #1			/* used it, so go save */
+	addne	r0, r2, #(PCB_VFPSTATE)	/* the state into the PCB */
+	blne	_C_LABEL(vfp_store)	/* and disable the VFP. */
 #endif
-	add	r0, r2, #(PCB_VFPSTATE)
-	bl	_C_LABEL(vfp_store)
-1:
-#endif		/* VFP */
 
-	/* r1 now free! */
+	/* r0-r3 now free! */
 
 	/* Third phase : restore saved context */
 
@@ -520,26 +504,12 @@ ENTRY(savectx)
 	add	r2, r0, #(PCB_R8)
 	stmia	r2, {r8-r13}
 #ifdef VFP
-	/*
-	 * vfp_store will clear pcpu->pc_vfpcthread, save 
-	 * registers and state, and modify the control as needed.
-	 * a future exception will bounce the backup settings in the fp unit.
-	 */
-	GET_PCPU(r7, r4)
-	ldr	r4, [r7, #(PC_VFPCTHREAD)]      /* vfp thread */
-	ldr	r2, [r7, #(PC_CURTHREAD)]       /* current thread */
-	cmp	r4, r2
-	bne	1f
-#ifdef SMP
-	ldr	r2, [r7, #(PC_CPU)]     /* last used on this cpu? */
-	ldr	r3, [r0, #(PCB_VFPCPU)]
-	cmp	r2, r3
-	bne	1f              /* no. these values are stale */
+	fmrx	r2, fpexc		/* If the VFP is enabled */
+	tst	r2, #(VFPEXC_EN)	/* the current thread has */
+	movne	r1, #1			/* used it, so go save */
+	addne	r0, r0, #(PCB_VFPSTATE)	/* the state into the PCB */
+	blne	_C_LABEL(vfp_store)	/* and disable the VFP. */
 #endif
-	add	r0, r0, #(PCB_VFPSTATE)
-	bl	_C_LABEL(vfp_store)
-1:
-#endif		/* VFP */
 	add	sp, sp, #4;
 	ldmfd	sp!, {r4-r7, pc}
 END(savectx)
diff --git a/sys/arm/arm/vfp.c b/sys/arm/arm/vfp.c
index 8ee38bb..d370da4 100644
--- a/sys/arm/arm/vfp.c
+++ b/sys/arm/arm/vfp.c
@@ -1,4 +1,5 @@
-/*
+/*-
+ * Copyright (c) 2014 Ian Lepore <ian@freebsd.org>
  * Copyright (c) 2012 Mark Tinguely
  *
  * All rights reserved.
@@ -34,6 +35,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/proc.h>
 #include <sys/kernel.h>
 
+#include <machine/armreg.h>
 #include <machine/frame.h>
 #include <machine/fp.h>
 #include <machine/pcb.h>
@@ -41,12 +43,8 @@ __FBSDID("$FreeBSD$");
 #include <machine/vfp.h>
 
 /* function prototypes */
-unsigned int get_coprocessorACR(void);
 static int vfp_bounce(u_int, u_int, struct trapframe *, int);
 static void vfp_restore(struct vfp_state *);
-void	vfp_discard(void);
-void	vfp_store(struct vfp_state *);
-void	set_coprocessorACR(u_int);
 
 extern int vfp_exists;
 static struct undefined_handler vfp10_uh, vfp11_uh;
@@ -64,7 +62,20 @@ static int is_d32;
     val; \
 })
 
-u_int
+/*
+ * Work around an issue with GCC where the asm it generates is not unified
+ * syntax and fails to assemble because it expects the ldcleq instruction in the
+ * form ldc<c>l, not in the UAL form ldcl<c>, and similar for stcleq.
+ */
+#ifdef __clang__
+#define	LDCLNE  "ldclne "
+#define	STCLNE  "stclne "
+#else
+#define	LDCLNE  "ldcnel "
+#define	STCLNE  "stcnel "
+#endif
+
+static u_int
 get_coprocessorACR(void)
 {
 	u_int val;
@@ -72,7 +83,7 @@ get_coprocessorACR(void)
 	return val;
 }
 
-void
+static void
 set_coprocessorACR(u_int val)
 {
 	__asm __volatile("mcr p15, 0, %0, c1, c0, 2\n\t"
@@ -136,147 +147,103 @@ SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL);
 static int
 vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code)
 {
-	u_int fpexc;
+	u_int cpu, fpexc;
 	struct pcb *curpcb;
-	struct thread *vfptd;
-	int i;
 
-	if (!vfp_exists)
-		return 1;		/* vfp does not exist */
-	i = disable_interrupts(I32_bit|F32_bit);
-	fpexc = fmrx(VFPEXC);		/* read the vfp exception reg */
+	if ((code & FAULT_USER) == 0)
+		panic("undefined floating point instruction in supervisor mode");
+
+	critical_enter();
+
+	/*
+	 * If the VFP is already on and we got an undefined instruction, then
+	 * something tried to executate a truly invalid instruction that maps to
+	 * the VFP.
+	 */
+	fpexc = fmrx(VFPEXC);
 	if (fpexc & VFPEXC_EN) {
-		vfptd = PCPU_GET(vfpcthread);
-		/* did the kernel call the vfp or exception that expect us
-		 * to emulate the command. Newer hardware does not require
-		 * emulation, so we don't emulate yet.
-		 */
-#ifdef SMP
-		/* don't save if newer registers are on another processor */
-		if (vfptd /* && (vfptd == curthread) */ &&
-		   (vfptd->td_pcb->pcb_vfpcpu == PCPU_GET(cpu)))
-#else
-		/* someone did not save their registers, */
-		if (vfptd /* && (vfptd == curthread) */)
-#endif
-			vfp_store(&vfptd->td_pcb->pcb_vfpstate);
-
-		fpexc &= ~VFPEXC_EN;
-		fmxr(VFPEXC, fpexc);	/* turn vfp hardware off */
-		if (vfptd == curthread) {
-			/* kill the process - we do not handle emulation */
-			restore_interrupts(i);
-			killproc(curthread->td_proc, "vfp emulation");
-			return 1;
-		}
-		/* should not happen. someone did not save their context */
-		printf("vfp_bounce: vfpcthread: %p curthread: %p\n",
-			vfptd, curthread);
+		/* kill the process - we do not handle emulation */
+		critical_exit();
+		killproc(curthread->td_proc, "vfp emulation");
+		return 1;
 	}
-	fpexc |= VFPEXC_EN;
-	fmxr(VFPEXC, fpexc);	/* enable the vfp and repeat command */
-	curpcb = curthread->td_pcb;
-	/* If we were the last process to use the VFP, the process did not
-	 * use a VFP on another processor, then the registers in the VFP
-	 * will still be ours and are current. Eventually, we will make the
-	 * restore smarter.
+
+	/*
+	 * If the last time this thread used the VFP it was on this core, and
+	 * the last thread to use the VFP on this core was this thread, then the
+	 * VFP state is valid, otherwise restore this thread's state to the VFP.
 	 */
-	vfp_restore(&curpcb->pcb_vfpstate);
-#ifdef SMP
-	curpcb->pcb_vfpcpu = PCPU_GET(cpu);
-#endif
-	PCPU_SET(vfpcthread, curthread);
-	restore_interrupts(i);
-	return 0;
+	fmxr(VFPEXC, fpexc | VFPEXC_EN);
+	curpcb = curthread->td_pcb;
+	cpu = PCPU_GET(cpu);
+	if (curpcb->pcb_vfpcpu != cpu || curthread != PCPU_GET(fpcurthread)) {
+		vfp_restore(&curpcb->pcb_vfpstate);
+		curpcb->pcb_vfpcpu = cpu;
+		PCPU_SET(fpcurthread, curthread);
+	}
+
+	critical_exit();
+	return (0);
 }
 
-/* vfs_store is called from from a VFP command to restore the registers and
- * turn on the VFP hardware.
- * Eventually we will use the information that this process was the last
- * to use the VFP hardware and bypass the restore, just turn on the hardware.
+/*
+ * Restore the given state to the VFP hardware.
  */
 static void
 vfp_restore(struct vfp_state *vfpsave)
 {
 	u_int vfpscr = 0;
 
-	/*
-	 * Work around an issue with GCC where the asm it generates is
-	 * not unified syntax and fails to assemble because it expects
-	 * the ldcleq instruction in the form ldc<c>l, not in the UAL
-	 * form ldcl<c>, and similar for stcleq.
-	 */
-#ifdef __clang__
-#define	ldclne	"ldclne"
-#define	stclne	"stclne"
-#else
-#define	ldclne	"ldcnel"
-#define	stclne	"stcnel"
-#endif
-	if (vfpsave) {
-		__asm __volatile("ldc	p10, c0, [%1], #128\n" /* d0-d15 */
+	__asm __volatile("ldc	p10, c0, [%1], #128\n" /* d0-d15 */
 			"cmp	%2, #0\n"		/* -D16 or -D32? */
-			ldclne"	p11, c0, [%1], #128\n"	/* d16-d31 */
+			LDCLNE "p11, c0, [%1], #128\n"	/* d16-d31 */
 			"addeq	%1, %1, #128\n"		/* skip missing regs */
 			"ldr	%0, [%1]\n"		/* set old vfpscr */
 			"mcr	p10, 7, %0, cr1, c0, 0\n"
 			: "=&r" (vfpscr) : "r" (vfpsave), "r" (is_d32) : "cc");
-	}
 }
 
-/* vfs_store is called from switch to save the vfp hardware registers
- * into the pcb before switching to another process.
- * we already know that the new process is different from this old
- * process and that this process last used the VFP registers.
- * Below we check to see if the VFP has been enabled since the last
- * register save.
- * This routine will exit with the VFP turned off. The next VFP user
- * will trap to restore its registers and turn on the VFP hardware.
+/*
+ * If the VFP is on, save its current state and turn it off if requested to do
+ * so.  If the VFP is not on, does not change the values at *vfpsave.  Caller is
+ * responsible for preventing a context switch while this is running.
  */
 void
-vfp_store(struct vfp_state *vfpsave)
+vfp_store(struct vfp_state *vfpsave, boolean_t disable_vfp)
 {
-	u_int tmp, vfpscr = 0;
+	u_int tmp, vfpscr;
 
 	tmp = fmrx(VFPEXC);		/* Is the vfp enabled? */
-	if (vfpsave && (tmp & VFPEXC_EN)) {
-		__asm __volatile("stc	p11, c0, [%1], #128\n" /* d0-d15 */
+	if (tmp & VFPEXC_EN) {
+		__asm __volatile(
+			"stc	p11, c0, [%1], #128\n"  /* d0-d15 */
 			"cmp	%2, #0\n"		/* -D16 or -D32? */
-			stclne"	p11, c0, [%1], #128\n"	/* d16-d31 */
+			STCLNE "p11, c0, [%1], #128\n"	/* d16-d31 */
 			"addeq	%1, %1, #128\n"		/* skip missing regs */
 			"mrc	p10, 7, %0, cr1, c0, 0\n" /* fmxr(VFPSCR) */
 			"str	%0, [%1]\n"		/* save vfpscr */
 			: "=&r" (vfpscr) : "r" (vfpsave), "r" (is_d32) : "cc");
+		if (disable_vfp)
+			fmxr(VFPEXC , tmp & ~VFPEXC_EN);
 	}
-#undef ldcleq
-#undef stcleq
-
-#ifndef SMP
-		/* eventually we will use this information for UP also */
-	PCPU_SET(vfpcthread, 0);
-#endif
-	tmp &= ~VFPEXC_EN;	/* disable the vfp hardware */
-	fmxr(VFPEXC , tmp);
 }
 
-/* discard the registers at cpu_thread_free() when fpcurthread == td.
- * Turn off the VFP hardware.
+/*
+ * If the VFP hardware is on, the current thread was using it but now that
+ * thread is dying.  Turn off the VFP and set pcpu fpcurthread to 0, to indicate
+ * that the VFP hardware state does not belong to any thread.   Called only from
+ * cpu_throw(), so we don't have to worry about a context switch here.
  */
 void
 vfp_discard()
 {
-	u_int tmp = 0;
+	u_int tmp;
 
-	/*
-	 * No need to protect the access to vfpcthread by disabling
-	 * interrupts, since it's called from cpu_throw(), who is called
-	 * with interrupts disabled.
-	 */
-	  
-	PCPU_SET(vfpcthread, 0);	/* permanent forget about reg */
 	tmp = fmrx(VFPEXC);
-	tmp &= ~VFPEXC_EN;		/* turn off VFP hardware */
-	fmxr(VFPEXC, tmp);
+	if (tmp & VFPEXC_EN) {
+		fmxr(VFPEXC, tmp & ~VFPEXC_EN);
+		PCPU_SET(fpcurthread, 0);
+	}
 }
 
 #endif
diff --git a/sys/arm/include/pcpu.h b/sys/arm/include/pcpu.h
index 1771a8e..ee79343 100644
--- a/sys/arm/include/pcpu.h
+++ b/sys/arm/include/pcpu.h
@@ -46,9 +46,8 @@ struct vmspace;
 	unsigned int pc_vfpsid;						\
 	unsigned int pc_vfpmvfr0;					\
 	unsigned int pc_vfpmvfr1;					\
-	struct thread *pc_vfpcthread;					\
 	struct pmap *pc_curpmap;					\
-	char __pad[133]
+	char __pad[137]
 #else
 #define PCPU_MD_FIELDS							\
 	char __pad[157]
diff --git a/sys/arm/include/vfp.h b/sys/arm/include/vfp.h
index 623f0d0..9023821 100644
--- a/sys/arm/include/vfp.h
+++ b/sys/arm/include/vfp.h
@@ -126,6 +126,10 @@
 #define COPROC10		(0x3 << 20)
 #define COPROC11		(0x3 << 22)
 
+#ifndef LOCORE
 void    vfp_init(void);
+void    vfp_store(struct vfp_state *, boolean_t);
+void    vfp_discard(void);
+#endif
 
 #endif