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

4 files changed, 381 insertions, 0 deletions

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;
+	}
+}