/*
|
* 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 <assert.h>
|
#include <memory.h>
|
#include <copperplate/heapobj.h>
|
#include <copperplate/threadobj.h>
|
#include <vxworks/errnoLib.h>
|
#include "reference.h"
|
#include "taskLib.h"
|
#include "msgQLib.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 thread
|
* base object directly.
|
*/
|
|
#define mq_magic 0x4a5b6c7d
|
|
struct msgholder {
|
int size;
|
struct holder link;
|
/* Payload data follows. */
|
};
|
|
static struct wind_mq *find_mq_from_id(MSG_Q_ID qid)
|
{
|
struct wind_mq *mq = mainheap_deref(qid, struct wind_mq);
|
|
if (mq == NULL || ((uintptr_t)mq & (sizeof(uintptr_t)-1)) != 0 ||
|
mq->magic != mq_magic)
|
return NULL;
|
|
return mq;
|
}
|
|
static void mq_finalize(struct syncobj *sobj)
|
{
|
struct wind_mq *mq = container_of(sobj, struct wind_mq, sobj);
|
heapobj_destroy(&mq->pool);
|
xnfree(mq);
|
}
|
fnref_register(libvxworks, mq_finalize);
|
|
MSG_Q_ID msgQCreate(int maxMsgs, int maxMsgLength, int options)
|
{
|
int sobj_flags = 0, ret;
|
struct wind_mq *mq;
|
struct service svc;
|
|
if (threadobj_irq_p()) {
|
errno = S_intLib_NOT_ISR_CALLABLE;
|
return (MSG_Q_ID)0;
|
}
|
|
if ((options & ~MSG_Q_PRIORITY) || maxMsgs <= 0) {
|
errno = S_msgQLib_INVALID_QUEUE_TYPE;
|
return (MSG_Q_ID)0;
|
}
|
|
if (maxMsgLength < 0) {
|
errno = S_msgQLib_INVALID_MSG_LENGTH;
|
return (MSG_Q_ID)0;
|
}
|
|
CANCEL_DEFER(svc);
|
|
mq = xnmalloc(sizeof(*mq));
|
if (mq == NULL)
|
goto fail_cballoc;
|
|
/*
|
* The message pool must come from the main heap because of
|
* mq->msg_list (this queue head and messages from the pool
|
* must share the same allocation base). Create the heap
|
* object accordingly.
|
*/
|
if (heapobj_init_array(&mq->pool, NULL, maxMsgLength +
|
sizeof(struct msgholder), maxMsgs))
|
goto fail_bufalloc;
|
|
if (options & MSG_Q_PRIORITY)
|
sobj_flags = SYNCOBJ_PRIO;
|
|
ret = syncobj_init(&mq->sobj, CLOCK_COPPERPLATE, sobj_flags,
|
fnref_put(libvxworks, mq_finalize));
|
if (ret)
|
goto fail_syncinit;
|
|
mq->options = options;
|
mq->maxmsg = maxMsgs;
|
mq->msgsize = maxMsgLength;
|
mq->msgcount = 0;
|
list_init(&mq->msg_list);
|
|
mq->magic = mq_magic;
|
|
CANCEL_RESTORE(svc);
|
|
return mainheap_ref(mq, MSG_Q_ID);
|
|
fail_syncinit:
|
heapobj_destroy(&mq->pool);
|
fail_bufalloc:
|
xnfree(mq);
|
fail_cballoc:
|
errno = S_memLib_NOT_ENOUGH_MEMORY;
|
|
CANCEL_RESTORE(svc);
|
|
return (MSG_Q_ID)0;
|
}
|
|
STATUS msgQDelete(MSG_Q_ID msgQId)
|
{
|
struct syncstate syns;
|
struct wind_mq *mq;
|
struct service svc;
|
|
if (threadobj_irq_p()) {
|
errno = S_intLib_NOT_ISR_CALLABLE;
|
return ERROR;
|
}
|
|
mq = find_mq_from_id(msgQId);
|
if (mq == NULL)
|
goto objid_error;
|
|
CANCEL_DEFER(svc);
|
|
if (syncobj_lock(&mq->sobj, &syns)) {
|
CANCEL_RESTORE(svc);
|
objid_error:
|
errno = S_objLib_OBJ_ID_ERROR;
|
return ERROR;
|
}
|
|
mq->magic = ~mq_magic; /* Prevent further reference. */
|
syncobj_destroy(&mq->sobj, &syns);
|
|
CANCEL_RESTORE(svc);
|
|
return OK;
|
}
|
|
int msgQReceive(MSG_Q_ID msgQId, char *buffer, UINT maxNBytes, int timeout)
|
{
|
struct wind_queue_wait *wait = NULL;
|
struct timespec ts, *timespec;
|
struct msgholder *msg = NULL;
|
UINT nbytes = (UINT)ERROR;
|
struct syncstate syns;
|
struct wind_mq *mq;
|
struct service svc;
|
int ret;
|
|
if (threadobj_irq_p()) {
|
errno = S_intLib_NOT_ISR_CALLABLE;
|
return ERROR;
|
}
|
|
mq = find_mq_from_id(msgQId);
|
if (mq == NULL)
|
goto objid_error;
|
|
CANCEL_DEFER(svc);
|
|
if (syncobj_lock(&mq->sobj, &syns)) {
|
CANCEL_RESTORE(svc);
|
objid_error:
|
errno = S_objLib_OBJ_ID_ERROR;
|
return ERROR;
|
}
|
|
retry:
|
if (!list_empty(&mq->msg_list)) {
|
mq->msgcount--;
|
msg = list_pop_entry(&mq->msg_list, struct msgholder, link);
|
nbytes = msg->size;
|
if (nbytes > maxNBytes)
|
nbytes = maxNBytes;
|
if (nbytes > 0)
|
memcpy(buffer, msg + 1, nbytes);
|
heapobj_free(&mq->pool, msg);
|
syncobj_drain(&mq->sobj);
|
goto done;
|
}
|
|
if (timeout == NO_WAIT) {
|
errno = S_objLib_OBJ_UNAVAILABLE;
|
goto done;
|
}
|
|
if (timeout != WAIT_FOREVER) {
|
timespec = &ts;
|
clockobj_ticks_to_timeout(&wind_clock, timeout, timespec);
|
} else
|
timespec = NULL;
|
|
wait = threadobj_prepare_wait(struct wind_queue_wait);
|
wait->ptr = __moff(buffer);
|
wait->size = maxNBytes;
|
|
ret = syncobj_wait_grant(&mq->sobj, timespec, &syns);
|
if (ret == -EIDRM) {
|
errno = S_objLib_OBJ_DELETED;
|
goto out;
|
}
|
if (ret == -ETIMEDOUT) {
|
errno = S_objLib_OBJ_TIMEOUT;
|
goto done;
|
}
|
nbytes = wait->size;
|
if (nbytes == -1L) /* No direct copy? */
|
goto retry;
|
syncobj_drain(&mq->sobj);
|
done:
|
syncobj_unlock(&mq->sobj, &syns);
|
out:
|
if (wait)
|
threadobj_finish_wait();
|
|
CANCEL_RESTORE(svc);
|
|
return nbytes;
|
}
|
|
STATUS msgQSend(MSG_Q_ID msgQId, const char *buffer, UINT bytes,
|
int timeout, int prio)
|
{
|
struct timespec ts, *timespec;
|
struct wind_queue_wait *wait;
|
struct threadobj *thobj;
|
struct msgholder *msg;
|
struct syncstate syns;
|
struct wind_mq *mq;
|
struct service svc;
|
int ret = ERROR;
|
UINT maxbytes;
|
|
CANCEL_DEFER(svc);
|
|
mq = find_mq_from_id(msgQId);
|
if (mq == NULL)
|
goto objid_error;
|
|
if (syncobj_lock(&mq->sobj, &syns)) {
|
CANCEL_RESTORE(svc);
|
objid_error:
|
errno = S_objLib_OBJ_ID_ERROR;
|
return ERROR;
|
}
|
|
if (bytes > mq->msgsize) {
|
errno = S_msgQLib_INVALID_MSG_LENGTH;
|
goto fail;
|
}
|
|
thobj = syncobj_peek_grant(&mq->sobj);
|
if (thobj && threadobj_local_p(thobj)) {
|
/* Fast path: direct copy to the receiver's buffer. */
|
wait = threadobj_get_wait(thobj);
|
maxbytes = wait->size;
|
if (bytes > maxbytes)
|
bytes = maxbytes;
|
if (bytes > 0)
|
memcpy(__mptr(wait->ptr), buffer, bytes);
|
wait->size = bytes;
|
goto done;
|
}
|
|
if (mq->msgcount < mq->maxmsg)
|
goto enqueue;
|
|
if (timeout == NO_WAIT) {
|
errno = S_objLib_OBJ_UNAVAILABLE;
|
goto fail;
|
}
|
|
if (threadobj_irq_p()) {
|
errno = S_msgQLib_NON_ZERO_TIMEOUT_AT_INT_LEVEL;
|
goto fail;
|
}
|
|
if (timeout != WAIT_FOREVER) {
|
timespec = &ts;
|
clockobj_ticks_to_timeout(&wind_clock, timeout, timespec);
|
} else
|
timespec = NULL;
|
|
do {
|
ret = syncobj_wait_drain(&mq->sobj, timespec, &syns);
|
if (ret == -EIDRM) {
|
errno = S_objLib_OBJ_DELETED;
|
ret = ERROR;
|
goto out;
|
}
|
if (ret == -ETIMEDOUT) {
|
errno = S_objLib_OBJ_TIMEOUT;
|
ret = ERROR;
|
goto fail;
|
}
|
} while (mq->msgcount >= mq->maxmsg);
|
|
enqueue:
|
msg = heapobj_alloc(&mq->pool, bytes + sizeof(*msg));
|
if (msg == NULL) {
|
errno = S_memLib_NOT_ENOUGH_MEMORY;
|
ret = ERROR;
|
goto fail;
|
}
|
|
mq->msgcount++;
|
assert(mq->msgcount <= mq->maxmsg); /* Paranoid. */
|
msg->size = bytes;
|
holder_init(&msg->link);
|
|
if (bytes > 0)
|
memcpy(msg + 1, buffer, bytes);
|
|
if (prio == MSG_PRI_NORMAL)
|
list_append(&msg->link, &mq->msg_list);
|
else
|
list_prepend(&msg->link, &mq->msg_list);
|
|
if (thobj) {
|
/*
|
* We could not copy the message directly to the
|
* remote buffer, tell the thread to pull it from the
|
* pool.
|
*/
|
wait = threadobj_get_wait(thobj);
|
wait->size = -1UL;
|
}
|
done:
|
if (thobj) /* Wakeup waiter. */
|
syncobj_grant_to(&mq->sobj, thobj);
|
|
ret = OK;
|
fail:
|
syncobj_unlock(&mq->sobj, &syns);
|
out:
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
|
int msgQNumMsgs(MSG_Q_ID msgQId)
|
{
|
struct syncstate syns;
|
struct wind_mq *mq;
|
struct service svc;
|
int msgcount;
|
|
mq = find_mq_from_id(msgQId);
|
if (mq == NULL)
|
goto objid_error;
|
|
CANCEL_DEFER(svc);
|
|
if (syncobj_lock(&mq->sobj, &syns)) {
|
CANCEL_RESTORE(svc);
|
objid_error:
|
errno = S_objLib_OBJ_ID_ERROR;
|
return ERROR;
|
}
|
|
msgcount = mq->msgcount;
|
syncobj_unlock(&mq->sobj, &syns);
|
|
CANCEL_RESTORE(svc);
|
|
return msgcount;
|
}
|
