* Fix exception handling so that it actually works. We can now handle

  exceptions from both kernel and user mode.
* Fix context switching so that we can switch back to a proc which we
  switched away from (we were saving the state in the wrong place).
* Implement lazy switching of the high-fp state. This needs to be looked
  at again for SMP to cope with the case of a process migrating from one
  processor to another while it has the high-fp state.
* Make setregs() work properly. I still think this should be called
  cpu_exec() or something.
* Various other minor fixes.

With this lot, we can execve() /sbin/init and we get all the way up to its
first syscall. At that point, we stop because syscall handling is not done
yet.

16 files changed, 605 insertions, 447 deletions

diff --git a/sys/ia64/ia64/exception.S b/sys/ia64/ia64/exception.S
index e2ed911..ed815ad 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
-	;;
-	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
+(p1)	br.cond.dpnt.few 1f		// don't switch bs if kernel
 	;;
-(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 e2ed911..ed815ad 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
-	;;
-	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
+(p1)	br.cond.dpnt.few 1f		// don't switch bs if kernel
 	;;
-(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 0a5e43a..17a4eb9 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 93608b7..45d32c1 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 93608b7..45d32c1 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 301e010..612c3f0 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 17f08a6..4ddd5e0 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 17f08a6..4ddd5e0 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 e2aa9d2..7adb66e 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
+	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,ret0 ;;
+	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
@@ -281,6 +306,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()
  *
  * Arrange for a function to be invoked neatly, after a cpu_switch().
diff --git a/sys/ia64/ia64/trap.c b/sys/ia64/ia64/trap.c
index 6bc6439..361c2cf 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 b73e8bb..b0a8955 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 79f3727..f14c20e 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 4046bff..07c44e7 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 49a5c54..928389c 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 4aa8566..eff85c2 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 302cd6a..31a06af 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