Take the same approach for i386 as that for ia64 and amd64. Use

the userland version of [gs]etcontext to switch between a thread
and the UTS scheduler (and back again).  This also fixes a bug
in i386 _thr_setcontext() which wasn't properly restoring the
context.

Reviewed by:	davidxu

9 files changed, 150 insertions, 339 deletions

diff --git a/lib/libkse/arch/i386/Makefile.inc b/lib/libkse/arch/i386/Makefile.inc
index 98e32eb..c5cbe18 100644
--- a/lib/libkse/arch/i386/Makefile.inc
+++ b/lib/libkse/arch/i386/Makefile.inc
@@ -2,4 +2,4 @@
 
 .PATH:	${.CURDIR}/arch/${MACHINE_ARCH}/${MACHINE_ARCH}
 
-SRCS+=	ksd.c thr_enter_uts.S thr_getcontext.S thr_switch.S
+SRCS+=	ksd.c thr_enter_uts.S thr_getcontext.S
diff --git a/lib/libkse/arch/i386/i386/thr_enter_uts.S b/lib/libkse/arch/i386/i386/thr_enter_uts.S
index a617b72..526fc9e 100644
--- a/lib/libkse/arch/i386/i386/thr_enter_uts.S
+++ b/lib/libkse/arch/i386/i386/thr_enter_uts.S
@@ -28,74 +28,18 @@
 #include <machine/asm.h>
 __FBSDID("$FreeBSD$");
 
-/*
- * Where do we define these?
- */
-#define	MC_SIZE			640	/* sizeof mcontext_t */
-#define	UC_MC_OFFSET		16	/* offset to mcontext from ucontext */
-#define	MC_LEN_OFFSET		80	/* offset to mc_len from mcontext */
-#define	MC_FP_CW_OFFSET		96	/* offset to FP control word */
-#define	MC_FPFMT_OFFSET		84	/* offset to mc_fpformat from mcontext */
-#define MC_FPFMT_NODEV		0x10000
-#define	MC_OWNEDFP_OFFSET	88	/* offset to mc_ownedfp from mcontext */
-#define	MC_OWNEDFP_NONE 	0x20000
-#define	KM_STACK_SP_OFFSET	36	/* offset to km_stack.ss_sp */
-#define	KM_STACK_SIZE_OFFSET	40	/* offset to km_stack.ss_sp */
-#define	KM_FUNC_OFFSET		32	/* offset to km_func */
 
 /*
- * int _thread_enter_uts(kse_thr_mailbox *tm, kse_mailbox *km);
- *
- * Returns 0 on success, -1 otherwise.
+ * _i386_enter_uts(struct kse_mailbox *km, kse_func_t uts, void *stack,
+ *     long stacksz);
+ * +4 = km, +8 = uts, +12 = stack, +16 = size
  */
-ENTRY(_thread_enter_uts)
-	movl	4(%esp), %eax		/* get address of context */
-	cmpl	$0, %eax		/* check for null pointer */
-	jne	1f
-	movl	$-1, %eax
-	jmp	2f
-1:	pushl	%edx			/* save value of edx */
-	movl	%eax, %edx		/* get address of context */
-	addl	$UC_MC_OFFSET, %edx	/* add offset to mcontext */
-	/*movl	%gs, 4(%edx)*/		/* we don't touch %gs */
-	movl	%fs, 8(%edx)
-	movl	%es, 12(%edx)
-	movl	%ds, 16(%edx)
-	movl	%edi, 20(%edx)
-	movl	%esi, 24(%edx)
-	movl	%ebp, 28(%edx)
-	movl	%ebx, 36(%edx)
-	movl	$0, 48(%edx)		/* store successful return in eax */
-	popl	%eax			/* get saved value of edx */
-	movl	%eax, 40(%edx)		/* save edx */
-	movl	%ecx, 44(%edx)
-	movl	(%esp), %eax		/* get return address */
-	movl	%eax, 60(%edx)		/* save return address */
-	movl	%ss, 76(%edx)
-	/*
-	 * Don't save floating point registers here.
-	 *
-	 * This is an explicit call to get the current context, so
-	 * the caller is done with the floating point registers.
-	 * Contexts formed by involuntary switches, such as signal delivery,
-	 * have floating point registers saved by the kernel.
-	 */
-	fnstcw	MC_FP_CW_OFFSET(%edx)
-	movl	$MC_OWNEDFP_NONE, MC_OWNEDFP_OFFSET(%edx) /* no FP */
-	movl	$MC_FPFMT_NODEV, MC_FPFMT_OFFSET(%edx) /* no FP */
-	pushfl				/* get eflags */
-	popl	%eax
-	movl	%eax, 68(%edx)		/* store eflags */
-	movl	%esp, %eax		/* setcontext pushes the return  */
-	addl	$4, %eax		/*   address onto the top of the */
-	movl	%eax, 72(%edx)		/*   stack; account for this     */
-	movl	$MC_SIZE, MC_LEN_OFFSET(%edx) /* context is now valid */
-	movl	8(%esp), %edx		/* get address of mailbox */
-	movl	KM_STACK_SP_OFFSET(%edx), %eax	/* get bottom of stack */
-	addl	KM_STACK_SIZE_OFFSET(%edx), %eax /* add length */
-	movl	%eax, %esp		/* switch to the uts's stack */
-	pushl	%edx			/* push the address of the mailbox */
-	pushl	KM_FUNC_OFFSET(%edx)	/* .. the uts can return to itself */
-	pushl	KM_FUNC_OFFSET(%edx)	/* push the address of the uts func */
-2:	ret
-
+ENTRY(_i386_enter_uts)
+	movl	%esp, %edx		/* save stack */
+	movl	12(%edx), %eax		/* get bottom of stack */
+	addl	16(%edx), %eax		/* add length */
+	movl	%eax, %esp		/* switch to uts stack */
+	pushl	4(%edx)			/* push the address of the mailbox */
+	pushl	8(%edx)			/* .. the uts can return to itself */
+	pushl	8(%edx)			/* .. the uts can return to itself */
+	ret
diff --git a/lib/libkse/arch/i386/i386/thr_getcontext.S b/lib/libkse/arch/i386/i386/thr_getcontext.S
index 82066ef..19afd06 100644
--- a/lib/libkse/arch/i386/i386/thr_getcontext.S
+++ b/lib/libkse/arch/i386/i386/thr_getcontext.S
@@ -30,7 +30,6 @@ __FBSDID("$FreeBSD$");
 /*
  * Where do we define these?
  */
-#define	UC_MC_OFFSET		16	/* offset to mcontext from ucontext */
 #define	MC_LEN_OFFSET		80	/* offset to mc_len from mcontext */
 #define	MC_LEN			640	/* mc_len <machine/ucontext.h> */
 #define	MC_FPFMT_OFFSET		84
@@ -45,27 +44,26 @@ __FBSDID("$FreeBSD$");
 #define	MC_FP_CW_OFFSET		96	/* offset to FP control word */
 
 /*
- * int thr_setcontext(ucontext_t *ucp)
+ * int thr_setcontext(mcontext_t *mcp, intptr_t val, intptr_t *loc)
  *
- * Restores the context in ucp.
+ * 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)
-	movl	4(%esp), %eax		/* get address of context and sigset */
-	cmpl	$0, %eax		/* check for null pointer */
+	movl	4(%esp), %edx		/* get address of mcontext */
+	cmpl	$0, %edx		/* check for null pointer */
 	jne	1f
 	movl	$-1, %eax
-	jmp	7f
-1:	addl	$UC_MC_OFFSET, %eax	/* add offset to mcontext */
-	cmpl	$MC_LEN, MC_LEN_OFFSET(%eax)	/* is context valid? */
+	jmp	8f
+1:	cmpl	$MC_LEN, MC_LEN_OFFSET(%edx)	/* is context valid? */
 	je	2f
 	movl	$-1, %eax		/* bzzzt, invalid context */
-	jmp	7f
-/*2:	movl	4(%edx), %gs*/		/* we don't touch %gs */
-2:	movl	8(%edx), %fs
+	jmp	8f
+2:	/*movl	4(%edx), %gs*/		/* we don't touch %gs */
+	movl	8(%edx), %fs
 	movl	12(%edx), %es
 	movl	16(%edx), %ds
 	movl	76(%edx), %ss
@@ -74,12 +72,9 @@ ENTRY(__thr_setcontext)
 	movl	28(%edx), %ebp
 	movl	%esp, %ecx		/* save current stack in ecx */
 	movl	72(%edx), %esp		/* switch to context defined stack */
-	movl	60(%edx), %eax		/* put return address at top of stack */
-	pushl	%eax
-	movl	44(%edx), %eax		/* get ecx from context, */
-	pushl	%eax			/*   push on top of stack */
-	movl	48(%edx), %eax		/* get eax from context, */
-	pushl	%eax			/*   push on top of stack */
+	pushl	60(%edx)		/* push return address on stack */
+	pushl	44(%edx)		/* push ecx on stack */
+	push	48(%edx)		/* push eax on stack */
 	/*
 	 * if (mc_fpowned == MC_OWNEDFP_FPU || mc_fpowned == MC_OWNEDFP_PCB) {
 	 *    if (mc_fpformat == MC_FPFMT_387)
@@ -102,32 +97,36 @@ ENTRY(__thr_setcontext)
 	jmp	6f
 5:	fninit
 	fldcw	MC_FP_CW_OFFSET(%edx)
-6:	pushl	68(%edx)		/* restore flags register */
-	popf
+6:	pushl	68(%edx)		/* push flags register on stack*/
 	movl	36(%edx), %ebx		/* restore ebx and edx */
 	movl	40(%edx), %edx
+	movl	12(%ecx), %eax		/* get 3rd arg (loc) */
+	cmpl	$0, %eax		/* do nothing if loc == null */
+	je	7f
+	movl	8(%ecx), %ecx		/* get 2nd arg (val) */
+	movl	%ecx, (%eax)		/* set loc = val */
+7:	popfl				/* restore flags after test */
 	popl	%eax			/* restore eax and ecx last */
 	popl	%ecx
-7:	ret
+8:	ret
 
 /*
- * int thr_getcontext(ucontext_t *ucp);
+ * int thr_getcontext(mcontext_t *mcp);
  *
- * Returns 0 if there are no errors; -1 otherwise
+ * 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)
-	movl	4(%esp), %eax		/* get address of context */
-	cmpl	$0, %eax		/* check for null pointer */
+	pushl	%edx			/* save edx */
+	movl	8(%esp), %edx		/* get address of mcontext */
+	cmpl	$0, %edx		/* check for null pointer */
 	jne	1f
+	popl	%edx			/* restore edx and stack */
 	movl	$-1, %eax
 	jmp	2f
-	movl	4(%esp), %eax		/* get address of context and sigset */
-1:	pushl	%edx			/* save value of edx */
-	movl	8(%esp), %edx		/* get address of context */
-	addl	$UC_MC_OFFSET, %edx	/* add offset to mcontext */
-	/*movl	%gs, 4(%edx)*/		/* we don't touch %gs */
+1:	/*movl	%gs, 4(%edx)*/		/* we don't touch %gs */
 	movl	%fs, 8(%edx)
 	movl	%es, 12(%edx)
 	movl	%ds, 16(%edx)
@@ -136,7 +135,7 @@ ENTRY(__thr_getcontext)
 	movl	%esi, 24(%edx)
 	movl	%ebp, 28(%edx)
 	movl	%ebx, 36(%edx)
-	movl	$0, 48(%edx)		/* store successful return in eax */
+	movl	$1, 48(%edx)		/* store successful return in eax */
 	popl	%eax			/* get saved value of edx */
 	movl	%eax, 40(%edx)		/* save edx */
 	movl	%ecx, 44(%edx)
diff --git a/lib/libkse/arch/i386/include/pthread_md.h b/lib/libkse/arch/i386/include/pthread_md.h
index ad0dfd6..980680a 100644
--- a/lib/libkse/arch/i386/include/pthread_md.h
+++ b/lib/libkse/arch/i386/include/pthread_md.h
@@ -31,23 +31,14 @@
 #ifndef _PTHREAD_MD_H_
 #define	_PTHREAD_MD_H_
 
-#include <setjmp.h>
+#include <sys/kse.h>
 #include <ucontext.h>
 
-extern int _thr_setcontext(ucontext_t *);
-extern int _thr_getcontext(ucontext_t *);
+extern int _thr_setcontext(mcontext_t *, intptr_t, intptr_t *);
+extern int _thr_getcontext(mcontext_t *);
 
-/*
- * These are needed to ensure an application doesn't attempt to jump
- * between stacks of different threads.  They return the stack of
- * jmp_buf, sigjmp_buf, and ucontext respectively.
- */
-#define	GET_STACK_JB(jb)	((unsigned long)((jb)[0]._jb[2]))
-#define	GET_STACK_SJB(sjb)	((unsigned long)((sjb)[0]._sjb[2]))
-#define	GET_STACK_UC(ucp)	((unsigned long)((ucp)->uc_mcontext.mc_esp))
-
-#define	THR_GETCONTEXT(ucp)	_thr_getcontext(ucp)
-#define	THR_SETCONTEXT(ucp)	_thr_setcontext(ucp)
+#define	THR_GETCONTEXT(ucp)	_thr_getcontext(&(ucp)->uc_mcontext);
+#define	THR_SETCONTEXT(ucp)	_thr_setcontext(&(ucp)->uc_mcontext, NULL, NULL);
 
 #define	THR_ALIGNBYTES	15
 #define	THR_ALIGN(td)	(((unsigned)(td) + THR_ALIGNBYTES) & ~THR_ALIGNBYTES)
@@ -63,4 +54,32 @@ struct ksd {
 	long	size;
 };
 
+extern void _i386_enter_uts(struct kse_mailbox *, kse_func_t, void *, long);
+
+static __inline int
+_thread_enter_uts(struct kse_thr_mailbox *tmbx, struct kse_mailbox *kmbx)
+{
+	int ret;
+
+	ret = _thr_getcontext(&tmbx->tm_context.uc_mcontext);
+	if (ret == 0) {
+		_i386_enter_uts(kmbx, kmbx->km_func,
+		    kmbx->km_stack.ss_sp, kmbx->km_stack.ss_size);
+		/* We should not reach here. */
+		return (-1);
+	}
+	else if (ret < 0)
+		return (-1);
+	return (0);
+}
+
+static __inline int
+_thread_switch(struct kse_thr_mailbox *tmbx, struct kse_thr_mailbox **loc)
+{
+	_thr_setcontext(&tmbx->tm_context.uc_mcontext,
+	    (intptr_t)tmbx, (intptr_t *)loc);
+	/* We should not reach here. */
+	return (-1);
+}
+
 #endif
diff --git a/lib/libpthread/arch/i386/Makefile.inc b/lib/libpthread/arch/i386/Makefile.inc
index 98e32eb..c5cbe18 100644
--- a/lib/libpthread/arch/i386/Makefile.inc
+++ b/lib/libpthread/arch/i386/Makefile.inc
@@ -2,4 +2,4 @@
 
 .PATH:	${.CURDIR}/arch/${MACHINE_ARCH}/${MACHINE_ARCH}
 
-SRCS+=	ksd.c thr_enter_uts.S thr_getcontext.S thr_switch.S
+SRCS+=	ksd.c thr_enter_uts.S thr_getcontext.S
diff --git a/lib/libpthread/arch/i386/i386/thr_enter_uts.S b/lib/libpthread/arch/i386/i386/thr_enter_uts.S
index a617b72..526fc9e 100644
--- a/lib/libpthread/arch/i386/i386/thr_enter_uts.S
+++ b/lib/libpthread/arch/i386/i386/thr_enter_uts.S
@@ -28,74 +28,18 @@
 #include <machine/asm.h>
 __FBSDID("$FreeBSD$");
 
-/*
- * Where do we define these?
- */
-#define	MC_SIZE			640	/* sizeof mcontext_t */
-#define	UC_MC_OFFSET		16	/* offset to mcontext from ucontext */
-#define	MC_LEN_OFFSET		80	/* offset to mc_len from mcontext */
-#define	MC_FP_CW_OFFSET		96	/* offset to FP control word */
-#define	MC_FPFMT_OFFSET		84	/* offset to mc_fpformat from mcontext */
-#define MC_FPFMT_NODEV		0x10000
-#define	MC_OWNEDFP_OFFSET	88	/* offset to mc_ownedfp from mcontext */
-#define	MC_OWNEDFP_NONE 	0x20000
-#define	KM_STACK_SP_OFFSET	36	/* offset to km_stack.ss_sp */
-#define	KM_STACK_SIZE_OFFSET	40	/* offset to km_stack.ss_sp */
-#define	KM_FUNC_OFFSET		32	/* offset to km_func */
 
 /*
- * int _thread_enter_uts(kse_thr_mailbox *tm, kse_mailbox *km);
- *
- * Returns 0 on success, -1 otherwise.
+ * _i386_enter_uts(struct kse_mailbox *km, kse_func_t uts, void *stack,
+ *     long stacksz);
+ * +4 = km, +8 = uts, +12 = stack, +16 = size
  */
-ENTRY(_thread_enter_uts)
-	movl	4(%esp), %eax		/* get address of context */
-	cmpl	$0, %eax		/* check for null pointer */
-	jne	1f
-	movl	$-1, %eax
-	jmp	2f
-1:	pushl	%edx			/* save value of edx */
-	movl	%eax, %edx		/* get address of context */
-	addl	$UC_MC_OFFSET, %edx	/* add offset to mcontext */
-	/*movl	%gs, 4(%edx)*/		/* we don't touch %gs */
-	movl	%fs, 8(%edx)
-	movl	%es, 12(%edx)
-	movl	%ds, 16(%edx)
-	movl	%edi, 20(%edx)
-	movl	%esi, 24(%edx)
-	movl	%ebp, 28(%edx)
-	movl	%ebx, 36(%edx)
-	movl	$0, 48(%edx)		/* store successful return in eax */
-	popl	%eax			/* get saved value of edx */
-	movl	%eax, 40(%edx)		/* save edx */
-	movl	%ecx, 44(%edx)
-	movl	(%esp), %eax		/* get return address */
-	movl	%eax, 60(%edx)		/* save return address */
-	movl	%ss, 76(%edx)
-	/*
-	 * Don't save floating point registers here.
-	 *
-	 * This is an explicit call to get the current context, so
-	 * the caller is done with the floating point registers.
-	 * Contexts formed by involuntary switches, such as signal delivery,
-	 * have floating point registers saved by the kernel.
-	 */
-	fnstcw	MC_FP_CW_OFFSET(%edx)
-	movl	$MC_OWNEDFP_NONE, MC_OWNEDFP_OFFSET(%edx) /* no FP */
-	movl	$MC_FPFMT_NODEV, MC_FPFMT_OFFSET(%edx) /* no FP */
-	pushfl				/* get eflags */
-	popl	%eax
-	movl	%eax, 68(%edx)		/* store eflags */
-	movl	%esp, %eax		/* setcontext pushes the return  */
-	addl	$4, %eax		/*   address onto the top of the */
-	movl	%eax, 72(%edx)		/*   stack; account for this     */
-	movl	$MC_SIZE, MC_LEN_OFFSET(%edx) /* context is now valid */
-	movl	8(%esp), %edx		/* get address of mailbox */
-	movl	KM_STACK_SP_OFFSET(%edx), %eax	/* get bottom of stack */
-	addl	KM_STACK_SIZE_OFFSET(%edx), %eax /* add length */
-	movl	%eax, %esp		/* switch to the uts's stack */
-	pushl	%edx			/* push the address of the mailbox */
-	pushl	KM_FUNC_OFFSET(%edx)	/* .. the uts can return to itself */
-	pushl	KM_FUNC_OFFSET(%edx)	/* push the address of the uts func */
-2:	ret
-
+ENTRY(_i386_enter_uts)
+	movl	%esp, %edx		/* save stack */
+	movl	12(%edx), %eax		/* get bottom of stack */
+	addl	16(%edx), %eax		/* add length */
+	movl	%eax, %esp		/* switch to uts stack */
+	pushl	4(%edx)			/* push the address of the mailbox */
+	pushl	8(%edx)			/* .. the uts can return to itself */
+	pushl	8(%edx)			/* .. the uts can return to itself */
+	ret
diff --git a/lib/libpthread/arch/i386/i386/thr_getcontext.S b/lib/libpthread/arch/i386/i386/thr_getcontext.S
index 82066ef..19afd06 100644
--- a/lib/libpthread/arch/i386/i386/thr_getcontext.S
+++ b/lib/libpthread/arch/i386/i386/thr_getcontext.S
@@ -30,7 +30,6 @@ __FBSDID("$FreeBSD$");
 /*
  * Where do we define these?
  */
-#define	UC_MC_OFFSET		16	/* offset to mcontext from ucontext */
 #define	MC_LEN_OFFSET		80	/* offset to mc_len from mcontext */
 #define	MC_LEN			640	/* mc_len <machine/ucontext.h> */
 #define	MC_FPFMT_OFFSET		84
@@ -45,27 +44,26 @@ __FBSDID("$FreeBSD$");
 #define	MC_FP_CW_OFFSET		96	/* offset to FP control word */
 
 /*
- * int thr_setcontext(ucontext_t *ucp)
+ * int thr_setcontext(mcontext_t *mcp, intptr_t val, intptr_t *loc)
  *
- * Restores the context in ucp.
+ * 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)
-	movl	4(%esp), %eax		/* get address of context and sigset */
-	cmpl	$0, %eax		/* check for null pointer */
+	movl	4(%esp), %edx		/* get address of mcontext */
+	cmpl	$0, %edx		/* check for null pointer */
 	jne	1f
 	movl	$-1, %eax
-	jmp	7f
-1:	addl	$UC_MC_OFFSET, %eax	/* add offset to mcontext */
-	cmpl	$MC_LEN, MC_LEN_OFFSET(%eax)	/* is context valid? */
+	jmp	8f
+1:	cmpl	$MC_LEN, MC_LEN_OFFSET(%edx)	/* is context valid? */
 	je	2f
 	movl	$-1, %eax		/* bzzzt, invalid context */
-	jmp	7f
-/*2:	movl	4(%edx), %gs*/		/* we don't touch %gs */
-2:	movl	8(%edx), %fs
+	jmp	8f
+2:	/*movl	4(%edx), %gs*/		/* we don't touch %gs */
+	movl	8(%edx), %fs
 	movl	12(%edx), %es
 	movl	16(%edx), %ds
 	movl	76(%edx), %ss
@@ -74,12 +72,9 @@ ENTRY(__thr_setcontext)
 	movl	28(%edx), %ebp
 	movl	%esp, %ecx		/* save current stack in ecx */
 	movl	72(%edx), %esp		/* switch to context defined stack */
-	movl	60(%edx), %eax		/* put return address at top of stack */
-	pushl	%eax
-	movl	44(%edx), %eax		/* get ecx from context, */
-	pushl	%eax			/*   push on top of stack */
-	movl	48(%edx), %eax		/* get eax from context, */
-	pushl	%eax			/*   push on top of stack */
+	pushl	60(%edx)		/* push return address on stack */
+	pushl	44(%edx)		/* push ecx on stack */
+	push	48(%edx)		/* push eax on stack */
 	/*
 	 * if (mc_fpowned == MC_OWNEDFP_FPU || mc_fpowned == MC_OWNEDFP_PCB) {
 	 *    if (mc_fpformat == MC_FPFMT_387)
@@ -102,32 +97,36 @@ ENTRY(__thr_setcontext)
 	jmp	6f
 5:	fninit
 	fldcw	MC_FP_CW_OFFSET(%edx)
-6:	pushl	68(%edx)		/* restore flags register */
-	popf
+6:	pushl	68(%edx)		/* push flags register on stack*/
 	movl	36(%edx), %ebx		/* restore ebx and edx */
 	movl	40(%edx), %edx
+	movl	12(%ecx), %eax		/* get 3rd arg (loc) */
+	cmpl	$0, %eax		/* do nothing if loc == null */
+	je	7f
+	movl	8(%ecx), %ecx		/* get 2nd arg (val) */
+	movl	%ecx, (%eax)		/* set loc = val */
+7:	popfl				/* restore flags after test */
 	popl	%eax			/* restore eax and ecx last */
 	popl	%ecx
-7:	ret
+8:	ret
 
 /*
- * int thr_getcontext(ucontext_t *ucp);
+ * int thr_getcontext(mcontext_t *mcp);
  *
- * Returns 0 if there are no errors; -1 otherwise
+ * 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)
-	movl	4(%esp), %eax		/* get address of context */
-	cmpl	$0, %eax		/* check for null pointer */
+	pushl	%edx			/* save edx */
+	movl	8(%esp), %edx		/* get address of mcontext */
+	cmpl	$0, %edx		/* check for null pointer */
 	jne	1f
+	popl	%edx			/* restore edx and stack */
 	movl	$-1, %eax
 	jmp	2f
-	movl	4(%esp), %eax		/* get address of context and sigset */
-1:	pushl	%edx			/* save value of edx */
-	movl	8(%esp), %edx		/* get address of context */
-	addl	$UC_MC_OFFSET, %edx	/* add offset to mcontext */
-	/*movl	%gs, 4(%edx)*/		/* we don't touch %gs */
+1:	/*movl	%gs, 4(%edx)*/		/* we don't touch %gs */
 	movl	%fs, 8(%edx)
 	movl	%es, 12(%edx)
 	movl	%ds, 16(%edx)
@@ -136,7 +135,7 @@ ENTRY(__thr_getcontext)
 	movl	%esi, 24(%edx)
 	movl	%ebp, 28(%edx)
 	movl	%ebx, 36(%edx)
-	movl	$0, 48(%edx)		/* store successful return in eax */
+	movl	$1, 48(%edx)		/* store successful return in eax */
 	popl	%eax			/* get saved value of edx */
 	movl	%eax, 40(%edx)		/* save edx */
 	movl	%ecx, 44(%edx)
diff --git a/lib/libpthread/arch/i386/i386/thr_switch.S b/lib/libpthread/arch/i386/i386/thr_switch.S
deleted file mode 100644
index f9c8b92..0000000
--- a/lib/libpthread/arch/i386/i386/thr_switch.S
+++ /dev/null
@@ -1,113 +0,0 @@
-/*
- * Copyright (c) 2002 Jonathan Mini <mini@freebsd.org>.
- * Copyright (c) 2001 Daniel Eischen <deischen@freebsd.org>.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Neither the name of the author nor the names of its contributors
- *    may be used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY DANIEL EISCHEN AND CONTRIBUTORS ``AS IS'' AND
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
- * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
- * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
- * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
- * SUCH DAMAGE.
- */
-
-#include <machine/asm.h>
-__FBSDID("$FreeBSD$");
-
-/*
- * Where do we define these?
- */
-#define	MC_SIZE			640	/* sizeof mcontext_t */
-#define	UC_MC_OFFSET		16	/* offset to mcontext from ucontext */
-#define	UC_MC_LEN_OFFSET	96	/* offset to mc_len from mcontext */
-#define	MC_FPREGS_OFFSET	96	/* offset to FP regs from mcontext */
-#define	MC_FP_CW_OFFSET		96	/* offset to FP control word */
-#define	MC_FPFMT_OFFSET		84	/* offset to mc_fpformat from mcontext */
-#define MC_FPFMT_NODEV		0x10000
-#define MC_FPFMT_387		0x10001
-#define	MC_FPFMT_XMM		0x10002
-#define	MC_OWNEDFP_OFFSET	88	/* offset to mc_ownedfp from mcontext */
-#define	MC_OWNEDFP_NONE 	0x20000
-#define	MC_OWNEDFP_FPU  	0x20001
-#define	MC_OWNEDFP_PCB  	0x20002
-
-
-/*
- * int _thread_switch(kse_thr_mailbox *td, thread_mailbox **curthreadp);
- *
- * Does not return on success, returns -1 otherwise.
- */
-ENTRY(_thread_switch)
-	movl	4(%esp), %edx		/* get address of thread_mailbox */
-					/* .. ucontext_t is at offset 0 */
-	cmpl	$0, %edx		/* check for null pointer */
-	jne	1f
-	movl	$-1, %eax
-	jmp	9f
-1:	cmpl	$MC_SIZE, UC_MC_LEN_OFFSET(%edx) /* is context valid? */
-	je	2f
-	movl	$-1, %eax		/* bzzzt, invalid context */
-	jmp	9f
-2:	movl	8(%esp), %ecx		/* get address of curthreadp */
-	movl	%edx, %ebx		/* save the pointer for later */
-	/*
-	 * From here on, we don't touch the old stack.
-	 */
-	addl	$UC_MC_OFFSET, %edx	/* add offset to mcontext */
-	/*movl	4(%edx), %gs*/		/* we don't touch %gs */
-	movl	8(%edx), %fs
-	movl	12(%edx), %es
-	movl	16(%edx), %ds
-	movl	76(%edx), %ss
-	movl	20(%edx), %edi
-	movl	24(%edx), %esi
-	movl	28(%edx), %ebp
-	movl	72(%edx), %esp		/* switch to context defined stack */
-	subl	$4, %esp		/* leave space for the return address */
-	movl	60(%edx), %eax		/* put return address at top of stack */
-	movl	%eax, (%esp)
-	cmpl	$MC_OWNEDFP_FPU, MC_OWNEDFP_OFFSET(%edx)
-	je	4f
-	cmpl	$MC_OWNEDFP_PCB, MC_OWNEDFP_OFFSET(%edx)
-	jne	6f
-4:	cmpl	$MC_FPFMT_387, MC_FPFMT_OFFSET(%edx)
-	jne	5f
-	frstor	MC_FPREGS_OFFSET(%edx) /* restore 387 FP regs */
-	jmp	7f
-5:	cmpl	$MC_FPFMT_XMM, MC_FPFMT_OFFSET(%edx)
-	jne	6f
-	fxrstor	MC_FPREGS_OFFSET(%edx) /* restore XMM/SSE FP regs */
-	jmp	7f
-6:	fninit
-	fldcw	MC_FP_CW_OFFSET(%edx)
-7:	movl	48(%edx), %eax		/* restore ax, bx, cx, dx */
-	pushl	68(%edx)		/* flags on stack */
-	pushl	36(%edx)		/* %ebx on stack */
-	pushl	44(%edx)		/* %ecx on stack */
-	movl	40(%edx), %edx		/* %edx */
-	/* 
-	 * all registers are now moved out of mailbox, 
-	 * it's safe to set current thread pointer
-	 */
-	cmpl	$0, %ecx
-	je	8f
-	movl	%ebx,(%ecx)
-8:	popl	%ecx			/* %ecx off stack */
-	popl	%ebx			/* %ebx off stack */
-	popfl				/* flags off stack */
-9:	ret				/* %eip off stack */
-
diff --git a/lib/libpthread/arch/i386/include/pthread_md.h b/lib/libpthread/arch/i386/include/pthread_md.h
index ad0dfd6..980680a 100644
--- a/lib/libpthread/arch/i386/include/pthread_md.h
+++ b/lib/libpthread/arch/i386/include/pthread_md.h
@@ -31,23 +31,14 @@
 #ifndef _PTHREAD_MD_H_
 #define	_PTHREAD_MD_H_
 
-#include <setjmp.h>
+#include <sys/kse.h>
 #include <ucontext.h>
 
-extern int _thr_setcontext(ucontext_t *);
-extern int _thr_getcontext(ucontext_t *);
+extern int _thr_setcontext(mcontext_t *, intptr_t, intptr_t *);
+extern int _thr_getcontext(mcontext_t *);
 
-/*
- * These are needed to ensure an application doesn't attempt to jump
- * between stacks of different threads.  They return the stack of
- * jmp_buf, sigjmp_buf, and ucontext respectively.
- */
-#define	GET_STACK_JB(jb)	((unsigned long)((jb)[0]._jb[2]))
-#define	GET_STACK_SJB(sjb)	((unsigned long)((sjb)[0]._sjb[2]))
-#define	GET_STACK_UC(ucp)	((unsigned long)((ucp)->uc_mcontext.mc_esp))
-
-#define	THR_GETCONTEXT(ucp)	_thr_getcontext(ucp)
-#define	THR_SETCONTEXT(ucp)	_thr_setcontext(ucp)
+#define	THR_GETCONTEXT(ucp)	_thr_getcontext(&(ucp)->uc_mcontext);
+#define	THR_SETCONTEXT(ucp)	_thr_setcontext(&(ucp)->uc_mcontext, NULL, NULL);
 
 #define	THR_ALIGNBYTES	15
 #define	THR_ALIGN(td)	(((unsigned)(td) + THR_ALIGNBYTES) & ~THR_ALIGNBYTES)
@@ -63,4 +54,32 @@ struct ksd {
 	long	size;
 };
 
+extern void _i386_enter_uts(struct kse_mailbox *, kse_func_t, void *, long);
+
+static __inline int
+_thread_enter_uts(struct kse_thr_mailbox *tmbx, struct kse_mailbox *kmbx)
+{
+	int ret;
+
+	ret = _thr_getcontext(&tmbx->tm_context.uc_mcontext);
+	if (ret == 0) {
+		_i386_enter_uts(kmbx, kmbx->km_func,
+		    kmbx->km_stack.ss_sp, kmbx->km_stack.ss_size);
+		/* We should not reach here. */
+		return (-1);
+	}
+	else if (ret < 0)
+		return (-1);
+	return (0);
+}
+
+static __inline int
+_thread_switch(struct kse_thr_mailbox *tmbx, struct kse_thr_mailbox **loc)
+{
+	_thr_setcontext(&tmbx->tm_context.uc_mcontext,
+	    (intptr_t)tmbx, (intptr_t *)loc);
+	/* We should not reach here. */
+	return (-1);
+}
+
 #endif