Enable FXSR all the time, and cope with NPX/FXSR conversions; from netbsd.

Pass SSE/SSE2/XCRYPT flags out via syctl, and prepare for being able to do
xcrypt-* in userland; ok naddy, tested a lot by pvalchev and jolan, also
works on amd64 in 32bit mode

8 files changed, 338 insertions, 50 deletions

diff --git a/sys/arch/i386/i386/locore.s b/sys/arch/i386/i386/locore.s
index 37ed4107fde..883e7f837c4 100644
--- a/sys/arch/i386/i386/locore.s
+++ b/sys/arch/i386/i386/locore.s
@@ -1,4 +1,4 @@
-/*	$OpenBSD: locore.s,v 1.76 2004/01/29 19:01:53 tedu Exp $	*/
+/*	$OpenBSD: locore.s,v 1.77 2004/02/01 19:05:23 deraadt Exp $	*/
 /*	$NetBSD: locore.s,v 1.145 1996/05/03 19:41:19 christos Exp $	*/
 
 /*-
@@ -1747,7 +1747,7 @@ IDTVEC(dna)
 	pushl	$T_DNA
 	INTRENTRY
 	pushl	_C_LABEL(curproc)
-	call	_C_LABEL(npxdna)
+	call	*_C_LABEL(npxdna_func)
 	addl	$4,%esp
 	testl	%eax,%eax
 	jz	calltrap
diff --git a/sys/arch/i386/i386/machdep.c b/sys/arch/i386/i386/machdep.c
index 04ce6163d49..d8307d55e26 100644
--- a/sys/arch/i386/i386/machdep.c
+++ b/sys/arch/i386/i386/machdep.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: machdep.c,v 1.267 2004/02/01 12:26:45 grange Exp $	*/
+/*	$OpenBSD: machdep.c,v 1.268 2004/02/01 19:05:23 deraadt Exp $	*/
 /*	$NetBSD: machdep.c,v 1.214 1996/11/10 03:16:17 thorpej Exp $	*/
 
 /*-
@@ -238,6 +238,11 @@ int	i386_fpu_present;
 int	i386_fpu_exception;
 int	i386_fpu_fdivbug;
 
+int	i386_use_fxsave;
+int	i386_has_sse;
+int	i386_has_sse2;
+int	i386_has_xcrypt;
+
 bootarg_t *bootargp;
 paddr_t avail_end;
 
@@ -2111,6 +2116,30 @@ identifycpu()
 	if (cpu_class >= CPUCLASS_486)
 		lcr0(rcr0() | CR0_WP);
 #endif
+
+#if defined(I686_CPU)
+	/*
+	 * If we have FXSAVE/FXRESTOR, use them.
+	 */
+	if (cpu_feature & CPUID_FXSR) {
+		i386_use_fxsave = 1;
+		lcr4(rcr4() | CR4_OSFXSR);
+
+		/*
+		 * If we have SSE/SSE2, enable XMM exceptions, and
+		 * notify userland.
+		 */
+		if (cpu_feature & (CPUID_SSE|CPUID_SSE2)) {
+			if (cpu_feature & CPUID_SSE)
+				i386_has_sse = 1;
+			if (cpu_feature & CPUID_SSE2)
+				i386_has_sse2 = 1;
+			lcr4(rcr4() | CR4_OSXMMEXCPT);
+		}
+	} else
+		i386_use_fxsave = 0;
+#endif /* I686_CPU */
+
 }
 
 #ifdef COMPAT_IBCS2
@@ -2617,6 +2646,7 @@ setregs(p, pack, stack, retval)
 	u_long stack;
 	register_t *retval;
 {
+	struct pcb *pcb = &p->p_addr->u_pcb;
 	struct pmap *pmap = vm_map_pmap(&p->p_vmspace->vm_map);
 	struct trapframe *tf = p->p_md.md_regs;
 
@@ -2631,6 +2661,11 @@ setregs(p, pack, stack, retval)
 #endif
 
 	p->p_md.md_flags &= ~MDP_USEDFPU;
+	if (i386_use_fxsave) {
+		pcb->pcb_savefpu.sv_xmm.sv_env.en_cw = __OpenBSD_NPXCW__;
+		pcb->pcb_savefpu.sv_xmm.sv_env.en_mxcsr = __INITIAL_MXCSR__;
+	} else
+		pcb->pcb_savefpu.sv_87.sv_env.en_cw = __OpenBSD_NPXCW__;
 
 	__asm("movw %w0,%%gs" : : "r" (LSEL(LUDATA_SEL, SEL_UPL)));
 	__asm("movw %w0,%%fs" : : "r" (LSEL(LUDATA_SEL, SEL_UPL)));
@@ -2837,6 +2872,13 @@ init386(paddr_t first_avail)
 	consinit();	/* XXX SHOULD NOT BE DONE HERE */
 			/* XXX here, until we can use bios for printfs */
 
+	/*
+	 * Saving SSE registers won't work if the save area isn't
+	 * 16-byte aligned.
+	 */
+	if (offsetof(struct user, u_pcb.pcb_savefpu) & 0xf)
+		panic("init386: pcb_savefpu not 16-byte aligned");
+
 	/* call pmap initialization to make new kernel address space */
 	pmap_bootstrap((vaddr_t)atdevbase + IOM_SIZE);
 
@@ -3225,6 +3267,14 @@ cpu_sysctl(name, namelen, oldp, oldlenp, newp, newlen, p)
 		return (sysctl_int(oldp, oldlenp, newp, newlen,
 		    &user_ldt_enable));
 #endif
+	case CPU_OSFXSR:
+		return (sysctl_rdint(oldp, oldlenp, newp, i386_use_fxsave));
+	case CPU_SSE:
+		return (sysctl_rdint(oldp, oldlenp, newp, i386_has_sse));
+	case CPU_SSE2:
+		return (sysctl_rdint(oldp, oldlenp, newp, i386_has_sse2));
+	case CPU_XCRYPT:
+		return (sysctl_rdint(oldp, oldlenp, newp, i386_has_xcrypt));
 	default:
 		return (EOPNOTSUPP);
 	}
diff --git a/sys/arch/i386/i386/process_machdep.c b/sys/arch/i386/i386/process_machdep.c
index 7db97a757fd..d75293f6a5c 100644
--- a/sys/arch/i386/i386/process_machdep.c
+++ b/sys/arch/i386/i386/process_machdep.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: process_machdep.c,v 1.13 2004/01/15 17:51:40 miod Exp $	*/
+/*	$OpenBSD: process_machdep.c,v 1.14 2004/02/01 19:05:23 deraadt Exp $	*/
 /*	$NetBSD: process_machdep.c,v 1.22 1996/05/03 19:42:25 christos Exp $	*/
 
 /*
@@ -78,22 +78,88 @@
 #endif
 
 static __inline struct trapframe *process_frame(struct proc *);
-static __inline struct save87 *process_fpframe(struct proc *);
+static __inline union savefpu *process_fpframe(struct proc *);
 
 static __inline struct trapframe *
-process_frame(p)
-	struct proc *p;
+process_frame(struct proc *p)
 {
 
 	return (p->p_md.md_regs);
 }
 
-static __inline struct save87 *
-process_fpframe(p)
-	struct proc *p;
+static __inline union savefpu *
+process_fpframe(struct proc *p)
+{
+
+	return (&p->p_addr->u_pcb.pcb_savefpu);
+}
+
+void
+process_xmm_to_s87(const struct savexmm *sxmm, struct save87 *s87)
+{
+	int i;
+
+	/* FPU control/status */
+	s87->sv_env.en_cw = sxmm->sv_env.en_cw;
+	s87->sv_env.en_sw = sxmm->sv_env.en_sw;
+	/* tag word handled below */
+	s87->sv_env.en_fip = sxmm->sv_env.en_fip;
+	s87->sv_env.en_fcs = sxmm->sv_env.en_fcs;
+	s87->sv_env.en_opcode = sxmm->sv_env.en_opcode;
+	s87->sv_env.en_foo = sxmm->sv_env.en_foo;
+	s87->sv_env.en_fos = sxmm->sv_env.en_fos;
+
+	/* Tag word and registers. */
+	for (i = 0; i < 8; i++) {
+		if (sxmm->sv_env.en_tw & (1U << i))
+			s87->sv_env.en_tw &= ~(3U << (i * 2));
+		else
+			s87->sv_env.en_tw |= (3U << (i * 2));
+
+		if (sxmm->sv_ex_tw & (1U << i))
+			s87->sv_ex_tw &= ~(3U << (i * 2));
+		else
+			s87->sv_ex_tw |= (3U << (i * 2));
+
+		memcpy(&s87->sv_ac[i].fp_bytes, &sxmm->sv_ac[i].fp_bytes,
+		    sizeof(s87->sv_ac[i].fp_bytes));
+	}
+
+	s87->sv_ex_sw = sxmm->sv_ex_sw;
+}
+
+void
+process_s87_to_xmm(const struct save87 *s87, struct savexmm *sxmm)
 {
+	int i;
+
+	/* FPU control/status */
+	sxmm->sv_env.en_cw = s87->sv_env.en_cw;
+	sxmm->sv_env.en_sw = s87->sv_env.en_sw;
+	/* tag word handled below */
+	sxmm->sv_env.en_fip = s87->sv_env.en_fip;
+	sxmm->sv_env.en_fcs = s87->sv_env.en_fcs;
+	sxmm->sv_env.en_opcode = s87->sv_env.en_opcode;
+	sxmm->sv_env.en_foo = s87->sv_env.en_foo;
+	sxmm->sv_env.en_fos = s87->sv_env.en_fos;
+
+	/* Tag word and registers. */
+	for (i = 0; i < 8; i++) {
+		if (((s87->sv_env.en_tw >> (i * 2)) & 3) == 3)
+			sxmm->sv_env.en_tw &= ~(1U << i);
+		else
+			sxmm->sv_env.en_tw |= (1U << i);
+
+		if (((s87->sv_ex_tw >> (i * 2)) & 3) == 3)
+			sxmm->sv_ex_tw &= ~(1U << i);
+		else
+			sxmm->sv_ex_tw |= (1U << i);
+
+		memcpy(&sxmm->sv_ac[i].fp_bytes, &s87->sv_ac[i].fp_bytes,
+		    sizeof(sxmm->sv_ac[i].fp_bytes));
+	}
 
-	return (&p->p_addr->u_pcb.pcb_savefpu.npx);
+	sxmm->sv_ex_sw = s87->sv_ex_sw;
 }
 
 int
@@ -142,14 +208,21 @@ process_read_fpregs(p, regs)
 {
 
 	if (p->p_md.md_flags & MDP_USEDFPU) {
-		struct save87 *frame = process_fpframe(p);
+		union savefpu *frame = process_fpframe(p);
 
 #if NNPX > 0
 		if (npxproc == p)
 			npxsave();
 #endif
 
-		bcopy(frame, regs, sizeof(*frame));
+		if (i386_use_fxsave) {
+			struct save87 s87;
+
+			/* XXX Yuck */
+			process_xmm_to_s87(&frame->sv_xmm, &s87);
+			memcpy(regs, &s87, sizeof(*regs));
+ 		} else
+			bcopy(frame, regs, sizeof(*frame));
 	} else
 		bzero(regs, sizeof(*regs));
 
@@ -231,15 +304,24 @@ process_write_fpregs(p, regs)
 	struct proc *p;
 	struct fpreg *regs;
 {
-	struct save87 *frame = process_fpframe(p);
+	union savefpu *frame = process_fpframe(p);
 
+	if (p->p_md.md_flags & MDP_USEDFPU) {
 #if NNPX > 0
-	if (npxproc == p)
-		npxdrop();
+		if (npxproc == p)
+			npxdrop();
 #endif
+	} else
+		p->p_md.md_flags |= MDP_USEDFPU;
 
-	p->p_md.md_flags |= MDP_USEDFPU;
-	bcopy(regs, frame, sizeof(*frame));
+	if (i386_use_fxsave) {
+		struct save87 s87;
+
+		/* XXX Yuck. */
+		memcpy(&s87, regs, sizeof(*regs));
+		process_s87_to_xmm(&s87, &frame->sv_xmm);
+	} else
+		memcpy(&frame->sv_87, regs, sizeof(*regs));
 
 	return (0);
 }
diff --git a/sys/arch/i386/include/cpu.h b/sys/arch/i386/include/cpu.h
index 2750fabdda6..ef70e5f86fc 100644
--- a/sys/arch/i386/include/cpu.h
+++ b/sys/arch/i386/include/cpu.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: cpu.h,v 1.56 2004/02/01 12:26:45 grange Exp $	*/
+/*	$OpenBSD: cpu.h,v 1.57 2004/02/01 19:05:23 deraadt Exp $	*/
 /*	$NetBSD: cpu.h,v 1.35 1996/05/05 19:29:26 christos Exp $	*/
 
 /*-
@@ -163,6 +163,10 @@ void fix_f00f(void);
 /* dkcsum.c */
 void	dkcsumattach(void);
 
+extern int i386_use_fxsave;
+extern int i386_has_sse;
+extern int i386_has_sse2;
+
 /* machdep.c */
 void	dumpconf(void);
 void	cpu_reset(void);
@@ -257,7 +261,11 @@ void	setconf(void);
 #define CPU_KBDRESET		10	/* keyboard reset under pcvt */
 #define CPU_APMHALT		11	/* halt -p hack */
 #define CPU_USERLDT		12
-#define	CPU_MAXID		13	/* number of valid machdep ids */
+#define CPU_OSFXSR		13	/* uses FXSAVE/FXRSTOR */
+#define CPU_SSE			14	/* supports SSE */
+#define CPU_SSE2		15	/* supports SSE2 */
+#define CPU_XCRYPT		16	/* supports VIA xcrypt in userland */
+#define CPU_MAXID		17	/* number of valid machdep ids */
 
 #define	CTL_MACHDEP_NAMES { \
 	{ 0, 0 }, \
@@ -273,6 +281,10 @@ void	setconf(void);
 	{ "kbdreset", CTLTYPE_INT }, \
 	{ "apmhalt", CTLTYPE_INT }, \
 	{ "userldt", CTLTYPE_INT }, \
+	{ "osfxsr", CTLTYPE_INT }, \
+	{ "sse", CTLTYPE_INT }, \
+	{ "sse2", CTLTYPE_INT }, \
+	{ "xcrypt", CTLTYPE_INT }, \
 }
 
 #endif /* !_I386_CPU_H_ */
diff --git a/sys/arch/i386/include/npx.h b/sys/arch/i386/include/npx.h
index c9fab4ffe9b..d6f906efb46 100644
--- a/sys/arch/i386/include/npx.h
+++ b/sys/arch/i386/include/npx.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: npx.h,v 1.5 2003/06/02 23:27:47 millert Exp $	*/
+/*	$OpenBSD: npx.h,v 1.6 2004/02/01 19:05:23 deraadt Exp $	*/
 /*	$NetBSD: npx.h,v 1.11 1994/10/27 04:16:11 cgd Exp $	*/
 
 /*-
@@ -86,9 +86,48 @@ struct	save87 {
 	u_long	sv_ex_tw;		/* tag word for last exception */
 };
 
-/* For the pcb. */
-union	fsave87 {
-	struct	save87 npx;
+/* Environment of FPU/MMX/SSE/SSE2. */
+struct envxmm {
+/*0*/	uint16_t en_cw;		/* FPU Control Word */
+	uint16_t en_sw;		/* FPU Status Word */
+	uint8_t  en_rsvd0;
+	uint8_t  en_tw;		/* FPU Tag Word (abridged) */
+	uint16_t en_opcode;	/* FPU Opcode */
+	uint32_t en_fip;	/* FPU Instruction Pointer */
+	uint16_t en_fcs;	/* FPU IP selector */
+	uint16_t en_rsvd1;
+/*16*/	uint32_t en_foo;	/* FPU Data pointer */
+	uint16_t en_fos;	/* FPU Data pointer selector */
+	uint16_t en_rsvd2;
+	uint32_t en_mxcsr;	/* MXCSR Register State */
+	uint32_t en_rsvd3;
+};
+
+/* FPU regsters in the extended save format. */
+struct fpaccxmm {
+	uint8_t	fp_bytes[10];
+	uint8_t	fp_rsvd[6];
+};
+
+/* SSE/SSE2 registers. */
+struct xmmreg {
+	uint8_t sse_bytes[16];
+};
+
+/* FPU/MMX/SSE/SSE2 context */
+struct savexmm {
+	struct envxmm sv_env;		/* control/status context */
+	struct fpaccxmm sv_ac[8];	/* ST/MM regs */
+	struct xmmreg sv_xmmregs[8];	/* XMM regs */
+	uint8_t sv_rsvd[16 * 14];
+	/* 512-bytes --- end of hardware portion of save area */
+	uint32_t sv_ex_sw;		/* saved SW from last exception */
+	uint32_t sv_ex_tw;		/* saved TW from last exception */
+};
+
+union savefpu {
+	struct save87 sv_87;
+	struct savexmm sv_xmm;
 #ifdef GPL_MATH_EMULATE
 	union i387_union gplemu;
 #else
@@ -109,6 +148,12 @@ struct	emcsts {
 #define	__OpenBSD_NPXCW__	0x127f
 
 /*
+ * The default MXCSR value at reset is 0x1f80, IA-32 Instruction
+ * Set Reference, pg. 3-369.
+ */
+#define __INITIAL_MXCSR__       0x1f80
+
+/*
  * The standard control word from finit is 0x37F, giving:
  *	round to nearest
  *	64-bit precision
@@ -133,4 +178,7 @@ struct	emcsts {
 
 #define	__INITIAL_NPXCW__	__OpenBSD_NPXCW__
 
+void    process_xmm_to_s87(const struct savexmm *, struct save87 *);
+void    process_s87_to_xmm(const struct save87 *, struct savexmm *);
+
 #endif /* !_I386_NPX_H_ */
diff --git a/sys/arch/i386/include/pcb.h b/sys/arch/i386/include/pcb.h
index 4a7c0094ef2..611ab5996a8 100644
--- a/sys/arch/i386/include/pcb.h
+++ b/sys/arch/i386/include/pcb.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: pcb.h,v 1.10 2004/02/01 12:26:45 grange Exp $	*/
+/*	$OpenBSD: pcb.h,v 1.11 2004/02/01 19:05:23 deraadt Exp $	*/
 /*	$NetBSD: pcb.h,v 1.21 1996/01/08 13:51:42 mycroft Exp $	*/
 
 /*-
@@ -62,10 +62,11 @@ struct pcb {
 #define	pcb_gs	pcb_tss.tss_gs
 #define	pcb_ldt_sel	pcb_tss.tss_ldt
 	int	pcb_tss_sel;
-        union	descriptor *pcb_ldt;	/* per process (user) LDT */
-        int	pcb_ldt_len;		/*      number of LDT entries */
+	union	descriptor *pcb_ldt;	/* per process (user) LDT */
+	int	pcb_ldt_len;		/*      number of LDT entries */
 	int	pcb_cr0;		/* saved image of CR0 */
-	union	fsave87 pcb_savefpu;	/* floating point state for 287/387 */
+	int	pcb_pad[2];		/* savefpu on 16-byte boundary */
+	union	savefpu pcb_savefpu;	/* floating point state for FPU */
 	struct	emcsts pcb_saveemc;	/* Cyrix EMC state */
 /*
  * Software pcb (extension)
diff --git a/sys/arch/i386/include/psl.h b/sys/arch/i386/include/psl.h
index 6d46152398d..2d71d97218b 100644
--- a/sys/arch/i386/include/psl.h
+++ b/sys/arch/i386/include/psl.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: psl.h,v 1.13 2003/06/02 23:27:47 millert Exp $	*/
+/*	$OpenBSD: psl.h,v 1.14 2004/02/01 19:05:23 deraadt Exp $	*/
 /*	$NetBSD: psl.h,v 1.30 1996/05/13 01:28:05 mycroft Exp $	*/
 
 /*-
@@ -58,6 +58,7 @@
 #define	PSL_VIF		0x00080000	/* virtual interrupt enable flag */
 #define	PSL_VIP		0x00100000	/* virtual interrupt pending flag */
 #define	PSL_ID		0x00200000	/* identification flag */
+#define	PSL_XCRYPT	0x20000000	/* VIA xcrypt: operation loaded */
 
 #define	PSL_MBO		0x00000002	/* must be one bits */
 #define	PSL_MBZ		0xffc08028	/* must be zero bits */
diff --git a/sys/arch/i386/isa/npx.c b/sys/arch/i386/isa/npx.c
index bfb3b4bc95c..b56a17f3875 100644
--- a/sys/arch/i386/isa/npx.c
+++ b/sys/arch/i386/isa/npx.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: npx.c,v 1.30 2003/12/31 19:17:41 deraadt Exp $	*/
+/*	$OpenBSD: npx.c,v 1.31 2004/02/01 19:05:21 deraadt Exp $	*/
 /*	$NetBSD: npx.c,v 1.57 1996/05/12 23:12:24 mycroft Exp $	*/
 
 #if 0
@@ -64,6 +64,12 @@
 #include <dev/isa/isavar.h>
 #include <i386/isa/icu.h>
 
+#if 0
+#define IPRINTF(x)      printf x
+#else
+#define IPRINTF(x)
+#endif
+
 /*
  * 387 and 287 Numeric Coprocessor Extension (NPX) Driver.
  *
@@ -96,8 +102,6 @@
 #define	clts()			__asm("clts")
 #define	stts()			lcr0(rcr0() | CR0_TS)
 
-int npxdna(struct proc *);
-void npxexit(void);
 int npxintr(void *);
 static int npxprobe1(struct isa_attach_args *);
 static void npxsave1(void);
@@ -136,6 +140,38 @@ extern int i386_fpu_present;
 extern int i386_fpu_exception;
 extern int i386_fpu_fdivbug;
 
+#ifdef I686_CPU
+#define        fxsave(addr)            __asm("fxsave %0" : "=m" (*addr))
+#define        fxrstor(addr)           __asm("fxrstor %0" : : "m" (*addr))
+#endif /* I686_CPU */
+
+static __inline void
+fpu_save(union savefpu *addr)
+{
+
+#ifdef I686_CPU
+	if (i386_use_fxsave) {
+		fxsave(&addr->sv_xmm);
+		/* FXSAVE doesn't FNINIT like FNSAVE does -- so do it here. */
+		fninit();
+	} else
+#endif /* I686_CPU */
+		fnsave(&addr->sv_87);
+}
+
+static int
+npxdna_notset(struct proc *p)
+{
+	panic("npxdna vector not initialized");
+}
+
+int    (*npxdna_func)(struct proc *) = npxdna_notset;
+int    npxdna_s87(struct proc *);
+#ifdef I686_CPU
+int    npxdna_xmm(struct proc *);
+#endif /* I686_CPU */
+void   npxexit(void);
+
 /*
  * Special interrupt handlers.  Someday intr0-intr15 will be used to count
  * interrupts.  We'll still need a special exception 16 handler.  The busy
@@ -351,6 +387,13 @@ npxattach(parent, self, aux)
 	}
 	lcr0(rcr0() | (CR0_TS));
 	i386_fpu_present = 1;
+
+#ifdef I686_CPU
+	if (i386_use_fxsave)
+		npxdna_func = npxdna_xmm;
+	else
+#endif /* I686_CPU */
+		npxdna_func = npxdna_s87;
 }
 
 /*
@@ -373,7 +416,7 @@ npxintr(arg)
 	void *arg;
 {
 	register struct proc *p = npxproc;
-	register struct save87 *addr;
+	union savefpu *addr;
 	struct intrframe *frame = arg;
 	int code;
 	union sigval sv;
@@ -401,17 +444,24 @@ npxintr(arg)
 	 * Find the address of npxproc's savefpu.  This is not necessarily
 	 * the one in curpcb.
 	 */
-	addr = &p->p_addr->u_pcb.pcb_savefpu.npx;
+	addr = &p->p_addr->u_pcb.pcb_savefpu;
 	/*
 	 * Save state.  This does an implied fninit.  It had better not halt
 	 * the cpu or we'll hang.
 	 */
-	fnsave(addr);
+	fpu_save(addr);
 	fwait();
 	/*
-	 * Restore control word (was clobbered by fnsave).
+	 * Restore control word (was clobbered by fpu_save).
 	 */
-	fldcw(&addr->sv_env.en_cw);
+	if (i386_use_fxsave) {
+		fldcw(&addr->sv_xmm.sv_env.en_cw);
+		/*
+		 * FNINIT doesn't affect MXCSR or the XMM registers;
+		 * no need to re-load MXCSR here.
+		 */
+	} else
+		fldcw(&addr->sv_87.sv_env.en_cw);
 	fwait();
 	/*
 	 * Remember the exception status word and tag word.  The current
@@ -421,8 +471,13 @@ npxintr(arg)
 	 * preserved the control word and will copy the status and tag
 	 * words, so the complete exception state can be recovered.
 	 */
-	addr->sv_ex_sw = addr->sv_env.en_sw;
-	addr->sv_ex_tw = addr->sv_env.en_tw;
+	if (i386_use_fxsave) {
+	        addr->sv_xmm.sv_ex_sw = addr->sv_xmm.sv_env.en_sw;
+	        addr->sv_xmm.sv_ex_tw = addr->sv_xmm.sv_env.en_tw;
+	} else {
+	        addr->sv_87.sv_ex_sw = addr->sv_87.sv_env.en_sw;
+	        addr->sv_87.sv_ex_tw = addr->sv_87.sv_env.en_tw;
+	}
 
 	/*
 	 * Pass exception to process.  If it's the current process, try to do
@@ -446,19 +501,19 @@ npxintr(arg)
 		 * Encode the appropriate code for detailed information on
 		 * this exception.
 		 */
-		if (addr->sv_ex_sw & EN_SW_IE)
+		if (addr->sv_87.sv_ex_sw & EN_SW_IE)
 			code = FPE_FLTINV;
 #ifdef notyet
 		else if (addr->sv_ex_sw & EN_SW_DE)
 			code = FPE_FLTDEN;
 #endif
-		else if (addr->sv_ex_sw & EN_SW_ZE)
+		else if (addr->sv_87.sv_ex_sw & EN_SW_ZE)
 			code = FPE_FLTDIV;
-		else if (addr->sv_ex_sw & EN_SW_OE)
+		else if (addr->sv_87.sv_ex_sw & EN_SW_OE)
 			code = FPE_FLTOVF;
-		else if (addr->sv_ex_sw & EN_SW_UE)
+		else if (addr->sv_87.sv_ex_sw & EN_SW_UE)
 			code = FPE_FLTUND;
-		else if (addr->sv_ex_sw & EN_SW_PE)
+		else if (addr->sv_87.sv_ex_sw & EN_SW_PE)
 			code = FPE_FLTRES;
 		else
 			code = 0;		/* XXX unknown */
@@ -494,14 +549,14 @@ npxintr(arg)
  * interrupt masked, it would be necessary to forcibly unmask the NPX interrupt
  * so that it could succeed.
  */
-static inline void
-npxsave1()
+static __inline void
+npxsave1(void)
 {
 	register struct pcb *pcb;
 
 	npx_nointr = 1;
 	pcb = &npxproc->p_addr->u_pcb;
-	fnsave(&pcb->pcb_savefpu);
+	fpu_save(&pcb->pcb_savefpu);
 	pcb->pcb_cr0 |= CR0_TS;
 	fwait();
 	npx_nointr = 0;
@@ -514,9 +569,48 @@ npxsave1()
  * Otherwise, we save the previous state, if necessary, and restore our last
  * saved state.
  */
+#ifdef I686_CPU
+int
+npxdna_xmm(struct proc *p)
+{
+
+#ifdef DIAGNOSTIC
+	if (cpl != 0 || npx_nointr != 0)
+	        panic("npxdna: masked");
+#endif
+
+	p->p_addr->u_pcb.pcb_cr0 &= ~CR0_TS;
+	clts();
+
+	/*
+	 * Initialize the FPU state to clear any exceptions.  If someone else
+	 * was using the FPU, save their state (which does an implicit
+	 * initialization).
+	 */
+	npx_nointr = 1;
+	if (npxproc != 0 && npxproc != p) {
+	        IPRINTF(("Save"));
+	        npxsave1();
+	} else {
+	        IPRINTF(("Init"));
+	        fninit();
+	        fwait();
+	}
+	npx_nointr = 0;
+	npxproc = p;
+
+	if ((p->p_md.md_flags & MDP_USEDFPU) == 0) {
+		fldcw(&p->p_addr->u_pcb.pcb_savefpu.sv_xmm.sv_env.en_cw);
+		p->p_md.md_flags |= MDP_USEDFPU;
+	} else
+		fxrstor(&p->p_addr->u_pcb.pcb_savefpu.sv_xmm);
+
+	return (1);
+}
+#endif /* I686_CPU */
+
 int
-npxdna(p)
-	struct proc *p;
+npxdna_s87(struct proc *p)
 {
 	static u_short control = __INITIAL_NPXCW__;
 
@@ -569,7 +663,7 @@ npxdna(p)
 		 * fnclex if it is the first FPU instruction after a context
 		 * switch.
 		 */
-		frstor(&p->p_addr->u_pcb.pcb_savefpu);
+		frstor(&p->p_addr->u_pcb.pcb_savefpu.sv_87);
 	}
 
 	return (1);