diff --git a/sys/ia64/ia64/exception.S b/sys/ia64/ia64/exception.S
index e2ed911c7316..ed815ad1f4b4 100644
--- a/sys/ia64/ia64/exception.S
+++ b/sys/ia64/ia64/exception.S
@@ -33,24 +33,31 @@
 /*
  * ar.k7 = curproc
  * ar.k6 = ksp
- * ar.k5 = globalp
+ * ar.k5 = kbsp
+ * ar.k4 = globalp
  */
 
 /*
  * Call exception_save_regs to preserve the interrupted state in a
- * trapframe and call trap() with the value of _n_ as an argument. We
- * arrange for trap() to return to exception_return which will restore 
- * the interrupted state before executing an rfi to resume it.
+ * trapframe. Note that we don't use a call instruction because we
+ * must be careful not to lose track of the RSE state. We then call
+ * trap() with the value of _n_ as an argument to handle the
+ * exception. We arrange for trap() to return to exception_restore
+ * which will restore the interrupted state before executing an rfi to
+ * resume it.
  */
-#define TRAP(_n_)					\
-	mov	r16=b0;					\
-	br.call.sptk.few b0=exception_save_regs;	\
-	alloc	r16=ar.pfs,0,0,2,0;			\
-	movl	r17=exception_return;			\
-	mov	out0=_n_;				\
-	mov	out1=sp;;				\
-	add	sp=-16,sp;;				\
-	mov	rp=r17;					\
+#define TRAP(_n_)				\
+	mov	r16=b0;				\
+1:	mov	r17=ip;;			\
+	add	r17=2f-1b,r17;;			\
+	mov	b0=r17;				\
+	br.sptk.few exception_save;		\
+2:	alloc	r14=ar.pfs,0,0,2,0;		\
+	movl	r15=exception_restore;		\
+	mov	out0=_n_;			\
+	mov	out1=sp;;			\
+	add	sp=-16,sp;;			\
+	mov	rp=r15;				\
 	br.call.sptk.few b6=trap
 	
 /*
@@ -77,15 +84,26 @@ ia64_vector_table:
 	thash	r18=r16
 	ttag	r19=r16
 	;;
+	add	r21=16,r18		// tag
 	add	r20=24,r18		// collision chain
 	;; 
-	ld8	r20=[r20]		// first entry
+	ld8	r21=[r21]		// check VHPT tag
+	;;
+	cmp.eq	p1,p2=r21,r19
+(p2)	br.dpnt.few 1f
+	;;
+	ld8	r21=[r18]		// read pte
+	;;
+	itc.i	r21			// insert pte
+	rfi				// done
+	;;
+1:	ld8	r20=[r20]		// first entry
 	;; 
 	rsm	psr.dt			// turn off data translations
 	;;
 	srlz.d				// serialize
 	;;
-1:	cmp.eq	p1,p2=r0,r20		// done?
+2:	cmp.eq	p1,p2=r0,r20		// done?
 (p1)	br.cond.spnt.few 9f		// bail if done
 	;;
 	add	r21=16,r20		// tag location
@@ -93,7 +111,7 @@ ia64_vector_table:
 	ld8	r21=[r21]		// read tag
 	;;
 	cmp.eq	p1,p2=r21,r19		// compare tags
-(p2)	br.cond.sptk.few 2f		// if not, read next in chain
+(p2)	br.cond.sptk.few 3f		// if not, read next in chain
 	;;
 	ld8	r21=[r20],8		// read pte
 	;; 
@@ -122,13 +140,17 @@ ia64_vector_table:
 	;;
 	rfi				// walker will retry the access
 	
-2:	add	r20=24,r20		// next in chain
+3:	add	r20=24,r20		// next in chain
 	;;
 	ld8	r20=[r20]		// read chain
-	br.cond.sptk.few 1b		// loop
+	br.cond.sptk.few 2b		// loop
 
 9:	mov	pr=r17,0x1ffff		// restore predicates
-	TRAP(1)				// die horribly
+	ssm	psr.dt
+	;;
+	srlz.d
+	;; 
+	TRAP(20)			// Page Not Present trap
 
 	.align	1024
 
@@ -140,15 +162,26 @@ ia64_vector_table:
 	thash	r18=r16
 	ttag	r19=r16
 	;;
+	add	r21=16,r18		// tag
 	add	r20=24,r18		// collision chain
 	;; 
-	ld8	r20=[r20]		// first entry
+	ld8	r21=[r21]		// check VHPT tag
+	;;
+	cmp.eq	p1,p2=r21,r19
+	br.dpnt.few 1f
+	;;
+	ld8	r21=[r18]		// read pte
+	;;
+	itc.d	r21			// insert pte
+	rfi				// done
+	;;
+1:	ld8	r20=[r20]		// first entry
 	;; 
 	rsm	psr.dt			// turn off data translations
 	;;
 	srlz.d				// serialize
 	;;
-1:	cmp.eq	p1,p2=r0,r20		// done?
+2:	cmp.eq	p1,p2=r0,r20		// done?
 (p1)	br.cond.spnt.few 9f		// bail if done
 	;;
 	add	r21=16,r20		// tag location
@@ -156,7 +189,7 @@ ia64_vector_table:
 	ld8	r21=[r21]		// read tag
 	;;
 	cmp.eq	p1,p2=r21,r19		// compare tags
-(p2)	br.cond.sptk.few 2f		// if not, read next in chain
+(p2)	br.cond.sptk.few 3f		// if not, read next in chain
 	;;
 	ld8	r21=[r20],8		// read pte
 	;; 
@@ -185,13 +218,17 @@ ia64_vector_table:
 	;;
 	rfi				// walker will retry the access
 	
-2:	add	r20=24,r20		// next in chain
+3:	add	r20=24,r20		// next in chain
 	;;
 	ld8	r20=[r20]		// read chain
-	br.cond.sptk.few 1b		// loop
+	br.cond.sptk.few 2b		// loop
 
 9:	mov	pr=r17,0x1ffff		// restore predicates
-	TRAP(2)				// die horribly
+	ssm	psr.dt
+	;;
+	srlz.d
+	;; 
+	TRAP(20)			// Page Not Present trap
 
 	.align	1024
 
@@ -779,13 +816,14 @@ ia64_vhpt:	.quad 0
 #define rB0	r31		/* overlay rIIP */
 
 /*
- * exception_return:	restore interrupted state
- *	
+ * exception_restore:	restore interrupted state
+ *
  * Arguments:
  *	sp+16	trapframe pointer
+ *	r4	ar.pfs before the alloc in TRAP()
  *
  */
-ENTRY(exception_return, 0)
+ENTRY(exception_restore, 0)
 
 	rsm	psr.ic|psr.dt		// disable interrupt collection and vm
 	add	r3=16,sp;
@@ -793,13 +831,17 @@ ENTRY(exception_return, 0)
 	srlz.d
 	dep	r3=0,r3,61,3		// physical address
 	;; 
+	add	r16=TF_CR_IPSR,r3
+	;;
+	ld8	rIPSR=[r16]
+	;; 
 	extr.u	r16=rIPSR,32,2		// extract ipsr.cpl
 	;;
 	cmp.eq	p1,p2=r0,r16		// test for return to kernel mode
 	;;
-(p1)	add	r16=SIZEOF_TRAPFRAME+16,sp  // restore ar.k6 (kernel sp)
+(p2)	add	r16=SIZEOF_TRAPFRAME+16,sp  // restore ar.k6 (kernel sp)
 	;; 
-(p1)	mov	ar.k6=r16
+(p2)	mov	ar.k6=r16
 	add	r1=SIZEOF_TRAPFRAME-16,r3 // r1=&tf_f[FRAME_F15]
 	add	r2=SIZEOF_TRAPFRAME-32,r3 // r2=&tf_f[FRAME_F14]
 	;;
@@ -900,20 +942,9 @@ ENTRY(exception_return, 0)
 	ld8	rBSPSTORE=[r1],-16	// r1=&tf_cr_pfs
 	ld8	rIFS=[r2],-16		// r2=&tf_ar_rsc
 	;;
-	ld8	rPFS=[r1],-16		// r1=&tf_pr
-	ld8	rRSC=[r2],-16		// r2=&tf_cr_ifa
+(p1)	br.cond.dpnt.few 1f		// don't switch bs if kernel
 	;;
-	ld8	rPR=[r1],-16		// r1=&tf_cr_isr
-	ld8	rIFA=[r2],-16		// r2=&tf_cr_ipsr
-	;;
-	ld8	rIIP=[r1]
-	ld8	rIPSR=[r2]
-	;;
-	extr.u	r16=rIPSR,32,2		// extract ipsr.cpl
-	;;
-	cmp.eq	p1,p2=r0,r17		// test for kernel mode
-	;;
-(p2)	br.cond.dpnt.few 1f		// don't switch bs if not user
+	alloc	r16=ar.pfs,0,0,0,0	// discard current frame
 	;;
 	sub	r16=rBSP,rBSPSTORE	// how many bytes to load?
 	;;
@@ -926,9 +957,21 @@ ENTRY(exception_return, 0)
 	mov	ar.bspstore=rBSPSTORE
 	;;
 	mov	ar.rnat=rRNAT
-
-1:	mov	r1=rR1
+	;;
+1:	ld8	rPFS=[r1],-16		// r1=&tf_pr
+	ld8	rRSC=[r2],-16		// r2=&tf_cr_ifa
+	;;
+	ld8	rPR=[r1],-16		// r1=&tf_cr_isr
+	ld8	rIFA=[r2],-16		// r2=&tf_cr_ipsr
+	;;
+	ld8	rISR=[r1],-16		// r1=&tf_cr_iip
+	ld8	rIPSR=[r2]
+	;;
+	ld8	rIIP=[r1]
+	;; 
+	mov	r1=rR1
 	mov	r2=rR2
+	mov	ar.pfs=rPFS
 	mov	cr.ifs=rIFS
 	mov	ar.rsc=rRSC
 	mov	pr=rPR,0x1ffff
@@ -938,11 +981,11 @@ ENTRY(exception_return, 0)
 	;;
 	rfi
 
-	END(exception_return)
+	END(exception_restore)
 	
 
 /*
- * exception_save_regs: save interrupted state
+ * exception_save: save interrupted state
  *
  * Arguments:
  *	b0	return address
@@ -951,7 +994,7 @@ ENTRY(exception_return, 0)
  * Return:
  *	sp	kernel stack pointer
  */
-ENTRY(exception_save_regs, 0)
+ENTRY(exception_save, 0)
 	rsm	psr.dt			// turn off data translations
 	;;
 	srlz.d				// serialize
@@ -969,10 +1012,8 @@ ENTRY(exception_save_regs, 0)
 	mov	rSP=sp			// save sp
 	;; 
 (p2)	mov	sp=ar.k6		// and switch to kernel stack
-	mov	r16=SIZEOF_TRAPFRAME
-	;;
-	sub	sp=sp,r16		// reserve trapframe
 	;;
+	add	sp=-SIZEOF_TRAPFRAME,sp	// reserve trapframe
 	mov	rR1=r1
 	mov	rR2=r2
 	;; 
@@ -992,16 +1033,13 @@ ENTRY(exception_save_regs, 0)
 	mov	rRSC=ar.rsc
 	mov	rPFS=ar.pfs
 	cover
-(p2)	mov	r16=ar.k7		// curproc
 	mov	rIFS=cr.ifs
 	;; 
-(p2)	add	r16=P_ADDR,r16		// &curproc->p_addr
 	mov	ar.rsc=0
 	;; 
-(p2)	ld8	r16=[r16]		// curproc->p_addr
 	mov	rBSPSTORE=ar.bspstore
 	;; 
-(p2)	add	r16=SIZEOF_USER,r16	// kernel backing store
+(p2)	mov	r16=ar.k5		// kernel backing store
 	mov	rRNAT=ar.rnat
 	mov	rBSP=ar.bsp
 	;; 
@@ -1143,13 +1181,13 @@ ENTRY(exception_save_regs, 0)
 	stf.spill [r1]=f15		// 
 	;; 
 	movl	r1=__gp			// kernel globals
-	mov	r13=ar.k5		// processor globals
+	mov	r13=ar.k4		// processor globals
 	ssm	psr.ic|psr.dt		// enable interrupts & translation
 	;;
 	srlz.d				// serialize
 
-	br.ret.sptk.few b0
+	br.sptk.few b0			// not br.ret - we were not br.call'ed
 
-	END(exception_save_regs)
+	END(exception_save)
 	
 
diff --git a/sys/ia64/ia64/exception.s b/sys/ia64/ia64/exception.s
index e2ed911c7316..ed815ad1f4b4 100644
--- a/sys/ia64/ia64/exception.s
+++ b/sys/ia64/ia64/exception.s
@@ -33,24 +33,31 @@
 /*
  * ar.k7 = curproc
  * ar.k6 = ksp
- * ar.k5 = globalp
+ * ar.k5 = kbsp
+ * ar.k4 = globalp
  */
 
 /*
  * Call exception_save_regs to preserve the interrupted state in a
- * trapframe and call trap() with the value of _n_ as an argument. We
- * arrange for trap() to return to exception_return which will restore 
- * the interrupted state before executing an rfi to resume it.
+ * trapframe. Note that we don't use a call instruction because we
+ * must be careful not to lose track of the RSE state. We then call
+ * trap() with the value of _n_ as an argument to handle the
+ * exception. We arrange for trap() to return to exception_restore
+ * which will restore the interrupted state before executing an rfi to
+ * resume it.
  */
-#define TRAP(_n_)					\
-	mov	r16=b0;					\
-	br.call.sptk.few b0=exception_save_regs;	\
-	alloc	r16=ar.pfs,0,0,2,0;			\
-	movl	r17=exception_return;			\
-	mov	out0=_n_;				\
-	mov	out1=sp;;				\
-	add	sp=-16,sp;;				\
-	mov	rp=r17;					\
+#define TRAP(_n_)				\
+	mov	r16=b0;				\
+1:	mov	r17=ip;;			\
+	add	r17=2f-1b,r17;;			\
+	mov	b0=r17;				\
+	br.sptk.few exception_save;		\
+2:	alloc	r14=ar.pfs,0,0,2,0;		\
+	movl	r15=exception_restore;		\
+	mov	out0=_n_;			\
+	mov	out1=sp;;			\
+	add	sp=-16,sp;;			\
+	mov	rp=r15;				\
 	br.call.sptk.few b6=trap
 	
 /*
@@ -77,15 +84,26 @@ ia64_vector_table:
 	thash	r18=r16
 	ttag	r19=r16
 	;;
+	add	r21=16,r18		// tag
 	add	r20=24,r18		// collision chain
 	;; 
-	ld8	r20=[r20]		// first entry
+	ld8	r21=[r21]		// check VHPT tag
+	;;
+	cmp.eq	p1,p2=r21,r19
+(p2)	br.dpnt.few 1f
+	;;
+	ld8	r21=[r18]		// read pte
+	;;
+	itc.i	r21			// insert pte
+	rfi				// done
+	;;
+1:	ld8	r20=[r20]		// first entry
 	;; 
 	rsm	psr.dt			// turn off data translations
 	;;
 	srlz.d				// serialize
 	;;
-1:	cmp.eq	p1,p2=r0,r20		// done?
+2:	cmp.eq	p1,p2=r0,r20		// done?
 (p1)	br.cond.spnt.few 9f		// bail if done
 	;;
 	add	r21=16,r20		// tag location
@@ -93,7 +111,7 @@ ia64_vector_table:
 	ld8	r21=[r21]		// read tag
 	;;
 	cmp.eq	p1,p2=r21,r19		// compare tags
-(p2)	br.cond.sptk.few 2f		// if not, read next in chain
+(p2)	br.cond.sptk.few 3f		// if not, read next in chain
 	;;
 	ld8	r21=[r20],8		// read pte
 	;; 
@@ -122,13 +140,17 @@ ia64_vector_table:
 	;;
 	rfi				// walker will retry the access
 	
-2:	add	r20=24,r20		// next in chain
+3:	add	r20=24,r20		// next in chain
 	;;
 	ld8	r20=[r20]		// read chain
-	br.cond.sptk.few 1b		// loop
+	br.cond.sptk.few 2b		// loop
 
 9:	mov	pr=r17,0x1ffff		// restore predicates
-	TRAP(1)				// die horribly
+	ssm	psr.dt
+	;;
+	srlz.d
+	;; 
+	TRAP(20)			// Page Not Present trap
 
 	.align	1024
 
@@ -140,15 +162,26 @@ ia64_vector_table:
 	thash	r18=r16
 	ttag	r19=r16
 	;;
+	add	r21=16,r18		// tag
 	add	r20=24,r18		// collision chain
 	;; 
-	ld8	r20=[r20]		// first entry
+	ld8	r21=[r21]		// check VHPT tag
+	;;
+	cmp.eq	p1,p2=r21,r19
+	br.dpnt.few 1f
+	;;
+	ld8	r21=[r18]		// read pte
+	;;
+	itc.d	r21			// insert pte
+	rfi				// done
+	;;
+1:	ld8	r20=[r20]		// first entry
 	;; 
 	rsm	psr.dt			// turn off data translations
 	;;
 	srlz.d				// serialize
 	;;
-1:	cmp.eq	p1,p2=r0,r20		// done?
+2:	cmp.eq	p1,p2=r0,r20		// done?
 (p1)	br.cond.spnt.few 9f		// bail if done
 	;;
 	add	r21=16,r20		// tag location
@@ -156,7 +189,7 @@ ia64_vector_table:
 	ld8	r21=[r21]		// read tag
 	;;
 	cmp.eq	p1,p2=r21,r19		// compare tags
-(p2)	br.cond.sptk.few 2f		// if not, read next in chain
+(p2)	br.cond.sptk.few 3f		// if not, read next in chain
 	;;
 	ld8	r21=[r20],8		// read pte
 	;; 
@@ -185,13 +218,17 @@ ia64_vector_table:
 	;;
 	rfi				// walker will retry the access
 	
-2:	add	r20=24,r20		// next in chain
+3:	add	r20=24,r20		// next in chain
 	;;
 	ld8	r20=[r20]		// read chain
-	br.cond.sptk.few 1b		// loop
+	br.cond.sptk.few 2b		// loop
 
 9:	mov	pr=r17,0x1ffff		// restore predicates
-	TRAP(2)				// die horribly
+	ssm	psr.dt
+	;;
+	srlz.d
+	;; 
+	TRAP(20)			// Page Not Present trap
 
 	.align	1024
 
@@ -779,13 +816,14 @@ ia64_vhpt:	.quad 0
 #define rB0	r31		/* overlay rIIP */
 
 /*
- * exception_return:	restore interrupted state
- *	
+ * exception_restore:	restore interrupted state
+ *
  * Arguments:
  *	sp+16	trapframe pointer
+ *	r4	ar.pfs before the alloc in TRAP()
  *
  */
-ENTRY(exception_return, 0)
+ENTRY(exception_restore, 0)
 
 	rsm	psr.ic|psr.dt		// disable interrupt collection and vm
 	add	r3=16,sp;
@@ -793,13 +831,17 @@ ENTRY(exception_return, 0)
 	srlz.d
 	dep	r3=0,r3,61,3		// physical address
 	;; 
+	add	r16=TF_CR_IPSR,r3
+	;;
+	ld8	rIPSR=[r16]
+	;; 
 	extr.u	r16=rIPSR,32,2		// extract ipsr.cpl
 	;;
 	cmp.eq	p1,p2=r0,r16		// test for return to kernel mode
 	;;
-(p1)	add	r16=SIZEOF_TRAPFRAME+16,sp  // restore ar.k6 (kernel sp)
+(p2)	add	r16=SIZEOF_TRAPFRAME+16,sp  // restore ar.k6 (kernel sp)
 	;; 
-(p1)	mov	ar.k6=r16
+(p2)	mov	ar.k6=r16
 	add	r1=SIZEOF_TRAPFRAME-16,r3 // r1=&tf_f[FRAME_F15]
 	add	r2=SIZEOF_TRAPFRAME-32,r3 // r2=&tf_f[FRAME_F14]
 	;;
@@ -900,20 +942,9 @@ ENTRY(exception_return, 0)
 	ld8	rBSPSTORE=[r1],-16	// r1=&tf_cr_pfs
 	ld8	rIFS=[r2],-16		// r2=&tf_ar_rsc
 	;;
-	ld8	rPFS=[r1],-16		// r1=&tf_pr
-	ld8	rRSC=[r2],-16		// r2=&tf_cr_ifa
+(p1)	br.cond.dpnt.few 1f		// don't switch bs if kernel
 	;;
-	ld8	rPR=[r1],-16		// r1=&tf_cr_isr
-	ld8	rIFA=[r2],-16		// r2=&tf_cr_ipsr
-	;;
-	ld8	rIIP=[r1]
-	ld8	rIPSR=[r2]
-	;;
-	extr.u	r16=rIPSR,32,2		// extract ipsr.cpl
-	;;
-	cmp.eq	p1,p2=r0,r17		// test for kernel mode
-	;;
-(p2)	br.cond.dpnt.few 1f		// don't switch bs if not user
+	alloc	r16=ar.pfs,0,0,0,0	// discard current frame
 	;;
 	sub	r16=rBSP,rBSPSTORE	// how many bytes to load?
 	;;
@@ -926,9 +957,21 @@ ENTRY(exception_return, 0)
 	mov	ar.bspstore=rBSPSTORE
 	;;
 	mov	ar.rnat=rRNAT
-
-1:	mov	r1=rR1
+	;;
+1:	ld8	rPFS=[r1],-16		// r1=&tf_pr
+	ld8	rRSC=[r2],-16		// r2=&tf_cr_ifa
+	;;
+	ld8	rPR=[r1],-16		// r1=&tf_cr_isr
+	ld8	rIFA=[r2],-16		// r2=&tf_cr_ipsr
+	;;
+	ld8	rISR=[r1],-16		// r1=&tf_cr_iip
+	ld8	rIPSR=[r2]
+	;;
+	ld8	rIIP=[r1]
+	;; 
+	mov	r1=rR1
 	mov	r2=rR2
+	mov	ar.pfs=rPFS
 	mov	cr.ifs=rIFS
 	mov	ar.rsc=rRSC
 	mov	pr=rPR,0x1ffff
@@ -938,11 +981,11 @@ ENTRY(exception_return, 0)
 	;;
 	rfi
 
-	END(exception_return)
+	END(exception_restore)
 	
 
 /*
- * exception_save_regs: save interrupted state
+ * exception_save: save interrupted state
  *
  * Arguments:
  *	b0	return address
@@ -951,7 +994,7 @@ ENTRY(exception_return, 0)
  * Return:
  *	sp	kernel stack pointer
  */
-ENTRY(exception_save_regs, 0)
+ENTRY(exception_save, 0)
 	rsm	psr.dt			// turn off data translations
 	;;
 	srlz.d				// serialize
@@ -969,10 +1012,8 @@ ENTRY(exception_save_regs, 0)
 	mov	rSP=sp			// save sp
 	;; 
 (p2)	mov	sp=ar.k6		// and switch to kernel stack
-	mov	r16=SIZEOF_TRAPFRAME
-	;;
-	sub	sp=sp,r16		// reserve trapframe
 	;;
+	add	sp=-SIZEOF_TRAPFRAME,sp	// reserve trapframe
 	mov	rR1=r1
 	mov	rR2=r2
 	;; 
@@ -992,16 +1033,13 @@ ENTRY(exception_save_regs, 0)
 	mov	rRSC=ar.rsc
 	mov	rPFS=ar.pfs
 	cover
-(p2)	mov	r16=ar.k7		// curproc
 	mov	rIFS=cr.ifs
 	;; 
-(p2)	add	r16=P_ADDR,r16		// &curproc->p_addr
 	mov	ar.rsc=0
 	;; 
-(p2)	ld8	r16=[r16]		// curproc->p_addr
 	mov	rBSPSTORE=ar.bspstore
 	;; 
-(p2)	add	r16=SIZEOF_USER,r16	// kernel backing store
+(p2)	mov	r16=ar.k5		// kernel backing store
 	mov	rRNAT=ar.rnat
 	mov	rBSP=ar.bsp
 	;; 
@@ -1143,13 +1181,13 @@ ENTRY(exception_save_regs, 0)
 	stf.spill [r1]=f15		// 
 	;; 
 	movl	r1=__gp			// kernel globals
-	mov	r13=ar.k5		// processor globals
+	mov	r13=ar.k4		// processor globals
 	ssm	psr.ic|psr.dt		// enable interrupts & translation
 	;;
 	srlz.d				// serialize
 
-	br.ret.sptk.few b0
+	br.sptk.few b0			// not br.ret - we were not br.call'ed
 
-	END(exception_save_regs)
+	END(exception_save)
 	
 
diff --git a/sys/ia64/ia64/genassym.c b/sys/ia64/ia64/genassym.c
index 0a5e43a96e29..17a4eb9c418e 100644
--- a/sys/ia64/ia64/genassym.c
+++ b/sys/ia64/ia64/genassym.c
@@ -86,6 +86,8 @@ ASSYM(VM_MAXUSER_ADDRESS, VM_MAXUSER_ADDRESS);
 
 ASSYM(SIZEOF_USER,	sizeof(struct user));
 
+ASSYM(TF_CR_IPSR,	offsetof(struct trapframe, tf_cr_ipsr));
+
 ASSYM(U_PCB_R4,		offsetof(struct user, u_pcb.pcb_r4));
 ASSYM(U_PCB_R5,		offsetof(struct user, u_pcb.pcb_r5));
 ASSYM(U_PCB_R6,		offsetof(struct user, u_pcb.pcb_r6));
@@ -115,6 +117,8 @@ ASSYM(U_PCB_PR,		offsetof(struct user, u_pcb.pcb_pr));
 ASSYM(U_PCB_SCHEDNEST,	offsetof(struct user, u_pcb.pcb_onfault));
 ASSYM(U_PCB_ONFAULT,	offsetof(struct user, u_pcb.pcb_onfault));
 
+ASSYM(U_PCB_HIGHFP,	offsetof(struct user, u_pcb.pcb_highfp));
+
 ASSYM(UC_MCONTEXT_MC_AR_BSP,  offsetof(ucontext_t, uc_mcontext.mc_ar_bsp));
 ASSYM(UC_MCONTEXT_MC_AR_RNAT, offsetof(ucontext_t, uc_mcontext.mc_ar_rnat));
 
diff --git a/sys/ia64/ia64/locore.S b/sys/ia64/ia64/locore.S
index 93608b7e8bd0..45d32c16b336 100644
--- a/sys/ia64/ia64/locore.S
+++ b/sys/ia64/ia64/locore.S
@@ -180,120 +180,6 @@ XENTRY(esigcode)
 	.quad	esigcode-sigcode
 	.text
 	
-/**************************************************************************/
-
-/*
- * savefpstate: Save a process's floating point state.
- *
- * Arguments:
- *	a0	'struct fpstate *' to save into
- */
-
-ENTRY(savefpstate, 1)
-#if 0
-	LDGP(pv)
-	/* save all of the FP registers */
-	lda	t1, FPREG_FPR_REGS(a0)	/* get address of FP reg. save area */
-	stt	$f0,   (0 * 8)(t1)	/* save first register, using hw name */
-	stt	$f1,   (1 * 8)(t1)	/* etc. */
-	stt	$f2,   (2 * 8)(t1)
-	stt	$f3,   (3 * 8)(t1)
-	stt	$f4,   (4 * 8)(t1)
-	stt	$f5,   (5 * 8)(t1)
-	stt	$f6,   (6 * 8)(t1)
-	stt	$f7,   (7 * 8)(t1)
-	stt	$f8,   (8 * 8)(t1)
-	stt	$f9,   (9 * 8)(t1)
-	stt	$f10, (10 * 8)(t1)
-	stt	$f11, (11 * 8)(t1)
-	stt	$f12, (12 * 8)(t1)
-	stt	$f13, (13 * 8)(t1)
-	stt	$f14, (14 * 8)(t1)
-	stt	$f15, (15 * 8)(t1)
-	stt	$f16, (16 * 8)(t1)
-	stt	$f17, (17 * 8)(t1)
-	stt	$f18, (18 * 8)(t1)
-	stt	$f19, (19 * 8)(t1)
-	stt	$f20, (20 * 8)(t1)
-	stt	$f21, (21 * 8)(t1)
-	stt	$f22, (22 * 8)(t1)
-	stt	$f23, (23 * 8)(t1)
-	stt	$f24, (24 * 8)(t1)
-	stt	$f25, (25 * 8)(t1)
-	stt	$f26, (26 * 8)(t1)
-	stt	$f27, (27 * 8)(t1)
-	stt	$f28, (28 * 8)(t1)
-	stt	$f29, (29 * 8)(t1)
-	stt	$f30, (30 * 8)(t1)
-
-	/*
-	 * Then save the FPCR; note that the necessary 'trapb's are taken
-	 * care of on kernel entry and exit.
-	 */
-	mf_fpcr	ft0
-	stt	ft0, FPREG_FPR_CR(a0)	/* store to FPCR save area */
-
-	RET
-#endif
-	END(savefpstate)
-
-/**************************************************************************/
-
-/*
- * restorefpstate: Restore a process's floating point state.
- *
- * Arguments:
- *	a0	'struct fpstate *' to restore from
- */
-
-ENTRY(restorefpstate, 1)
-#if 0
-	LDGP(pv)
-	/*
-	 * Restore the FPCR; note that the necessary 'trapb's are taken care of
-	 * on kernel entry and exit.
-	 */
-	ldt	ft0, FPREG_FPR_CR(a0)	/* load from FPCR save area */
-	mt_fpcr	ft0
-
-	/* Restore all of the FP registers. */
-	lda	t1, FPREG_FPR_REGS(a0)	/* get address of FP reg. save area */
-	ldt	$f0,   (0 * 8)(t1)	/* restore first reg., using hw name */
-	ldt	$f1,   (1 * 8)(t1)	/* etc. */
-	ldt	$f2,   (2 * 8)(t1)
-	ldt	$f3,   (3 * 8)(t1)
-	ldt	$f4,   (4 * 8)(t1)
-	ldt	$f5,   (5 * 8)(t1)
-	ldt	$f6,   (6 * 8)(t1)
-	ldt	$f7,   (7 * 8)(t1)
-	ldt	$f8,   (8 * 8)(t1)
-	ldt	$f9,   (9 * 8)(t1)
-	ldt	$f10, (10 * 8)(t1)
-	ldt	$f11, (11 * 8)(t1)
-	ldt	$f12, (12 * 8)(t1)
-	ldt	$f13, (13 * 8)(t1)
-	ldt	$f14, (14 * 8)(t1)
-	ldt	$f15, (15 * 8)(t1)
-	ldt	$f16, (16 * 8)(t1)
-	ldt	$f17, (17 * 8)(t1)
-	ldt	$f18, (18 * 8)(t1)
-	ldt	$f19, (19 * 8)(t1)
-	ldt	$f20, (20 * 8)(t1)
-	ldt	$f21, (21 * 8)(t1)
-	ldt	$f22, (22 * 8)(t1)
-	ldt	$f23, (23 * 8)(t1)
-	ldt	$f24, (24 * 8)(t1)
-	ldt	$f25, (25 * 8)(t1)
-	ldt	$f26, (26 * 8)(t1)
-	ldt	$f27, (27 * 8)(t1)
-	ldt	$f28, (28 * 8)(t1)
-	ldt	$f29, (29 * 8)(t1)
-	ldt	$f30, (30 * 8)(t1)
-
-	RET
-#endif
-	END(restorefpstate)
-
 /*
  * When starting init, call this to configure the process for user
  * mode.  This will be inherited by other processes.
diff --git a/sys/ia64/ia64/locore.s b/sys/ia64/ia64/locore.s
index 93608b7e8bd0..45d32c16b336 100644
--- a/sys/ia64/ia64/locore.s
+++ b/sys/ia64/ia64/locore.s
@@ -180,120 +180,6 @@ XENTRY(esigcode)
 	.quad	esigcode-sigcode
 	.text
 	
-/**************************************************************************/
-
-/*
- * savefpstate: Save a process's floating point state.
- *
- * Arguments:
- *	a0	'struct fpstate *' to save into
- */
-
-ENTRY(savefpstate, 1)
-#if 0
-	LDGP(pv)
-	/* save all of the FP registers */
-	lda	t1, FPREG_FPR_REGS(a0)	/* get address of FP reg. save area */
-	stt	$f0,   (0 * 8)(t1)	/* save first register, using hw name */
-	stt	$f1,   (1 * 8)(t1)	/* etc. */
-	stt	$f2,   (2 * 8)(t1)
-	stt	$f3,   (3 * 8)(t1)
-	stt	$f4,   (4 * 8)(t1)
-	stt	$f5,   (5 * 8)(t1)
-	stt	$f6,   (6 * 8)(t1)
-	stt	$f7,   (7 * 8)(t1)
-	stt	$f8,   (8 * 8)(t1)
-	stt	$f9,   (9 * 8)(t1)
-	stt	$f10, (10 * 8)(t1)
-	stt	$f11, (11 * 8)(t1)
-	stt	$f12, (12 * 8)(t1)
-	stt	$f13, (13 * 8)(t1)
-	stt	$f14, (14 * 8)(t1)
-	stt	$f15, (15 * 8)(t1)
-	stt	$f16, (16 * 8)(t1)
-	stt	$f17, (17 * 8)(t1)
-	stt	$f18, (18 * 8)(t1)
-	stt	$f19, (19 * 8)(t1)
-	stt	$f20, (20 * 8)(t1)
-	stt	$f21, (21 * 8)(t1)
-	stt	$f22, (22 * 8)(t1)
-	stt	$f23, (23 * 8)(t1)
-	stt	$f24, (24 * 8)(t1)
-	stt	$f25, (25 * 8)(t1)
-	stt	$f26, (26 * 8)(t1)
-	stt	$f27, (27 * 8)(t1)
-	stt	$f28, (28 * 8)(t1)
-	stt	$f29, (29 * 8)(t1)
-	stt	$f30, (30 * 8)(t1)
-
-	/*
-	 * Then save the FPCR; note that the necessary 'trapb's are taken
-	 * care of on kernel entry and exit.
-	 */
-	mf_fpcr	ft0
-	stt	ft0, FPREG_FPR_CR(a0)	/* store to FPCR save area */
-
-	RET
-#endif
-	END(savefpstate)
-
-/**************************************************************************/
-
-/*
- * restorefpstate: Restore a process's floating point state.
- *
- * Arguments:
- *	a0	'struct fpstate *' to restore from
- */
-
-ENTRY(restorefpstate, 1)
-#if 0
-	LDGP(pv)
-	/*
-	 * Restore the FPCR; note that the necessary 'trapb's are taken care of
-	 * on kernel entry and exit.
-	 */
-	ldt	ft0, FPREG_FPR_CR(a0)	/* load from FPCR save area */
-	mt_fpcr	ft0
-
-	/* Restore all of the FP registers. */
-	lda	t1, FPREG_FPR_REGS(a0)	/* get address of FP reg. save area */
-	ldt	$f0,   (0 * 8)(t1)	/* restore first reg., using hw name */
-	ldt	$f1,   (1 * 8)(t1)	/* etc. */
-	ldt	$f2,   (2 * 8)(t1)
-	ldt	$f3,   (3 * 8)(t1)
-	ldt	$f4,   (4 * 8)(t1)
-	ldt	$f5,   (5 * 8)(t1)
-	ldt	$f6,   (6 * 8)(t1)
-	ldt	$f7,   (7 * 8)(t1)
-	ldt	$f8,   (8 * 8)(t1)
-	ldt	$f9,   (9 * 8)(t1)
-	ldt	$f10, (10 * 8)(t1)
-	ldt	$f11, (11 * 8)(t1)
-	ldt	$f12, (12 * 8)(t1)
-	ldt	$f13, (13 * 8)(t1)
-	ldt	$f14, (14 * 8)(t1)
-	ldt	$f15, (15 * 8)(t1)
-	ldt	$f16, (16 * 8)(t1)
-	ldt	$f17, (17 * 8)(t1)
-	ldt	$f18, (18 * 8)(t1)
-	ldt	$f19, (19 * 8)(t1)
-	ldt	$f20, (20 * 8)(t1)
-	ldt	$f21, (21 * 8)(t1)
-	ldt	$f22, (22 * 8)(t1)
-	ldt	$f23, (23 * 8)(t1)
-	ldt	$f24, (24 * 8)(t1)
-	ldt	$f25, (25 * 8)(t1)
-	ldt	$f26, (26 * 8)(t1)
-	ldt	$f27, (27 * 8)(t1)
-	ldt	$f28, (28 * 8)(t1)
-	ldt	$f29, (29 * 8)(t1)
-	ldt	$f30, (30 * 8)(t1)
-
-	RET
-#endif
-	END(restorefpstate)
-
 /*
  * When starting init, call this to configure the process for user
  * mode.  This will be inherited by other processes.
diff --git a/sys/ia64/ia64/machdep.c b/sys/ia64/ia64/machdep.c
index 301e010125db..612c3f04d85e 100644
--- a/sys/ia64/ia64/machdep.c
+++ b/sys/ia64/ia64/machdep.c
@@ -87,6 +87,8 @@ struct cpuhead cpuhead;
 struct mtx	sched_lock;
 struct mtx	Giant;
 
+struct	user *proc0paddr;
+
 char machine[] = "ia64";
 SYSCTL_STRING(_hw, HW_MACHINE, machine, CTLFLAG_RD, machine, 0, "");
 
@@ -571,7 +573,7 @@ ia64_init()
 	/*
 	 * Init mapping for u page(s) for proc 0
 	 */
-	proc0.p_addr =
+	proc0paddr = proc0.p_addr =
 	    (struct user *)pmap_steal_memory(UPAGES * PAGE_SIZE);
 
 	/*
@@ -581,7 +583,7 @@ ia64_init()
 		size_t sz = round_page(UPAGES * PAGE_SIZE);
 		globalp = (struct globaldata *) pmap_steal_memory(sz);
 		globaldata_init(globalp, 0, sz);
-		ia64_set_k5((u_int64_t) globalp);
+		ia64_set_k4((u_int64_t) globalp);
 		PCPU_GET(next_asn) = 1;	/* 0 used for proc0 pmap */
 	}
 
@@ -1030,6 +1032,14 @@ setregs(struct proc *p, u_long entry, u_long stack, u_long ps_strings)
 	bzero(frame->tf_r, sizeof(frame->tf_r));
 	bzero(frame->tf_f, sizeof(frame->tf_f));
 	frame->tf_cr_iip = entry;
+	frame->tf_cr_ipsr = (IA64_PSR_IC
+			     /* | IA64_PSR_I XXX not yet */
+			     | IA64_PSR_IT
+			     | IA64_PSR_DT
+			     | IA64_PSR_RT
+			     | IA64_PSR_DFH
+			     | IA64_PSR_BN
+			     | IA64_PSR_CPL_USER);
 	frame->tf_r[FRAME_SP] = stack;
 	frame->tf_r[FRAME_R14] = ps_strings;
 
@@ -1040,6 +1050,8 @@ setregs(struct proc *p, u_long entry, u_long stack, u_long ps_strings)
 	frame->tf_ar_bspstore = p->p_md.md_bspstore;
 	frame->tf_ar_bsp = p->p_md.md_bspstore;
 	frame->tf_cr_ifs = (1L<<63); /* ifm=0, v=1 */
+	frame->tf_ar_rsc = 0xf;	/* user mode rsc */
+	frame->tf_ar_fpsr = IA64_FPSR_DEFAULT;
 
 	p->p_md.md_flags &= ~MDP_FPUSED;
 	ia64_fpstate_drop(p);
@@ -1257,64 +1269,32 @@ SYSCTL_INT(_machdep, CPU_WALLCLOCK, wall_cmos_clock,
 void
 ia64_fpstate_check(struct proc *p)
 {
-#if 0
-	/* TODO panic if p has fp enabled and p != fpcurproc */
-	if (p->p_addr->u_pcb.pcb_hw.apcb_flags & IA64_PCB_FLAGS_FEN)
-		if (p != fpcurproc)
+	if ((p->p_md.md_tf->tf_cr_ipsr & IA64_PSR_DFH) == 0)
+		if (p != PCPU_GET(fpcurproc))
 			panic("ia64_check_fpcurproc: bogus");
-#endif
 }
 
-#define SET_FEN(p) /* TODO set fp enable for p */
-
-#define CLEAR_FEN(p) /* TODO clear fp enable for p */
-
 /*
- * Save the floating point state in the pcb. Use this to get read-only
- * access to the floating point state. If write is true, the current
- * fp process is cleared so that fp state can safely be modified. The
- * process will automatically reload the changed state by generating a 
- * FEN trap.
+ * Save the high floating point state in the pcb. Use this to get
+ * read-only access to the floating point state. If write is true, the
+ * current fp process is cleared so that fp state can safely be
+ * modified. The process will automatically reload the changed state
+ * by generating a disabled fp trap.
  */
 void
 ia64_fpstate_save(struct proc *p, int write)
 {
-#if 0
-	if (p == fpcurproc) {
-		/*
-		 * If curproc != fpcurproc, then we need to enable FEN 
-		 * so that we can dump the fp state.
-		 */
-		ia64_pal_wrfen(1);
-
+	if (p == PCPU_GET(fpcurproc)) {
 		/*
 		 * Save the state in the pcb.
 		 */
-		savefpstate(&p->p_addr->u_pcb.pcb_fp);
+		savehighfp(p->p_addr->u_pcb.pcb_highfp);
 
 		if (write) {
-			/*
-			 * If fpcurproc == curproc, just ask the
-			 * PALcode to disable FEN, otherwise we must
-			 * clear the FEN bit in fpcurproc's pcb.
-			 */
-			if (fpcurproc == curproc)
-				ia64_pal_wrfen(0);
-			else
-				CLEAR_FEN(fpcurproc);
-			fpcurproc = NULL;
-		} else {
-			/*
-			 * Make sure that we leave FEN enabled if
-			 * curproc == fpcurproc. We must have at most
-			 * one process with FEN enabled. Note that FEN 
-			 * must already be set in fpcurproc's pcb.
-			 */
-			if (curproc != fpcurproc)
-				ia64_pal_wrfen(0);
+			p->p_md.md_tf->tf_cr_ipsr |= IA64_PSR_DFH;
+			PCPU_SET(fpcurproc, NULL);
 		}
 	}
-#endif
 }
 
 /*
@@ -1325,23 +1305,10 @@ ia64_fpstate_save(struct proc *p, int write)
 void
 ia64_fpstate_drop(struct proc *p)
 {
-#if 0
-	if (p == fpcurproc) {
-		if (p == curproc) {
-			/*
-			 * Disable FEN via the PALcode. This will
-			 * clear the bit in the pcb as well.
-			 */
-			ia64_pal_wrfen(0);
-		} else {
-			/*
-			 * Clear the FEN bit of the pcb.
-			 */
-			CLEAR_FEN(p);
-		}
-		fpcurproc = NULL;
+	if (p == PCPU_GET(fpcurproc)) {
+		p->p_md.md_tf->tf_cr_ipsr |= IA64_PSR_DFH;
+		PCPU_SET(fpcurproc, NULL);
 	}
-#endif
 }
 
 /*
@@ -1351,35 +1318,20 @@ ia64_fpstate_drop(struct proc *p)
 void
 ia64_fpstate_switch(struct proc *p)
 {
-#if 0
-	/*
-	 * Enable FEN so that we can access the fp registers.
-	 */
-	ia64_pal_wrfen(1);
-	if (fpcurproc) {
+	if (PCPU_GET(fpcurproc)) {
 		/*
 		 * Dump the old fp state if its valid.
 		 */
-		savefpstate(&fpcurproc->p_addr->u_pcb.pcb_fp);
-		CLEAR_FEN(fpcurproc);
+		savehighfp(PCPU_GET(fpcurproc)->p_addr->u_pcb.pcb_highfp);
+		PCPU_GET(fpcurproc)->p_md.md_tf->tf_cr_ipsr |= IA64_PSR_DFH;
 	}
 
 	/*
 	 * Remember the new FP owner and reload its state.
 	 */
-	fpcurproc = p;
-	restorefpstate(&fpcurproc->p_addr->u_pcb.pcb_fp);
-
-	/*
-	 * If the new owner is curproc, leave FEN enabled, otherwise
-	 * mark its PCB so that it gets FEN when we context switch to
-	 * it later.
-	 */
-	if (p != curproc) {
-		ia64_pal_wrfen(0);
-		SET_FEN(p);
-	}
+	PCPU_SET(fpcurproc, p);
+	restorehighfp(p->p_addr->u_pcb.pcb_highfp);
+	p->p_md.md_tf->tf_cr_ipsr &= ~IA64_PSR_DFH;
 
 	p->p_md.md_flags |= MDP_FPUSED;
-#endif
 }
diff --git a/sys/ia64/ia64/support.S b/sys/ia64/ia64/support.S
index 17f08a6ab12b..4ddd5e0889dc 100644
--- a/sys/ia64/ia64/support.S
+++ b/sys/ia64/ia64/support.S
@@ -217,7 +217,7 @@ ENTRY(copystr, 4)
 
 2:	cmp.eq	p6,p0=r0,in3
 (p6)	br.cond.dpnt.few 3f		// if (lenp != NULL)
-	sub	r14=in2,r14		// *lenp = (i - len)
+	sub	r14=r14,in2		// *lenp = (i - len)
 	;;
 	st8	[in3]=r14
 	
@@ -261,6 +261,7 @@ ENTRY(copyinstr, 4)
 	;;
 	br.call.sptk.few rp=copystr		// do the copy.
 	st8	[loc2]=r0			// kill the fault handler.
+	mov	ar.pfs=loc0			// restore ar.pfs
 	mov	rp=loc1				// restore ra.
 	br.ret.sptk.few rp			// ret0 left over from copystr
 
@@ -295,6 +296,7 @@ ENTRY(copyoutstr, 4)
 	;;
 	br.call.sptk.few rp=copystr		// do the copy.
 	st8	[loc2]=r0			// kill the fault handler.
+	mov	ar.pfs=loc0			// restore ar.pfs
 	mov	rp=loc1				// restore ra.
 	br.ret.sptk.few rp			// ret0 left over from copystr
 
@@ -386,7 +388,7 @@ ENTRY(copyin, 3)
 
 	movl	loc2=VM_MAXUSER_ADDRESS		// make sure that src addr
 	;; 
-	cmp.ltu	p6,p0=in0,loc2			// is in user space.
+	cmp.geu	p6,p0=in0,loc2			// is in user space.
 	;; 
 (p6)	br.cond.spnt.few copyerr		// if it's not, error out.
 	movl	r14=copyerr			// set up fault handler.
@@ -408,6 +410,7 @@ ENTRY(copyin, 3)
 	;;
 	br.call.sptk.few rp=bcopy		// do the copy.
 	st8	[loc2]=r0			// kill the fault handler.
+	mov	ar.pfs=loc0			// restore ar.pfs
 	mov	rp=loc1				// restore ra.
 	br.ret.sptk.few rp			// ret0 left over from bcopy
 	
@@ -420,7 +423,7 @@ ENTRY(copyout, 3)
 
 	movl	loc2=VM_MAXUSER_ADDRESS		// make sure that dest addr
 	;; 
-	cmp.ltu	p6,p0=in1,loc2			// is in user space.
+	cmp.geu	p6,p0=in1,loc2			// is in user space.
 	;; 
 (p6)	br.cond.spnt.few copyerr		// if it's not, error out.
 	movl	r14=copyerr			// set up fault handler.
@@ -442,6 +445,7 @@ ENTRY(copyout, 3)
 	;;
 	br.call.sptk.few rp=bcopy		// do the copy.
 	st8	[loc2]=r0			// kill the fault handler.
+	mov	ar.pfs=loc0			// restore ar.pfs
 	mov	rp=loc1				// restore ra.
 	br.ret.sptk.few rp			// ret0 left over from bcopy
 	
diff --git a/sys/ia64/ia64/support.s b/sys/ia64/ia64/support.s
index 17f08a6ab12b..4ddd5e0889dc 100644
--- a/sys/ia64/ia64/support.s
+++ b/sys/ia64/ia64/support.s
@@ -217,7 +217,7 @@ ENTRY(copystr, 4)
 
 2:	cmp.eq	p6,p0=r0,in3
 (p6)	br.cond.dpnt.few 3f		// if (lenp != NULL)
-	sub	r14=in2,r14		// *lenp = (i - len)
+	sub	r14=r14,in2		// *lenp = (i - len)
 	;;
 	st8	[in3]=r14
 	
@@ -261,6 +261,7 @@ ENTRY(copyinstr, 4)
 	;;
 	br.call.sptk.few rp=copystr		// do the copy.
 	st8	[loc2]=r0			// kill the fault handler.
+	mov	ar.pfs=loc0			// restore ar.pfs
 	mov	rp=loc1				// restore ra.
 	br.ret.sptk.few rp			// ret0 left over from copystr
 
@@ -295,6 +296,7 @@ ENTRY(copyoutstr, 4)
 	;;
 	br.call.sptk.few rp=copystr		// do the copy.
 	st8	[loc2]=r0			// kill the fault handler.
+	mov	ar.pfs=loc0			// restore ar.pfs
 	mov	rp=loc1				// restore ra.
 	br.ret.sptk.few rp			// ret0 left over from copystr
 
@@ -386,7 +388,7 @@ ENTRY(copyin, 3)
 
 	movl	loc2=VM_MAXUSER_ADDRESS		// make sure that src addr
 	;; 
-	cmp.ltu	p6,p0=in0,loc2			// is in user space.
+	cmp.geu	p6,p0=in0,loc2			// is in user space.
 	;; 
 (p6)	br.cond.spnt.few copyerr		// if it's not, error out.
 	movl	r14=copyerr			// set up fault handler.
@@ -408,6 +410,7 @@ ENTRY(copyin, 3)
 	;;
 	br.call.sptk.few rp=bcopy		// do the copy.
 	st8	[loc2]=r0			// kill the fault handler.
+	mov	ar.pfs=loc0			// restore ar.pfs
 	mov	rp=loc1				// restore ra.
 	br.ret.sptk.few rp			// ret0 left over from bcopy
 	
@@ -420,7 +423,7 @@ ENTRY(copyout, 3)
 
 	movl	loc2=VM_MAXUSER_ADDRESS		// make sure that dest addr
 	;; 
-	cmp.ltu	p6,p0=in1,loc2			// is in user space.
+	cmp.geu	p6,p0=in1,loc2			// is in user space.
 	;; 
 (p6)	br.cond.spnt.few copyerr		// if it's not, error out.
 	movl	r14=copyerr			// set up fault handler.
@@ -442,6 +445,7 @@ ENTRY(copyout, 3)
 	;;
 	br.call.sptk.few rp=bcopy		// do the copy.
 	st8	[loc2]=r0			// kill the fault handler.
+	mov	ar.pfs=loc0			// restore ar.pfs
 	mov	rp=loc1				// restore ra.
 	br.ret.sptk.few rp			// ret0 left over from bcopy
 	
diff --git a/sys/ia64/ia64/swtch.s b/sys/ia64/ia64/swtch.s
index e2aa9d2c52b6..7adb66e5e11a 100644
--- a/sys/ia64/ia64/swtch.s
+++ b/sys/ia64/ia64/swtch.s
@@ -161,8 +161,10 @@ ENTRY(cpu_switch, 0)
 	add	r16=GD_CURPROC,r13 ;;
 	ld8	r17=[r16] ;; 
 	add	r17=P_ADDR,r17 ;;
+	ld8	r17=[r17]
 
 	flushrs				// push out caller's dirty regs
+	mov	r2=ar.pfs
 	mov	r3=ar.unat		// caller's value for ar.unat
 	;;
 	mov	ar.rsc=0		// stop the RSE after the flush
@@ -214,13 +216,36 @@ ENTRY(cpu_switch, 0)
 	mov	ar.rsc=3		// turn RSE back on
 
 	br.call.sptk.few rp=chooseproc
-	add	r14=GD_CURPROC,r13 ;;
-	ld8	r14=[r14] ;;
-	cmp.eq	p6,p0=r14,ret0		// chooseproc() == curproc ?
-(p6)	br.dpnt.few 9f			// don't bother to restore
 
-	add	r15=P_ADDR,ret0 ;;
+	add	r14=GD_CURPROC,r13 ;;
+	ld8	r15=[r14] ;;
+
+#if 0
+	cmp.ne	p6,p0=r15,ret0		// chooseproc() == curproc ?
+(p6)	br.dptk.few 1f
+	;;
+	add	r17=P_ADDR,r15 ;;	// restore b0
+	ld8	r17=[r17] ;;
+	add	r17=U_PCB_B0,r17 ;;
+	ld8	r17=[r17] ;;
+	mov	b0=r17
+
+	br.sptk.few 9f			// don't bother to restore
+#endif
+
+1:
+	st8	[r14]=ret0
+	mov	ar.k7=ret0
+	mov	r4=ret0			// save from call
+	alloc	r15=ar.pfs,0,0,1,0	// create temporary output frame
+	;;
+	mov	out0=r4
+	br.call.sptk.few rp=pmap_activate // install RIDs etc.
+
+	add	r15=P_ADDR,r4 ;;
 	ld8	r15=[r15] ;;
+	add	r16=SIZEOF_USER,r15 ;;
+	mov	ar.k5=r16
 
 	add	r3=U_PCB_UNAT,r15	// point at NaT for r4..r7
 	mov	ar.rsc=0 ;;		// switch off the RSE
@@ -280,6 +305,322 @@ ENTRY(cpu_switch, 0)
 
 END(cpu_switch)
 
+/*
+ * savehighfp: Save f32-f127
+ *
+ * Arguments:
+ *	in0	array of struct ia64_fpreg
+ */
+ENTRY(savehighfp, 1)
+
+	add	r14=16,in0
+	;;
+	stf.spill [in0]=f32,32
+	stf.spill [r14]=f33,32
+	;; 
+	stf.spill [in0]=f34,32
+	stf.spill [r14]=f35,32
+	;; 
+	stf.spill [in0]=f36,32
+	stf.spill [r14]=f37,32
+	;; 
+	stf.spill [in0]=f38,32
+	stf.spill [r14]=f39,32
+	;; 
+	stf.spill [in0]=f40,32
+	stf.spill [r14]=f41,32
+	;; 
+	stf.spill [in0]=f42,32
+	stf.spill [r14]=f43,32
+	;; 
+	stf.spill [in0]=f44,32
+	stf.spill [r14]=f45,32
+	;; 
+	stf.spill [in0]=f46,32
+	stf.spill [r14]=f47,32
+	;; 
+	stf.spill [in0]=f48,32
+	stf.spill [r14]=f49,32
+	;; 
+	stf.spill [in0]=f50,32
+	stf.spill [r14]=f51,32
+	;; 
+	stf.spill [in0]=f52,32
+	stf.spill [r14]=f53,32
+	;; 
+	stf.spill [in0]=f54,32
+	stf.spill [r14]=f55,32
+	;; 
+	stf.spill [in0]=f56,32
+	stf.spill [r14]=f57,32
+	;; 
+	stf.spill [in0]=f58,32
+	stf.spill [r14]=f59,32
+	;; 
+	stf.spill [in0]=f60,32
+	stf.spill [r14]=f61,32
+	;; 
+	stf.spill [in0]=f62,32
+	stf.spill [r14]=f63,32
+	;; 
+	stf.spill [in0]=f64,32
+	stf.spill [r14]=f65,32
+	;; 
+	stf.spill [in0]=f66,32
+	stf.spill [r14]=f67,32
+	;; 
+	stf.spill [in0]=f68,32
+	stf.spill [r14]=f69,32
+	;; 
+	stf.spill [in0]=f70,32
+	stf.spill [r14]=f71,32
+	;; 
+	stf.spill [in0]=f72,32
+	stf.spill [r14]=f73,32
+	;; 
+	stf.spill [in0]=f74,32
+	stf.spill [r14]=f75,32
+	;; 
+	stf.spill [in0]=f76,32
+	stf.spill [r14]=f77,32
+	;; 
+	stf.spill [in0]=f78,32
+	stf.spill [r14]=f79,32
+	;; 
+	stf.spill [in0]=f80,32
+	stf.spill [r14]=f81,32
+	;; 
+	stf.spill [in0]=f82,32
+	stf.spill [r14]=f83,32
+	;; 
+	stf.spill [in0]=f84,32
+	stf.spill [r14]=f85,32
+	;; 
+	stf.spill [in0]=f86,32
+	stf.spill [r14]=f87,32
+	;; 
+	stf.spill [in0]=f88,32
+	stf.spill [r14]=f89,32
+	;; 
+	stf.spill [in0]=f90,32
+	stf.spill [r14]=f91,32
+	;; 
+	stf.spill [in0]=f92,32
+	stf.spill [r14]=f93,32
+	;; 
+	stf.spill [in0]=f94,32
+	stf.spill [r14]=f95,32
+	;; 
+	stf.spill [in0]=f96,32
+	stf.spill [r14]=f97,32
+	;; 
+	stf.spill [in0]=f98,32
+	stf.spill [r14]=f99,32
+	;; 
+	stf.spill [in0]=f100,32
+	stf.spill [r14]=f101,32
+	;; 
+	stf.spill [in0]=f102,32
+	stf.spill [r14]=f103,32
+	;; 
+	stf.spill [in0]=f104,32
+	stf.spill [r14]=f105,32
+	;; 
+	stf.spill [in0]=f106,32
+	stf.spill [r14]=f107,32
+	;; 
+	stf.spill [in0]=f108,32
+	stf.spill [r14]=f109,32
+	;; 
+	stf.spill [in0]=f110,32
+	stf.spill [r14]=f111,32
+	;; 
+	stf.spill [in0]=f112,32
+	stf.spill [r14]=f113,32
+	;; 
+	stf.spill [in0]=f114,32
+	stf.spill [r14]=f115,32
+	;; 
+	stf.spill [in0]=f116,32
+	stf.spill [r14]=f117,32
+	;; 
+	stf.spill [in0]=f118,32
+	stf.spill [r14]=f119,32
+	;; 
+	stf.spill [in0]=f120,32
+	stf.spill [r14]=f121,32
+	;; 
+	stf.spill [in0]=f122,32
+	stf.spill [r14]=f123,32
+	;; 
+	stf.spill [in0]=f124,32
+	stf.spill [r14]=f125,32
+	;; 
+	stf.spill [in0]=f126
+	stf.spill [r14]=f127
+	;; 
+	br.ret.sptk.few rp
+
+END(savehighfp)
+
+/*
+ * restorehighfp: Restore f32-f127
+ *
+ * Arguments:
+ *	in0	array of struct ia64_fpreg
+ */
+ENTRY(restorehighfp, 1)
+	
+	add	r14=16,in0
+	;;
+	ldf.fill f32=[in0],32
+	ldf.fill f33=[r14],32
+	;; 
+	ldf.fill f34=[in0],32
+	ldf.fill f35=[r14],32
+	;; 
+	ldf.fill f36=[in0],32
+	ldf.fill f37=[r14],32
+	;; 
+	ldf.fill f38=[in0],32
+	ldf.fill f39=[r14],32
+	;; 
+	ldf.fill f40=[in0],32
+	ldf.fill f41=[r14],32
+	;; 
+	ldf.fill f42=[in0],32
+	ldf.fill f43=[r14],32
+	;; 
+	ldf.fill f44=[in0],32
+	ldf.fill f45=[r14],32
+	;; 
+	ldf.fill f46=[in0],32
+	ldf.fill f47=[r14],32
+	;; 
+	ldf.fill f48=[in0],32
+	ldf.fill f49=[r14],32
+	;; 
+	ldf.fill f50=[in0],32
+	ldf.fill f51=[r14],32
+	;; 
+	ldf.fill f52=[in0],32
+	ldf.fill f53=[r14],32
+	;; 
+	ldf.fill f54=[in0],32
+	ldf.fill f55=[r14],32
+	;; 
+	ldf.fill f56=[in0],32
+	ldf.fill f57=[r14],32
+	;; 
+	ldf.fill f58=[in0],32
+	ldf.fill f59=[r14],32
+	;; 
+	ldf.fill f60=[in0],32
+	ldf.fill f61=[r14],32
+	;; 
+	ldf.fill f62=[in0],32
+	ldf.fill f63=[r14],32
+	;; 
+	ldf.fill f64=[in0],32
+	ldf.fill f65=[r14],32
+	;; 
+	ldf.fill f66=[in0],32
+	ldf.fill f67=[r14],32
+	;; 
+	ldf.fill f68=[in0],32
+	ldf.fill f69=[r14],32
+	;; 
+	ldf.fill f70=[in0],32
+	ldf.fill f71=[r14],32
+	;; 
+	ldf.fill f72=[in0],32
+	ldf.fill f73=[r14],32
+	;; 
+	ldf.fill f74=[in0],32
+	ldf.fill f75=[r14],32
+	;; 
+	ldf.fill f76=[in0],32
+	ldf.fill f77=[r14],32
+	;; 
+	ldf.fill f78=[in0],32
+	ldf.fill f79=[r14],32
+	;; 
+	ldf.fill f80=[in0],32
+	ldf.fill f81=[r14],32
+	;; 
+	ldf.fill f82=[in0],32
+	ldf.fill f83=[r14],32
+	;; 
+	ldf.fill f84=[in0],32
+	ldf.fill f85=[r14],32
+	;; 
+	ldf.fill f86=[in0],32
+	ldf.fill f87=[r14],32
+	;; 
+	ldf.fill f88=[in0],32
+	ldf.fill f89=[r14],32
+	;; 
+	ldf.fill f90=[in0],32
+	ldf.fill f91=[r14],32
+	;; 
+	ldf.fill f92=[in0],32
+	ldf.fill f93=[r14],32
+	;; 
+	ldf.fill f94=[in0],32
+	ldf.fill f95=[r14],32
+	;; 
+	ldf.fill f96=[in0],32
+	ldf.fill f97=[r14],32
+	;; 
+	ldf.fill f98=[in0],32
+	ldf.fill f99=[r14],32
+	;; 
+	ldf.fill f100=[in0],32
+	ldf.fill f101=[r14],32
+	;; 
+	ldf.fill f102=[in0],32
+	ldf.fill f103=[r14],32
+	;; 
+	ldf.fill f104=[in0],32
+	ldf.fill f105=[r14],32
+	;; 
+	ldf.fill f106=[in0],32
+	ldf.fill f107=[r14],32
+	;; 
+	ldf.fill f108=[in0],32
+	ldf.fill f109=[r14],32
+	;; 
+	ldf.fill f110=[in0],32
+	ldf.fill f111=[r14],32
+	;; 
+	ldf.fill f112=[in0],32
+	ldf.fill f113=[r14],32
+	;; 
+	ldf.fill f114=[in0],32
+	ldf.fill f115=[r14],32
+	;; 
+	ldf.fill f116=[in0],32
+	ldf.fill f117=[r14],32
+	;; 
+	ldf.fill f118=[in0],32
+	ldf.fill f119=[r14],32
+	;; 
+	ldf.fill f120=[in0],32
+	ldf.fill f121=[r14],32
+	;; 
+	ldf.fill f122=[in0],32
+	ldf.fill f123=[r14],32
+	;; 
+	ldf.fill f124=[in0],32
+	ldf.fill f125=[r14],32
+	;; 
+	ldf.fill f126=[in0]
+	ldf.fill f127=[r14]
+	;; 
+	br.ret.sptk.few rp
+
+END(restorehighfp)
+
 /*
  * switch_trampoline()
  *
diff --git a/sys/ia64/ia64/trap.c b/sys/ia64/ia64/trap.c
index 6bc64398c120..361c2cf71e7b 100644
--- a/sys/ia64/ia64/trap.c
+++ b/sys/ia64/ia64/trap.c
@@ -310,7 +310,7 @@ trap(int vector, struct trapframe *framep)
 		 * on exit from the kernel, if proc == fpcurproc,
 		 * FP is enabled.
 		 */
-		if (fpcurproc == p) {
+		if (PCPU_GET(fpcurproc) == p) {
 			printf("trap: fp disabled for fpcurproc == %p", p);
 			goto dopanic;
 		}
@@ -320,6 +320,8 @@ trap(int vector, struct trapframe *framep)
 		break;
 
 	case IA64_VEC_PAGE_NOT_PRESENT:
+	case IA64_VEC_INST_ACCESS_RIGHTS:
+	case IA64_VEC_DATA_ACCESS_RIGHTS:
 	{
 		vm_offset_t va = framep->tf_cr_ifa;
 		struct vmspace *vm = NULL;
@@ -378,7 +380,9 @@ trap(int vector, struct trapframe *framep)
 			map = &vm->vm_map;
 		}
 
-		if (framep->tf_cr_isr & (IA64_ISR_X | IA64_ISR_R))
+		if (framep->tf_cr_isr & IA64_ISR_X)
+			ftype = VM_PROT_EXECUTE;
+		else if (framep->tf_cr_isr & IA64_ISR_R)
 			ftype = VM_PROT_READ;
 		else
 			ftype = VM_PROT_WRITE;
diff --git a/sys/ia64/ia64/vm_machdep.c b/sys/ia64/ia64/vm_machdep.c
index b73e8bb0bd3f..b0a8955d22b6 100644
--- a/sys/ia64/ia64/vm_machdep.c
+++ b/sys/ia64/ia64/vm_machdep.c
@@ -246,7 +246,7 @@ cpu_fork(p1, p2, flags)
 		 * Setup the child's pcb so that its ar.bspstore
 		 * starts just above the region which we copied. This
 		 * should work since the child will normally return
-		 * straight into exception_return.
+		 * straight into exception_restore.
 		 */
 		up->u_pcb.pcb_bspstore = (u_int64_t)
 			(p2bs + (p1->p_md.md_tf->tf_ar_bsp
@@ -255,8 +255,8 @@ cpu_fork(p1, p2, flags)
 
 		/*
 		 * Arrange for continuation at child_return(), which
-		 * will return to exception_return().  Note that the child
-		 * process doesn't stay in the kernel for long!
+		 * will return to exception_restore().  Note that the
+		 * child process doesn't stay in the kernel for long!
 		 *
 		 * We should really deal with the function descriptor
 		 * for child_return in switch_trampoline so that a
@@ -265,7 +265,7 @@ cpu_fork(p1, p2, flags)
 		 */
 		up->u_pcb.pcb_sp = (u_int64_t)p2tf - 16;	
 		up->u_pcb.pcb_r4 = FDESC_FUNC(child_return);
-		up->u_pcb.pcb_r5 = FDESC_FUNC(exception_return);
+		up->u_pcb.pcb_r5 = FDESC_FUNC(exception_restore);
 		up->u_pcb.pcb_r6 = (u_int64_t)p2;
 		up->u_pcb.pcb_b0 = FDESC_FUNC(switch_trampoline);
 
diff --git a/sys/ia64/include/cpu.h b/sys/ia64/include/cpu.h
index 79f3727db997..f14c20eb9ab8 100644
--- a/sys/ia64/include/cpu.h
+++ b/sys/ia64/include/cpu.h
@@ -131,7 +131,6 @@ struct reg;
 struct rpb;
 struct trapframe;
 
-extern struct proc *fpcurproc;
 extern struct rpb *hwrpb;
 extern volatile int mc_expected, mc_received;
 
@@ -142,7 +141,7 @@ void	child_return __P((struct proc *p));
 u_int64_t console_restart __P((u_int64_t, u_int64_t, u_int64_t));
 void	do_sir __P((void));
 void	dumpconf __P((void));
-void	exception_return __P((void));				/* MAGIC */
+void	exception_restore __P((void));				/* MAGIC */
 void	frametoreg __P((struct trapframe *, struct reg *));
 long	fswintrberr __P((void));				/* MAGIC */
 int	ia64_pa_access __P((u_long));
diff --git a/sys/ia64/include/globals.h b/sys/ia64/include/globals.h
index 4046bff8c2e4..07c44e71da0d 100644
--- a/sys/ia64/include/globals.h
+++ b/sys/ia64/include/globals.h
@@ -52,7 +52,6 @@ register struct globaldata *globalp __asm__("r13");
 #define	curproc		PCPU_GET(curproc)
 #define	idleproc	PCPU_GET(idleproc)
 #define	curpcb		PCPU_GET(curpcb)
-#define	fpcurproc	PCPU_GET(fpcurproc)
 #define	switchtime	PCPU_GET(switchtime)
 #define	switchticks	PCPU_GET(switchticks)
 #define cpuid		PCPU_GET(cpuno)
diff --git a/sys/ia64/include/pcb.h b/sys/ia64/include/pcb.h
index 49a5c545a8eb..928389c2ef95 100644
--- a/sys/ia64/include/pcb.h
+++ b/sys/ia64/include/pcb.h
@@ -62,6 +62,8 @@ struct pcb {
 
 	u_int64_t		pcb_onfault;	/* for copy faults */
 	u_int64_t		pcb_accessaddr;	/* for [fs]uswintr */
+
+	struct ia64_fpreg	pcb_highfp[96];	/* f32-f127 */
 };
 
 /*
diff --git a/sys/ia64/include/proc.h b/sys/ia64/include/proc.h
index 4aa8566094c4..eff85c21754d 100644
--- a/sys/ia64/include/proc.h
+++ b/sys/ia64/include/proc.h
@@ -37,6 +37,7 @@
 
 struct mdproc {
 	u_long		md_flags;
+	struct user	*md_uservirt;	/* virtual address of p_addr */
 	vm_offset_t	md_bspstore;	/* initial ar.bspstore */
 	struct	trapframe *md_tf;	/* trap/syscall registers */
 };
diff --git a/sys/ia64/include/reg.h b/sys/ia64/include/reg.h
index 302cd6a3668e..31a06af6e86e 100644
--- a/sys/ia64/include/reg.h
+++ b/sys/ia64/include/reg.h
@@ -56,8 +56,8 @@ struct dbreg {
 
 struct proc;
 
-void	restorefpstate __P((struct fpreg *));
-void	savefpstate __P((struct fpreg *));
+void	restorehighfp __P((struct ia64_fpreg *));
+void	savehighfp __P((struct ia64_fpreg *));
 void	setregs __P((struct proc *, u_long, u_long, u_long));
 #endif