--- npx.c	Wed May 31 09:50:49 2006
+++ /usr/src/sys/i386/isa/npx.c	Thu Jun  1 04:09:09 2006
@@ -45,7 +45,6 @@
 #include <sys/malloc.h>
 #include <sys/module.h>
 #include <sys/mutex.h>
-#include <sys/mutex.h>
 #include <sys/proc.h>
 #include <sys/smp.h>
 #include <sys/sysctl.h>
@@ -81,11 +80,6 @@
  * 387 and 287 Numeric Coprocessor Extension (NPX) Driver.
  */
 
-/* Configuration flags. */
-#define	NPX_DISABLE_I586_OPTIMIZED_BCOPY	(1 << 0)
-#define	NPX_DISABLE_I586_OPTIMIZED_BZERO	(1 << 1)
-#define	NPX_DISABLE_I586_OPTIMIZED_COPYIO	(1 << 2)
-
 #if defined(__GNUCLIKE_ASM) && !defined(lint)
 
 #define	fldcw(addr)		__asm("fldcw %0" : : "m" (*(addr)))
@@ -127,17 +121,17 @@
 #ifdef CPU_ENABLE_SSE
 #define GET_FPU_CW(thread) \
 	(cpu_fxsr ? \
-		(thread)->td_pcb->pcb_save.sv_xmm.sv_env.en_cw : \
-		(thread)->td_pcb->pcb_save.sv_87.sv_env.en_cw)
+		(thread)->td_savefpu->sv_xmm.sv_env.en_cw : \
+		(thread)->td_savefpu->sv_87.sv_env.en_cw)
 #define GET_FPU_SW(thread) \
 	(cpu_fxsr ? \
-		(thread)->td_pcb->pcb_save.sv_xmm.sv_env.en_sw : \
-		(thread)->td_pcb->pcb_save.sv_87.sv_env.en_sw)
+		(thread)->td_savefpu->sv_xmm.sv_env.en_sw : \
+		(thread)->td_savefpu->sv_87.sv_env.en_sw)
 #else /* CPU_ENABLE_SSE */
 #define GET_FPU_CW(thread) \
-	(thread->td_pcb->pcb_save.sv_87.sv_env.en_cw)
+	(thread->td_savefpu->sv_87.sv_env.en_cw)
 #define GET_FPU_SW(thread) \
-	(thread->td_pcb->pcb_save.sv_87.sv_env.en_sw)
+	(thread->td_savefpu->sv_87.sv_env.en_sw)
 #endif /* CPU_ENABLE_SSE */
 
 typedef u_char bool_t;
@@ -152,10 +146,6 @@
 static	void	npx_identify(driver_t *driver, device_t parent);
 static	void	npx_intr(void *);
 static	int	npx_probe(device_t dev);
-#ifdef I586_CPU_XXX
-static	long	timezero(const char *funcname,
-		    void (*func)(void *buf, size_t len));
-#endif /* I586_CPU */
 
 int	hw_float;		/* XXX currently just alias for npx_exists */
 
@@ -421,19 +411,9 @@
 		npx_cleanstate_ready = 1;
 		intr_restore(s);
 	}
-#ifdef I586_CPU_XXX
-	if (cpu_class == CPUCLASS_586 && npx_ex16 && npx_exists &&
-	    timezero("i586_bzero()", i586_bzero) <
-	    timezero("bzero()", bzero) * 4 / 5) {
-		if (!(flags & NPX_DISABLE_I586_OPTIMIZED_BCOPY))
-			bcopy_vector = i586_bcopy;
-		if (!(flags & NPX_DISABLE_I586_OPTIMIZED_BZERO))
-			bzero_vector = i586_bzero;
-		if (!(flags & NPX_DISABLE_I586_OPTIMIZED_COPYIO)) {
-			copyin_vector = i586_copyin;
-			copyout_vector = i586_copyout;
-		}
-	}
+#if defined(I686_CPU)
+	if (cpu_feature & CPUID_SSE2)
+		memcpy_vector = i686_memcpy;
 #endif
 
 	return (0);		/* XXX unused */
@@ -480,7 +460,7 @@
 
 	savecrit = intr_disable();
 	if (curthread == PCPU_GET(fpcurthread))
-		npxsave(&PCPU_GET(curpcb)->pcb_save);
+		npxsave(curthread->td_savefpu);
 	intr_restore(savecrit);
 #ifdef NPX_DEBUG
 	if (npx_exists) {
@@ -807,7 +787,7 @@
 		 * fnclex if it is the first FPU instruction after a context
 		 * switch.
 		 */
-		fpurstor(&pcb->pcb_save);
+		fpurstor(curthread->td_savefpu);
 	}
 	intr_restore(s);
 
@@ -910,7 +890,7 @@
 		return (_MC_FPOWNED_FPU);
 	} else {
 		intr_restore(s);
-		bcopy(&td->td_pcb->pcb_save, addr, sizeof(*addr));
+		bcopy(td->td_savefpu, addr, sizeof(*addr));
 		return (_MC_FPOWNED_PCB);
 	}
 }
@@ -938,7 +918,7 @@
 		intr_restore(s);
 	} else {
 		intr_restore(s);
-		bcopy(addr, &td->td_pcb->pcb_save, sizeof(*addr));
+		bcopy(addr, td->td_savefpu, sizeof(*addr));
 	}
 	curthread->td_pcb->pcb_flags |= PCB_NPXINITDONE;
 }
@@ -1002,36 +982,6 @@
 #endif
 		frstor(addr);
 }
-
-#ifdef I586_CPU_XXX
-static long
-timezero(funcname, func)
-	const char *funcname;
-	void (*func)(void *buf, size_t len);
-
-{
-	void *buf;
-#define	BUFSIZE		1048576
-	long usec;
-	struct timeval finish, start;
-
-	buf = malloc(BUFSIZE, M_TEMP, M_NOWAIT);
-	if (buf == NULL)
-		return (BUFSIZE);
-	microtime(&start);
-	(*func)(buf, BUFSIZE);
-	microtime(&finish);
-	usec = 1000000 * (finish.tv_sec - start.tv_sec) +
-	    finish.tv_usec - start.tv_usec;
-	if (usec <= 0)
-		usec = 1;
-	if (bootverbose)
-		printf("%s bandwidth = %u kBps\n", funcname,
-		    (u_int32_t)(((BUFSIZE >> 10) * 1000000) / usec));
-	free(buf, M_TEMP);
-	return (usec);
-}
-#endif /* I586_CPU */
 
 static device_method_t npx_methods[] = {
 	/* Device interface */
--- md_var.h	Wed May 31 09:50:41 2006
+++ /usr/src/sys/i386/include/md_var.h	Thu Jun  1 01:12:54 2006
@@ -38,8 +38,11 @@
 
 extern	void	(*bcopy_vector)(const void *from, void *to, size_t len);
 extern	void	(*bzero_vector)(void *buf, size_t len);
-extern	int	(*copyin_vector)(const void *udaddr, void *kaddr, size_t len);
-extern	int	(*copyout_vector)(const void *kaddr, void *udaddr, size_t len);
+/*
+ * arguments handled by upper level functions (memcpy, copyin, copyout, etc.)
+ * it assumes: esi = from, edi = to, ecx = len
+ */
+extern	void	(*memcpy_vector)(void);
 
 extern	long	Maxmem;
 extern	u_int	basemem;	/* PA of original top of base memory */
@@ -87,10 +90,7 @@
 void	enable_sse(void);
 void	fillw(int /*u_short*/ pat, void *base, size_t cnt);
 void	i486_bzero(void *buf, size_t len);
-void	i586_bcopy(const void *from, void *to, size_t len);
-void	i586_bzero(void *buf, size_t len);
-int	i586_copyin(const void *udaddr, void *kaddr, size_t len);
-int	i586_copyout(const void *kaddr, void *udaddr, size_t len);
+void	i686_memcpy(void);
 void	i686_pagezero(void *addr);
 void	sse2_pagezero(void *addr);
 void	init_AMD_Elan_sc520(void);
--- pcpu.h	Wed May 31 09:50:58 2006
+++ /usr/src/sys/i386/include/pcpu.h	Thu Jun  1 01:13:00 2006
@@ -38,6 +38,8 @@
 #include <machine/segments.h>
 #include <machine/tss.h>
 
+#define	FPUSIZE	512
+
 /*
  * The SMP parts are setup in pmap.c and locore.s for the BSP, and
  * mp_machdep.c sets up the data for the AP's to "see" when they awake.
@@ -54,7 +56,9 @@
 	struct	segment_descriptor *pc_fsgs_gdt;			\
 	int	pc_currentldt;						\
 	u_int	pc_acpi_id;						\
-	u_int	pc_apic_id
+	u_int	pc_apic_id;						\
+	int	pc_fpu_lock;						\
+	char	pc_savefpu[FPUSIZE] __aligned(16)
 
 #if defined(lint)
  
--- proc.h	Wed May 31 09:51:06 2006
+++ /usr/src/sys/i386/include/proc.h	Thu Jun  1 01:13:06 2006
@@ -49,11 +49,14 @@
 struct mdthread {
 	int	md_spinlock_count;	/* (k) */
 	register_t md_saved_flags;	/* (k) */
+	union savefpu *md_savefpu;	/* (k) */
 };
 
 struct mdproc {
 	struct proc_ldt *md_ldt;	/* (j) per-process ldt */
 };
+
+#define	td_savefpu	td_md.md_savefpu
 
 #ifdef	_KERNEL
 
--- genassym.c	Wed May 31 09:50:27 2006
+++ /usr/src/sys/i386/i386/genassym.c	Thu Jun  1 01:12:24 2006
@@ -84,6 +84,9 @@
 ASSYM(TD_PCB, offsetof(struct thread, td_pcb));
 ASSYM(TD_PROC, offsetof(struct thread, td_proc));
 ASSYM(TD_MD, offsetof(struct thread, td_md));
+ASSYM(TD_SAVEFPU, offsetof(struct thread, td_md) + offsetof(struct mdthread,
+							md_savefpu));
+ASSYM(TD_PINNED, offsetof(struct thread, td_pinned));
 
 ASSYM(P_MD, offsetof(struct proc, p_md));
 ASSYM(MD_LDT, offsetof(struct mdproc, md_ldt));
@@ -198,6 +201,8 @@
 ASSYM(PC_CURRENTLDT, offsetof(struct pcpu, pc_currentldt));
 ASSYM(PC_CPUID, offsetof(struct pcpu, pc_cpuid));
 ASSYM(PC_CURPMAP, offsetof(struct pcpu, pc_curpmap));
+ASSYM(PC_FPU_LOCK, offsetof(struct pcpu, pc_fpu_lock));
+ASSYM(PC_SAVEFPU, offsetof(struct pcpu, pc_savefpu));
 
 #ifdef DEV_APIC
 ASSYM(LA_VER, offsetof(struct LAPIC, version));
--- support.s	Wed May 31 09:51:22 2006
+++ /usr/src/sys/i386/i386/support.s	Thu Jun  1 04:02:12 2006
@@ -39,8 +39,71 @@
 
 #include "assym.s"
 
-#define IDXSHIFT	10
+#if defined(I686_CPU) && defined(DEV_NPX)
+/*
+ * FPU handling macros.
+ * FPU save/restore routines are coded with preprocessor in order to mantain
+ * either good flexibility and good peroformances.
+ * We need to pin curthread in order to avoid thread migration (not-consistent
+ * state of per-cpu datas).
+ */
+
+/*
+ * thread pinning/unpinning routines -- inlined in C codes
+ */
+#define	SCHED_PIN(reg)			\
+	addl	$1,TD_PINNED(reg)
+
+#define SCHED_UNPIN(reg)		\
+	subl	$1,TD_PINNED(reg)
+
+/*
+ * Interrupts are disabled along fpu context saving in order to avoid races.
+ * It is not convenient disabling for the whole time of the copy takes (so
+ * we pin curthead).
+ */
+#define FPU_PICKUP(val)			\
+	cmpl	$val,%ecx ;		\
+	jb	generic_memcpy ;	\
+	movl	%edi,%eax ;		\
+	andl	$15,%eax ;		\
+	jnz	generic_memcpy ;	\
+	movl	%esi,%eax ;		\
+	andl	$15,%eax ;		\
+	jnz	generic_memcpy ;	\
+	movl	PCPU(CURTHREAD),%eax ;	\
+	SCHED_PIN(%eax) ;		\
+	btsl	$1,PCPU(FPU_LOCK) ;	\
+	jc	memcpy_unpin ;		\
+	movl	TD_SAVEFPU(%eax),%edx ;	\
+	pushl	%edx ;			\
+	cli ;				\
+	cmpl	$0,PCPU(FPCURTHREAD) ;	\
+	je	1f ;			\
+	fxsave	(%edx) ;		\
+	movl	$0,PCPU(FPCURTHREAD) ;	\
+	clts ;				\
+	fninit ;			\
+1:					\
+	PCPU_ADDR(SAVEFPU,%edx) ;	\
+	movl	%edx,TD_SAVEFPU(%eax) ;	\
+	clts ;				\
+	movl	%eax,PCPU(FPCURTHREAD) ;\
+	sti
 
+#define FPU_DROP			\
+	movl	%cr0,%eax ;		\
+	orb	$CR0_TS,%al ;		\
+	movl	%eax,%cr0 ;		\
+	popl	%edx ;			\
+	movl	PCPU(CURTHREAD),%eax ;	\
+	movl	%edx,TD_SAVEFPU(%eax) ;	\
+	movl	$0,PCPU(FPCURTHREAD) ;	\
+	movl	$0,PCPU(FPU_LOCK) ;	\
+	SCHED_UNPIN(%eax)
+
+#endif
+ 
 	.data
 	.globl	bcopy_vector
 bcopy_vector:
@@ -48,17 +111,9 @@
 	.globl	bzero_vector
 bzero_vector:
 	.long	generic_bzero
-	.globl	copyin_vector
-copyin_vector:
-	.long	generic_copyin
-	.globl	copyout_vector
-copyout_vector:
-	.long	generic_copyout
-#if defined(I586_CPU) && defined(DEV_NPX)
-kernel_fpu_lock:
-	.byte	0xfe
-	.space	3
-#endif
+	.globl	memcpy_vector
+memcpy_vector:
+	.long	generic_memcpy
 	ALIGN_DATA
 	.globl	intrcnt, eintrcnt
 intrcnt:
@@ -199,164 +254,6 @@
 	ret
 #endif
 
-#if defined(I586_CPU) && defined(DEV_NPX)
-ENTRY(i586_bzero)
-	movl	4(%esp),%edx
-	movl	8(%esp),%ecx
-
-	/*
-	 * The FPU register method is twice as fast as the integer register
-	 * method unless the target is in the L1 cache and we pre-allocate a
-	 * cache line for it (then the integer register method is 4-5 times
-	 * faster).  However, we never pre-allocate cache lines, since that
-	 * would make the integer method 25% or more slower for the common
-	 * case when the target isn't in either the L1 cache or the L2 cache.
-	 * Thus we normally use the FPU register method unless the overhead
-	 * would be too large.
-	 */
-	cmpl	$256,%ecx	/* empirical; clts, fninit, smsw cost a lot */
-	jb	intreg_i586_bzero
-
-	/*
-	 * The FPU registers may belong to an application or to fastmove()
-	 * or to another invocation of bcopy() or ourself in a higher level
-	 * interrupt or trap handler.  Preserving the registers is
-	 * complicated since we avoid it if possible at all levels.  We
-	 * want to localize the complications even when that increases them.
-	 * Here the extra work involves preserving CR0_TS in TS.
-	 * `fpcurthread != NULL' is supposed to be the condition that all the
-	 * FPU resources belong to an application, but fpcurthread and CR0_TS
-	 * aren't set atomically enough for this condition to work in
-	 * interrupt handlers.
-	 *
-	 * Case 1: FPU registers belong to the application: we must preserve
-	 * the registers if we use them, so we only use the FPU register
-	 * method if the target size is large enough to amortize the extra
-	 * overhead for preserving them.  CR0_TS must be preserved although
-	 * it is very likely to end up as set.
-	 *
-	 * Case 2: FPU registers belong to fastmove(): fastmove() currently
-	 * makes the registers look like they belong to an application so
-	 * that cpu_switch() and savectx() don't have to know about it, so
-	 * this case reduces to case 1.
-	 *
-	 * Case 3: FPU registers belong to the kernel: don't use the FPU
-	 * register method.  This case is unlikely, and supporting it would
-	 * be more complicated and might take too much stack.
-	 *
-	 * Case 4: FPU registers don't belong to anyone: the FPU registers
-	 * don't need to be preserved, so we always use the FPU register
-	 * method.  CR0_TS must be preserved although it is very likely to
-	 * always end up as clear.
-	 */
-	cmpl	$0,PCPU(FPCURTHREAD)
-	je	i586_bz1
-
-	/*
-	 * XXX don't use the FPU for cases 1 and 2, since preemptive
-	 * scheduling of ithreads broke these cases.  Note that we can
-	 * no longer get here from an interrupt handler, since the
-	 * context sitch to the interrupt handler will have saved the
-	 * FPU state.
-	 */
-	jmp	intreg_i586_bzero
-
-	cmpl	$256+184,%ecx		/* empirical; not quite 2*108 more */
-	jb	intreg_i586_bzero
-	sarb	$1,kernel_fpu_lock
-	jc	intreg_i586_bzero
-	smsw	%ax
-	clts
-	subl	$108,%esp
-	fnsave	0(%esp)
-	jmp	i586_bz2
-
-i586_bz1:
-	sarb	$1,kernel_fpu_lock
-	jc	intreg_i586_bzero
-	smsw	%ax
-	clts
-	fninit				/* XXX should avoid needing this */
-i586_bz2:
-	fldz
-
-	/*
-	 * Align to an 8 byte boundary (misalignment in the main loop would
-	 * cost a factor of >= 2).  Avoid jumps (at little cost if it is
-	 * already aligned) by always zeroing 8 bytes and using the part up
-	 * to the _next_ alignment position.
-	 */
-	fstl	0(%edx)
-	addl	%edx,%ecx		/* part of %ecx -= new_%edx - %edx */
-	addl	$8,%edx
-	andl	$~7,%edx
-	subl	%edx,%ecx
-
-	/*
-	 * Similarly align `len' to a multiple of 8.
-	 */
-	fstl	-8(%edx,%ecx)
-	decl	%ecx
-	andl	$~7,%ecx
-
-	/*
-	 * This wouldn't be any faster if it were unrolled, since the loop
-	 * control instructions are much faster than the fstl and/or done
-	 * in parallel with it so their overhead is insignificant.
-	 */
-fpureg_i586_bzero_loop:
-	fstl	0(%edx)
-	addl	$8,%edx
-	subl	$8,%ecx
-	cmpl	$8,%ecx
-	jae	fpureg_i586_bzero_loop
-
-	cmpl	$0,PCPU(FPCURTHREAD)
-	je	i586_bz3
-
-	/* XXX check that the condition for cases 1-2 stayed false. */
-i586_bzero_oops:
-	int	$3
-	jmp	i586_bzero_oops
-
-	frstor	0(%esp)
-	addl	$108,%esp
-	lmsw	%ax
-	movb	$0xfe,kernel_fpu_lock
-	ret
-
-i586_bz3:
-	fstp	%st(0)
-	lmsw	%ax
-	movb	$0xfe,kernel_fpu_lock
-	ret
-
-intreg_i586_bzero:
-	/*
-	 * `rep stos' seems to be the best method in practice for small
-	 * counts.  Fancy methods usually take too long to start up due
-	 * to cache and BTB misses.
-	 */
-	pushl	%edi
-	movl	%edx,%edi
-	xorl	%eax,%eax
-	shrl	$2,%ecx
-	cld
-	rep
-	stosl
-	movl	12(%esp),%ecx
-	andl	$3,%ecx
-	jne	1f
-	popl	%edi
-	ret
-
-1:
-	rep
-	stosb
-	popl	%edi
-	ret
-#endif /* I586_CPU && defined(DEV_NPX) */
-
 ENTRY(sse2_pagezero)
 	pushl	%ebx
 	movl	8(%esp),%ecx
@@ -518,177 +415,107 @@
 	cld
 	ret
 
-#if defined(I586_CPU) && defined(DEV_NPX)
-ENTRY(i586_bcopy)
-	pushl	%esi
+/*
+ * Note: memcpy does not support overlapping copies
+ */
+ENTRY(memcpy)
 	pushl	%edi
-	movl	12(%esp),%esi
-	movl	16(%esp),%edi
-	movl	20(%esp),%ecx
-
-	movl	%edi,%eax
-	subl	%esi,%eax
-	cmpl	%ecx,%eax			/* overlapping && src < dst? */
-	jb	1f
-
-	cmpl	$1024,%ecx
-	jb	small_i586_bcopy
-
-	sarb	$1,kernel_fpu_lock
-	jc	small_i586_bcopy
-	cmpl	$0,PCPU(FPCURTHREAD)
-	je	i586_bc1
-
-	/* XXX turn off handling of cases 1-2, as above. */
-	movb	$0xfe,kernel_fpu_lock
-	jmp	small_i586_bcopy
-
-	smsw	%dx
-	clts
-	subl	$108,%esp
-	fnsave	0(%esp)
-	jmp	4f
-
-i586_bc1:
-	smsw	%dx
-	clts
-	fninit				/* XXX should avoid needing this */
-
-	ALIGN_TEXT
-4:
-	pushl	%ecx
-#define	DCACHE_SIZE	8192
-	cmpl	$(DCACHE_SIZE-512)/2,%ecx
-	jbe	2f
-	movl	$(DCACHE_SIZE-512)/2,%ecx
-2:
-	subl	%ecx,0(%esp)
-	cmpl	$256,%ecx
-	jb	5f			/* XXX should prefetch if %ecx >= 32 */
 	pushl	%esi
-	pushl	%ecx
-	ALIGN_TEXT
-3:
-	movl	0(%esi),%eax
-	movl	32(%esi),%eax
-	movl	64(%esi),%eax
-	movl	96(%esi),%eax
-	movl	128(%esi),%eax
-	movl	160(%esi),%eax
-	movl	192(%esi),%eax
-	movl	224(%esi),%eax
-	addl	$256,%esi
-	subl	$256,%ecx
-	cmpl	$256,%ecx
-	jae	3b
-	popl	%ecx
+	movl	12(%esp),%edi
+	movl	16(%esp),%esi
+	movl	20(%esp),%ecx
+	call	*memcpy_vector
 	popl	%esi
-5:
-	ALIGN_TEXT
-large_i586_bcopy_loop:
-	fildq	0(%esi)
-	fildq	8(%esi)
-	fildq	16(%esi)
-	fildq	24(%esi)
-	fildq	32(%esi)
-	fildq	40(%esi)
-	fildq	48(%esi)
-	fildq	56(%esi)
-	fistpq	56(%edi)
-	fistpq	48(%edi)
-	fistpq	40(%edi)
-	fistpq	32(%edi)
-	fistpq	24(%edi)
-	fistpq	16(%edi)
-	fistpq	8(%edi)
-	fistpq	0(%edi)
-	addl	$64,%esi
-	addl	$64,%edi
-	subl	$64,%ecx
-	cmpl	$64,%ecx
-	jae	large_i586_bcopy_loop
-	popl	%eax
-	addl	%eax,%ecx
-	cmpl	$64,%ecx
-	jae	4b
-
-	cmpl	$0,PCPU(FPCURTHREAD)
-	je	i586_bc2
-
-	/* XXX check that the condition for cases 1-2 stayed false. */
-i586_bcopy_oops:
-	int	$3
-	jmp	i586_bcopy_oops
-
-	frstor	0(%esp)
-	addl	$108,%esp
-i586_bc2:
-	lmsw	%dx
-	movb	$0xfe,kernel_fpu_lock
+	popl	%edi
+	movl	4(%esp),%eax
+	ret
 
 /*
- * This is a duplicate of the main part of generic_bcopy.  See the comments
- * there.  Jumping into generic_bcopy would cost a whole 0-1 cycles and
- * would mess up high resolution profiling.
+ * generic memcpy onfault routine -- copyin/copyout
  */
 	ALIGN_TEXT
-small_i586_bcopy:
+generic_onfault:
+	addl	$4,%esp		/* skip ret value */
+	ret
+
+/*
+ * It assumes: edi = to, esi = from, ecx = len. Doesn't handle ret value
+ */
+ENTRY(generic_memcpy)
+	pushl	$generic_onfault
+	movl	%ecx,%eax
 	shrl	$2,%ecx
 	cld
-	rep
-	movsl
-	movl	20(%esp),%ecx
+	rep	movsl
+	movl	%eax,%ecx
 	andl	$3,%ecx
-	rep
-	movsb
-	popl	%edi
-	popl	%esi
+	rep	movsb
+	addl	$4,%esp
 	ret
 
+#if defined(I686_CPU) && defined(DEV_NPX)
+/*
+ * optimized memcpy onfault routine -- copyin/copyout
+ */
 	ALIGN_TEXT
-1:
-	addl	%ecx,%edi
-	addl	%ecx,%esi
-	decl	%edi
-	decl	%esi
-	andl	$3,%ecx
-	std
-	rep
-	movsb
-	movl	20(%esp),%ecx
-	shrl	$2,%ecx
-	subl	$3,%esi
-	subl	$3,%edi
-	rep
-	movsl
-	popl	%edi
-	popl	%esi
-	cld
+fpu_onfault:
+	FPU_DROP
+	addl	$4,%esp
 	ret
-#endif /* I586_CPU && defined(DEV_NPX) */
 
 /*
- * Note: memcpy does not support overlapping copies
+ * unpin trampoline
  */
-ENTRY(memcpy)
-	pushl	%edi
-	pushl	%esi
-	movl	12(%esp),%edi
-	movl	16(%esp),%esi
-	movl	20(%esp),%ecx
-	movl	%edi,%eax
-	shrl	$2,%ecx				/* copy by 32-bit words */
-	cld					/* nope, copy forwards */
-	rep
-	movsl
-	movl	20(%esp),%ecx
-	andl	$3,%ecx				/* any bytes left? */
-	rep
-	movsb
-	popl	%esi
-	popl	%edi
-	ret
+	ALIGN_TEXT
+memcpy_unpin:
+	SCHED_UNPIN(%eax)
+	jmp	generic_memcpy
+
+/*
+ * optimized memcpy function
+ */
+ENTRY(i686_memcpy)
+	FPU_PICKUP(2048);
+	pushl	$fpu_onfault
+	movl	%ecx,%eax
+	shrl	$7,%ecx
+
+	SUPERALIGN_TEXT
+2:
+	movdqa	(%esi),%xmm0
+	movdqa	16(%esi),%xmm1
+	movdqa	32(%esi),%xmm2
+	movdqa	48(%esi),%xmm3
+	movdqa	64(%esi),%xmm4
+	movdqa	80(%esi),%xmm5
+	movdqa	96(%esi),%xmm6
+	movdqa	112(%esi),%xmm7
+	addl	$128,%esi
+	movdqa	%xmm0,(%edi)
+	movdqa	%xmm1,16(%edi)
+	movdqa	%xmm2,32(%edi)
+	movdqa	%xmm3,48(%edi)
+	movdqa	%xmm4,64(%edi)
+	movdqa	%xmm5,80(%edi)
+	movdqa	%xmm6,96(%edi)
+	movdqa	%xmm7,112(%edi)
+	addl	$128,%edi
+	loop	2b
+
+	SUPERALIGN_TEXT
+	movl	%eax,%ecx
+	andl	$127,%ecx
+	shrl	$2,%ecx
+	cld
+	rep	movsl
+	movl	%eax,%ecx
+	andl	$3,%ecx
+	rep	movsb
+	addl	$4,%esp
 
+	FPU_DROP
+	ret
+#endif
 
 /*****************************************************************************/
 /* copyout and fubyte family                                                 */
@@ -709,22 +536,25 @@
  */
 
 /*
+ * Last step onfault handling routine
+ */
+	ALIGN_TEXT
+stack_onfault:
+	ret
+
+/*
  * copyout(from_kernel, to_user, len)  - MP SAFE
  */
 ENTRY(copyout)
-	MEXITCOUNT
-	jmp	*copyout_vector
-
-ENTRY(generic_copyout)
-	movl	PCPU(CURPCB),%eax
-	movl	$copyout_fault,PCB_ONFAULT(%eax)
 	pushl	%esi
 	pushl	%edi
-	pushl	%ebx
+	pushl	$copyout_fault2
+	movl	PCPU(CURPCB),%eax
+	movl	$stack_onfault,PCB_ONFAULT(%eax)
 	movl	16(%esp),%esi
 	movl	20(%esp),%edi
-	movl	24(%esp),%ebx
-	testl	%ebx,%ebx			/* anything to do? */
+	movl	24(%esp),%ecx
+	testl	%ecx,%ecx			/* anything to do? */
 	jz	done_copyout
 
 	/*
@@ -738,8 +568,8 @@
 	 * First, prevent address wrapping.
 	 */
 	movl	%edi,%eax
-	addl	%ebx,%eax
-	jc	copyout_fault
+	addl	%ecx,%eax
+	jc	copyout_fault1
 /*
  * XXX STOP USING VM_MAXUSER_ADDRESS.
  * It is an end address, not a max, so every time it is used correctly it
@@ -747,26 +577,13 @@
  * by one error in several places.
  */
 	cmpl	$VM_MAXUSER_ADDRESS,%eax
-	ja	copyout_fault
+	ja	copyout_fault1
 
-	/* bcopy(%esi, %edi, %ebx) */
-	movl	%ebx,%ecx
-
-#if defined(I586_CPU) && defined(DEV_NPX)
-	ALIGN_TEXT
-slow_copyout:
-#endif
-	shrl	$2,%ecx
-	cld
-	rep
-	movsl
-	movb	%bl,%cl
-	andb	$3,%cl
-	rep
-	movsb
+	/* memcpy(%edi, %esi, %ecx); */
+	call	*memcpy_vector
 
 done_copyout:
-	popl	%ebx
+	addl	$4,%esp
 	popl	%edi
 	popl	%esi
 	xorl	%eax,%eax
@@ -775,8 +592,9 @@
 	ret
 
 	ALIGN_TEXT
-copyout_fault:
-	popl	%ebx
+copyout_fault1:
+	addl	$4,%esp
+copyout_fault2:
 	popl	%edi
 	popl	%esi
 	movl	PCPU(CURPCB),%edx
@@ -784,103 +602,32 @@
 	movl	$EFAULT,%eax
 	ret
 
-#if defined(I586_CPU) && defined(DEV_NPX)
-ENTRY(i586_copyout)
-	/*
-	 * Duplicated from generic_copyout.  Could be done a bit better.
-	 */
-	movl	PCPU(CURPCB),%eax
-	movl	$copyout_fault,PCB_ONFAULT(%eax)
-	pushl	%esi
-	pushl	%edi
-	pushl	%ebx
-	movl	16(%esp),%esi
-	movl	20(%esp),%edi
-	movl	24(%esp),%ebx
-	testl	%ebx,%ebx			/* anything to do? */
-	jz	done_copyout
-
-	/*
-	 * Check explicitly for non-user addresses.  If 486 write protection
-	 * is being used, this check is essential because we are in kernel
-	 * mode so the h/w does not provide any protection against writing
-	 * kernel addresses.
-	 */
-
-	/*
-	 * First, prevent address wrapping.
-	 */
-	movl	%edi,%eax
-	addl	%ebx,%eax
-	jc	copyout_fault
-/*
- * XXX STOP USING VM_MAXUSER_ADDRESS.
- * It is an end address, not a max, so every time it is used correctly it
- * looks like there is an off by one error, and of course it caused an off
- * by one error in several places.
- */
-	cmpl	$VM_MAXUSER_ADDRESS,%eax
-	ja	copyout_fault
-
-	/* bcopy(%esi, %edi, %ebx) */
-3:
-	movl	%ebx,%ecx
-	/*
-	 * End of duplicated code.
-	 */
-
-	cmpl	$1024,%ecx
-	jb	slow_copyout
-
-	pushl	%ecx
-	call	fastmove
-	addl	$4,%esp
-	jmp	done_copyout
-#endif /* I586_CPU && defined(DEV_NPX) */
-
 /*
  * copyin(from_user, to_kernel, len) - MP SAFE
  */
 ENTRY(copyin)
-	MEXITCOUNT
-	jmp	*copyin_vector
-
-ENTRY(generic_copyin)
-	movl	PCPU(CURPCB),%eax
-	movl	$copyin_fault,PCB_ONFAULT(%eax)
 	pushl	%esi
 	pushl	%edi
-	movl	12(%esp),%esi			/* caddr_t from */
-	movl	16(%esp),%edi			/* caddr_t to */
-	movl	20(%esp),%ecx			/* size_t  len */
+	pushl	$copyin_fault2
+	movl	PCPU(CURPCB),%eax
+	movl	$stack_onfault,PCB_ONFAULT(%eax)
+	movl	16(%esp),%esi			/* caddr_t from */
+	movl	20(%esp),%edi			/* caddr_t to */
+	movl	24(%esp),%ecx			/* size_t  len */
 
 	/*
 	 * make sure address is valid
 	 */
 	movl	%esi,%edx
 	addl	%ecx,%edx
-	jc	copyin_fault
+	jc	copyin_fault1
 	cmpl	$VM_MAXUSER_ADDRESS,%edx
-	ja	copyin_fault
+	ja	copyin_fault1
 
-#if defined(I586_CPU) && defined(DEV_NPX)
-	ALIGN_TEXT
-slow_copyin:
-#endif
-	movb	%cl,%al
-	shrl	$2,%ecx				/* copy longword-wise */
-	cld
-	rep
-	movsl
-	movb	%al,%cl
-	andb	$3,%cl				/* copy remaining bytes */
-	rep
-	movsb
+	/* memcpy(%edi, %esi, %ecx); */
+	call	*memcpy_vector
 
-#if defined(I586_CPU) && defined(DEV_NPX)
-	ALIGN_TEXT
-done_copyin:
-#endif
+	addl	$4,%esp
 	popl	%edi
 	popl	%esi
 	xorl	%eax,%eax
@@ -889,255 +636,15 @@
 	ret
 
 	ALIGN_TEXT
-copyin_fault:
-	popl	%edi
-	popl	%esi
-	movl	PCPU(CURPCB),%edx
-	movl	$0,PCB_ONFAULT(%edx)
-	movl	$EFAULT,%eax
-	ret
-
-#if defined(I586_CPU) && defined(DEV_NPX)
-ENTRY(i586_copyin)
-	/*
-	 * Duplicated from generic_copyin.  Could be done a bit better.
-	 */
-	movl	PCPU(CURPCB),%eax
-	movl	$copyin_fault,PCB_ONFAULT(%eax)
-	pushl	%esi
-	pushl	%edi
-	movl	12(%esp),%esi			/* caddr_t from */
-	movl	16(%esp),%edi			/* caddr_t to */
-	movl	20(%esp),%ecx			/* size_t  len */
-
-	/*
-	 * make sure address is valid
-	 */
-	movl	%esi,%edx
-	addl	%ecx,%edx
-	jc	copyin_fault
-	cmpl	$VM_MAXUSER_ADDRESS,%edx
-	ja	copyin_fault
-	/*
-	 * End of duplicated code.
-	 */
-
-	cmpl	$1024,%ecx
-	jb	slow_copyin
-
-	pushl	%ebx			/* XXX prepare for fastmove_fault */
-	pushl	%ecx
-	call	fastmove
-	addl	$8,%esp
-	jmp	done_copyin
-#endif /* I586_CPU && defined(DEV_NPX) */
-
-#if defined(I586_CPU) && defined(DEV_NPX)
-/* fastmove(src, dst, len)
-	src in %esi
-	dst in %edi
-	len in %ecx		XXX changed to on stack for profiling
-	uses %eax and %edx for tmp. storage
- */
-/* XXX use ENTRY() to get profiling.  fastmove() is actually a non-entry. */
-ENTRY(fastmove)
-	pushl	%ebp
-	movl	%esp,%ebp
-	subl	$PCB_SAVEFPU_SIZE+3*4,%esp
-
-	movl	8(%ebp),%ecx
-	cmpl	$63,%ecx
-	jbe	fastmove_tail
-
-	testl	$7,%esi	/* check if src addr is multiple of 8 */
-	jnz	fastmove_tail
-
-	testl	$7,%edi	/* check if dst addr is multiple of 8 */
-	jnz	fastmove_tail
-
-	/* XXX grab FPU context atomically. */
-	cli
-
-/* if (fpcurthread != NULL) { */
-	cmpl	$0,PCPU(FPCURTHREAD)
-	je	6f
-/*    fnsave(&curpcb->pcb_savefpu); */
-	movl	PCPU(CURPCB),%eax
-	fnsave	PCB_SAVEFPU(%eax)
-/*   FPCURTHREAD = NULL; */
-	movl	$0,PCPU(FPCURTHREAD)
-/* } */
-6:
-/* now we own the FPU. */
-
-/*
- * The process' FP state is saved in the pcb, but if we get
- * switched, the cpu_switch() will store our FP state in the
- * pcb.  It should be possible to avoid all the copying for
- * this, e.g., by setting a flag to tell cpu_switch() to
- * save the state somewhere else.
- */
-/* tmp = curpcb->pcb_savefpu; */
-	movl	%ecx,-12(%ebp)
-	movl	%esi,-8(%ebp)
-	movl	%edi,-4(%ebp)
-	movl	%esp,%edi
-	movl	PCPU(CURPCB),%esi
-	addl	$PCB_SAVEFPU,%esi
-	cld
-	movl	$PCB_SAVEFPU_SIZE>>2,%ecx
-	rep
-	movsl
-	movl	-12(%ebp),%ecx
-	movl	-8(%ebp),%esi
-	movl	-4(%ebp),%edi
-/* stop_emulating(); */
-	clts
-/* fpcurthread = curthread; */
-	movl	PCPU(CURTHREAD),%eax
-	movl	%eax,PCPU(FPCURTHREAD)
-	movl	PCPU(CURPCB),%eax
-
-	/* XXX end of atomic FPU context grab. */
-	sti
-
-	movl	$fastmove_fault,PCB_ONFAULT(%eax)
-4:
-	movl	%ecx,-12(%ebp)
-	cmpl	$1792,%ecx
-	jbe	2f
-	movl	$1792,%ecx
-2:
-	subl	%ecx,-12(%ebp)
-	cmpl	$256,%ecx
-	jb	5f
-	movl	%ecx,-8(%ebp)
-	movl	%esi,-4(%ebp)
-	ALIGN_TEXT
-3:
-	movl	0(%esi),%eax
-	movl	32(%esi),%eax
-	movl	64(%esi),%eax
-	movl	96(%esi),%eax
-	movl	128(%esi),%eax
-	movl	160(%esi),%eax
-	movl	192(%esi),%eax
-	movl	224(%esi),%eax
-	addl	$256,%esi
-	subl	$256,%ecx
-	cmpl	$256,%ecx
-	jae	3b
-	movl	-8(%ebp),%ecx
-	movl	-4(%ebp),%esi
-5:
-	ALIGN_TEXT
-fastmove_loop:
-	fildq	0(%esi)
-	fildq	8(%esi)
-	fildq	16(%esi)
-	fildq	24(%esi)
-	fildq	32(%esi)
-	fildq	40(%esi)
-	fildq	48(%esi)
-	fildq	56(%esi)
-	fistpq	56(%edi)
-	fistpq	48(%edi)
-	fistpq	40(%edi)
-	fistpq	32(%edi)
-	fistpq	24(%edi)
-	fistpq	16(%edi)
-	fistpq	8(%edi)
-	fistpq	0(%edi)
-	addl	$-64,%ecx
-	addl	$64,%esi
-	addl	$64,%edi
-	cmpl	$63,%ecx
-	ja	fastmove_loop
-	movl	-12(%ebp),%eax
-	addl	%eax,%ecx
-	cmpl	$64,%ecx
-	jae	4b
-
-	/* XXX ungrab FPU context atomically. */
-	cli
-
-/* curpcb->pcb_savefpu = tmp; */
-	movl	%ecx,-12(%ebp)
-	movl	%esi,-8(%ebp)
-	movl	%edi,-4(%ebp)
-	movl	PCPU(CURPCB),%edi
-	addl	$PCB_SAVEFPU,%edi
-	movl	%esp,%esi
-	cld
-	movl	$PCB_SAVEFPU_SIZE>>2,%ecx
-	rep
-	movsl
-	movl	-12(%ebp),%ecx
-	movl	-8(%ebp),%esi
-	movl	-4(%ebp),%edi
-
-/* start_emulating(); */
-	smsw	%ax
-	orb	$CR0_TS,%al
-	lmsw	%ax
-/* fpcurthread = NULL; */
-	movl	$0,PCPU(FPCURTHREAD)
-
-	/* XXX end of atomic FPU context ungrab. */
-	sti
-
-	ALIGN_TEXT
-fastmove_tail:
-	movl	PCPU(CURPCB),%eax
-	movl	$fastmove_tail_fault,PCB_ONFAULT(%eax)
-
-	movb	%cl,%al
-	shrl	$2,%ecx				/* copy longword-wise */
-	cld
-	rep
-	movsl
-	movb	%al,%cl
-	andb	$3,%cl				/* copy remaining bytes */
-	rep
-	movsb
-
-	movl	%ebp,%esp
-	popl	%ebp
-	ret
-
-	ALIGN_TEXT
-fastmove_fault:
-	/* XXX ungrab FPU context atomically. */
-	cli
-
-	movl	PCPU(CURPCB),%edi
-	addl	$PCB_SAVEFPU,%edi
-	movl	%esp,%esi
-	cld
-	movl	$PCB_SAVEFPU_SIZE>>2,%ecx
-	rep
-	movsl
-
-	smsw	%ax
-	orb	$CR0_TS,%al
-	lmsw	%ax
-	movl	$0,PCPU(FPCURTHREAD)
-
-	/* XXX end of atomic FPU context ungrab. */
-	sti
-
-fastmove_tail_fault:
-	movl	%ebp,%esp
-	popl	%ebp
-	addl	$8,%esp
-	popl	%ebx
+copyin_fault1:
+	addl	$4,%esp
+copyin_fault2:
 	popl	%edi
 	popl	%esi
 	movl	PCPU(CURPCB),%edx
 	movl	$0,PCB_ONFAULT(%edx)
 	movl	$EFAULT,%eax
 	ret
-#endif /* I586_CPU && defined(DEV_NPX) */
 
 /*
  * casuptr.  Compare and set user pointer.  Returns -1 or the current value.
--- swtch.s	Wed May 31 09:51:28 2006
+++ /usr/src/sys/i386/i386/swtch.s	Thu Jun  1 01:12:34 2006
@@ -143,8 +143,7 @@
 	/* have we used fp, and need a save? */
 	cmpl	%ecx,PCPU(FPCURTHREAD)
 	jne	1f
-	addl	$PCB_SAVEFPU,%edx		/* h/w bugs make saving complicated */
-	pushl	%edx
+	pushl	TD_SAVEFPU(%ecx)		/* h/w bugs make saving complicated */
 	call	npxsave				/* do it in a big C function */
 	popl	%eax
 1:
@@ -372,8 +371,7 @@
 	je	1f
 
 	pushl	%ecx
-	movl	TD_PCB(%eax),%eax
-	leal	PCB_SAVEFPU(%eax),%eax
+	movl	TD_SAVEFPU(%eax),%eax
 	pushl	%eax
 	pushl	%eax
 	call	npxsave
--- vm86bios.s	Wed May 31 09:51:42 2006
+++ /usr/src/sys/i386/i386/vm86bios.s	Thu Jun  1 01:12:41 2006
@@ -72,9 +72,7 @@
 	testl	%ecx,%ecx
 	je 	1f			/* no curproc/npxproc */
 	pushl	%edx
-	movl	TD_PCB(%ecx),%ecx
-	addl	$PCB_SAVEFPU,%ecx
-	pushl	%ecx
+	pushl	TD_SAVEFPU(%ecx)
 	call	npxsave
 	popl	%ecx
 	popl	%edx			/* recover our pcb */
@@ -91,8 +89,12 @@
 	movsl				/* copy frame to new stack */
 
 	movl	PCPU(CURPCB),%eax
+	movl	PCPU(CURTHREAD),%ebx
 	pushl	%eax			/* save curpcb */
+	pushl	TD_SAVEFPU(%ebx)	/* save fpu pointer */
 	movl	%edx,PCPU(CURPCB)	/* set curpcb to vm86pcb */
+	leal	PCB_SAVEFPU(%edx),%eax
+	movl	%eax,TD_SAVEFPU(%ebx)
 
 	movl	PCPU(TSS_GDT),%ebx	/* entry in GDT */
 	movl	0(%ebx),%eax
@@ -166,6 +168,8 @@
 	movl	$GPROC0_SEL*8,%esi	/* GSEL(entry, SEL_KPL) */
 	ltr	%si
 	
+	movl	PCPU(CURTHREAD),%eax
+	popl	TD_SAVEFPU(%eax)	/* restore fpu pointer */
 	popl	PCPU(CURPCB)		/* restore curpcb/curproc */
 	movl	SCR_ARGFRAME(%edx),%edx	/* original stack frame */
 	movl	TF_TRAPNO(%edx),%eax	/* return (trapno) */
--- vm_machdep.c	Wed May 31 09:51:50 2006
+++ /usr/src/sys/i386/i386/vm_machdep.c	Thu Jun  1 01:12:46 2006
@@ -172,7 +172,7 @@
 		td1->td_pcb->pcb_gs = rgs();
 	savecrit = intr_disable();
 	if (PCPU_GET(fpcurthread) == td1)
-		npxsave(&td1->td_pcb->pcb_save);
+		npxsave(td1->td_savefpu);
 	intr_restore(savecrit);
 #endif
 
@@ -358,6 +358,7 @@
 
 	td->td_pcb = (struct pcb *)(td->td_kstack +
 	    td->td_kstack_pages * PAGE_SIZE) - 1;
+	td->td_savefpu = &td->td_pcb->pcb_save;
 	td->td_frame = (struct trapframe *)((caddr_t)td->td_pcb - 16) - 1;
 	td->td_pcb->pcb_ext = NULL; 
 }
