1/*
2 * Copyright (c) 2004 Suleiman Souhlal
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 * 1. Redistributions of source code must retain the above copyright
9 *    notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 *    notice, this list of conditions and the following disclaimer in the
12 *    documentation and/or other materials provided with the distribution.
13 *
14 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
15 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
18 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24 * SUCH DAMAGE.
25 */
26
27#include <sys/cdefs.h>
28__FBSDID("$FreeBSD$");
29
30#include <sys/param.h>
31
32#include <stdarg.h>
33#include <stdlib.h>
34#include <unistd.h>
35#include <stdio.h>
36#include <ucontext.h>
37
38__weak_reference(__makecontext, makecontext);
39
40void _ctx_done(ucontext_t *ucp);
41void _ctx_start(void);
42
43void
44_ctx_done(ucontext_t *ucp)
45{
46	if (ucp->uc_link == NULL)
47		exit(0);
48	else {
49		/* invalidate context */
50		ucp->uc_mcontext.mc_len = 0;
51
52		setcontext((const ucontext_t *)ucp->uc_link);
53
54		abort(); /* should never return from above call */
55	}
56}
57
58void
59__makecontext(ucontext_t *ucp, void (*start)(void), int argc, ...)
60{
61	mcontext_t *mc;
62	char *sp;
63	va_list ap;
64	int i, regargs, stackargs;
65
66	/* Sanity checks */
67	if ((ucp == NULL) || (argc < 0) || (argc > NCARGS)
68	    || (ucp->uc_stack.ss_sp == NULL)
69	    || (ucp->uc_stack.ss_size < MINSIGSTKSZ)) {
70		/* invalidate context */
71		ucp->uc_mcontext.mc_len = 0;
72		return;
73	}
74
75	/*
76	 * The stack must have space for the frame pointer, saved
77	 * link register, overflow arguments, and be 16-byte
78	 * aligned.
79	 */
80	stackargs = (argc > 8) ? argc - 8 : 0;
81	sp = (char *) ucp->uc_stack.ss_sp + ucp->uc_stack.ss_size
82		- sizeof(uint32_t)*(stackargs + 2);
83	sp = (char *)((uint32_t)sp & ~0x1f);
84
85	mc = &ucp->uc_mcontext;
86
87	/*
88	 * Up to 8 register args. Assumes all args are 32-bit and
89	 * integer only. Not sure how to cater for floating point,
90	 * although 64-bit args will work if aligned correctly
91	 * in the arg list.
92	 */
93	regargs = (argc > 8) ? 8 : argc;
94	va_start(ap, argc);
95	for (i = 0; i < regargs; i++)
96		mc->mc_gpr[3 + i] = va_arg(ap, uint32_t);
97
98	/*
99	 * Overflow args go onto the stack
100	 */
101	if (argc > 8) {
102		uint32_t *argp;
103
104		/* Skip past frame pointer and saved LR */
105		argp = (uint32_t *)sp + 2;
106
107		for (i = 0; i < stackargs; i++)
108			*argp++ = va_arg(ap, uint32_t);
109	}
110	va_end(ap);
111
112	/*
113	 * Use caller-saved regs 14/15 to hold params that _ctx_start
114	 * will use to invoke the user-supplied func
115	 */
116	mc->mc_srr0 = (uint32_t) _ctx_start;
117	mc->mc_gpr[1] = (uint32_t) sp;		/* new stack pointer */
118	mc->mc_gpr[14] = (uint32_t) start;	/* r14 <- start */
119	mc->mc_gpr[15] = (uint32_t) ucp;	/* r15 <- ucp */
120}
121