1*433d6423SLionel Sambuc #include <sys/cdefs.h>
2*433d6423SLionel Sambuc #include <namespace.h>
3*433d6423SLionel Sambuc #include <lib.h>
4*433d6423SLionel Sambuc #include <machine/stackframe.h>
5*433d6423SLionel Sambuc #include <sys/cdefs.h>
6*433d6423SLionel Sambuc #include <ucontext.h>
7*433d6423SLionel Sambuc #include <signal.h>
8*433d6423SLionel Sambuc #include <stdarg.h>
9*433d6423SLionel Sambuc #include <stdlib.h>
10*433d6423SLionel Sambuc #include <unistd.h>
11*433d6423SLionel Sambuc #include <stdint.h>
12*433d6423SLionel Sambuc #include <stdio.h>
13*433d6423SLionel Sambuc
14*433d6423SLionel Sambuc void ctx_start(void (*)(void), int, ...);
15*433d6423SLionel Sambuc
16*433d6423SLionel Sambuc /*===========================================================================*
17*433d6423SLionel Sambuc * setuctx *
18*433d6423SLionel Sambuc *===========================================================================*/
setuctx(const ucontext_t * ucp)19*433d6423SLionel Sambuc int setuctx(const ucontext_t *ucp)
20*433d6423SLionel Sambuc {
21*433d6423SLionel Sambuc int r;
22*433d6423SLionel Sambuc
23*433d6423SLionel Sambuc if (ucp == NULL) {
24*433d6423SLionel Sambuc errno = EFAULT;
25*433d6423SLionel Sambuc return(-1);
26*433d6423SLionel Sambuc }
27*433d6423SLionel Sambuc
28*433d6423SLionel Sambuc if (!(ucp->uc_flags & _UC_IGNSIGM)) {
29*433d6423SLionel Sambuc /* Set signal mask */
30*433d6423SLionel Sambuc if ((r = sigprocmask(SIG_SETMASK, &ucp->uc_sigmask, NULL)) == -1)
31*433d6423SLionel Sambuc return(r);
32*433d6423SLionel Sambuc }
33*433d6423SLionel Sambuc
34*433d6423SLionel Sambuc if (!(ucp->uc_flags & _UC_IGNFPU)) {
35*433d6423SLionel Sambuc if ((r = setmcontext(&(ucp->uc_mcontext))) == -1)
36*433d6423SLionel Sambuc return(r);
37*433d6423SLionel Sambuc }
38*433d6423SLionel Sambuc
39*433d6423SLionel Sambuc return(0);
40*433d6423SLionel Sambuc }
41*433d6423SLionel Sambuc
42*433d6423SLionel Sambuc
43*433d6423SLionel Sambuc /*===========================================================================*
44*433d6423SLionel Sambuc * getuctx *
45*433d6423SLionel Sambuc *===========================================================================*/
getuctx(ucontext_t * ucp)46*433d6423SLionel Sambuc int getuctx(ucontext_t *ucp)
47*433d6423SLionel Sambuc {
48*433d6423SLionel Sambuc int r;
49*433d6423SLionel Sambuc
50*433d6423SLionel Sambuc if (ucp == NULL) {
51*433d6423SLionel Sambuc errno = EFAULT;
52*433d6423SLionel Sambuc return(-1);
53*433d6423SLionel Sambuc }
54*433d6423SLionel Sambuc
55*433d6423SLionel Sambuc if (!(ucp->uc_flags & _UC_IGNSIGM)) {
56*433d6423SLionel Sambuc /* Get signal mask */
57*433d6423SLionel Sambuc if ((r = sigprocmask(0, NULL, &ucp->uc_sigmask)) == -1)
58*433d6423SLionel Sambuc return(r);
59*433d6423SLionel Sambuc }
60*433d6423SLionel Sambuc
61*433d6423SLionel Sambuc if (!(ucp->uc_flags & _UC_IGNFPU)) {
62*433d6423SLionel Sambuc if ((r = getmcontext(&(ucp->uc_mcontext))) != 0)
63*433d6423SLionel Sambuc return(r);
64*433d6423SLionel Sambuc }
65*433d6423SLionel Sambuc
66*433d6423SLionel Sambuc return(0);
67*433d6423SLionel Sambuc }
68*433d6423SLionel Sambuc
69*433d6423SLionel Sambuc
70*433d6423SLionel Sambuc /*===========================================================================*
71*433d6423SLionel Sambuc * makecontext *
72*433d6423SLionel Sambuc *===========================================================================*/
makecontext(ucontext_t * ucp,void (* func)(void),int argc,...)73*433d6423SLionel Sambuc void makecontext(ucontext_t *ucp, void (*func)(void), int argc, ...)
74*433d6423SLionel Sambuc {
75*433d6423SLionel Sambuc va_list ap;
76*433d6423SLionel Sambuc unsigned int *stack_top;
77*433d6423SLionel Sambuc
78*433d6423SLionel Sambuc /* There are a number of situations that are erroneous, but we can't actually
79*433d6423SLionel Sambuc tell the caller something is wrong, because this is a void function.
80*433d6423SLionel Sambuc Instead, mcontext_t contains a magic field that has to be set
81*433d6423SLionel Sambuc properly before it can be used. */
82*433d6423SLionel Sambuc if (ucp == NULL) {
83*433d6423SLionel Sambuc return;
84*433d6423SLionel Sambuc } else if ((ucp->uc_stack.ss_sp == NULL) ||
85*433d6423SLionel Sambuc (ucp->uc_stack.ss_size < MINSIGSTKSZ)) {
86*433d6423SLionel Sambuc ucp->uc_mcontext.mc_magic = 0;
87*433d6423SLionel Sambuc _UC_MACHINE_SET_STACK(ucp, 0);
88*433d6423SLionel Sambuc return;
89*433d6423SLionel Sambuc }
90*433d6423SLionel Sambuc
91*433d6423SLionel Sambuc if (ucp->uc_mcontext.mc_magic == MCF_MAGIC) {
92*433d6423SLionel Sambuc #if defined(__i386__)
93*433d6423SLionel Sambuc /* The caller provides a pointer to a stack that we can use to run our
94*433d6423SLionel Sambuc context on. When the context starts, control is given to a wrapped
95*433d6423SLionel Sambuc start routine, which calls a function and cleans up the stack
96*433d6423SLionel Sambuc afterwards. The wrapper needs the address of that function on the
97*433d6423SLionel Sambuc stack.
98*433d6423SLionel Sambuc The stack will be prepared as follows:
99*433d6423SLionel Sambuc func() - start routine
100*433d6423SLionel Sambuc arg1 - first argument
101*433d6423SLionel Sambuc ...
102*433d6423SLionel Sambuc argn - last argument
103*433d6423SLionel Sambuc ucp - context, esp points here when `func' returns
104*433d6423SLionel Sambuc _ctx_start pops the address of `func' from the stack and calls it.
105*433d6423SLionel Sambuc The stack will then be setup with all arguments for `func'. When
106*433d6423SLionel Sambuc `func' returns, _ctx_start cleans up the stack such that ucp is at
107*433d6423SLionel Sambuc the top of the stack, ready to be used by resumecontext.
108*433d6423SLionel Sambuc Resumecontext, in turn, checks whether another context is ready to
109*433d6423SLionel Sambuc be executed (i.e., uc_link != NULL) or exit(2)s the process. */
110*433d6423SLionel Sambuc
111*433d6423SLionel Sambuc /* Find the top of the stack from which we grow downwards. */
112*433d6423SLionel Sambuc stack_top = (unsigned int *) ((uintptr_t ) ucp->uc_stack.ss_sp +
113*433d6423SLionel Sambuc ucp->uc_stack.ss_size);
114*433d6423SLionel Sambuc
115*433d6423SLionel Sambuc /* Align the arguments to 16 bytes (we might lose a few bytes of stack
116*433d6423SLionel Sambuc space here).*/
117*433d6423SLionel Sambuc stack_top = (unsigned int *) ((uintptr_t) stack_top & ~0xf);
118*433d6423SLionel Sambuc
119*433d6423SLionel Sambuc /* Make room for 'func', the `func' routine arguments, and ucp. */
120*433d6423SLionel Sambuc stack_top -= (1 + argc + 1);
121*433d6423SLionel Sambuc
122*433d6423SLionel Sambuc /* Adjust the machine context to point to the top of this stack and the
123*433d6423SLionel Sambuc program counter to the context start wrapper. */
124*433d6423SLionel Sambuc _UC_MACHINE_SET_EBP(ucp, 0); /* Clear frame pointer */
125*433d6423SLionel Sambuc _UC_MACHINE_SET_STACK(ucp, (reg_t) stack_top);
126*433d6423SLionel Sambuc _UC_MACHINE_SET_PC(ucp, (reg_t) ctx_start);
127*433d6423SLionel Sambuc
128*433d6423SLionel Sambuc *stack_top++ = (uintptr_t) func;
129*433d6423SLionel Sambuc
130*433d6423SLionel Sambuc /* Copy arguments to the stack. */
131*433d6423SLionel Sambuc va_start(ap, argc);
132*433d6423SLionel Sambuc while (argc-- > 0) {
133*433d6423SLionel Sambuc *stack_top++ = va_arg(ap, uintptr_t);
134*433d6423SLionel Sambuc }
135*433d6423SLionel Sambuc va_end(ap);
136*433d6423SLionel Sambuc
137*433d6423SLionel Sambuc /* Store ucp on the stack */
138*433d6423SLionel Sambuc *stack_top = (uintptr_t) ucp;
139*433d6423SLionel Sambuc
140*433d6423SLionel Sambuc /* Set ESI to point to the base of the stack where ucp is stored, so
141*433d6423SLionel Sambuc that the wrapper function knows how to clean up the stack after
142*433d6423SLionel Sambuc calling `func' (i.e., how to adjust ESP). */
143*433d6423SLionel Sambuc _UC_MACHINE_SET_ESI(ucp, (reg_t) stack_top);
144*433d6423SLionel Sambuc
145*433d6423SLionel Sambuc
146*433d6423SLionel Sambuc /* If we ran out of stack space, invalidate stack pointer. Eventually,
147*433d6423SLionel Sambuc swapcontext will choke on this and return ENOMEM. */
148*433d6423SLionel Sambuc if (stack_top == ucp->uc_stack.ss_sp) {
149*433d6423SLionel Sambuc _UC_MACHINE_SET_STACK(ucp, 0);
150*433d6423SLionel Sambuc }
151*433d6423SLionel Sambuc #elif defined(__arm__)
152*433d6423SLionel Sambuc /* The caller provides a pointer to a stack that we can use to run our
153*433d6423SLionel Sambuc context on. When the context starts, control is given to the
154*433d6423SLionel Sambuc requested function. When the function finishes, it returns to the
155*433d6423SLionel Sambuc _ctx_start wrapper that calls resumecontext (after setting up
156*433d6423SLionel Sambuc resumecontext's parameter).
157*433d6423SLionel Sambuc
158*433d6423SLionel Sambuc The first four arguments for the function will be passed in
159*433d6423SLionel Sambuc regs r0-r3 as specified by the ABI, and the rest will go on
160*433d6423SLionel Sambuc the stack. The ucp is saved in r4 so that we can
161*433d6423SLionel Sambuc eventually pass it to resumecontext. The r4 register is
162*433d6423SLionel Sambuc callee-preserved, so the ucp will remain valid in r4 when
163*433d6423SLionel Sambuc _ctx_start runs. _ctx_start will move the ucp from r4 into
164*433d6423SLionel Sambuc r0, so that the ucp is the first paramater for resumecontext.
165*433d6423SLionel Sambuc Then, _ctx_start will call resumecontext. Resumecontext, in turn,
166*433d6423SLionel Sambuc checks whether another context is ready to be executed
167*433d6423SLionel Sambuc (i.e., uc_link != NULL) or exit(2)s the process. */
168*433d6423SLionel Sambuc
169*433d6423SLionel Sambuc /* Find the top of the stack from which we grow downwards. */
170*433d6423SLionel Sambuc stack_top = (unsigned int *) ((uintptr_t ) ucp->uc_stack.ss_sp +
171*433d6423SLionel Sambuc ucp->uc_stack.ss_size);
172*433d6423SLionel Sambuc
173*433d6423SLionel Sambuc /* Align the arguments to 16 bytes (we might lose a few bytes of stack
174*433d6423SLionel Sambuc space here).*/
175*433d6423SLionel Sambuc stack_top = (unsigned int *) ((uintptr_t) stack_top & ~0xf);
176*433d6423SLionel Sambuc
177*433d6423SLionel Sambuc /* Make room for `func' routine arguments that don't fit in r0-r3 */
178*433d6423SLionel Sambuc if (argc > 4)
179*433d6423SLionel Sambuc stack_top -= argc - 4;
180*433d6423SLionel Sambuc
181*433d6423SLionel Sambuc /* Adjust the machine context to point to the top of this stack and the
182*433d6423SLionel Sambuc program counter to the 'func' entry point. Set lr to ctx_start, so
183*433d6423SLionel Sambuc ctx_start runs after 'func'. Save ucp in r4 */
184*433d6423SLionel Sambuc _UC_MACHINE_SET_FP(ucp, 0); /* Clear frame pointer */
185*433d6423SLionel Sambuc _UC_MACHINE_SET_STACK(ucp, (reg_t) stack_top);
186*433d6423SLionel Sambuc _UC_MACHINE_SET_PC(ucp, (reg_t) func);
187*433d6423SLionel Sambuc _UC_MACHINE_SET_LR(ucp, (reg_t) ctx_start);
188*433d6423SLionel Sambuc _UC_MACHINE_SET_R4(ucp, (reg_t) ucp);
189*433d6423SLionel Sambuc
190*433d6423SLionel Sambuc /* Copy arguments to r0-r3 and stack. */
191*433d6423SLionel Sambuc va_start(ap, argc);
192*433d6423SLionel Sambuc /* Pass up to four arguments in registers. */
193*433d6423SLionel Sambuc if (argc-- > 0)
194*433d6423SLionel Sambuc _UC_MACHINE_SET_R0(ucp, va_arg(ap, uintptr_t));
195*433d6423SLionel Sambuc if (argc-- > 0)
196*433d6423SLionel Sambuc _UC_MACHINE_SET_R1(ucp, va_arg(ap, uintptr_t));
197*433d6423SLionel Sambuc if (argc-- > 0)
198*433d6423SLionel Sambuc _UC_MACHINE_SET_R2(ucp, va_arg(ap, uintptr_t));
199*433d6423SLionel Sambuc if (argc-- > 0)
200*433d6423SLionel Sambuc _UC_MACHINE_SET_R3(ucp, va_arg(ap, uintptr_t));
201*433d6423SLionel Sambuc /* Pass the rest on the stack. */
202*433d6423SLionel Sambuc while (argc-- > 0) {
203*433d6423SLionel Sambuc *stack_top++ = va_arg(ap, uintptr_t);
204*433d6423SLionel Sambuc }
205*433d6423SLionel Sambuc va_end(ap);
206*433d6423SLionel Sambuc
207*433d6423SLionel Sambuc /* If we ran out of stack space, invalidate stack pointer. Eventually,
208*433d6423SLionel Sambuc swapcontext will choke on this and return ENOMEM. */
209*433d6423SLionel Sambuc if (stack_top == ucp->uc_stack.ss_sp) {
210*433d6423SLionel Sambuc _UC_MACHINE_SET_STACK(ucp, 0);
211*433d6423SLionel Sambuc }
212*433d6423SLionel Sambuc #else
213*433d6423SLionel Sambuc # error "Unsupported platform"
214*433d6423SLionel Sambuc #endif
215*433d6423SLionel Sambuc }
216*433d6423SLionel Sambuc }
217*433d6423SLionel Sambuc
218*433d6423SLionel Sambuc
219*433d6423SLionel Sambuc /*===========================================================================*
220*433d6423SLionel Sambuc * swapcontext *
221*433d6423SLionel Sambuc *===========================================================================*/
swapcontext(ucontext_t * oucp,const ucontext_t * ucp)222*433d6423SLionel Sambuc int swapcontext(ucontext_t *oucp, const ucontext_t *ucp)
223*433d6423SLionel Sambuc {
224*433d6423SLionel Sambuc int r;
225*433d6423SLionel Sambuc
226*433d6423SLionel Sambuc if ((oucp == NULL) || (ucp == NULL)) {
227*433d6423SLionel Sambuc errno = EFAULT;
228*433d6423SLionel Sambuc return(-1);
229*433d6423SLionel Sambuc }
230*433d6423SLionel Sambuc
231*433d6423SLionel Sambuc if (_UC_MACHINE_STACK(ucp) == 0) {
232*433d6423SLionel Sambuc /* No stack space. Bail out. */
233*433d6423SLionel Sambuc errno = ENOMEM;
234*433d6423SLionel Sambuc return(-1);
235*433d6423SLionel Sambuc }
236*433d6423SLionel Sambuc
237*433d6423SLionel Sambuc oucp->uc_flags &= ~_UC_SWAPPED;
238*433d6423SLionel Sambuc r = getcontext(oucp);
239*433d6423SLionel Sambuc if ((r == 0) && !(oucp->uc_flags & _UC_SWAPPED)) {
240*433d6423SLionel Sambuc oucp->uc_flags |= _UC_SWAPPED;
241*433d6423SLionel Sambuc r = setcontext(ucp);
242*433d6423SLionel Sambuc }
243*433d6423SLionel Sambuc
244*433d6423SLionel Sambuc return(r);
245*433d6423SLionel Sambuc }
246*433d6423SLionel Sambuc
247*433d6423SLionel Sambuc
248*433d6423SLionel Sambuc /*===========================================================================*
249*433d6423SLionel Sambuc * resumecontext *
250*433d6423SLionel Sambuc *===========================================================================*/
251*433d6423SLionel Sambuc __dead
resumecontext(ucontext_t * ucp)252*433d6423SLionel Sambuc void resumecontext(ucontext_t *ucp)
253*433d6423SLionel Sambuc {
254*433d6423SLionel Sambuc if (ucp->uc_link == NULL) exit(0);
255*433d6423SLionel Sambuc
256*433d6423SLionel Sambuc /* Error handling? Where should the error go to? */
257*433d6423SLionel Sambuc (void) setcontext((const ucontext_t *) ucp->uc_link);
258*433d6423SLionel Sambuc
259*433d6423SLionel Sambuc exit(1); /* Never reached */
260*433d6423SLionel Sambuc }
261*433d6423SLionel Sambuc
262