signal.c

xenomai 很好的linux实时补丁

 * @see http://www.opengroup.org/onlinepubs/000095399/functions/pthread_sigmask.html
 * 
 */
int pthread_sigmask (int how, const sigset_t *user_set, sigset_t *user_oset)

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

    if (!cur)
        return EPERM;

    emptyset(&unblocked);

    xnlock_get_irqsave(&nklock, s);

    if (oset)
        *oset = cur->sigmask;

    if (!set)
        goto unlock_and_exit;

    if (xnthread_signaled_p(&cur->threadbase))
        /* Call xnpod_schedule to deliver any soon-to-be-blocked pending
           signal, after this call, no signal is pending. */
        xnpod_schedule();

    switch (how)
        {

        case SIG_BLOCK:

            orset(&cur->sigmask, &cur->sigmask, set);
            break;

        case SIG_UNBLOCK:
            /* Mark as pending any signal which was received while
               blocked and is going to be unblocked. */
            andset(&unblocked, set, &cur->blocked_received.mask);
            nandset(&cur->sigmask, &cur->pending.mask, &unblocked);
            break;

        case SIG_SETMASK:

            nandset(&unblocked, &cur->blocked_received.mask, set);
            cur->sigmask = *set;
            break;

        default:

            xnlock_put_irqrestore(&nklock, s);
            return EINVAL;
        }
        
    /* Handle any unblocked signal. */
    if (!isemptyset(&unblocked))
        {
        pse51_siginfo_t *si, *next = NULL;

        cur->threadbase.signals = 0;

        while ((si = pse51_getsigq(&cur->blocked_received, &unblocked, &next)))
            {
            int sig = si->info.si_signo;
            unsigned prio;

            prio = sig < SIGRTMIN ? sig + SIGRTMAX : sig;
            addset(&cur->pending.mask, si->info.si_signo);
            insertpqf(&cur->pending.list, &si->link, prio);
            cur->threadbase.signals = 1;

            if (!next)
                break;
            }

        /* Let pse51_dispatch_signals do the job. */
        if (cur->threadbase.signals)
            xnpod_schedule();
        }

 unlock_and_exit:

    xnlock_put_irqrestore(&nklock, s);

    return 0;
}

static int pse51_sigtimedwait_inner (const sigset_t *user_set,
                                     siginfo_t *si,
                                     xnticks_t to)
{
    pse51_sigset_t non_blocked, *set = user2pse51_sigset(user_set);
    pse51_siginfo_t *received;
    pthread_t thread;
    int err = 0;
    spl_t s;

    thread = pse51_current_thread();

    if (!thread)
        return EPERM;
    
    /* All signals in "set" must be blocked in order for sigwait to
       work reliably. */
    nandset(&non_blocked, set, &thread->sigmask);
    if (!isemptyset(&non_blocked))
        return EINVAL;

    xnlock_get_irqsave(&nklock, s);

    received = pse51_getsigq(&thread->blocked_received, set, NULL);
    
    if (!received)
        {
        if (xnpod_unblockable_p())
            {
            err = EPERM;
            goto unlock_and_ret;
            }

        err = clock_adjust_timeout(&to, CLOCK_MONOTONIC);

        if (err)
            {
            if (err == ETIMEDOUT)
                err = EAGAIN;
            
            goto unlock_and_ret;
            }
        
        thread_cancellation_point(&thread->threadbase);

        xnpod_suspend_thread(&thread->threadbase, XNDELAY, to, NULL);

        thread_cancellation_point(&thread->threadbase);

        if (xnthread_test_flags(&thread->threadbase, XNBREAK))
            {
            if (!(received = pse51_getsigq(&thread->blocked_received,
                                           set,
                                           NULL)))
                err = EINTR;
            }
        else if (xnthread_test_flags(&thread->threadbase, XNTIMEO))
            err = EAGAIN;
        }

    if (!err)
        {
        *si = received->info;
        if (si->si_code == SI_QUEUE || si->si_code == SI_USER)
            pse51_delete_siginfo(received);
        else if (si->si_code == SI_TIMER)
            pse51_timer_notified(received);
        /* Nothing to be done for SI_MESQ. */
        }

  unlock_and_ret:
    xnlock_put_irqrestore(&nklock, s);

    return err;
}

/**
 * Wait for signals.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/sigwait.html
 * 
 */
int sigwait (const sigset_t *user_set, int *sig)
{
    siginfo_t info;
    int err;

    do
        {
        err = pse51_sigtimedwait_inner(user_set, &info, XN_INFINITE);
        }
    while (err == EINTR);

    if (!err)
        *sig = info.si_signo;

    return err;
}

/**
 * Wait for signals.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/sigwaitinfo.html
 * 
 */
int sigwaitinfo (const sigset_t *__restrict__ user_set,
                 siginfo_t *__restrict__ info)
{
    siginfo_t loc_info;
    int err;

    if (!info)
        info = &loc_info;

    do
        {
        err = pse51_sigtimedwait_inner(user_set, info, XN_INFINITE);
        }
    while (err == EINTR);

    /* Sigwaitinfo does not have the same behaviour as sigwait, errors are
       returned through errno. */
    if (err)
        {
        thread_set_errno(err);
        return -1;
        }
    
    return 0;
}

/**
 * Wait during a bounded time for signals.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/sigtimedwait.html
 * 
 */
int sigtimedwait (const sigset_t *__restrict__ user_set,
                  siginfo_t *__restrict__ info,
                  const struct timespec *__restrict__ timeout)
{
    xnticks_t abs_timeout;
    int err;

    if (timeout)
        {
        if ((unsigned long) timeout->tv_nsec >= ONE_BILLION)
            {
            err = EINVAL;
            goto out;
            }

        abs_timeout = clock_get_ticks(CLOCK_MONOTONIC)+ts2ticks_ceil(timeout)+1;
        }
    else
        abs_timeout = XN_INFINITE;

    do 
        {
        err = pse51_sigtimedwait_inner(user_set, info, abs_timeout);
        }
    while (err == EINTR);

  out:
    if (err)
        {
        thread_set_errno(err);
        return -1;
        }

    return 0;
}

static void pse51_dispatch_signals (xnsigmask_t sigs)

{
    pse51_siginfo_t *si, *next = NULL;
    pse51_sigset_t saved_mask;
    pthread_t thread;
    spl_t s;

    xnlock_get_irqsave(&nklock, s);

    thread = pse51_current_thread();
    
    saved_mask = thread->sigmask;

    while ((si = pse51_getsigq(&thread->pending, &thread->pending.mask, &next)))
        {
        struct sigaction *action = &actions[si->info.si_signo - 1];
        siginfo_t info = si->info;

        if (si->info.si_code == SI_TIMER)
            pse51_timer_notified(si);

        if (si->info.si_code == SI_QUEUE || si->info.si_code == SI_USER)
            pse51_delete_siginfo(si);

        /* Nothing to be done for SI_MESQ. */

        if (action->sa_handler != SIG_IGN)
            {
            siginfo_handler_t *info_handler =
                (siginfo_handler_t *) action->sa_sigaction;
            sighandler_t handler = action->sa_handler;

            if (handler == SIG_DFL)
                handler = pse51_default_handler;

            thread->sigmask = *user2pse51_sigset(&action->sa_mask);

            if (testbits(action->sa_flags, SA_ONESHOT))
                action->sa_handler = SIG_DFL;

            if (!testbits(action->sa_flags, SA_SIGINFO)
                || handler == pse51_default_handler)
                handler(info.si_signo);
            else
                info_handler(info.si_signo, &info, NULL);
            }

        if (!next)
            break;
        }

    thread->sigmask = saved_mask;
    thread->threadbase.signals = 0;

    xnlock_put_irqrestore(&nklock, s);
}

#if defined(__KERNEL__) && defined(CONFIG_XENO_OPT_PERVASIVE)
static void pse51_dispatch_shadow_signals (xnsigmask_t sigs)
{
    /* Migrate to secondary mode in order to get the signals delivered by
       Linux. */
    xnshadow_relax(1);
}

static void pse51_signal_handle_request (void *cookie)
{
    int cpuid = smp_processor_id(), reqnum;
    struct pse51_signals_threadsq_t *rq = &pse51_signals_threadsq[cpuid];

    while ((reqnum = rq->out) != rq->in)
        {
        pthread_t thread = rq->thread[reqnum];
        pse51_siginfo_t *si;
        spl_t s;
        
        rq->out = (reqnum + 1) & (SIG_MAX_REQUESTS - 1);

        xnlock_get_irqsave(&nklock, s);

        thread->threadbase.signals = 0;

        while ((si = pse51_getsigq(&thread->pending,
                                   &thread->pending.mask,
                                   NULL)))
            {
            siginfo_t info = si->info;
                
            if (si->info.si_code == SI_TIMER)
                pse51_timer_notified(si);
            
            if (si->info.si_code == SI_QUEUE || si->info.si_code == SI_USER)
                pse51_delete_siginfo(si);
            /* Nothing to be done for SI_MESQ. */

            /* Release the big lock, before calling a function which may
               reschedule. */
            xnlock_put_irqrestore(&nklock, s);
            
            send_sig_info(info.si_signo,
                          &info,
                          xnthread_user_task(&thread->threadbase));

            xnlock_get_irqsave(&nklock, s);
            
            thread->threadbase.signals = 0;
            }

        xnlock_put_irqrestore(&nklock, s);
        }
}
#endif /* __KERNEL__  && CONFIG_XENO_OPT_PERVASIVE*/

void pse51_signal_init_thread (pthread_t newthread, const pthread_t parent)

{
    emptyset(&newthread->blocked_received.mask);
    initpq(&newthread->blocked_received.list, xnqueue_up, SIGRTMAX + SIGRTMIN);
    emptyset(&newthread->pending.mask);
    initpq(&newthread->pending.list, xnqueue_up, SIGRTMAX + SIGRTMIN);

    /* parent may be NULL if pthread_create is not called from a pse51 thread. */
    if (parent)
        newthread->sigmask = parent->sigmask;
    else
        emptyset(&newthread->sigmask);

#if defined(__KERNEL__) && defined(CONFIG_XENO_OPT_PERVASIVE)
    if (testbits(newthread->threadbase.status, XNSHADOW))
        newthread->threadbase.asr = &pse51_dispatch_shadow_signals;
    else
#endif /* __KERNEL__  && CONFIG_XENO_OPT_PERVASIVE*/
        newthread->threadbase.asr = &pse51_dispatch_signals;

    newthread->threadbase.asrmode = 0;
    newthread->threadbase.asrimask = 0;
}

/* Unqueue, and free any pending siginfo structure. Assume we are called nklock
   locked, IRQ off. */
void pse51_signal_cleanup_thread (pthread_t thread)
{
    pse51_sigqueue_t *queue = &thread->pending;
    pse51_siginfo_t *si;

    while (queue)
        {
        while ((si = pse51_getsigq(queue, &queue->mask, NULL)))
            {
            if (si->info.si_code == SI_TIMER)
                pse51_timer_notified(si);
            
            if (si->info.si_code == SI_QUEUE || si->info.si_code == SI_USER)
                pse51_delete_siginfo(si);

            /* Nothing to be done for SI_MESQ. */
            }

        queue = (queue == &thread->pending ? &thread->blocked_received : NULL);
        }
}

void pse51_signal_pkg_init (void)

{
    int i;

    /* Fill the pool. */
    initpq(&pse51_infos_free_list, xnqueue_up, 1);
    for (i = 0; i < PSE51_SIGQUEUE_MAX; i++)
        pse51_delete_siginfo(&pse51_infos_pool[i]);

    for (i = 1; i <= SIGRTMAX; i++)
        {
        actions[i-1].sa_handler = SIG_DFL;
        emptyset(user2pse51_sigset(&actions[i-1].sa_mask));
        actions[i-1].sa_flags = 0;
        }

#if defined(__KERNEL__) && defined(CONFIG_XENO_OPT_PERVASIVE)
    pse51_signals_apc = rthal_apc_alloc("posix_signals_handler",
                                        &pse51_signal_handle_request,
                                        NULL);

    if (pse51_signals_apc < 0)
        printk("Unable to allocate APC: %d !\n", pse51_signals_apc);
#endif /* __KERNEL__  && CONFIG_XENO_OPT_PERVASIVE*/
}

void pse51_signal_pkg_cleanup (void)

{
#ifdef CONFIG_XENO_OPT_DEBUG
    int i;

    for (i = 0; i < PSE51_SIGQUEUE_MAX; i++)
        if (pse51_infos_pool[i].info.si_signo)
            xnprintf("Posix siginfo structure %p was not freed, freeing now.\n",
                     &pse51_infos_pool[i].info);
#endif /* CONFIG_XENO_OPT_DEBUG */

#if defined(__KERNEL__) && defined(CONFIG_XENO_OPT_PERVASIVE)
    rthal_apc_free(pse51_signals_apc);
#endif /* __KERNEL__  && CONFIG_XENO_OPT_PERVASIVE*/
}

static void pse51_default_handler (int sig)

{
    pthread_t cur = pse51_current_thread();
    
    xnpod_fatal("Thread %s received unhandled signal %d.\n",
                thread_name(cur), sig);
}

/*@}*/

EXPORT_SYMBOL(sigemptyset);
EXPORT_SYMBOL(sigfillset);
EXPORT_SYMBOL(sigaddset);
EXPORT_SYMBOL(sigdelset);
EXPORT_SYMBOL(sigismember);
EXPORT_SYMBOL(pthread_kill);
EXPORT_SYMBOL(pthread_sigmask);
EXPORT_SYMBOL(pse51_sigaction);
EXPORT_SYMBOL(pse51_sigqueue);

EXPORT_SYMBOL(sigpending);
EXPORT_SYMBOL(sigwait);
EXPORT_SYMBOL(sigwaitinfo);
EXPORT_SYMBOL(sigtimedwait);