mq.c

来自「xenomai 很好的linux实时补丁」· C语言 代码 · 共 879 行 · 第 1/2 页

        if (direct & msg.used)
            return 0;
            
        if (xnthread_test_flags(cur, XNRMID))
            return EBADF;

        if (xnthread_test_flags(cur, XNTIMEO))
            return ETIMEDOUT;

        if (xnthread_test_flags(cur, XNBREAK))
            return EINTR;
        }
}

/**
 * Try during a bounded time to send a message to a message queue.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/mq_timedsend.html
 * 
 */
int mq_timedsend(mqd_t fd,
                 const char * buffer,
                 size_t len,
                 unsigned prio,
                 const struct timespec *abs_timeout)
{
    xnticks_t timeout;
    int err;
    spl_t s;

    if((unsigned long) abs_timeout->tv_nsec >= ONE_BILLION)
        {
        thread_set_errno(EINVAL);
        return -1;
        }

    timeout = ts2ticks_ceil(abs_timeout) + 1;

    xnlock_get_irqsave(&nklock, s);
    err = pse51_mq_timedsend_inner(fd, buffer, len, prio, timeout);
    xnlock_put_irqrestore(&nklock, s);

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

    return 0;
}

/**
 * Send a message to a message queue.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/mq_send.html
 * 
 */
int mq_send(mqd_t fd, const char *buffer, size_t len, unsigned prio)
{
    int err;
    spl_t s;

    xnlock_get_irqsave(&nklock, s);
    err = pse51_mq_timedsend_inner(fd, buffer, len, prio, XN_INFINITE);
    xnlock_put_irqrestore(&nklock, s);

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

    return 0;
}

/**
 * Register the current thread to be notified of message arrival at an empty
 * message queue.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/mq_notify.html
 * 
 */
int mq_notify(mqd_t fd, const struct sigevent *evp)
{
    pthread_t thread = pse51_current_thread();
    pse51_desc_t *desc;
    pse51_mq_t *mq;
    int err;
    spl_t s;

    if (evp && ((evp->sigev_notify != SIGEV_SIGNAL &&
		 evp->sigev_notify != SIGEV_NONE) ||
                (unsigned) (evp->sigev_signo - 1) > SIGRTMAX - 1))
        {
        err = EINVAL;
        goto error;
        }

    if (xnpod_asynch_p() || !thread)
        {
        err = EPERM;
        goto error;
        }

    xnlock_get_irqsave(&nklock, s);

    err = pse51_desc_get(&desc, fd, PSE51_MQ_MAGIC);

    if(err)
        goto unlock_and_error;

    mq = node2mq(pse51_desc_node(desc));

    if (mq->target && mq->target != thread)
        {
        err = EBUSY;
        goto unlock_and_error;
        }

    if (!evp || evp->sigev_notify == SIGEV_NONE)
        /* Here, mq->target == pse51_current_thread() or NULL. */
        mq->target = NULL;
    else
        {
        mq->target = thread;
        mq->si.info.si_signo = evp->sigev_signo;
        mq->si.info.si_code = SI_MESGQ;
        mq->si.info.si_value = evp->sigev_value;
        }

    xnlock_put_irqrestore(&nklock, s);
    return 0;

  unlock_and_error:
    xnlock_put_irqrestore(&nklock, s);
  error:
    thread_set_errno(err);
    return -1;
}

/**
 * Try during a bounded time to receive a message from a message queue.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/mq_timedreceive.html
 * 
 */
ssize_t mq_timedreceive(mqd_t fd,
                        char *__restrict__ buffer,
                        size_t len,
                        unsigned *__restrict__ priop,
                        const struct timespec *__restrict__ abs_timeout)
{
    xnticks_t timeout;
    int err;
    spl_t s;

    if((unsigned long) abs_timeout->tv_nsec >= ONE_BILLION)
        {
        thread_set_errno(EINVAL);
        return -1;
        }

    timeout = ts2ticks_ceil(abs_timeout) + 1;

    xnlock_get_irqsave(&nklock, s);
    err = pse51_mq_timedrcv_inner(fd, buffer, &len, priop, timeout);
    xnlock_put_irqrestore(&nklock, s);

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

    return len;    
}

/**
 * Receive a message from a message queue.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/mq_receive.html
 * 
 */
ssize_t mq_receive(mqd_t fd, char *buffer, size_t len, unsigned *priop)
{
    int err;
    spl_t s;

    xnlock_get_irqsave(&nklock, s);
    err = pse51_mq_timedrcv_inner(fd, buffer, &len, priop, XN_INFINITE);
    xnlock_put_irqrestore(&nklock, s);

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

    return len;    
}


/**
 * Open a message queue.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/mq_open.html
 * 
 */
mqd_t mq_open(const char *name, int oflags, ...)
{
    struct mq_attr *attr;
    xnsynch_t done_synch;
    pse51_node_t *node;
    pse51_desc_t *desc;
    spl_t s, ignored;
    pse51_mq_t *mq;
    mode_t mode;
    va_list ap;
    int err;
    
    xnlock_get_irqsave(&nklock, s);

    err = pse51_node_get(&node, name, PSE51_MQ_MAGIC, oflags);

    if(err)
        goto error;

    if(node)
        {
        mq = node2mq(node);
        goto got_mq;
        }

    /* Here, we know that we must create a message queue. */
    mq = (pse51_mq_t *) xnmalloc(sizeof(*mq));
    
    if(!mq)
        {
        err = ENOMEM;
        goto error;
        }

    err = pse51_node_add_start(&mq->nodebase, name, PSE51_MQ_MAGIC, &done_synch);
    if(err)
        goto error;
    xnlock_clear_irqon(&nklock);

    /* Release the global lock while creating the message queue. */
    va_start(ap, oflags);
    mode = va_arg(ap, int); /* unused */
    attr = va_arg(ap, struct mq_attr *);
    va_end(ap);    

    err = pse51_mq_init(mq, attr);

    xnlock_get_irqsave(&nklock, ignored);
    pse51_node_add_finished(&mq->nodebase, err);
    if(err)
        {
        xnlock_put_irqrestore(&nklock, s);
        goto err_free_mq;
        }

    inith(&mq->link);
    appendq(&pse51_mqq, &mq->link);

    /* Whether found or created, here we have a valid message queue. */
  got_mq:
    err = pse51_desc_create(&desc, &mq->nodebase);

    if(err)
        goto err_put_mq;

    pse51_desc_setflags(desc, oflags & (O_NONBLOCK | PSE51_PERMS_MASK));

    xnlock_put_irqrestore(&nklock, s);

    return (mqd_t) pse51_desc_fd(desc);


  err_put_mq:
    pse51_node_put(&mq->nodebase);

    if(pse51_node_removed_p(&mq->nodebase))
        {
        /* mq is no longer referenced, we may destroy it. */
        xnlock_put_irqrestore(&nklock, s);

        pse51_mq_destroy(mq);
  err_free_mq:
        xnfree(mq);
        }
    else
  error:
        xnlock_put_irqrestore(&nklock, s);

    thread_set_errno(err);

    return (mqd_t) -1;
}

/**
 * Close a message queue.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/mq_close.html
 * 
 */
int mq_close(mqd_t fd)
{
    pse51_desc_t *desc;
    pse51_mq_t *mq;
    spl_t s;
    int err;

    if (xnpod_asynch_p() || !xnpod_root_p())
        {
        err = EPERM;
        goto error;
        }

    xnlock_get_irqsave(&nklock, s);

    err = pse51_desc_get(&desc, fd, PSE51_MQ_MAGIC);

    if(err)
        goto err_unlock;

    mq = node2mq(pse51_desc_node(desc));
    
    err = pse51_desc_destroy(desc);

    if(err)
        goto err_unlock;

    err = pse51_node_put(&mq->nodebase);

    if(err)
        goto err_unlock;
    
    if(pse51_node_removed_p(&mq->nodebase))
        {
        xnlock_put_irqrestore(&nklock, s);

        pse51_mq_destroy(mq);
        xnfree(mq);
        }
    else
        xnlock_put_irqrestore(&nklock, s);

    return 0;

  err_unlock:
    xnlock_put_irqrestore(&nklock, s);
  error:
    thread_set_errno(err);
    return -1;
}

/**
 * Unlink a message queue.
 *
 * @see http://www.opengroup.org/onlinepubs/000095399/functions/mq_unlink.html
 * 
 */
int mq_unlink(const char *name)
{
    pse51_node_t *node;
    pse51_mq_t *mq;
    spl_t s;
    int err;

    if (xnpod_asynch_p() || !xnpod_root_p())
        {
        err = EPERM;
        goto error;
        }

    xnlock_get_irqsave(&nklock, s);

    err = pse51_node_remove(&node, name, PSE51_MQ_MAGIC);

    if(!err && pse51_node_removed_p(node))
        {
        xnlock_put_irqrestore(&nklock, s);

        mq = node2mq(node);
        pse51_mq_destroy(mq);
        xnfree(mq);
        }
    else
        xnlock_put_irqrestore(&nklock, s);

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

    return 0;
}

int pse51_mq_pkg_init(void)
{
    initq(&pse51_mqq);
    return 0;
}

void pse51_mq_pkg_cleanup(void)
{
    xnholder_t *holder;

    while ((holder = getheadq(&pse51_mqq)))
        {
        pse51_mq_t *mq = link2mq(holder);
        pse51_node_t *node;
#ifdef CONFIG_XENO_OPT_DEBUG
        xnprintf("Posix message queue %s was not unlinked, unlinking now.\n",
                 mq->nodebase.name);
#endif /* CONFIG_XENO_OPT_DEBUG */
        pse51_node_remove(&node, mq->nodebase.name, PSE51_MQ_MAGIC);
        pse51_mq_destroy(mq);
        xnfree(mq);
        }
}

/*@}*/

EXPORT_SYMBOL(mq_open);
EXPORT_SYMBOL(mq_getattr);
EXPORT_SYMBOL(mq_setattr);
EXPORT_SYMBOL(mq_send);
EXPORT_SYMBOL(mq_timedsend);
EXPORT_SYMBOL(mq_receive);
EXPORT_SYMBOL(mq_timedreceive);
EXPORT_SYMBOL(mq_close);
EXPORT_SYMBOL(mq_unlink);