~ljy/RK3588_XEN.git

/*
 * Copyright (C) 2008 Philippe Gerum <rpm@xenomai.org>.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library 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
 * Lesser General Public License for more details.
 
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA.
 */
 
#include <stdio.h>
#include <errno.h>
#include <stdlib.h>
#include <limits.h>
#include <boilerplate/ancillaries.h>
#include <copperplate/heapobj.h>
#include <vxworks/errnoLib.h>
#include "reference.h"
#include "taskLib.h"
#include "semLib.h"
#include "tickLib.h"
 
/*
 * XXX: In order to keep the following services callable from
 * non-VxWorks tasks (but still Xenomai ones, though), make sure
 * to never depend on the wind_task struct, but rather on the basic
 * thread object directly.
 */
 
#define sem_magic    0x2a3b4c5d
 
static struct wind_sem *alloc_sem(int options, const struct wind_sem_ops *ops)
{
    struct wind_sem *sem;
 
    sem = xnmalloc(sizeof(*sem));
    if (sem == NULL) {
        errno = S_memLib_NOT_ENOUGH_MEMORY;
        return NULL;
    }
 
    sem->options = options;
    sem->semops = ops;
    sem->magic = sem_magic;
 
    return sem;
}
 
static STATUS xsem_take(struct wind_sem *sem, int timeout)
{
    struct timespec ts, *timespec;
    struct syncstate syns;
    struct service svc;
    STATUS ret = OK;
 
    if (threadobj_irq_p())
        return S_intLib_NOT_ISR_CALLABLE;
 
    CANCEL_DEFER(svc);
 
    if (syncobj_lock(&sem->u.xsem.sobj, &syns)) {
        ret = S_objLib_OBJ_ID_ERROR;
        goto out;
    }
 
    if (--sem->u.xsem.value >= 0)
        goto done;
 
    if (timeout == NO_WAIT) {
        sem->u.xsem.value++;
        ret = S_objLib_OBJ_UNAVAILABLE;
        goto done;
    }
 
    if (timeout != WAIT_FOREVER) {
        timespec = &ts;
        clockobj_ticks_to_timeout(&wind_clock, timeout, timespec);
    } else
        timespec = NULL;
 
    ret = syncobj_wait_grant(&sem->u.xsem.sobj, timespec, &syns);
    if (ret == -EIDRM) {
        ret = S_objLib_OBJ_DELETED;
        goto out;
    }
    if (ret) {
        sem->u.xsem.value++;
        if (ret == -ETIMEDOUT)
            ret = S_objLib_OBJ_TIMEOUT;
        else if (ret == -EINTR)
            ret = OK;    /* Flushed. */
    }
done:
    syncobj_unlock(&sem->u.xsem.sobj, &syns);
out:
    CANCEL_RESTORE(svc);
 
    return ret;
}
 
static STATUS xsem_give(struct wind_sem *sem)
{
    struct syncstate syns;
    struct service svc;
    STATUS ret = OK;
 
    CANCEL_DEFER(svc);
 
    if (syncobj_lock(&sem->u.xsem.sobj, &syns)) {
        ret = S_objLib_OBJ_ID_ERROR;
        goto out;
    }
 
    if (sem->u.xsem.value >= sem->u.xsem.maxvalue) {
        if (sem->u.xsem.maxvalue == INT_MAX)
            /* No wrap around. */
            ret = S_semLib_INVALID_OPERATION;
    } else if (++sem->u.xsem.value <= 0)
        syncobj_grant_one(&sem->u.xsem.sobj);
 
    syncobj_unlock(&sem->u.xsem.sobj, &syns);
out:
    CANCEL_RESTORE(svc);
 
    return ret;
}
 
static STATUS xsem_flush(struct wind_sem *sem)
{
    struct syncstate syns;
    struct service svc;
    STATUS ret = OK;
 
    CANCEL_DEFER(svc);
 
    if (syncobj_lock(&sem->u.xsem.sobj, &syns)) {
        ret = S_objLib_OBJ_ID_ERROR;
        goto out;
    }
 
    syncobj_flush(&sem->u.xsem.sobj);
 
    syncobj_unlock(&sem->u.xsem.sobj, &syns);
out:
    CANCEL_RESTORE(svc);
 
    return ret;
}
 
static void sem_finalize(struct syncobj *sobj)
{
    struct wind_sem *sem = container_of(sobj, struct wind_sem, u.xsem.sobj);
    xnfree(sem);
}
fnref_register(libvxworks, sem_finalize);
 
static STATUS xsem_delete(struct wind_sem *sem)
{
    struct syncstate syns;
    struct service svc;
    int ret = OK;
 
    if (threadobj_irq_p())
        return S_intLib_NOT_ISR_CALLABLE;
 
    CANCEL_DEFER(svc);
 
    if (syncobj_lock(&sem->u.xsem.sobj, &syns)) {
        ret = S_objLib_OBJ_ID_ERROR;
        goto out;
    }
 
    sem->magic = ~sem_magic; /* Prevent further reference. */
    syncobj_destroy(&sem->u.xsem.sobj, &syns);
out:
    CANCEL_RESTORE(svc);
 
    return ret;
}
 
static const struct wind_sem_ops xsem_ops = {
    .take = xsem_take,
    .give = xsem_give,
    .flush = xsem_flush,
    .delete = xsem_delete
};
 
static SEM_ID alloc_xsem(int options, int initval, int maxval)
{
    int sobj_flags = 0, ret;
    struct wind_sem *sem;
 
    if (options & ~SEM_Q_PRIORITY) {
        errno = S_semLib_INVALID_OPTION;
        return (SEM_ID)0;
    }
 
    sem = alloc_sem(options, &xsem_ops);
    if (sem == NULL) {
        errno = S_memLib_NOT_ENOUGH_MEMORY;
        return (SEM_ID)0;
    }
 
    if (options & SEM_Q_PRIORITY)
        sobj_flags = SYNCOBJ_PRIO;
 
    sem->u.xsem.value = initval;
    sem->u.xsem.maxvalue = maxval;
    ret = syncobj_init(&sem->u.xsem.sobj, CLOCK_COPPERPLATE, sobj_flags,
               fnref_put(libvxworks, sem_finalize));
    if (ret) {
        xnfree(sem);
        errno = S_memLib_NOT_ENOUGH_MEMORY;
        return (SEM_ID)0;
    }
 
    return mainheap_ref(sem, SEM_ID);
}
 
static STATUS msem_take(struct wind_sem *sem, int timeout)
{
    struct wind_task *current;
    struct timespec ts;
    int ret;
 
    if (threadobj_irq_p())
        return S_intLib_NOT_ISR_CALLABLE;
 
    /*
     * We allow threads from other APIs to grab a VxWorks mutex
     * ignoring the safe option in such a case.
     */
    current = wind_task_current();
    if (current && (sem->options & SEM_DELETE_SAFE))
        __RT(pthread_mutex_lock(&current->safelock));
 
    if (timeout == NO_WAIT) {
        ret = __RT(pthread_mutex_trylock(&sem->u.msem.lock));
        goto check;
    }
 
    if  (timeout == WAIT_FOREVER) {
        ret = __RT(pthread_mutex_lock(&sem->u.msem.lock));
        goto check;
    }
 
    __clockobj_ticks_to_timeout(&wind_clock, CLOCK_REALTIME, timeout, &ts);
    ret = __RT(pthread_mutex_timedlock(&sem->u.msem.lock, &ts));
check:
    switch (ret) {
    case 0:
        return OK;
    case EINVAL:
        ret = S_objLib_OBJ_ID_ERROR;
        break;
    case EBUSY:
        ret = S_objLib_OBJ_UNAVAILABLE;
        break;
    case ETIMEDOUT:
        ret = S_objLib_OBJ_TIMEOUT;
        break;
    case EOWNERDEAD:
    case ENOTRECOVERABLE:
        warning("owner of mutex-type semaphore %p died", sem);
        ret = S_objLib_OBJ_UNAVAILABLE;
        break;
    }
 
    if (current != NULL && (sem->options & SEM_DELETE_SAFE))
        __RT(pthread_mutex_unlock(&current->safelock));
 
    return ret;
}
 
static STATUS msem_give(struct wind_sem *sem)
{
    struct wind_task *current;
    int ret;
 
    if (threadobj_irq_p())
        return S_intLib_NOT_ISR_CALLABLE;
 
    ret = __RT(pthread_mutex_unlock(&sem->u.msem.lock));
    if (ret == EINVAL)
        return S_objLib_OBJ_ID_ERROR;
    if (ret == EPERM)
        return S_semLib_INVALID_OPERATION;
 
    if (sem->options & SEM_DELETE_SAFE) {
        current = wind_task_current();
        if (current)
            __RT(pthread_mutex_unlock(&current->safelock));
    }
 
    return OK;
}
 
static STATUS msem_flush(struct wind_sem *sem)
{
    return S_semLib_INVALID_OPERATION;
}
 
static STATUS msem_delete(struct wind_sem *sem)
{
    int ret;
 
    if (threadobj_irq_p())
        return S_intLib_NOT_ISR_CALLABLE;
 
    ret = __RT(pthread_mutex_destroy(&sem->u.msem.lock));
    if (ret == EINVAL)
        return S_objLib_OBJ_ID_ERROR;
    /*
     * XXX: We depart from the spec here since we can't flush, but
     * we tell the caller about any pending task instead.
     */
    if (ret == EBUSY)
        return S_semLib_INVALID_OPERATION;
    else
        xnfree(sem);
 
    return OK;
}
 
static const struct wind_sem_ops msem_ops = {
    .take = msem_take,
    .give = msem_give,
    .flush = msem_flush,
    .delete = msem_delete
};
 
SEM_ID semBCreate(int options, SEM_B_STATE state)
{
    struct service svc;
    SEM_ID sem_id;
 
    CANCEL_DEFER(svc);
    sem_id = alloc_xsem(options, state, 1);
    CANCEL_RESTORE(svc);
 
    return sem_id;
}
 
SEM_ID semCCreate(int options, int count)
{
    struct service svc;
    SEM_ID sem_id;
 
    CANCEL_DEFER(svc);
    sem_id = alloc_xsem(options, count, INT_MAX);
    CANCEL_RESTORE(svc);
 
    return sem_id;
}
 
SEM_ID semMCreate(int options)
{
    pthread_mutexattr_t mattr;
    struct wind_sem *sem;
    struct service svc;
 
    if (options & ~(SEM_Q_PRIORITY|SEM_DELETE_SAFE|SEM_INVERSION_SAFE)) {
        errno = S_semLib_INVALID_OPTION;
        return (SEM_ID)0;
    }
 
    if ((options & SEM_Q_PRIORITY) == 0) {
        if (options & SEM_INVERSION_SAFE) {
            errno = S_semLib_INVALID_QUEUE_TYPE; /* C'mon... */
            return (SEM_ID)0;
        }
    }
 
    CANCEL_DEFER(svc);
 
    sem = alloc_sem(options, &msem_ops);
    if (sem == NULL) {
        errno = S_memLib_NOT_ENOUGH_MEMORY;
        CANCEL_RESTORE(svc);
        return (SEM_ID)0;
    }
 
    /*
     * XXX: POSIX-wise, we have a few issues with emulating
     * VxWorks semaphores of the mutex kind.
     *
     * VxWorks flushes any kind of semaphore upon deletion
     * (however, explicit semFlush() is not allowed on the mutex
     * kind though); but POSIX doesn't implement such mechanism on
     * its mutex object. At the same time, we need priority
     * inheritance when SEM_INVERSION_SAFE is passed, so we can't
     * emulate VxWorks mutex semaphores using condvars. Since the
     * only way to get priority inheritance is to use a POSIX
     * mutex, we choose not to emulate flushing in semDelete(),
     * but keep inversion-safe locking possible.
     *
     * The same way, we don't support FIFO ordering for mutexes,
     * since this would require to handle them as recursive binary
     * semaphores with ownership, for no obvious upside.
     * Logically speaking, relying on recursion without any
     * consideration for priority while serializing threads is
     * just asking for troubles anyway.
     */
    pthread_mutexattr_init(&mattr);
    pthread_mutexattr_settype(&mattr, PTHREAD_MUTEX_RECURSIVE);
    /* pthread_mutexattr_setrobust() might not be implemented. */
    pthread_mutexattr_setrobust(&mattr, PTHREAD_MUTEX_ROBUST_NP);
    if (options & SEM_INVERSION_SAFE)
        pthread_mutexattr_setprotocol(&mattr, PTHREAD_PRIO_INHERIT);
    pthread_mutexattr_setpshared(&mattr, mutex_scope_attribute);
 
    __RT(pthread_mutex_init(&sem->u.msem.lock, &mattr));
    pthread_mutexattr_destroy(&mattr);
 
    CANCEL_RESTORE(svc);
 
    return mainheap_ref(sem, SEM_ID);
}
 
static struct wind_sem *find_sem_from_id(SEM_ID sem_id)
{
    struct wind_sem *sem = mainheap_deref(sem_id, struct wind_sem);
 
    if (sem == NULL || ((uintptr_t)sem & (sizeof(uintptr_t)-1)) != 0 ||
        sem->magic != sem_magic)
        return NULL;
 
    return sem;
}
 
#define do_sem_op(sem_id, op, args...)            \
do {                            \
    struct wind_sem *sem;                \
    struct service svc;                \
    STATUS ret;                    \
                            \
    sem = find_sem_from_id(sem_id);            \
    if (sem == NULL) {                \
        errno = S_objLib_OBJ_ID_ERROR;        \
        return ERROR;                \
    }                        \
                            \
    CANCEL_DEFER(svc);            \
    ret = sem->semops->op(sem , ##args);        \
    CANCEL_RESTORE(svc);            \
    if (ret) {                    \
        errno = ret;                \
        ret = ERROR;                \
    }                        \
                            \
    return ret;                    \
} while(0)
 
STATUS semDelete(SEM_ID sem_id)
{
    do_sem_op(sem_id, delete);
}
 
STATUS semGive(SEM_ID sem_id)
{
    do_sem_op(sem_id, give);
}
 
STATUS semTake(SEM_ID sem_id, int timeout)
{
    do_sem_op(sem_id, take, timeout);
}
 
STATUS semFlush(SEM_ID sem_id)
{
    do_sem_op(sem_id, flush);
}