cond.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_cond Condition variables services.
 *
 * Condition variables services.
 *
 * A condition variable is a synchronization object that allows threads to
 * suspend execution until some predicate on shared data is satisfied. The basic
 * operations on conditions are: signal the condition (when the predicate
 * becomes true), and wait for the condition, suspending the thread execution
 * until another thread signals the condition.
 *
 * A condition variable must always be associated with a mutex, to avoid the
 * race condition where a thread prepares to wait on a condition variable and
 * another thread signals the condition just before the first thread actually
 * waits on it.
 *
 * Before it can be used, a condition variable has to be initialized with
 * pthread_cond_init(). An attribute object, which reference may be passed to
 * this service, allows to select the features of the created condition
 * variable, namely the @a clock used by the pthread_cond_timedwait() service
 * (@a CLOCK_REALTIME is used by default).
 *
 * There is no support for the @a pshared attribute; condition variables created
 * by Xenomai POSIX skin may be shared by kernel-space modules and user-space
 * processes through shared memory.
 *
 * Note that only pthread_cond_init() may be used to initialize a condition
 * variable, using the static initializer @a PTHREAD_COND_INITIALIZER is
 * not supported.
 *
 *@{*/

#include <posix/mutex.h>
#include <posix/cond.h>

typedef struct pse51_cond {
    xnsynch_t synchbase;
    xnholder_t link;            /* Link in pse51_condq */

#define link2cond(laddr)                                                \
    ((pse51_cond_t *)(((char *)laddr) - offsetof(pse51_cond_t, link)))

    pthread_condattr_t attr;
    struct __shadow_mutex *mutex;
} pse51_cond_t;

static pthread_condattr_t default_cond_attr;

static xnqueue_t pse51_condq;

static void cond_destroy_internal (pse51_cond_t *cond)

{
    removeq(&pse51_condq, &cond->link);
    /* synchbase wait queue may not be empty only when this function is called
       from pse51_cond_pkg_cleanup, hence the absence of xnpod_schedule(). */
    xnsynch_destroy(&cond->synchbase);
    xnfree(cond);
}

/**
 * Initialize a condition variable.
 *
 * This service initializes the condition variable @a cnd, using the condition
 * variable attributes object @a attr. If @a attr is @a NULL or this service is
 * called from user-space, default attributes are used (see
 * pthread_condattr_init()).
 *
 * @param cnd the condition variable to be initialized;
 *
 * @param attr the condition variable attributes object.
 *
 * @return 0 on succes,
 * @return an error number if:
 * - EINVAL, the condition variable attributes object @a attr is invalid or
 *   uninitialized;
 * - EBUSY, the condition variable @a cnd was already initialized;
 * - ENOMEM, insufficient memory exists in the system heap to initialize the
 *   condition variable, increase CONFIG_XENO_OPT_SYS_HEAPSZ.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/pthread_cond_init.html
 * 
 */
int pthread_cond_init (pthread_cond_t *cnd, const pthread_condattr_t *attr)

{
    struct __shadow_cond *shadow = &((union __xeno_cond *) cnd)->shadow_cond;
    xnflags_t synch_flags = XNSYNCH_PRIO | XNSYNCH_NOPIP;
    pse51_cond_t *cond;
    spl_t s;

    if (!attr)
        attr = &default_cond_attr;

    xnlock_get_irqsave(&nklock, s);

    if (attr->magic != PSE51_COND_ATTR_MAGIC)
	{
        xnlock_put_irqrestore(&nklock, s);
        return EINVAL;
	}

    if (shadow->magic == PSE51_COND_MAGIC)
        {
        xnholder_t *holder;
        for(holder = getheadq(&pse51_condq); holder;
            holder = nextq(&pse51_condq, holder))
            if (holder == &shadow->cond->link)
                {
                /* cond is already in the queue. */
                xnlock_put_irqrestore(&nklock, s);
                return EBUSY;
                }
        }

    cond = (pse51_cond_t *) xnmalloc(sizeof(*cond));
    if (!cond)
        {
        xnlock_put_irqrestore(&nklock, s);
        return ENOMEM;
        }

    shadow->magic = PSE51_COND_MAGIC;
    shadow->cond = cond;

    xnsynch_init(&cond->synchbase, synch_flags);
    inith(&cond->link);
    cond->attr = *attr;
    cond->mutex = NULL;

    appendq(&pse51_condq, &cond->link);

    xnlock_put_irqrestore(&nklock, s);

    return 0;    
}

/**
 * Destroy a condition variable.
 *
 * This service destroys the condition variable @a cnd, if no thread is
 * currently blocked on it. The condition variable becomes invalid for all
 * condition variable services (they all return the EINVAL error) except
 * pthread_cond_init().
 *
 * @param cnd the condition variable to be destroyed.
 *
 * @return 0 on succes,
 * @return an error number if:
 * - EINVAL, the condition variable @a cnd is invalid;
 * - EBUSY, some thread is currently blocked on the condition variable.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/pthread_cond_destroy.html
 * 
 */
int pthread_cond_destroy (pthread_cond_t *cnd)

{
    struct __shadow_cond *shadow = &((union __xeno_cond *) cnd)->shadow_cond;
    pse51_cond_t *cond;
    spl_t s;

    xnlock_get_irqsave(&nklock, s);

    if (!pse51_obj_active(shadow, PSE51_COND_MAGIC, struct __shadow_cond))
	{
        xnlock_put_irqrestore(&nklock, s);
        return EINVAL;
	}

    cond = shadow->cond;

    if (xnsynch_nsleepers(&cond->synchbase))
	{
        xnlock_put_irqrestore(&nklock, s);
        return EBUSY;
	}

    cond_destroy_internal(cond);
    pse51_mark_deleted(shadow);
    
    xnlock_put_irqrestore(&nklock, s);

    return 0;
}

/* must be called with nklock locked, interrupts off. */
static inline int mutex_save_count(xnthread_t *cur,
                                   struct __shadow_mutex *shadow,
                                   unsigned *count_ptr)
{
    pse51_mutex_t *mutex;

    if (!pse51_obj_active(shadow, PSE51_MUTEX_MAGIC, struct __shadow_mutex))
        return EINVAL;

    mutex = shadow->mutex;

    if (xnsynch_owner(&mutex->synchbase) != cur || mutex->count == 0)
        return EPERM;

    *count_ptr = mutex->count;
    mutex->count = 0;

    xnsynch_wakeup_one_sleeper(&mutex->synchbase);
    /* Do not reschedule here, releasing the mutex and suspension must be done
       atomically in pthread_cond_*wait. */
    
    return 0;
}

/* must be called with nklock locked, interrupts off. */
static inline void mutex_restore_count(xnthread_t *cur,
                                       struct __shadow_mutex *shadow,
                                       unsigned count)
{
    pse51_mutex_t *mutex = shadow->mutex;

    /* Relock the mutex */
    mutex_timedlock_internal(cur, shadow, XN_INFINITE);

    /* Restore the mutex lock count. */
    mutex->count = count;
}

int pse51_cond_timedwait_internal(struct __shadow_cond *shadow,
                                  struct __shadow_mutex *mutex,
                                  xnticks_t to)
{
    xnthread_t *cur = xnpod_current_thread();
    pse51_cond_t *cond;
    unsigned count;
    spl_t s;
    int err;

    if (!shadow || !mutex)
        return EINVAL;

    if (xnpod_unblockable_p())
        return EPERM;

    xnlock_get_irqsave(&nklock, s);

    thread_cancellation_point(cur);

    cond = shadow->cond;

    /* If another thread waiting for cond does not use the same mutex */
    if (!pse51_obj_active(shadow, PSE51_COND_MAGIC, struct __shadow_cond)
       || (cond->mutex && cond->mutex != mutex))
	{
        err = EINVAL;
        goto unlock_and_return;
	}

    err = clock_adjust_timeout(&to, cond->attr.clock);

    if(err)
        goto unlock_and_return;
    
    /* Unlock mutex, with its previous recursive lock count stored
       in "count". */
    err = mutex_save_count(cur, mutex, &count);