path: root/firmware/asm/thread-unix.c

/***************************************************************************
 *             __________               __   ___.
 *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
 *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
 *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
 *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
 *                     \/            \/     \/    \/            \/
 * $Id$
 *
 * Copyright (C) 2011 by Thomas Martitz
 *
 * Generic unix threading support
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
 * KIND, either express or implied.
 *
 ****************************************************************************/

#include <stdlib.h>
#include <stdbool.h>
#include <signal.h>
#include <stdio.h>
#include <setjmp.h>
#include <unistd.h>
#include <pthread.h>
#include <errno.h>
#include "debug.h"

static volatile bool sig_handler_called;
static volatile jmp_buf tramp_buf;
static volatile jmp_buf bootstrap_buf;
static void (*thread_func)(void);
static const int trampoline_sig = SIGUSR1;
static pthread_t main_thread;

static struct ctx {
    jmp_buf thread_buf;
} thread_bufs[MAXTHREADS];
static threadbit_t free_thread_bufs;
static struct ctx* thread_context, *target_context;

static void trampoline(int sig);
static void bootstrap_context(void) __attribute__((noinline));

static void init_thread_bufs(void)
{
    for (unsigned int bufidx = 0; bufidx < MAXTHREADS; bufidx++)
        threadbit_set_bit(&free_thread_bufs, bufidx);
}

static struct ctx * alloc_thread_buf(void)
{
    unsigned int bufidx = threadbit_ffs(&free_thread_bufs);
    threadbit_clear_bit(&free_thread_bufs, bufidx);
    return &thread_bufs[bufidx];
}

static void free_thread_buf(struct ctx *threadbuf)
{
    unsigned int bufidx = threadbuf - thread_bufs;
    threadbit_set_bit(&free_thread_bufs, bufidx);
}

/* The *_context functions are heavily based on Gnu pth
 * http://www.gnu.org/software/pth/
 *
 * adjusted to work in a multi-thread environment to
 * offer a ucontext-like API
 */

/*
 * VARIANT 2: THE SIGNAL STACK TRICK
 *
 * This uses sigstack/sigaltstack() and friends and is really the
 * most tricky part of Pth. When you understand the following
 * stuff you're a good Unix hacker and then you've already
 * understood the gory ingredients of Pth.  So, either welcome to
 * the club of hackers, or do yourself a favor and skip this ;)
 *
 * The ingenious fact is that this variant runs really on _all_ POSIX
 * compliant systems without special platform kludges.  But be _VERY_
 * carefully when you change something in the following code. The slightest
 * change or reordering can lead to horribly broken code.  Really every
 * function call in the following case is intended to be how it is, doubt
 * me...
 *
 * For more details we strongly recommend you to read the companion
 * paper ``Portable Multithreading -- The Signal Stack Trick for
 * User-Space Thread Creation'' from Ralf S. Engelschall. A copy of the
 * draft of this paper you can find in the file rse-pmt.ps inside the
 * GNU Pth distribution.
 */

static int make_context(struct ctx *ctx, void (*f)(void), char *sp, size_t stack_size)
{
    struct sigaction sa;
    struct sigaction osa;
    stack_t ss;
    stack_t oss;
    sigset_t osigs;
    sigset_t sigs;

    disable_irq();
    /*
     * Preserve the trampoline_sig signal state, block trampoline_sig,
     * and establish our signal handler. The signal will
     * later transfer control onto the signal stack.
     */
    sigemptyset(&sigs);
    sigaddset(&sigs, trampoline_sig);
    sigprocmask(SIG_BLOCK, &sigs, &osigs);
    sa.sa_handler = trampoline;
    sigemptyset(&sa.sa_mask);
    sa.sa_flags = SA_ONSTACK;
    if (sigaction(trampoline_sig, &sa, &osa) != 0)
    {
        DEBUGF("%s(): %s\n", __func__, strerror(errno));
        return false;
    }
    /*
     * Set the new stack.
     *
     * For sigaltstack we're lucky [from sigaltstack(2) on
     * FreeBSD 3.1]: ``Signal stacks are automatically adjusted
     * for the direction of stack growth and alignment
     * requirements''
     *
     * For sigstack we have to decide ourself [from sigstack(2)
     * on Solaris 2.6]: ``The direction of stack growth is not
     * indicated in the historical definition of struct sigstack.
     * The only way to portably establish a stack pointer is for
     * the application to determine stack growth direction.''
     */
    ss.ss_sp    = sp;
    ss.ss_size  = stack_size;
    ss.ss_flags = 0;
    if (sigaltstack(&ss, &oss) < 0)
    {
        DEBUGF("%s(): %s\n", __func__, strerror(errno));
        return false;
    }

    /*
     * Now transfer control onto the signal stack and set it up.
     * It will return immediately via "return" after the setjmp()
     * was performed. Be careful here with race conditions.  The
     * signal can be delivered the first time sigsuspend() is
     * called.
     */
    sig_handler_called = false;
    main_thread = pthread_self();
    sigfillset(&sigs);
    sigdelset(&sigs, trampoline_sig);
    pthread_kill(main_thread, trampoline_sig);
    while(!sig_handler_called)
        sigsuspend(&sigs);

    /*
     * Inform the system that we are back off the signal stack by
     * removing the alternative signal stack. Be careful here: It
     * first has to be disabled, before it can be removed.
     */
    sigaltstack(NULL, &ss);
    ss.ss_flags = SS_DISABLE;
    if (sigaltstack(&ss, NULL) < 0)
    {
        DEBUGF("%s(): %s\n", __func__, strerror(errno));
        return false;
    }
    sigaltstack(NULL, &ss);
    if (!(ss.ss_flags & SS_DISABLE))
    {
        DEBUGF("%s(): %s\n", __func__, strerror(errno));
        return false;
    }
    if (!(oss.ss_flags & SS_DISABLE))
        sigaltstack(&oss, NULL);

    /*
     * Restore the old trampoline_sig signal handler and mask
     */
    sigaction(trampoline_sig, &osa, NULL);
    sigprocmask(SIG_SETMASK, &osigs, NULL);

    /*
     * Tell the trampoline and bootstrap function where to dump
     * the new machine context, and what to do afterwards...
     */
    thread_func = f;
    thread_context  = ctx;

    /*
     * Now enter the trampoline again, but this time not as a signal
     * handler. Instead we jump into it directly. The functionally
     * redundant ping-pong pointer arithmentic is neccessary to avoid
     * type-conversion warnings related to the `volatile' qualifier and
     * the fact that `jmp_buf' usually is an array type.
     */
    if (setjmp(*((jmp_buf *)&bootstrap_buf)) == 0)
        longjmp(*((jmp_buf *)&tramp_buf), 1);

    /*
     * Ok, we returned again, so now we're finished
     */
    enable_irq();
    return true;
}

static void trampoline(int sig)
{
    (void)sig;
    /* sanity check, no other thread should be here */
    if (pthread_self() != main_thread)
        return;

    if (setjmp(*((jmp_buf *)&tramp_buf)) == 0)
    {
        sig_handler_called = true;
        return;
    }
    /* longjump'd back in */
    bootstrap_context();
}

void bootstrap_context(void)
{
    /* copy to local storage so we can spawn further threads
     * in the meantime */
    void (*thread_entry)(void) = thread_func;
    struct ctx *t = thread_context;
    
    /*
     * Save current machine state (on new stack) and
     * go back to caller until we're scheduled for real...
     */
    if (setjmp(t->thread_buf) == 0)
        longjmp(*((jmp_buf *)&bootstrap_buf), 1);

    /*
     * The new thread is now running: GREAT!
     * Now we just invoke its init function....
     */
    thread_entry();
    DEBUGF("thread left\n");
    free_thread_buf(t);
    thread_exit();
}

static inline void set_context(struct ctx *c)
{
    longjmp(c->thread_buf, 1);
}

static inline void swap_context(struct ctx *old, struct ctx *new)
{
    if (!old || setjmp(old->thread_buf) == 0)
        longjmp(new->thread_buf, 1);
}

static inline void get_context(struct ctx *c)
{
    setjmp(c->thread_buf);
}


static void setup_thread(struct regs *context);

#define INIT_MAIN_THREAD
static void init_main_thread(void *addr)
{
    /* get a context for the main thread so that we can jump to it from
     * other threads */
    struct regs *context = (struct regs*)addr;
    init_thread_bufs();
    context->uc = alloc_thread_buf();
    get_context(context->uc);
}

#define THREAD_STARTUP_INIT(core, thread, function) \
    ({ (thread)->context.stack_size = (thread)->stack_size, \
       (thread)->context.stack = (uintptr_t)(thread)->stack; \
       (thread)->context.start = function; })



/*
 * Prepare context to make the thread runnable by calling swapcontext on it
 */
static void setup_thread(struct regs *context)
{
    void (*fn)(void) = context->start;
    context->uc = alloc_thread_buf();
    while (!make_context(context->uc, fn, (char*)context->stack, context->stack_size))
        DEBUGF("Thread creation failed. Retrying");
}


/*
 * Save the ucontext_t pointer for later use in swapcontext()
 *
 * Cannot do getcontext() here, because jumping back to the context
 * resumes after the getcontext call (i.e. store_context), but we need
 * to resume from load_context()
 */
static inline void store_context(void* addr)
{
    struct regs *r = (struct regs*)addr;
    target_context = r->uc;
}

/*
 * Perform context switch
 */
static inline void load_context(const void* addr)
{
    struct regs *r = (struct regs*)addr;
    if (UNLIKELY(r->start))
    {
        setup_thread(r);
        r->start = NULL;
    }
    swap_context(target_context, r->uc);
    target_context = NULL;
}