firmware/target/hosted/thread-unix.c

   1 #include <stdlib.h>
   2 #include <stdbool.h>
   3 #include <signal.h>
   4 #include <stdio.h>
   5 #include <setjmp.h>
   6 #include <unistd.h>
   7 #include <pthread.h>
   8 #include <errno.h>
   9 #include "debug.h"
  10
  11 static volatile bool sig_handler_called;
  12 static volatile jmp_buf tramp_buf;
  13 static volatile jmp_buf bootstrap_buf;
  14 static void (*thread_func)(void);
  15 static const int trampoline_sig = SIGUSR1;
  16 static pthread_t main_thread;
  17
  18 static struct ctx {
  19     jmp_buf thread_buf;
  20 } thread_bufs[MAXTHREADS];
  21 static struct ctx* thread_context, *target_context;
  22 static int curr_uc;
  23
  24 static void trampoline(int sig);
  25 static void bootstrap_context(void) __attribute__((noinline));
  26
  27 /* The *_context functions are heavily based on Gnu pth
  28  * http://www.gnu.org/software/pth/
  29  *
  30  * adjusted to work in a multi-thread environment to
  31  * offer a ucontext-like API
  32  */
  33
  34 /*
  35  * VARIANT 2: THE SIGNAL STACK TRICK
  36  *
  37  * This uses sigstack/sigaltstack() and friends and is really the
  38  * most tricky part of Pth. When you understand the following
  39  * stuff you're a good Unix hacker and then you've already
  40  * understood the gory ingredients of Pth.  So, either welcome to
  41  * the club of hackers, or do yourself a favor and skip this ;)
  42  *
  43  * The ingenious fact is that this variant runs really on _all_ POSIX
  44  * compliant systems without special platform kludges.  But be _VERY_
  45  * carefully when you change something in the following code. The slightest
  46  * change or reordering can lead to horribly broken code.  Really every
  47  * function call in the following case is intended to be how it is, doubt
  48  * me...
  49  *
  50  * For more details we strongly recommend you to read the companion
  51  * paper ``Portable Multithreading -- The Signal Stack Trick for
  52  * User-Space Thread Creation'' from Ralf S. Engelschall. A copy of the
  53  * draft of this paper you can find in the file rse-pmt.ps inside the
  54  * GNU Pth distribution.
  55  */
  56
  57 static int make_context(struct ctx *ctx, void (*f)(void), char *sp, size_t stack_size)
  58 {
  59     struct sigaction sa;
  60     struct sigaction osa;
  61     stack_t ss;
  62     stack_t oss;
  63     sigset_t osigs;
  64     sigset_t sigs;
  65
  66     disable_irq();
  67     /*
  68      * Preserve the trampoline_sig signal state, block trampoline_sig,
  69      * and establish our signal handler. The signal will
  70      * later transfer control onto the signal stack.
  71      */
  72     sigemptyset(&sigs);
  73     sigaddset(&sigs, trampoline_sig);
  74     sigprocmask(SIG_BLOCK, &sigs, &osigs);
  75     sa.sa_handler = trampoline;
  76     sigemptyset(&sa.sa_mask);
  77     sa.sa_flags = SA_ONSTACK;
  78     if (sigaction(trampoline_sig, &sa, &osa) != 0)
  79     {
  80         DEBUGF("%s(): %s\n", __func__, strerror(errno));
  81         return false;
  82     }
  83     /*
  84      * Set the new stack.
  85      *
  86      * For sigaltstack we're lucky [from sigaltstack(2) on
  87      * FreeBSD 3.1]: ``Signal stacks are automatically adjusted
  88      * for the direction of stack growth and alignment
  89      * requirements''
  90      *
  91      * For sigstack we have to decide ourself [from sigstack(2)
  92      * on Solaris 2.6]: ``The direction of stack growth is not
  93      * indicated in the historical definition of struct sigstack.
  94      * The only way to portably establish a stack pointer is for
  95      * the application to determine stack growth direction.''
  96      */
  97     ss.ss_sp    = sp;
  98     ss.ss_size  = stack_size;
  99     ss.ss_flags = 0;
 100     if (sigaltstack(&ss, &oss) < 0)
 101     {
 102         DEBUGF("%s(): %s\n", __func__, strerror(errno));
 103         return false;
 104     }
 105
 106     /*
 107      * Now transfer control onto the signal stack and set it up.
 108      * It will return immediately via "return" after the setjmp()
 109      * was performed. Be careful here with race conditions.  The
 110      * signal can be delivered the first time sigsuspend() is
 111      * called.
 112      */
 113     sig_handler_called = false;
 114     main_thread = pthread_self();
 115     sigfillset(&sigs);
 116     sigdelset(&sigs, trampoline_sig);
 117     pthread_kill(main_thread, trampoline_sig);
 118     while(!sig_handler_called)
 119         sigsuspend(&sigs);
 120
 121     /*
 122      * Inform the system that we are back off the signal stack by
 123      * removing the alternative signal stack. Be careful here: It
 124      * first has to be disabled, before it can be removed.
 125      */
 126     sigaltstack(NULL, &ss);
 127     ss.ss_flags = SS_DISABLE;
 128     if (sigaltstack(&ss, NULL) < 0)
 129     {
 130         DEBUGF("%s(): %s\n", __func__, strerror(errno));
 131         return false;
 132     }
 133     sigaltstack(NULL, &ss);
 134     if (!(ss.ss_flags & SS_DISABLE))
 135     {
 136         DEBUGF("%s(): %s\n", __func__, strerror(errno));
 137         return false;
 138     }
 139     if (!(oss.ss_flags & SS_DISABLE))
 140         sigaltstack(&oss, NULL);
 141
 142     /*
 143      * Restore the old trampoline_sig signal handler and mask
 144      */
 145     sigaction(trampoline_sig, &osa, NULL);
 146     sigprocmask(SIG_SETMASK, &osigs, NULL);
 147
 148     /*
 149      * Tell the trampoline and bootstrap function where to dump
 150      * the new machine context, and what to do afterwards...
 151      */
 152     thread_func = f;
 153     thread_context  = ctx;
 154
 155     /*
 156      * Now enter the trampoline again, but this time not as a signal
 157      * handler. Instead we jump into it directly. The functionally
 158      * redundant ping-pong pointer arithmentic is neccessary to avoid
 159      * type-conversion warnings related to the `volatile' qualifier and
 160      * the fact that `jmp_buf' usually is an array type.
 161      */
 162     if (setjmp(*((jmp_buf *)&bootstrap_buf)) == 0)
 163         longjmp(*((jmp_buf *)&tramp_buf), 1);
 164
 165     /*
 166      * Ok, we returned again, so now we're finished
 167      */
 168     enable_irq();
 169     return true;
 170 }
 171
 172 static void trampoline(int sig)
 173 {
 174     (void)sig;
 175     /* sanity check, no other thread should be here */
 176     if (pthread_self() != main_thread)
 177         return;
 178
 179     if (setjmp(*((jmp_buf *)&tramp_buf)) == 0)
 180     {
 181         sig_handler_called = true;
 182         return;
 183     }
 184     /* longjump'd back in */
 185     bootstrap_context();
 186 }
 187
 188 void bootstrap_context(void)
 189 {
 190     /* copy to local storage so we can spawn further threads
 191      * in the meantime */
 192     void (*thread_entry)(void) = thread_func;
 193     struct ctx *t = thread_context;
 194
 195     /*
 196      * Save current machine state (on new stack) and
 197      * go back to caller until we're scheduled for real...
 198      */
 199     if (setjmp(t->thread_buf) == 0)
 200         longjmp(*((jmp_buf *)&bootstrap_buf), 1);
 201
 202     /*
 203      * The new thread is now running: GREAT!
 204      * Now we just invoke its init function....
 205      */
 206     thread_entry();
 207     DEBUGF("thread left\n");
 208     thread_exit();
 209 }
 210
 211 static inline void set_context(struct ctx *c)
 212 {
 213     longjmp(c->thread_buf, 1);
 214 }
 215
 216 static inline void swap_context(struct ctx *old, struct ctx *new)
 217 {
 218     if (setjmp(old->thread_buf) == 0)
 219         longjmp(new->thread_buf, 1);
 220 }
 221
 222 static inline void get_context(struct ctx *c)
 223 {
 224     setjmp(c->thread_buf);
 225 }
 226
 227
 228 static void setup_thread(struct regs *context);
 229
 230 #define INIT_MAIN_THREAD
 231 static void init_main_thread(void *addr)
 232 {
 233     /* get a context for the main thread so that we can jump to it from
 234      * other threads */
 235     struct regs *context = (struct regs*)addr;
 236     context->uc = &thread_bufs[curr_uc++];
 237     get_context(context->uc);
 238 }
 239
 240 #define THREAD_STARTUP_INIT(core, thread, function) \
 241     ({ (thread)->context.stack_size = (thread)->stack_size, \
 242        (thread)->context.stack = (uintptr_t)(thread)->stack; \
 243        (thread)->context.start = function; })
 244
 245
 246
 247 /*
 248  * Prepare context to make the thread runnable by calling swapcontext on it
 249  */
 250 static void setup_thread(struct regs *context)
 251 {
 252     void (*fn)(void) = context->start;
 253     context->uc = &thread_bufs[curr_uc++];
 254     while (!make_context(context->uc, fn, (char*)context->stack, context->stack_size))
 255         DEBUGF("Thread creation failed. Retrying");
 256 }
 257
 258
 259 /*
 260  * Save the ucontext_t pointer for later use in swapcontext()
 261  *
 262  * Cannot do getcontext() here, because jumping back to the context
 263  * resumes after the getcontext call (i.e. store_context), but we need
 264  * to resume from load_context()
 265  */
 266 static inline void store_context(void* addr)
 267 {
 268     struct regs *r = (struct regs*)addr;
 269     target_context = r->uc;
 270 }
 271
 272 /*
 273  * Perform context switch
 274  */
 275 static inline void load_context(const void* addr)
 276 {
 277     struct regs *r = (struct regs*)addr;
 278     if (UNLIKELY(r->start))
 279     {
 280         setup_thread(r);
 281         r->start = NULL;
 282     }
 283     swap_context(target_context, r->uc);
 284 }
 285
 286 /*
 287  * play nice with the host and sleep while waiting for the tick */
 288 extern void wait_for_interrupt(void);
 289 static inline void core_sleep(void)
 290 {
 291     enable_irq();
 292     wait_for_interrupt();
 293 }
 294