firmware/asm/thread-unix.c

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