Add getcontext, setcontext, makecontext, and swapcontext. These

functions are defined in SUSv2 and the latest POSIX spec.

Thanks to Bernd Walter <ticso@cicely8.cicely.de> for helping debug my
alpha assembly.

Approved by:	-arch

11 files changed, 1035 insertions, 1 deletions

diff --git a/lib/libc/alpha/gen/Makefile.inc b/lib/libc/alpha/gen/Makefile.inc
index d316fdb..209a55d 100644
--- a/lib/libc/alpha/gen/Makefile.inc
+++ b/lib/libc/alpha/gen/Makefile.inc
@@ -8,6 +8,7 @@ SRCS+=	sigsetjmp.S
 SRCS+=	__divqu.S __divq.S __divlu.S __divl.S
 SRCS+=	__remqu.S __remq.S __remlu.S __reml.S
 SRCS+=	rfork_thread.S
+SRCS+=	_ctx_start.S getcontext.S makecontext.c
 
 CLEANFILES+=	__divqu.S __divq.S __divlu.S __divl.S
 CLEANFILES+=	__remqu.S __remq.S __remlu.S __reml.S
diff --git a/lib/libc/alpha/gen/_ctx_start.S b/lib/libc/alpha/gen/_ctx_start.S
new file mode 100644
index 0000000..f17d1d5
--- /dev/null
+++ b/lib/libc/alpha/gen/_ctx_start.S
@@ -0,0 +1,49 @@
+/*
+ * 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 THE AUTHOR 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.
+ */
+
+/*
+ * $FreeBSD$
+ */
+
+#include <machine/asm.h>
+
+
+/*
+ * A0-A5 are the first 6 arguments to the start routine with the
+ * remaining arguments (if any) placed on the stack.  S0 is the
+ * address of the user-supplied start routine, and S1 is the pointer
+ * to the ucontext.
+ */
+	.set	noreorder
+LEAF(_ctx_start,0)
+	mov	s0, t12
+	jsr	ra, (s0)	/* call start routine; args already set */
+	LDGP(ra)
+	mov	s1, a0		/* load A0 (arg 1) with pointer to ucontext */
+	CALL(_ctx_done)		/* call context completion routine */
+	CALL(abort)		/* should never return from above call */
+	RET
+END(_ctx_start)
diff --git a/lib/libc/alpha/gen/getcontext.S b/lib/libc/alpha/gen/getcontext.S
new file mode 100644
index 0000000..92a15fd
--- /dev/null
+++ b/lib/libc/alpha/gen/getcontext.S
@@ -0,0 +1,377 @@
+/*
+ * 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 THE AUTHOR 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.
+ */
+/*
+ * Copyright (c) 1994, 1995 Carnegie-Mellon University.
+ * All rights reserved.
+ *
+ * Author: Chris G. Demetriou
+ * 
+ * Permission to use, copy, modify and distribute this software and
+ * its documentation is hereby granted, provided that both the copyright
+ * notice and this permission notice appear in all copies of the
+ * software, derivative works or modified versions, and any portions
+ * thereof, and that both notices appear in supporting documentation.
+ * 
+ * CARNEGIE MELLON ALLOWS FREE USE OF THIS SOFTWARE IN ITS "AS IS" 
+ * CONDITION.  CARNEGIE MELLON DISCLAIMS ANY LIABILITY OF ANY KIND 
+ * FOR ANY DAMAGES WHATSOEVER RESULTING FROM THE USE OF THIS SOFTWARE.
+ * 
+ * Carnegie Mellon requests users of this software to return to
+ *
+ *  Software Distribution Coordinator  or  Software.Distribution@CS.CMU.EDU
+ *  School of Computer Science
+ *  Carnegie Mellon University
+ *  Pittsburgh PA 15213-3890
+ *
+ * any improvements or extensions that they make and grant Carnegie the
+ * rights to redistribute these changes.
+ *
+ */
+
+/*
+ * $FreeBSD$
+ */
+
+#include <machine/asm.h>
+
+/* #include <machine/frame.h> */
+#define	FRAME_V0		0
+#define	FRAME_T0		1
+#define	FRAME_T1		2
+#define	FRAME_T2		3
+#define	FRAME_T3		4
+#define	FRAME_T4		5
+#define	FRAME_T5		6
+#define	FRAME_T6		7
+#define	FRAME_T7		8
+#define	FRAME_S0		9
+#define	FRAME_S1		10
+#define	FRAME_S2		11
+#define	FRAME_S3		12
+#define	FRAME_S4		13
+#define	FRAME_S5		14
+#define	FRAME_S6		15
+#define	FRAME_A3		16
+#define	FRAME_A4		17
+#define	FRAME_A5		18
+#define	FRAME_RA		23
+#define	FRAME_T12		24
+#define	FRAME_AT		25
+#define	FRAME_SP		26
+#define FRAME_TRAPARG_A0	28
+#define FRAME_TRAPARG_A1	29
+#define FRAME_TRAPARG_A2	30
+#define	FRAME_PC		(FRAME_TRAPARG_A2 + 1 + 1)
+
+/* #include <machine/reg.h> */
+#define	R_V0	0
+#define	R_T0	1
+#define	R_T1	2
+#define	R_T2	3
+#define	R_T3	4
+#define	R_T4	5
+#define	R_T5	6
+#define	R_T6	7
+#define	R_T7	8
+#define	R_S0	9
+#define	R_S1	10
+#define	R_S2	11
+#define	R_S3	12
+#define	R_S4	13
+#define	R_S5	14
+#define	R_S6	15
+#define	R_A0	16
+#define	R_A1	17
+#define	R_A2	18
+#define	R_A3	19
+#define	R_A4	20
+#define	R_A5	21
+#define	R_T8	22
+#define	R_T9	23
+#define	R_T10	24
+#define	R_T11	25
+#define	R_RA	26
+#define	R_T12	27
+#define	R_SP	30
+#define	R_ZERO	31
+
+/*
+ * XXX - The rev id's are defined in <machine/ucontext.h>
+ */
+#define	UC_FMT_OFFSET	73*8 + 4*4	/* offset to format from ucontext */
+#define	REV0_SIGFRAME	0x0001		/* rev R0 sigcontext format */
+#define	REV0_TRAPFRAME	0x0002		/* rev R0 trapframe format */
+
+/*
+ * int setcontext(const ucontext_t *);
+ *
+ *   The format of the context is verified at the beginning.
+ *   Returns -1 if invalid format or sigprocmask fails.
+ */
+	.set	noreorder
+XLEAF(setcontext, 1)
+LEAF(__setcontext, 1)
+	LDGP(pv)
+	bne	a0, Lsc1			/* argument null? */
+Lscbad:	ldiq	v0, -1				/* return -1 */
+	br	Lscend
+Lsc1:	ldl	t1, UC_FMT_OFFSET(a0)		/* is mcontext valid format? */
+	ldil	t0, REV0_TRAPFRAME
+	cmpeq	t0, t1, t0			/* is it trapframe format? */
+	bne	t0, Lsc_sm			/* if so, get signal mask */
+	ldil	t0, REV0_SIGFRAME
+	cmpeq	t0, t1, t0			/* is it sigcontext format? */
+	beq	t0, Lscbad
+	/* supposedly sigcontext format, check magic number */
+	ldiq	t0, 0xACEDBADE			/* check magic number */
+	ldq	t1, ((R_ZERO + 3) * 8)(a0)	/* magic in mc_regs[R_ZERO] */
+	cmpeq	t0, t1, t0
+	beq	t0, Lscbad
+	/*
+	 * set current signal mask
+	 */
+Lsc_sm:	lda	sp, -16(sp)			/* save some space on stack */
+	stq	ra, 0(sp)			/* save ra */
+	stq	a0, 8(sp)			/* save ptr to ucontext */
+	mov	a0, a1				/* set:	 &ucp->uc_sigmask */
+	mov	zero, a2			/* oset: NULL */
+	ldiq	a0, 3				/* how:	 SIG_SETMASK */
+	CALL(_sigprocmask)			/* set new signal mask */
+	ldq	a0, 8(sp)			/* restore ptr to ucontext */
+	ldq	ra, 0(sp)			/* restore ra */
+	lda	sp, 16(sp)			/* restore stack */
+	bne	v0, Lscbad			/* check for error */
+	/* restore floating point regs first */
+	ldq	t0, ((71 + 3) * 8)(a0)		/* if FP regs not saved, */
+	beq	t0, Lsc2			/*   skip setting FP regs */
+	ldt	$f0,  ((37 + 3) * 8)(a0)	/* restore FP regs using */
+	ldt	$f1,  ((38 + 3) * 8)(a0)	/*   hw name */
+	ldt	$f2,  ((39 + 3) * 8)(a0)
+	ldt	$f3,  ((40 + 3) * 8)(a0)
+	ldt	$f4,  ((41 + 3) * 8)(a0)
+	ldt	$f5,  ((42 + 3) * 8)(a0)
+	ldt	$f6,  ((43 + 3) * 8)(a0)
+	ldt	$f7,  ((44 + 3) * 8)(a0)
+	ldt	$f8,  ((45 + 3) * 8)(a0)
+	ldt	$f9,  ((46 + 3) * 8)(a0)
+	ldt	$f10, ((47 + 3) * 8)(a0)
+	ldt	$f11, ((48 + 3) * 8)(a0)
+	ldt	$f12, ((49 + 3) * 8)(a0)
+	ldt	$f13, ((50 + 3) * 8)(a0)
+	ldt	$f14, ((51 + 3) * 8)(a0)
+	ldt	$f15, ((52 + 3) * 8)(a0)
+	ldt	$f16, ((53 + 3) * 8)(a0)
+	ldt	$f17, ((54 + 3) * 8)(a0)
+	ldt	$f18, ((55 + 3) * 8)(a0)
+	ldt	$f19, ((56 + 3) * 8)(a0)
+	ldt	$f20, ((57 + 3) * 8)(a0)
+	ldt	$f21, ((58 + 3) * 8)(a0)
+	ldt	$f22, ((59 + 3) * 8)(a0)
+	ldt	$f23, ((60 + 3) * 8)(a0)
+	ldt	$f24, ((61 + 3) * 8)(a0)
+	ldt	$f25, ((62 + 3) * 8)(a0)
+	ldt	$f26, ((63 + 3) * 8)(a0)
+	ldt	$f27, ((64 + 3) * 8)(a0)
+	.set noat
+	ldt	$f28, ((65 + 3) * 8)(a0)
+	.set at
+	ldt	$f29, ((66 + 3) * 8)(a0)
+	ldt	$f30, ((67 + 3) * 8)(a0)
+	/* $f31 is hardwired zero */
+	ldt	ft0, ((69 + 3) * 8)(a0)		/* restore FP control reg */
+	mt_fpcr ft0
+Lsc2:	ldil	t0, REV0_SIGFRAME		/* check the context format */
+	ldl	t1, UC_FMT_OFFSET(a0)		/*   again. */
+	cmpeq	t0, t1, t0			/* is it sigcontext format? */
+	bne	t0, Lsc_sc
+	/* trapframe format */
+	ldq	v0, ((FRAME_V0 + 3) * 8)(a0)	/* set v0 */
+	ldq	t0, ((FRAME_T0 + 3) * 8)(a0)	/* set t0-t7 */
+	ldq	t1, ((FRAME_T1 + 3) * 8)(a0)
+	ldq	t2, ((FRAME_T2 + 3) * 8)(a0)
+	ldq	t3, ((FRAME_T3 + 3) * 8)(a0)
+	ldq	t4, ((FRAME_T4 + 3) * 8)(a0)
+	ldq	t5, ((FRAME_T5 + 3) * 8)(a0)
+	ldq	t6, ((FRAME_T6 + 3) * 8)(a0)
+	ldq	t7, ((FRAME_T7 + 3) * 8)(a0)
+	ldq	s0, ((FRAME_S0 + 3) * 8)(a0)	/* set s0-s6 */
+	ldq	s1, ((FRAME_S1 + 3) * 8)(a0)
+	ldq	s2, ((FRAME_S2 + 3) * 8)(a0)
+	ldq	s3, ((FRAME_S3 + 3) * 8)(a0)
+	ldq	s4, ((FRAME_S4 + 3) * 8)(a0)
+	ldq	s5, ((FRAME_S5 + 3) * 8)(a0)
+	ldq	s6, ((FRAME_S6 + 3) * 8)(a0)
+	ldq	a1, ((FRAME_TRAPARG_A1 + 3) * 8)(a0)	/* set a1-a5 */
+	ldq	a2, ((FRAME_TRAPARG_A2 + 3) * 8)(a0)
+	ldq	a3, ((FRAME_A3 + 3) * 8)(a0)
+	ldq	a4, ((FRAME_A4 + 3) * 8)(a0)
+	ldq	a5, ((FRAME_A5 + 3) * 8)(a0)
+	ldq	ra, ((FRAME_RA + 3) * 8)(a0)
+	ldq	sp, ((FRAME_SP + 3) * 8)(a0)
+	.set noat
+	ldq	at_reg, ((FRAME_PC + 3) * 8)(a0) /* PC at time of trap? */
+	.set at
+	ldq	a0, ((FRAME_TRAPARG_A0 + 3) * 8)(a0)	/* restore a0 last */
+	br	Lscend			/* return to PC or RA? */
+Lsc_sc:	/* sigcontext format */
+	ldq	v0, ((R_V0 + 3) * 8)(a0)	/* set v0 */
+	ldq	t0, ((R_T0 + 3) * 8)(a0)	/* set t0-t7 */
+	ldq	t1, ((R_T1 + 3) * 8)(a0)
+	ldq	t2, ((R_T2 + 3) * 8)(a0)
+	ldq	t3, ((R_T3 + 3) * 8)(a0)
+	ldq	t4, ((R_T4 + 3) * 8)(a0)
+	ldq	t5, ((R_T5 + 3) * 8)(a0)
+	ldq	t6, ((R_T6 + 3) * 8)(a0)
+	ldq	t7, ((R_T7 + 3) * 8)(a0)
+	ldq	s0, ((R_S0 + 3) * 8)(a0)	/* set s0-s6 */
+	ldq	s1, ((R_S1 + 3) * 8)(a0)
+	ldq	s2, ((R_S2 + 3) * 8)(a0)
+	ldq	s3, ((R_S3 + 3) * 8)(a0)
+	ldq	s4, ((R_S4 + 3) * 8)(a0)
+	ldq	s5, ((R_S5 + 3) * 8)(a0)
+	ldq	s6, ((R_S6 + 3) * 8)(a0)
+	ldq	a1, ((R_A1 + 3) * 8)(a0)	/* set a1-a5 */
+	ldq	a2, ((R_A2 + 3) * 8)(a0)
+	ldq	a3, ((R_A3 + 3) * 8)(a0)
+	ldq	a4, ((R_A4 + 3) * 8)(a0)
+	ldq	a5, ((R_A5 + 3) * 8)(a0)
+	ldq	ra, ((R_RA + 3) * 8)(a0)
+	ldq	sp, ((R_SP + 3) * 8)(a0)
+	ldq	a0, ((R_A0 + 3) * 8)(a0)	/* restore a0 last */
+Lscend:	RET
+END(__setcontext)
+
+
+/*
+ * int getcontext(ucontext_t *ucp);
+ *
+ *   Always save in trapframe format.  Floating point registers are
+ *   saved but may be optimized away later (see comments below).
+ */
+XLEAF(getcontext, 1)
+LEAF(__getcontext, 1)
+	LDGP(pv)
+	bne	a0, Lgc1			/* argument null? */
+	ldiq	v0, -1				/* return -1 */
+	br	Lgcend
+Lgc1:	ldiq	v0, 0				/* {gs}etcontext returns 0, */
+	stq	v0, ((FRAME_V0 + 3) * 8)(a0)	/*   so save 0 in v0 */
+	stq	t0, ((FRAME_T0 + 3) * 8)(a0)	/* save t0-t7 */
+	stq	t1, ((FRAME_T1 + 3) * 8)(a0)
+	stq	t2, ((FRAME_T2 + 3) * 8)(a0)
+	stq	t3, ((FRAME_T3 + 3) * 8)(a0)
+	stq	t4, ((FRAME_T4 + 3) * 8)(a0)
+	stq	t5, ((FRAME_T5 + 3) * 8)(a0)
+	stq	t6, ((FRAME_T6 + 3) * 8)(a0)
+	stq	t7, ((FRAME_T7 + 3) * 8)(a0)
+	stq	s0, ((FRAME_S0 + 3) * 8)(a0)	/* save s0-s6 */
+	stq	s1, ((FRAME_S1 + 3) * 8)(a0)
+	stq	s2, ((FRAME_S2 + 3) * 8)(a0)
+	stq	s3, ((FRAME_S3 + 3) * 8)(a0)
+	stq	s4, ((FRAME_S4 + 3) * 8)(a0)
+	stq	s5, ((FRAME_S5 + 3) * 8)(a0)
+	stq	s6, ((FRAME_S6 + 3) * 8)(a0)
+	stq	a0, ((FRAME_TRAPARG_A0 + 3) * 8)(a0)	/* save a0-a5 */
+	stq	a1, ((FRAME_TRAPARG_A1 + 3) * 8)(a0)
+	stq	a2, ((FRAME_TRAPARG_A2 + 3) * 8)(a0)
+	stq	a3, ((FRAME_A3 + 3) * 8)(a0)
+	stq	a4, ((FRAME_A4 + 3) * 8)(a0)
+	stq	a5, ((FRAME_A5 + 3) * 8)(a0)
+	stq	ra, ((FRAME_RA + 3) * 8)(a0)
+	stq	sp, ((FRAME_SP + 3) * 8)(a0)
+	ldiq	t0, REV0_TRAPFRAME		/* store trapframe format in */
+	stq	t0, UC_FMT_OFFSET(a0)		/*   ucp->uc-rev */
+	/*
+	 * get current signal mask
+	 *
+	 * XXX - Since a1 is destroyed, does it need to be saved and restored?
+	 */
+	mov	a0, s0				/* save ptr to ucontext */
+	mov	a0, a2				/* oset: &ucp->uc_sigmask */
+	mov	zero, a1			/* set:	 NULL */
+	ldiq	a0, 3				/* how:	 SIG_SETMASK */
+	CALL(_sigprocmask)			/* see what's blocked */
+	mov	s0, a0				/* restore ptr to ucontext */
+	ldq	ra, ((FRAME_RA + 3) * 8)(a0)	/* restore ra */
+	ldq	s0, ((FRAME_S0 + 3) * 8)(a0)	/* restore s0 */
+	beq	v0, Lgc2			/* check for error */
+	ldiq	v0, -1				/* return -1 */
+	br	Lgcend
+Lgc2:
+	/*
+	 * XXX - Do we really need to save floating point registers?
+	 *
+	 * This is an explicit call to get the current context, so
+	 * shouldn't the caller be done with the floating point registers?
+	 * Contexts formed by involuntary switches, such as signal delivery,
+	 * should have floating point registers saved by the kernel.
+	 */
+#if 1
+	stq	zero, ((71 + 3) * 8)(a0)	/* FP regs are not saved */
+#else
+	ldiq	t0, 1				/* say we've used FP,  */
+	stq	t0,  ((71 + 3) * 8)(a0)		/*   mc_ownedfp = 1  */
+	stt	$f0,  ((37 + 3) * 8)(a0)	/* save first register, using */
+	stt	$f1,  ((38 + 3) * 8)(a0)	/*   hw name etc. */
+	stt	$f2,  ((39 + 3) * 8)(a0)
+	stt	$f3,  ((40 + 3) * 8)(a0)
+	stt	$f4,  ((41 + 3) * 8)(a0)
+	stt	$f5,  ((42 + 3) * 8)(a0)
+	stt	$f6,  ((43 + 3) * 8)(a0)
+	stt	$f7,  ((44 + 3) * 8)(a0)
+	stt	$f8,  ((45 + 3) * 8)(a0)
+	stt	$f9,  ((46 + 3) * 8)(a0)
+	stt	$f10, ((47 + 3) * 8)(a0)
+	stt	$f11, ((48 + 3) * 8)(a0)
+	stt	$f12, ((49 + 3) * 8)(a0)
+	stt	$f13, ((50 + 3) * 8)(a0)
+	stt	$f14, ((51 + 3) * 8)(a0)
+	stt	$f15, ((52 + 3) * 8)(a0)
+	stt	$f16, ((53 + 3) * 8)(a0)
+	stt	$f17, ((54 + 3) * 8)(a0)
+	stt	$f18, ((55 + 3) * 8)(a0)
+	stt	$f19, ((56 + 3) * 8)(a0)
+	stt	$f20, ((57 + 3) * 8)(a0)
+	stt	$f21, ((58 + 3) * 8)(a0)
+	stt	$f22, ((59 + 3) * 8)(a0)
+	stt	$f23, ((60 + 3) * 8)(a0)
+	stt	$f24, ((61 + 3) * 8)(a0)
+	stt	$f25, ((62 + 3) * 8)(a0)
+	stt	$f26, ((63 + 3) * 8)(a0)
+	stt	$f27, ((64 + 3) * 8)(a0)
+	.set noat
+	stt	$f28, ((65 + 3) * 8)(a0)
+	.set at
+	stt	$f29, ((66 + 3) * 8)(a0)
+	stt	$f30, ((67 + 3) * 8)(a0)
+	/* $f31 is hardwired zero */
+#endif
+	mf_fpcr ft0				/* get FP control reg */
+	stt	ft0, ((69 + 3) * 8)(a0)		/* and store it in mc_fpcr */
+	stq	zero, ((70 + 3) * 8)(a0)	/* FP software control XXX */
+	mov	zero, v0			/* return zero */
+Lgcend:	RET
+END(__getcontext)
diff --git a/lib/libc/alpha/gen/makecontext.c b/lib/libc/alpha/gen/makecontext.c
new file mode 100644
index 0000000..fa9856d
--- /dev/null
+++ b/lib/libc/alpha/gen/makecontext.c
@@ -0,0 +1,171 @@
+/*
+ * Copyright (c) 2001 Daniel M. 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 THE AUTHOR 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 <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/signal.h>
+
+#include <errno.h>
+#include <stdarg.h>
+#include <ucontext.h>
+#include <unistd.h>
+
+
+/* Prototypes */
+extern void _ctx_start(int argc, ...);
+
+
+__weak_reference(__makecontext, makecontext);
+
+void
+_ctx_done (ucontext_t *ucp)
+{
+	if (ucp->uc_link == NULL)
+		exit(0);
+	else {
+		/*
+		 * Since this context has finished, don't allow it
+		 * to be restarted without being reinitialized (via
+		 * setcontext or swapcontext).
+		 */
+		ucp->uc_mcontext.mc_format = 0;
+
+		/* Set context to next one in link */
+		/* XXX - what to do for error, abort? */
+		setcontext((const ucontext_t *)ucp->uc_link);
+		abort();	/* should never get here */
+	}
+}
+
+void
+__makecontext(ucontext_t *ucp, void (*start)(void), int argc, ...)
+{
+	va_list 	ap;
+	char		*stack_top;
+	intptr_t	*argp;
+	int		i;
+
+	if (ucp == NULL)
+		return;
+	else if ((ucp->uc_stack.ss_sp == NULL) ||
+	    (ucp->uc_stack.ss_size < MINSIGSTKSZ)) {
+		/*
+		 * This should really return -1 with errno set to ENOMEM
+		 * or something, but the spec says that makecontext is
+		 * a void function.   At least make sure that the context
+		 * isn't valid so it can't be used without an error.
+		 */
+		ucp->uc_mcontext.mc_format = 0;
+	}
+	/* XXX - Do we want to sanity check argc? */
+	else if ((argc < 0) || (argc > NCARGS)) {
+		ucp->uc_mcontext.mc_format = 0;
+	}
+	/*
+	 * Make sure the context is valid.  For now, we only allow
+	 * trapframe format contexts to be used for makecontext.
+	 */
+	else if (ucp->uc_mcontext.mc_format == __UC_REV0_SIGFRAME) {
+		/*
+		 * Alpha passes the first 6 parameters in registers and
+		 * remaining parameters on the stack.  Set up the context
+		 * accordingly, with the user start routine in register
+		 * S0, and the context start wrapper (_ctx_start) in the
+		 * program counter and return address.  The context must
+		 * be in trapframe format.
+		 *
+		 * Note: The context start wrapper needs to retrieve the
+		 *       ucontext pointer.  Place this in register S1
+		 *       which must be saved by the callee.
+		 */
+		stack_top = (char *)(ucp->uc_stack.ss_sp +
+		    ucp->uc_stack.ss_size - sizeof(double));
+		stack_top = (char *)ALIGN(stack_top);
+
+		/*
+		 * Adjust top of stack to allow for any additional integer
+		 * arguments beyond 6.
+		 */
+		if (argc > 6)
+			stack_top = stack_top - (sizeof(intptr_t) * (argc - 6));
+
+		argp = (intptr_t *)stack_top;
+
+		va_start(ap, argc);
+		for (i = 0; i < argc; i++) {
+			switch (i) {
+			case 0:	ucp->uc_mcontext.mc_regs[FRAME_TRAPARG_A0] =
+				    (unsigned long)va_arg(ap, intptr_t);
+				break;
+
+			case 1: ucp->uc_mcontext.mc_regs[FRAME_TRAPARG_A1] =
+				    (unsigned long)va_arg(ap, intptr_t);
+				break;
+
+			case 2:	ucp->uc_mcontext.mc_regs[FRAME_TRAPARG_A2] =
+				    (unsigned long)va_arg(ap, intptr_t);
+				break;
+
+			case 3:	ucp->uc_mcontext.mc_regs[FRAME_A3] =
+				    (unsigned long)va_arg(ap, intptr_t);
+				break;
+
+			case 4:	ucp->uc_mcontext.mc_regs[FRAME_A4] =
+				    (unsigned long)va_arg(ap, intptr_t);
+				break;
+
+			case 5:	ucp->uc_mcontext.mc_regs[FRAME_A5] =
+				    (unsigned long)va_arg(ap, intptr_t);
+				break;
+
+			default:
+				*argp = va_arg(ap, intptr_t);
+				argp++;
+				break;
+			}
+		}
+		va_end(ap);
+
+		/*
+		 * The start routine and ucontext are placed in registers
+		 * S0 and S1 respectively.
+		 */
+		ucp->uc_mcontext.mc_regs[FRAME_S0] = (unsigned long)start;
+		ucp->uc_mcontext.mc_regs[FRAME_S1] = (unsigned long)ucp;
+
+		/*
+		 * Set the machine context to point to the top of the stack,
+		 * and the program counter and return address to the context
+		 * start wrapper.
+		 */
+		ucp->uc_mcontext.mc_regs[FRAME_SP] = (unsigned long)stack_top;
+		ucp->uc_mcontext.mc_regs[FRAME_PC] = (unsigned long)_ctx_start;
+		ucp->uc_mcontext.mc_regs[FRAME_RA] = (unsigned long)_ctx_start;
+		ucp->uc_mcontext.mc_regs[FRAME_T12] = (unsigned long)_ctx_start;
+	}
+}
diff --git a/lib/libc/amd64/gen/Makefile.inc b/lib/libc/amd64/gen/Makefile.inc
index 66b0759..f5db83f 100644
--- a/lib/libc/amd64/gen/Makefile.inc
+++ b/lib/libc/amd64/gen/Makefile.inc
@@ -3,3 +3,4 @@
 
 SRCS+=	_setjmp.S alloca.S fabs.S frexp.c infinity.c isinf.c ldexp.c modf.S \
 	rfork_thread.S setjmp.S sigsetjmp.S
+SRCS+=	_ctx_start.S getcontext.S makecontext.c
diff --git a/lib/libc/gen/Makefile.inc b/lib/libc/gen/Makefile.inc
index 3cc848d..167851c 100644
--- a/lib/libc/gen/Makefile.inc
+++ b/lib/libc/gen/Makefile.inc
@@ -27,7 +27,7 @@ SRCS+=  __xuname.c _pthread_stubs.c _rand48.c _spinlock_stub.c _thread_init.c \
 	setproctitle.c \
 	shmat.c shmctl.c shmdt.c shmget.c siginterrupt.c siglist.c signal.c \
 	sigsetops.c sleep.c srand48.c stringlist.c strtofflags.c \
-	sysconf.c sysctl.c sysctlbyname.c sysctlnametomib.c \
+	sysconf.c sysctl.c sysctlbyname.c sysctlnametomib.c swapcontext.c \
 	syslog.c telldir.c termios.c time.c times.c timezone.c ttyname.c \
 	ttyslot.c ualarm.c uname.c unvis.c usleep.c utime.c valloc.c vis.c \
 	wait.c wait3.c waitpid.c
diff --git a/lib/libc/gen/swapcontext.c b/lib/libc/gen/swapcontext.c
new file mode 100644
index 0000000..5434c0f
--- /dev/null
+++ b/lib/libc/gen/swapcontext.c
@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2001 Daniel M. 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 THE AUTHOR 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 <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <errno.h>
+#include <signal.h>
+#include <ucontext.h>
+
+__weak_reference(__swapcontext, swapcontext);
+
+int
+__swapcontext(ucontext_t *oucp, const ucontext_t *ucp)
+{
+	volatile int	swapped;
+	int		ret;
+
+	if (oucp == NULL || ucp == NULL) {
+		errno = EINVAL;
+		ret = -1;
+	}
+	else {
+		swapped = 0;
+		if ((ret = getcontext(oucp) == 0) && (swapped == 0)) {
+			swapped = 1;
+			ret = setcontext(ucp);
+		}
+	}
+	return (ret);
+}
diff --git a/lib/libc/i386/gen/Makefile.inc b/lib/libc/i386/gen/Makefile.inc
index 66b0759..f5db83f 100644
--- a/lib/libc/i386/gen/Makefile.inc
+++ b/lib/libc/i386/gen/Makefile.inc
@@ -3,3 +3,4 @@
 
 SRCS+=	_setjmp.S alloca.S fabs.S frexp.c infinity.c isinf.c ldexp.c modf.S \
 	rfork_thread.S setjmp.S sigsetjmp.S
+SRCS+=	_ctx_start.S getcontext.S makecontext.c
diff --git a/lib/libc/i386/gen/_ctx_start.S b/lib/libc/i386/gen/_ctx_start.S
new file mode 100644
index 0000000..593a570
--- /dev/null
+++ b/lib/libc/i386/gen/_ctx_start.S
@@ -0,0 +1,53 @@
+/*
+ * 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 THE AUTHOR 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.
+ */
+
+/*
+ * $FreeBSD$
+ */
+
+#include <machine/asm.h>
+
+/*
+ * _ctx_start((void *func)(int arg1, ..., argn),
+ *            int arg1, ..., argn, ucontext_t *ucp)
+ *
+ * 0(%esp)		- func
+ * 4(%esp)		- arg1
+ * 8(%esp)		- arg2
+ * ...
+ * (4*n)(%esp)		- argn
+ * (4*(n + 1))(%esp)	- ucp, %ebp setup to point here (base of stack)
+ */
+ENTRY(_ctx_start)
+	popl	%eax		/* get start function */
+	call	*%eax		/* call start function */
+	movl	%ebp, %esp	/*
+				 * setup stack for completion routine;
+				 * ucp is now at top of stack
+				 */
+	call	_ctx_done	/* should never return */
+	call	abort		/* fubar */
+	ret
diff --git a/lib/libc/i386/gen/getcontext.S b/lib/libc/i386/gen/getcontext.S
new file mode 100644
index 0000000..c3feb2a
--- /dev/null
+++ b/lib/libc/i386/gen/getcontext.S
@@ -0,0 +1,168 @@
+/*
+ * 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.
+ */
+
+/*
+ * $FreeBSD$
+ */
+
+#include <machine/asm.h>
+
+/*
+ * Where do we define these?
+ */
+#define	UC_MC_VALID		0x0001	/* __UC_MC_VALID <machine/ucontext.h> */
+#define	UC_FP_VALID		0x0002	/* __UC_FP_VALID <machine/ucontext.h> */
+#define	MC_FP_VALID		UC_MC_VALID | UC_FP_VALID
+#define	UC_MC_OFFSET		16	/* offset to mcontext from ucontext */
+#define	UC_FLAGS_OFFSET		208	/* offset to flags from ucontext */
+#define	MC_FLAGS_OFFSET		192	/* offset to flags from mcontext */
+#define	MC_FP_REGS_OFFSET	80	/* offset to FP regs from mcontext */
+#define	MC_FP_CW_OFFSET		80	/* offset to FP control word */
+
+/*
+ * int setcontext(ucontext_t *ucp);
+ *
+ *   calls sigprocmask(int how, sigset_t *set, sigset_t *oset);
+ *
+ * Returns 0 if there are no errors; -1 otherwise
+ */
+	.weak CNAME(setcontext)
+	.set CNAME(setcontext),CNAME(__setcontext)
+ENTRY(__setcontext)
+	movl	4(%esp), %eax		/* get address of context and sigset */
+	cmpl	$0, %eax		/* check for null pointer */
+	jne	1f
+	movl	$-1, %eax
+	jmp	5f
+1:	testl	$UC_MC_VALID, UC_FLAGS_OFFSET(%eax) /* is context valid? */
+	jnz	2f
+	movl	$-1, %eax		/* bzzzt, invalid context */
+	jmp	5f
+2:	PIC_PROLOGUE
+	pushl	$0			/* oset = NULL */
+	pushl	%eax			/* set = &ucp->uc_sigmask */
+	pushl	$3			/* how = SIG_SETMASK */
+	call	PIC_PLT(CNAME(_sigprocmask))
+	addl	$12, %esp
+	PIC_EPILOGUE
+	testl	%eax, %eax		/* check for error */
+	jnz	5f
+	movl	4(%esp), %edx		/* get address of context */
+	addl	$UC_MC_OFFSET, %edx	/* add offset to mcontext */
+	movl	4(%edx), %gs
+	movl	8(%edx), %fs
+	movl	12(%edx), %es
+	movl	16(%edx), %ds
+	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)
+	testl	$UC_FP_VALID, MC_FLAGS_OFFSET(%edx) /* are FP regs valid? */
+	jz	3f
+	frstor	MC_FP_REGS_OFFSET(%edx) /* restore FP regs */
+	jmp	4f
+3:	fninit
+	fldcw	MC_FP_CW_OFFSET(%edx)
+4:	movl	68(%edx), %eax		/* restore flags register */
+	sahf
+	movl	48(%edx), %eax		/* restore ax, bx, cx, and dx last */
+	movl	36(%edx), %ebx
+	movl	44(%edx), %ecx
+	movl	40(%edx), %edx
+5:	ret
+
+/*
+ * int getcontext(ucontext_t *ucp);
+ *
+ *   calls sigprocmask(int how, sigset_t *set, sigset_t *oset);
+ *
+ * Returns 0 if there are no errors; -1 otherwise
+ */
+	.weak CNAME(getcontext)
+	.set CNAME(getcontext),CNAME(__getcontext)
+ENTRY(__getcontext)
+	movl	4(%esp), %eax		/* get address of context */
+	cmpl	$0, %eax		/* check for null pointer */
+	jne	1f
+	movl	$-1, %eax
+	jmp	2f
+	movl	4(%esp), %eax		/* get address of context and sigset */
+1:	PIC_PROLOGUE
+	pushl	%eax			/* oset = &ucp->uc_sigmask */
+	pushl	$0			/* set = NULL */
+	pushl	$3			/* how = SIG_SETMASK */
+	call	PIC_PLT(CNAME(_sigprocmask))
+	addl	$12, %esp
+	PIC_EPILOGUE
+	testl	%eax, %eax		/* check for error */
+	jnz	2f
+	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)
+	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 */
+	/*
+	 * XXX - Do we really need to save floating point registers?
+	 *
+	 * This is an explicit call to get the current context, so
+	 * shouldn't the caller be done with the floating point registers?
+	 * Contexts formed by involuntary switches, such as signal delivery,
+	 * should have floating point registers saved by the kernel.
+	 *
+	 * As of this writing, the kernel doesn't yet save the FPU state
+	 * on signal delivery, so a setcontext on the interrupted context
+	 * may yield incorrect results regardless.
+	 */
+#if 1
+	fnstcw	MC_FP_CW_OFFSET(%edx)
+	movl	$UC_MC_VALID, MC_FLAGS_OFFSET(%edx) /* mcontext valid, no FP */
+#else
+	fnsave	MC_FP_REGS_OFFSET(%edx) /* save FP regs */
+	movl	$MC_FP_VALID, MC_FLAGS_OFFSET(%edx) /* mcontext and FP valid */
+#endif
+	lahf				/* get eflags */
+	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	40(%edx), %edx		/* restore edx -- is this needed? */
+	xorl	%eax, %eax		/* return 0 */
+2:	ret
diff --git a/lib/libc/i386/gen/makecontext.c b/lib/libc/i386/gen/makecontext.c
new file mode 100644
index 0000000..1334b5a
--- /dev/null
+++ b/lib/libc/i386/gen/makecontext.c
@@ -0,0 +1,159 @@
+/*
+ * Copyright (c) 2001 Daniel M. 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 THE AUTHOR 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 <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/signal.h>
+
+#include <errno.h>
+#include <stdarg.h>
+#include <ucontext.h>
+#include <unistd.h>
+
+/* Prototypes */
+extern void _ctx_start(ucontext_t *, int argc, ...);
+
+
+__weak_reference(__makecontext, makecontext);
+
+void
+_ctx_done (ucontext_t *ucp)
+{
+	if (ucp->uc_link == NULL)
+		exit(0);
+	else {
+		/*
+		 * Since this context has finished, don't allow it
+		 * to be restarted without being reinitialized (via
+		 * setcontext or swapcontext).
+		 */
+		ucp->uc_mcontext.mc_flags = 0;
+
+		/* Set context to next one in link */
+		/* XXX - what to do for error, abort? */
+		setcontext((const ucontext_t *)ucp->uc_link);
+		abort();	/* should never get here */
+	}
+}
+
+void
+__makecontext(ucontext_t *ucp, void (*start)(void), int argc, ...)
+{
+	va_list		ap;
+	char		*stack_top;
+	intptr_t	*argp;
+	int		i;
+
+	if (ucp == NULL)
+		return;
+	else if ((ucp->uc_stack.ss_sp == NULL) ||
+	    (ucp->uc_stack.ss_size < MINSIGSTKSZ)) {
+		/*
+		 * This should really return -1 with errno set to ENOMEM
+		 * or something, but the spec says that makecontext is
+		 * a void function.   At least make sure that the context
+		 * isn't valid so it can't be used without an error.
+		 */
+		ucp->uc_mcontext.mc_flags = 0;
+	}
+	/* XXX - Do we want to sanity check argc? */
+	else if ((argc < 0) || (argc > NCARGS)) {
+		ucp->uc_mcontext.mc_flags = 0;
+	}
+	/* Make sure the context is valid. */
+	else if ((ucp->uc_mcontext.mc_flags & __UC_MC_VALID) != 0) {
+		/*
+		 * Arrange the stack as follows:
+		 *
+		 *	_ctx_start()	- context start wrapper
+		 *	start()		- user start routine
+		 * 	arg1
+		 *	...
+		 *	argn
+		 *	ucp		- this context, %ebp points here
+		 *
+		 * When the context is started, control will return to
+		 * the context start wrapper which will pop the user
+		 * start routine from the top of the stack.  After that,
+		 * the top of the stack will be setup with all arguments
+		 * necessary for calling the start routine.  When the
+		 * start routine returns, the context wrapper then sets
+		 * the stack pointer to %ebp which was setup to point to
+		 * the base of the stack (and where ucp is stored).  It
+		 * will then call _ctx_done() to swap in the next context
+		 * (uc_link != 0) or exit the program (uc_link == 0).
+		 */
+		stack_top = (char *)(ucp->uc_stack.ss_sp +
+		    ucp->uc_stack.ss_size - sizeof(double));
+		stack_top = (char *)ALIGN(stack_top);
+
+		/*
+		 * Adjust top of stack to allow for 3 pointers (return
+		 * address, _ctx_start, and ucp) and argc arguments.
+		 * We allow the arguments to be pointers also.
+		 */
+		stack_top = stack_top - (sizeof(intptr_t) * (3 + argc));
+		argp = (intptr_t *)stack_top;
+
+		/*
+		 * Setup the top of the stack with the user start routine
+		 * followed by all of its aguments and the pointer to the
+		 * ucontext.  We need to leave a spare spot at the top of
+		 * the stack because setcontext will move eip to the top
+		 * of the stack before returning.
+		 */
+		*argp = (intptr_t)_ctx_start;  /* overwritten with same value */
+		argp++;
+		*argp = (intptr_t)start;
+		argp++;
+
+		/* Add all the arguments: */
+		va_start(ap, argc);
+		for (i = 0; i < argc; i++) {
+			*argp = va_arg(ap, intptr_t);
+			argp++;
+		}
+		va_end(ap);
+
+		/* The ucontext is placed at the bottom of the stack. */
+		*argp = (intptr_t)ucp;
+
+		/*
+		 * Set the machine context to point to the top of the
+		 * stack and the program counter to the context start
+		 * wrapper.  Note that setcontext() pushes the return
+		 * address onto the top of the stack, so allow for this
+		 * by adjusting the stack downward 1 slot.  Also set
+		 * %ebp to point to the base of the stack where ucp
+		 * is stored.
+		 */
+		ucp->uc_mcontext.mc_ebp = (int)argp;
+		ucp->uc_mcontext.mc_esp = (int)stack_top + sizeof(caddr_t);
+		ucp->uc_mcontext.mc_eip = (int)_ctx_start;
+	}
+}