--- sys/sparc64/sparc64/machdep.c.orig	Thu Aug  5 09:42:40 2004
+++ sys/sparc64/sparc64/machdep.c	Tue Sep 28 15:11:08 2004
@@ -623,6 +623,11 @@ set_mcontext(struct thread *td, const mc
 	tf = td->td_frame;
 	pcb = td->td_pcb;
 	wstate = tf->tf_wstate;
+	/* XXXKSE Make sure the pre-trap windows are spilled.
+	 * This is needed for kse_switchin(2) for correct switching between
+	 * threads stacks.
+	 */
+	flushw();
 	bcopy(mc, tf, sizeof(*tf));
 	tf->tf_wstate = wstate;
 	if ((mc->mc_fprs & FPRS_FEF) != 0) {
--- sys/sparc64/sparc64/vm_machdep.c.orig	Thu Aug  5 09:42:43 2004
+++ sys/sparc64/sparc64/vm_machdep.c	Thu Sep 16 16:31:26 2004
@@ -141,6 +141,7 @@ cpu_thread_setup(struct thread *td)
 
 	pcb = (struct pcb *)((td->td_kstack + td->td_kstack_pages * PAGE_SIZE -
 	    sizeof(struct pcb)) & ~0x3fUL);
+	pcb->pcb_nsaved = 0;
 	td->td_frame = (struct trapframe *)pcb - 1;
 	td->td_pcb = pcb;
 }
--- sys/sparc64/sparc64/trap.c.orig	Thu Aug  5 09:42:43 2004
+++ sys/sparc64/sparc64/trap.c	Fri Sep 24 10:09:06 2004
@@ -559,7 +559,8 @@ syscall(struct trapframe *tf)
 	narg = callp->sy_narg & SYF_ARGMASK;
 
 	if (narg <= regcnt) {
-		argp = &tf->tf_out[reg];
+		argp = args;
+		bcopy(&tf->tf_out[reg], args, sizeof(args[0]) * regcnt);
 		error = 0;
 	} else {
 		KASSERT(narg <= sizeof(args) / sizeof(args[0]),
--- lib/libpthread/arch/sparc64/include/pthread_md.h.orig	Thu Aug  5 09:29:06 2004
+++ lib/libpthread/arch/sparc64/include/pthread_md.h	Thu Sep 30 14:16:50 2004
@@ -232,6 +232,9 @@ _thread_switch(struct kcb *kcb, struct t
 
 	_tcb_set(kcb, tcb);
 	mc = &tcb->tcb_tmbx.tm_context.uc_mcontext;
+
+	/* XXX Setup new tcb_tp pointer in machine context */
+	mc->mc_global[6] = (uint64_t)_tp;
 	if (_libkse_debug == 0) {
 		tcb->tcb_tmbx.tm_lwp = kcb->kcb_kmbx.km_lwp;
 		if (setmbox)
--- lib/libpthread/arch/sparc64/sparc64/thr_getcontext.S.orig	Mon Aug  9 12:20:25 2004
+++ lib/libpthread/arch/sparc64/sparc64/thr_getcontext.S	Thu Sep 30 14:18:53 2004
@@ -27,24 +27,66 @@
 #include <machine/asm.h>
 __FBSDID("$FreeBSD: src/lib/libpthread/arch/sparc64/sparc64/thr_getcontext.S,v 1.3 2003/10/09 20:52:17 deischen Exp $");
 
+#include <machine/fsr.h>
+#include <machine/tstate.h>
+
 #include "assym.s"
 
+/*
+ * int thr_getcontext(mcontext_t *mcp);
+ *
+ * Returns -1 if there is an error, 0 no errors; 1 upon return
+ * from a setcontext().
+ */
 	.weak	CNAME(_thr_getcontext)
 	.set	CNAME(_thr_getcontext),CNAME(__thr_getcontext)
 ENTRY(__thr_getcontext)
 	add	%o7, 8, %o1
 	add	%o1, 4, %o2
+	stx	%g6, [%o0 + MC_GLOBAL + (6 * 8)] /* Current tcb_tp pointer */
+	stx	%g7, [%o0 + MC_GLOBAL + (7 * 8)] /* XXX %g7 not used? */
 	stx	%sp, [%o0 + MC_OUT + (6 * 8)]
 	stx	%o1, [%o0 + MC_TPC]
 	stx	%o2, [%o0 + MC_TNPC]
-	mov	MC_VALID_FLAGS, %l0		/* Validate the context. */
-	stx	%l0, [%o0 + MC_FLAGS]
-	mov	1, %l0
-	stx	%l0, [%o0 + MC_OUT + (0 * 8)]	/* return 1 when resumed */
-	retl
-	mov	0, %o0				/* return 0 */
+	rd	%y, %g1				/* XXX %y deprecated */
+	stx	%g1, [%o0 + MC_Y]
+	rd	%fprs, %g1
+	stx	%g1, [%o0 + MC_FPRS]
+	wr	%g0, FPRS_FEF, %fprs		/* Enable FPU */
+	stx	%fsr, [%o0 + MC_FSR]		/* Store FPU Status Register */
+	rd	%gsr, %g2
+	stx	%g2, [%o0 + MC_GSR]		/* Store %gsr */
+	wr	%g1, 0, %fprs			/* Restore %fprs */
+	andcc	%g1, FPRS_FEF, %g0		/* FPU enabled? */
+	bz,a,pt	%xcc, 1f
+	 nop
+	std	%f0,  [%o0 + MC_FP + (0 * 64)]	/* Store FPU registers */
+	std	%f16, [%o0 + MC_FP + (1 * 64)]
+	std	%f32, [%o0 + MC_FP + (2 * 64)]
+	std	%f48, [%o0 + MC_FP + (3 * 64)]
+
+1:
+	set	(TSTATE_IE | TSTATE_PEF), %g1
+	stx	%g1, [%o0 + MC_TSTATE]		/* XXX Emulated %tstate */
+	mov	T_RESERVED, %g1
+	stx	%g1, [%o0 + MC_TYPE]		/* XXX Emulated trap type */
+	clr	[%o0 + MC_WSTATE]		/* XXX Emulated %wstate */
+
+	mov	MC_VALID_FLAGS, %o3		/* Validate the context. */
+	stx	%o3, [%o0 + MC_FLAGS]
+	mov	1, %o3
+	stx	%o3, [%o0 + MC_OUT + (0 * 8)]	/* return 1 when resumed */
+	jmpl	%o7 + 8, %g0
+	 clr	%o0				/* return 0 */
 END(__thr_getcontext)
 
+/*
+ * int thr_setcontext(mcontext_t *mcp, intptr_t val, intptr_t *loc)
+ *
+ * Restores the context in mcp.
+ *
+ * Returns 0 if there are no errors; -1 otherwise
+ */
 	.weak	CNAME(_thr_setcontext)
 	.set	CNAME(_thr_setcontext),CNAME(__thr_setcontext)
 ENTRY(__thr_setcontext)
@@ -53,28 +95,59 @@ ENTRY(__thr_setcontext)
 	mov	%i0, %l0
 	mov	%i1, %l1
 	mov	%i2, %l2
+	ldx	[%l0 + MC_FLAGS], %l5
+	cmp	%l5, MC_VALID_FLAGS
+	be,pt	%icc, 2f			/* XXX Checking lower 32bit */
+	 nop
+	return	%i7 + 8
+	 mov	-1, %i0				/* Return -1 */
+2:
 	ldx	[%l0 + MC_GLOBAL + (1 * 8)], %g1
 	ldx	[%l0 + MC_GLOBAL + (2 * 8)], %g2
 	ldx	[%l0 + MC_GLOBAL + (3 * 8)], %g3
 	ldx	[%l0 + MC_GLOBAL + (4 * 8)], %g4
 	ldx	[%l0 + MC_GLOBAL + (5 * 8)], %g5
-	ldx	[%l0 + MC_GLOBAL + (6 * 8)], %g6
-	ldx	[%l0 + MC_GLOBAL + (7 * 8)], %g7
-	ldx	[%l0 + MC_OUT + (0 * 8)], %i0
+	ldx	[%l0 + MC_GLOBAL + (6 * 8)], %g6 /* Current tcb_tp pointer */
+	ldx	[%l0 + MC_GLOBAL + (7 * 8)], %g7 /* XXX %g7 not used? */
+	ldx	[%l0 + MC_OUT + (0 * 8)], %i0	/* Return value */
 	ldx	[%l0 + MC_OUT + (1 * 8)], %i1
 	ldx     [%l0 + MC_OUT + (2 * 8)], %i2
 	ldx	[%l0 + MC_OUT + (3 * 8)], %i3
 	ldx	[%l0 + MC_OUT + (4 * 8)], %i4
 	ldx	[%l0 + MC_OUT + (5 * 8)], %i5
-	ldx	[%l0 + MC_OUT + (6 * 8)], %i6
+	ldx	[%l0 + MC_OUT + (6 * 8)], %fp	/* %fp */
 	ldx	[%l0 + MC_OUT + (7 * 8)], %i7
+	ldx	[%l0 + MC_FPRS], %l5
+	ldx	[%l0 + MC_GSR], %l4
+	ldx	[%l0 + MC_Y], %l3
+	wr	%g0, FPRS_FEF, %fprs		/* Enable FPU */
+	andcc	%l5, FPRS_FEF, %g0		/* FPU has been enabled? */
+	bz,a,pt	%xcc, 3f
+	 nop
+	ldd	[%l0 + MC_FP + (0 * 64)], %f0	/* Restore FPU registers */
+	ldd	[%l0 + MC_FP + (1 * 64)], %f16
+	ldd	[%l0 + MC_FP + (2 * 64)], %f32
+	ldd	[%l0 + MC_FP + (3 * 64)], %f48
+3:
+	ldx	[%l0 + MC_FSR], %fsr		/* Restore %fsr */
+	wr	%l4, 0, %gsr			/* Restore %gsr */
+	wr	%g0, 0, %fprs			/* Disable FPU */
+	wr	%l3, 0, %y			/* XXX %y deprecated */
+	wr	%l5, 0, %fprs			/* Restore %fprs */
 	ldx	[%l0 + MC_TPC], %l4
 	ldx	[%l0 + MC_TNPC], %l3
-	brz	%l2, 1f
-	nop
-	stx	%l1, [%l2]
-1:	jmpl	%l3, %g0
-	return	%l4
+	ldx	[%l0 + MC_TSTATE], %l5
+	brnz,a,pn	%l2, 1f
+	 stx	%l1, [%l2]			/* Set mbox */
+1:	
+	srlx	%l5, TSTATE_CCR_SHIFT, %l1
+	and	%l1, CCR_MASK, %l1
+	srlx	%l5, TSTATE_ASI_SHIFT, %l2
+	and	%l2, ASI_MASK, %l2
+	wr	%l2, 0, %asi
+	wr	%l1, 0, %ccr
+	jmpl	%l4, %g0
+	 return	%l3
 END(__thr_setcontext)
 
 ENTRY(_sparc64_enter_uts)
@@ -83,5 +156,5 @@ ENTRY(_sparc64_enter_uts)
 	add	%i2, %i3, %i2
 	sub	%i2, SPOFF + CCFSZ, %sp
 	jmpl	%i0, %g0
-	mov	%i1, %o0
+	 mov	%i1, %o0
 END(_sparc64_enter_uts)
--- lib/libpthread/arch/sparc64/sparc64/assym.s.orig	Thu Oct  9 18:48:09 2003
+++ lib/libpthread/arch/sparc64/sparc64/assym.s	Thu Sep 30 13:26:09 2004
@@ -11,5 +11,22 @@
 #define	MC_VALID_FLAGS	0x1
 #define	MC_GLOBAL	0x0
 #define	MC_OUT		0x40
-#define	MC_TPC		0xc8
+#define	MC_LOCAL	0x80
+#define	MC_IN		0xc0
+#define MC_FP		0x100
+#define MC_SIZEOF	512
+
+#define	MC_FPRS		0x80
+#define	MC_FSR		0x88
+#define	MC_GSR		0x90
 #define	MC_TNPC		0xc0
+#define	MC_TPC		0xc8
+#define MC_TSTATE	0xd0
+#define MC_TYPE		0xd8
+#define MC_Y		0xe0
+#define MC_WSTATE	0xe8
+
+#define CCR_MASK	0xff
+#define ASI_MASK	0xff
+#define T_SYSCALL	48
+#define T_RESERVED	0
