signal.c

xenomai 很好的linux实时补丁

/*
 * Written by Gilles Chanteperdrix <[e-mail]>.
 *
 * 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 program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */

/**
 * @ingroup posix
 * @defgroup posix_signal Signals services.
 *
 * Signals management services.
 * 
 *@{*/

#if defined(__KERNEL__) && defined(CONFIG_XENO_OPT_PERVASIVE)
#include <nucleus/shadow.h>
#endif /* __KERNEL__  && CONFIG_XENO_OPT_PERVASIVE*/
#include <asm/xenomai/system.h> /* For xnlock. */
#include <posix/timer.h>        /* For pse51_timer_notified. */
#include <posix/sig.h>

static void pse51_default_handler (int sig);

typedef void siginfo_handler_t(int, siginfo_t *, void *);

#define user2pse51_sigset(set) ((pse51_sigset_t *)(set))
#define PSE51_SIGQUEUE_MAX 64

#define SIGRTMAX 64
static struct sigaction actions[SIGRTMAX];
static pse51_siginfo_t pse51_infos_pool[PSE51_SIGQUEUE_MAX];
#ifdef CONFIG_SMP
static xnlock_t pse51_infos_lock;
#endif
static xnpqueue_t pse51_infos_free_list;

#if defined(__KERNEL__) && defined(CONFIG_XENO_OPT_PERVASIVE)
#define SIG_MAX_REQUESTS 64 /* Must be a ^2 */

static int pse51_signals_apc;

static struct pse51_signals_threadsq_t {
    int in, out;
    pthread_t thread [SIG_MAX_REQUESTS];
} pse51_signals_threadsq [XNARCH_NR_CPUS];

static void pse51_signal_schedule_request (pthread_t thread)
{
    int cpuid = rthal_processor_id(), reqnum;
    struct pse51_signals_threadsq_t *rq = &pse51_signals_threadsq[cpuid];
    spl_t s;

    /* Signal the APC, to have it delegate signals to Linux. */
    splhigh(s);
    reqnum = rq->in;
    rq->thread[reqnum] = thread;
    rq->in = (reqnum + 1) & (SIG_MAX_REQUESTS - 1);
    splexit(s);

    rthal_apc_schedule(pse51_signals_apc);
}
#endif /* __KERNEL__  && CONFIG_XENO_OPT_PERVASIVE*/

static pse51_siginfo_t *pse51_new_siginfo (int sig, int code, union sigval value)
{
    xnpholder_t *holder;
    pse51_siginfo_t *si;
    spl_t s;

    xnlock_get_irqsave(&pse51_infos_lock, s);
    holder = getpq(&pse51_infos_free_list);
    xnlock_put_irqrestore(&pse51_infos_lock, s);

    if (!holder)
        return NULL;

    si = link2siginfo(holder);
    si->info.si_signo = sig;
    si->info.si_code = code;
    si->info.si_value = value;

    return si;
}

static void pse51_delete_siginfo (pse51_siginfo_t *si)
{
    spl_t s;

    initph(&si->link);
    si->info.si_signo = 0;      /* Used for debugging. */

    xnlock_get_irqsave(&pse51_infos_lock, s);
    insertpql(&pse51_infos_free_list, &si->link, 0);
    xnlock_put_irqrestore(&pse51_infos_lock, s);
}

static inline void emptyset (pse51_sigset_t *set) {

    *set = 0ULL;
}

static inline void fillset (pse51_sigset_t *set) {

    *set = ~0ULL;
}

static inline void addset (pse51_sigset_t *set, int sig) {

    *set |= ((pse51_sigset_t) 1 << (sig - 1));
}

static inline void delset (pse51_sigset_t *set, int sig) {

    *set &= ~((pse51_sigset_t) 1 << (sig - 1));
}

static inline int ismember (const pse51_sigset_t *set, int sig) {

    return (*set & ((pse51_sigset_t) 1 << (sig - 1))) != 0;
}

static inline int isemptyset (const pse51_sigset_t *set)
{
    return (*set) == 0ULL;
}

static inline void andset (pse51_sigset_t *set,
                           const pse51_sigset_t *left,
                           const pse51_sigset_t *right)
{
    *set = (*left) & (*right);
}

static inline void orset (pse51_sigset_t *set,
                          const pse51_sigset_t *left,
                          const pse51_sigset_t *right)
{
    *set = (*left) | (*right);
}

static inline void nandset (pse51_sigset_t *set,
                            const pse51_sigset_t *left,
                            const pse51_sigset_t *right)
{
    *set = (*left) & ~(*right);
}


/**
 * Initialize and empty a signal set.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/sigemptyset.html
 * 
 */
int sigemptyset (sigset_t *user_set)

{
    pse51_sigset_t *set = user2pse51_sigset(user_set);
    
    emptyset(set);

    return 0;
}

/**
 * Initialize and fill a signal set.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/sigfillset.html
 * 
 */
int sigfillset (sigset_t *user_set)

{
    pse51_sigset_t *set = user2pse51_sigset(user_set);
    
    fillset(set);

    return 0;
}

/**
 * Add a signal to a signal set.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/sigaddset.html
 * 
 */
int sigaddset (sigset_t *user_set, int sig)

{
    pse51_sigset_t *set = user2pse51_sigset(user_set);
    
    if ((unsigned ) (sig - 1) > SIGRTMAX - 1)
        {
        thread_set_errno(EINVAL);
        return -1;
        }

    addset(set, sig);

    return 0;
}

/**
 * Delete a signal from a signal set.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/sigdelset.html
 * 
 */
int sigdelset (sigset_t *user_set, int sig)

{
    pse51_sigset_t *set = user2pse51_sigset(user_set);
    
    if ((unsigned ) (sig - 1) > SIGRTMAX - 1)
        {
        thread_set_errno(EINVAL);
        return -1;
        }

    delset(set, sig);

    return 0;
}

/**
 * Test for a signal in a signal set.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/sigismember.html
 * 
 */
int sigismember (const sigset_t *user_set, int sig)

{
    pse51_sigset_t *set=user2pse51_sigset(user_set);

    if ((unsigned ) (sig - 1) > SIGRTMAX - 1)
        {
        thread_set_errno(EINVAL);
        return -1;
        }

    return ismember(set,sig);
}

/* Must be called with nklock lock, irqs off, may reschedule. */
void pse51_sigqueue_inner (pthread_t thread, pse51_siginfo_t *si)
{
    unsigned prio;
    int signum;

    if (!pse51_obj_active(thread, PSE51_THREAD_MAGIC,struct pse51_thread))
        return;

    signum = si->info.si_signo;
    /* Since signals below SIGRTMIN are not real-time, they should be treated
       after real-time signals, hence their priority. */
    prio = signum < SIGRTMIN ? signum + SIGRTMAX : signum;

    initph(&si->link);
    
    if (ismember(&thread->sigmask, signum))
        {
        addset(&thread->blocked_received.mask, signum);
        insertpqf(&thread->blocked_received.list, &si->link, prio);
        }    
    else
        {
        addset(&thread->pending.mask, signum);
        insertpqf(&thread->pending.list, &si->link, prio);
        thread->threadbase.signals = 1;
        }

#if defined(__KERNEL__) && defined(CONFIG_XENO_OPT_PERVASIVE)
    pse51_signal_schedule_request(thread);
#endif /* __KERNEL__  && CONFIG_XENO_OPT_PERVASIVE*/

    if (thread == pse51_current_thread()
        || xnpod_unblock_thread(&thread->threadbase))
        xnpod_schedule();
}

void pse51_sigunqueue (pthread_t thread, pse51_siginfo_t *si)
{
    pse51_sigqueue_t *queue;
    xnpholder_t *next;

    if (ismember(&thread->sigmask, si->info.si_signo))
        queue = &thread->blocked_received;
    else
        queue = &thread->pending;

    /* If si is the only signal queued with its signal number, clear the
       mask. We do not have "prevpq", we hence use findpq, even though this is
       much less efficient. */
    next = nextpq(&queue->list, &si->link);
    if ((!next || next->prio != si->link.prio)
       && findpqh(&queue->list, si->link.prio) == &si->link)
        delset(&queue->mask, si->info.si_signo);

    removepq(&queue->list, &si->link);
}


/* Unqueue any siginfo of "queue" whose signal number is member of "set",
   starting with "start". If "start" is NULL, start from the list head. */
static pse51_siginfo_t *pse51_getsigq (pse51_sigqueue_t *queue,
                                       pse51_sigset_t *set,
                                       pse51_siginfo_t **start)
{
    xnpholder_t *holder, *next;
    pse51_siginfo_t *si;

    next = (start && *start) ? &(*start)->link : getheadpq(&queue->list);

    while ((holder = next))
        {
        next = nextpq(&queue->list, holder);
        si = link2siginfo(holder);
        
        if (ismember(set, si->info.si_signo))
            goto found;
        }

    if (start)
        *start = NULL;

    return NULL;

  found:
    removepq(&queue->list, holder);
    if (!next || next->prio != holder->prio)
        delset(&queue->mask, si->info.si_signo);

    if (start)
        *start = next ? link2siginfo(next) : NULL;
    
    return si;
}

/**
 * Examine and change a signal action.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/sigaction.html
 * 
 */
int sigaction (int sig, const struct sigaction *action, struct sigaction *old)

{
    spl_t s;

    if ((unsigned) (sig - 1) > SIGRTMAX - 1)
        {
        thread_set_errno(EINVAL);
        return -1;
        }

    if (action && testbits(action->sa_flags, ~SIGACTION_FLAGS))
        {
        thread_set_errno(ENOTSUP);
        return -1;
        }

    xnlock_get_irqsave(&nklock, s);

    if (old)
        *old = actions[sig - 1];

    if (action)
        {
        struct sigaction *dest_action = &actions[sig - 1];
            
        *dest_action = *action;

        if (!(testbits(action->sa_flags, SA_NOMASK)))
            addset(user2pse51_sigset(&dest_action->sa_mask), sig);
        }

    xnlock_put_irqrestore(&nklock, s);

    return 0;
}

int sigqueue (pthread_t thread, int sig, union sigval value)
{
    pse51_siginfo_t *si = NULL; /* Avoid spurious warning. */
    spl_t s;

    if ((unsigned) sig > SIGRTMAX)
        return EINVAL;

    if (sig)
        {
        si = pse51_new_siginfo(sig, SI_QUEUE, value);

        if (!si)
            return EAGAIN;
        }

    xnlock_get_irqsave(&nklock, s);    

    if (!pse51_obj_active(thread, PSE51_THREAD_MAGIC,struct pse51_thread))
        {
        xnlock_put_irqrestore(&nklock, s);
        return ESRCH;
        }

    if (sig)
        pse51_sigqueue_inner(thread, si);

    xnlock_put_irqrestore(&nklock, s);

    return 0;
}

/**
 * Send a signal to a thread.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/pthread_kill.html
 * 
 */
int pthread_kill (pthread_t thread, int sig)
{
    pse51_siginfo_t *si = NULL;
    spl_t s;

    if ((unsigned) sig > SIGRTMAX)
        return EINVAL;

    if (sig)
        {
        si = pse51_new_siginfo(sig, SI_USER, (union sigval) 0);

        if (!si)
            return EAGAIN;
        }

    xnlock_get_irqsave(&nklock, s);    

    if (!pse51_obj_active(thread, PSE51_THREAD_MAGIC,struct pse51_thread))
        {
        xnlock_put_irqrestore(&nklock, s);
        return ESRCH;
        }

    if (sig)
        pse51_sigqueue_inner(thread, si);

    xnlock_put_irqrestore(&nklock, s);

    return 0;
}

/**
 * Examine pending signals.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/sigpending.html
 * 
 */
int sigpending (sigset_t *user_set)

{
    pse51_sigset_t *set = user2pse51_sigset(user_set);
    pthread_t cur = pse51_current_thread();
    spl_t s;

    if (!cur)
        {
        thread_set_errno(EPERM);
        return -1;
        }

    /* Lock nklock, in order to prevent pthread_kill from modifying
     * blocked_received while we are reading */
    xnlock_get_irqsave(&nklock, s);  

    *set = cur->blocked_received.mask;

    xnlock_put_irqrestore(&nklock, s);

    return 0;
}

/**
 * Examine and change the set of signals blocked by a thread.
 *