sem.c

xenomai 很好的linux实时补丁

        goto error;

    named_sem = node2sem(node);
    
    if (pse51_node_removed_p(&named_sem->nodebase))
        sem_destroy_internal(&named_sem->sembase);

    xnlock_put_irqrestore(&nklock, s);

    return 0;

  error:
    thread_set_errno(err);

    return -1;
}

static inline int sem_trywait_internal (struct __shadow_sem *shadow)

{
    pse51_sem_t *sem;

    if (shadow->magic != PSE51_SEM_MAGIC
        && shadow->magic != PSE51_NAMED_SEM_MAGIC)
        return EINVAL;

    sem = shadow->sem;
 
    if (sem->value == 0)
        return EAGAIN;

    --sem->value;

    return 0;
}

/**
 * Attempt to lock a semaphore.
 *
 * This service is equivalent to sem_wait(), except that it returns immediately
 * if the semaphore @a sm is currently locked, and that it is not a cancellation
 * point.
 *
 * @param sm the semaphore to be locked.
 *
 * @retval 0 on success;
 * @retval -1 with @a errno set if:
 * - EINVAL, the specified semaphore is invalid or uninitialized;
 * - EAGAIN, the specified semaphore is currently locked.
 *
 * http://www.opengroup.org/onlinepubs/000095399/functions/sem_trywait.html
 * 
 */
int sem_trywait (sem_t *sm)

{
    struct __shadow_sem *shadow = &((union __xeno_sem *) sm)->shadow_sem;
    int err;
    spl_t s;
    
    xnlock_get_irqsave(&nklock, s);

    err = sem_trywait_internal(shadow);

    xnlock_put_irqrestore(&nklock, s);
    
    if (err)
        {
        thread_set_errno(err);
        return -1;
        }

    return 0;
}

static inline int sem_timedwait_internal (struct __shadow_sem *shadow,
                                          xnticks_t to)
{
    pse51_sem_t *sem = shadow->sem;
    xnthread_t *cur;
    int err;

    if (xnpod_unblockable_p())
        return EPERM;

    cur = xnpod_current_thread();

    if ((err = sem_trywait_internal(shadow)) != EAGAIN)
        return err;

    if((err = clock_adjust_timeout(&to, CLOCK_REALTIME)))
        return err;

    thread_cancellation_point(cur);

    xnsynch_sleep_on(&sem->synchbase, to);
            
    /* Handle cancellation requests. */
    thread_cancellation_point(cur);

    if (xnthread_test_flags(cur, XNRMID))
        return EINVAL;
    
    if (xnthread_test_flags(cur, XNBREAK))
        return EINTR;
    
    if (xnthread_test_flags(cur, XNTIMEO))
        return ETIMEDOUT;

    return 0;
}

/**
 * Lock a semaphore.
 *
 * This service locks the semaphore @a sm if it is currently unlocked (i.e. if
 * its value is greater than 0). If the semaphore is currently locked, the
 * calling thread is suspended until the semaphore is unlocked, or a signal is
 * delivered to the calling thread.
 *
 * This service is a cancellation point for Xenomai POSIX skin threads (created
 * with the pthread_create() service). When such a thread is cancelled while
 * blocked in a call to this service, the semaphore state is left unchanged
 * before the cancellation cleanup handlers are called.
 *
 * @param sm the semaphore to be locked.
 *
 * @retval 0 on success;
 * @retval -1 with @a errno set if:
 * - EPERM, the caller context is invalid;
 * - EINVAL, the semaphore is invalid or uninitialized;
 * - EINTR, the caller was interrupted by a signal while blocked in this
 *   service.
 *
 * @par Valid contexts:
 * - Xenomai kernel-space thread,
 * - Xenomai user-space thread (switches to primary mode).
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/sem_wait.html
 * 
 */
int sem_wait (sem_t *sm)

{
    struct __shadow_sem *shadow = &((union __xeno_sem *) sm)->shadow_sem;
    spl_t s;
    int err;

    xnlock_get_irqsave(&nklock, s);
    err = sem_timedwait_internal(shadow, XN_INFINITE);
    xnlock_put_irqrestore(&nklock, s);

    if(err)
        {
        thread_set_errno(err);
        return -1;
        }
    
    return 0;
}

/**
 * Attempt, during a bounded time, to lock a semaphore.
 *
 * This serivce is equivalent to sem_wait(), except that the caller is only
 * blocked until the timeout @a abs_timeout expires.
 *
 * @param sm the semaphore to be locked;
 *
 * @param abs_timeout the timeout, expressed as an absolute value of the
 * CLOCK_REALTIME clock.
 *
 * @retval 0 on success;
 * @retval -1 with @a errno set if:
 * - EPERM, the caller context is invalid;
 * - EINVAL, the semaphore is invalid or uninitialized;
 * - EINVAL, the specified timeout is invalid;
 * - EINTR, the caller was interrupted by a signal while blocked in this
 *   service;
 * - ETIMEDOUT, the semaphore could not be locked and the specified timeout
 *   expired.
 *
 * @par Valid contexts:
 * - Xenomai kernel-space thread,
 * - Xenomai user-space thread (switches to primary mode).
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/sem_timedwait.html
 * 
 */
int sem_timedwait (sem_t *sm, const struct timespec *abs_timeout)

{
    struct __shadow_sem *shadow = &((union __xeno_sem *) sm)->shadow_sem;
    spl_t s;
    int err;

    xnlock_get_irqsave(&nklock, s);
    err = sem_timedwait_internal(shadow, ts2ticks_ceil(abs_timeout)+1);
    xnlock_put_irqrestore(&nklock, s);

    if(err)
        {
        thread_set_errno(err);
        return -1;
        }
    
    return 0;
}

/**
 * Unlock a semaphore.
 *
 * This service unlocks the semaphore @a sm.
 *
 * If no thread is currently blocked on this semaphore, its count is
 * incremented, otherwise the highest priority thread is unblocked.
 *
 * @param sm the semaphore to be unlocked.
 *
 * @retval 0 on success;
 * @retval -1 with errno set if:
 * - EINVAL, the specified semaphore is invalid or uninitialized;
 * - EAGAIN, the semaphore count is @a SEM_VALUE_MAX.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/sem_post.html
 * 
 */
int sem_post (sem_t *sm)

{
    struct __shadow_sem *shadow = &((union __xeno_sem *) sm)->shadow_sem;
    pse51_sem_t *sem;
    spl_t s;
    
    xnlock_get_irqsave(&nklock, s);

    if (shadow->magic != PSE51_SEM_MAGIC
        && shadow->magic != PSE51_NAMED_SEM_MAGIC)
        {
        thread_set_errno(EINVAL);
        goto error;
        }

    sem = shadow->sem;

    if (sem->value == SEM_VALUE_MAX)
        {
        thread_set_errno(EAGAIN);
        goto error;
        }

    if(xnsynch_wakeup_one_sleeper(&sem->synchbase) != NULL)
        xnpod_schedule();
    else
        ++sem->value;

    xnlock_put_irqrestore(&nklock, s);

    return 0;

 error:

    xnlock_put_irqrestore(&nklock, s);

    return -1;
}

/**
 * Get the value of a semaphore.
 *
 * This service stores at the address @a value, the current count of the
 * semaphore @a sm. The state of the semaphore is unchanged.
 *
 * If the semaphore is currently locked, the value stored is zero.
 *
 * @param sm a semaphore;
 *
 * @param value address where the semaphore count will be stored on success.
 *
 * @retval 0 on success;
 * @retval -1 with @a errno set if:
 * - EINVAL, the semaphore is invalid or uninitialized.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/sem_getvalue.html
 * 
 */
int sem_getvalue (sem_t *sm, int *value)

{
    struct __shadow_sem *shadow = &((union __xeno_sem *) sm)->shadow_sem;
    pse51_sem_t *sem;
    spl_t s;

    xnlock_get_irqsave(&nklock, s);

    if (shadow->magic != PSE51_SEM_MAGIC
        && shadow->magic != PSE51_NAMED_SEM_MAGIC)
        {
        xnlock_put_irqrestore(&nklock, s);
        thread_set_errno(EINVAL);
        return -1;
        }

    sem = shadow->sem;

    *value = sem->value;

    xnlock_put_irqrestore(&nklock, s);

    return 0;
}

#if defined(__KERNEL__) && defined(CONFIG_XENO_OPT_PERVASIVE)
/* Must be called nklock locked, irq off. */
unsigned long pse51_usem_open (struct __shadow_sem *shadow,
                               struct mm_struct *mm,
                               unsigned long uaddr)
{
    xnholder_t *holder;
    pse51_uptr_t *uptr;
    nsem_t *nsem;

    if (shadow->magic != PSE51_NAMED_SEM_MAGIC)
        return 0;

    nsem = sem2named_sem(shadow->sem);
     
    for(holder = getheadq(&nsem->userq);
        holder;
        holder = nextq(&nsem->userq, holder))
        {
        pse51_uptr_t *uptr = link2uptr(holder);

        if (uptr->mm == mm)
            {
            ++uptr->refcnt;
            return uptr->uaddr;
            }
        }

    uptr = (pse51_uptr_t *) xnmalloc(sizeof(*uptr));
    uptr->mm = mm;
    uptr->uaddr = uaddr;
    uptr->refcnt = 1;
    inith(&uptr->link);
    appendq(&nsem->userq, &uptr->link);
    return uaddr;
}

/* Must be called nklock locked, irq off. */
int pse51_usem_close (struct __shadow_sem *shadow, struct mm_struct *mm)
{
    nsem_t *nsem;
    pse51_uptr_t *uptr = NULL;
    xnholder_t *holder;

    if (shadow->magic != PSE51_NAMED_SEM_MAGIC)
        return -EINVAL;

    nsem = sem2named_sem(shadow->sem);
 
    for(holder = getheadq(&nsem->userq);
        holder;
        holder = nextq(&nsem->userq, holder))
        {
        uptr = link2uptr(holder);

        if (uptr->mm == mm)
            {
            if (--uptr->refcnt)
                return 0;
            break;
            }
        }

    if (!uptr)
        return -EINVAL;

    removeq(&nsem->userq, &uptr->link);
    xnfree(uptr);
    return 1;
}

/* Must be called nklock locked, irq off. */
void pse51_usems_cleanup (pse51_sem_t *sem)
{
    nsem_t *nsem = sem2named_sem(sem);
    xnholder_t *holder;
    
    while((holder = getheadq(&nsem->userq)))
        {
        pse51_uptr_t *uptr = link2uptr(holder);

#ifdef CONFIG_XENO_OPT_DEBUG
        xnprintf("POSIX semaphore \"%s\" binding for user process"
                 " discarded.\n", nsem->nodebase.name);
#endif /* CONFIG_XENO_OPT_DEBUG */

        removeq(&nsem->userq, &uptr->link);
        xnfree(uptr);
        }
}
#endif /* __KERNEL__ && CONFIG_XENO_OPT_PERVASIVE */

void pse51_sem_pkg_init (void) {

    initq(&pse51_semq);
}

void pse51_sem_pkg_cleanup (void)

{
    xnholder_t *holder;
    spl_t s;

    xnlock_get_irqsave(&nklock, s);

    while ((holder = getheadq(&pse51_semq)) != NULL)
        {
        pse51_sem_t *sem = link2sem(holder);

#if defined(__KERNEL__) && defined(CONFIG_XENO_OPT_PERVASIVE)
        if (sem->magic == PSE51_NAMED_SEM_MAGIC)
            pse51_usems_cleanup(sem);
#endif /* __KERNEL__ && CONFIG_XENO_OPT_PERVASIVE */

#ifdef CONFIG_XENO_OPT_DEBUG
        if (sem->magic == PSE51_SEM_MAGIC)
            xnprintf("POSIX semaphore %p discarded.\n",
                     sem);
        else
            xnprintf("POSIX semaphore \"%s\" discarded.\n",
                     sem2named_sem(sem)->nodebase.name);
#endif /* CONFIG_XENO_OPT_DEBUG */
        sem_destroy_internal(sem);
        }

    xnlock_put_irqrestore(&nklock, s);
}

/*@}*/

EXPORT_SYMBOL(pse51_sem_init);
EXPORT_SYMBOL(sem_destroy);
EXPORT_SYMBOL(sem_post);
EXPORT_SYMBOL(sem_trywait);
EXPORT_SYMBOL(sem_wait);
EXPORT_SYMBOL(sem_timedwait);
EXPORT_SYMBOL(sem_getvalue);
EXPORT_SYMBOL(sem_open);
EXPORT_SYMBOL(sem_close);
EXPORT_SYMBOL(sem_unlink);