/*
|
* 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 <stdint.h>
|
#include <errno.h>
|
#include <stdlib.h>
|
#include <memory.h>
|
#include <boilerplate/ancillaries.h>
|
#include <copperplate/threadobj.h>
|
#include <copperplate/heapobj.h>
|
#include <copperplate/clockobj.h>
|
#include <copperplate/cluster.h>
|
#include <psos/psos.h>
|
#include "internal.h"
|
#include "reference.h"
|
#include "task.h"
|
#include "queue.h"
|
#include "tm.h"
|
|
#define queue_magic 0x8181fdfd
|
|
struct cluster psos_queue_table;
|
|
static unsigned long anon_qids;
|
|
struct msgholder {
|
int size;
|
struct holder link;
|
/* Payload data follows. */
|
};
|
|
static struct psos_queue *get_queue_from_id(u_long qid, int *err_r)
|
{
|
struct psos_queue *q = mainheap_deref(qid, struct psos_queue);
|
|
if (q == NULL || ((uintptr_t)q & (sizeof(uintptr_t)-1)) != 0)
|
goto objid_error;
|
|
if (q->magic == queue_magic)
|
return q;
|
|
if (q->magic == ~queue_magic) {
|
*err_r = ERR_OBJDEL;
|
return NULL;
|
}
|
|
if ((q->magic >> 16) == 0x8181) {
|
*err_r = ERR_OBJTYPE;
|
return NULL;
|
}
|
|
objid_error:
|
*err_r = ERR_OBJID;
|
|
return NULL;
|
}
|
|
static void queue_finalize(struct syncobj *sobj)
|
{
|
struct psos_queue *q = container_of(sobj, struct psos_queue, sobj);
|
xnfree(q);
|
}
|
fnref_register(libpsos, queue_finalize);
|
|
static u_long __q_create(const char *name, u_long count,
|
u_long flags, u_long maxlen, u_long *qid_r)
|
{
|
struct psos_queue *q;
|
struct service svc;
|
int sobj_flags = 0;
|
int ret = SUCCESS;
|
char short_name[5];
|
|
CANCEL_DEFER(svc);
|
|
q = xnmalloc(sizeof(*q));
|
if (q == NULL) {
|
ret = ERR_NOQCB;
|
goto out;
|
}
|
|
if (name == NULL || *name == '\0')
|
sprintf(q->name, "q%lu", ++anon_qids);
|
else {
|
name = psos_trunc_name(short_name, name);
|
namecpy(q->name, name);
|
}
|
|
if (flags & Q_PRIOR)
|
sobj_flags = SYNCOBJ_PRIO;
|
|
q->flags = flags;
|
q->maxmsg = (flags & Q_LIMIT) ? count : 0;
|
q->maxlen = maxlen;
|
ret = syncobj_init(&q->sobj, CLOCK_COPPERPLATE, sobj_flags,
|
fnref_put(libpsos, queue_finalize));
|
if (ret) {
|
ret = ERR_NOQCB;
|
goto fail_syncinit;
|
}
|
|
list_init(&q->msg_list);
|
q->msgcount = 0;
|
q->magic = queue_magic;
|
*qid_r = mainheap_ref(q, u_long);
|
|
if (cluster_addobj_dup(&psos_queue_table, q->name, &q->cobj)) {
|
warning("cannot register queue: %s", q->name);
|
ret = ERR_OBJID;
|
goto fail_register;
|
}
|
|
CANCEL_RESTORE(svc);
|
|
return 0;
|
|
fail_register:
|
syncobj_uninit(&q->sobj);
|
fail_syncinit:
|
xnfree(q);
|
out:
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
|
u_long q_create(const char *name,
|
u_long count, u_long flags, u_long *qid_r)
|
{
|
return __q_create(name, count, flags & ~Q_VARIABLE, sizeof(u_long[4]), qid_r);
|
}
|
|
u_long q_vcreate(const char *name, u_long flags,
|
u_long count, u_long maxlen, u_long *qid_r)
|
{
|
return __q_create(name, count, flags | Q_VARIABLE, maxlen, qid_r);
|
}
|
|
static u_long __q_delete(u_long qid, u_long flags)
|
{
|
struct syncstate syns;
|
struct msgholder *msg;
|
struct psos_queue *q;
|
struct service svc;
|
int ret, emptyq;
|
|
q = get_queue_from_id(qid, &ret);
|
if (q == NULL)
|
return ret;
|
|
CANCEL_DEFER(svc);
|
|
if (syncobj_lock(&q->sobj, &syns))
|
return ERR_OBJDEL;
|
|
if (((flags ^ q->flags) & Q_VARIABLE)) {
|
syncobj_unlock(&q->sobj, &syns);
|
CANCEL_RESTORE(svc);
|
return (flags & Q_VARIABLE) ? ERR_NOTVARQ: ERR_VARQ;
|
|
}
|
|
emptyq = list_empty(&q->msg_list);
|
if (!emptyq) {
|
do {
|
msg = list_pop_entry(&q->msg_list,
|
struct msgholder, link);
|
xnfree(msg);
|
} while (!list_empty(&q->msg_list));
|
}
|
|
cluster_delobj(&psos_queue_table, &q->cobj);
|
q->magic = ~queue_magic; /* Prevent further reference. */
|
ret = syncobj_destroy(&q->sobj, &syns);
|
CANCEL_RESTORE(svc);
|
if (ret)
|
return ERR_TATQDEL;
|
|
return emptyq ? SUCCESS : ERR_MATQDEL;
|
}
|
|
u_long q_delete(u_long qid)
|
{
|
return __q_delete(qid, 0);
|
}
|
|
u_long q_vdelete(u_long qid)
|
{
|
return __q_delete(qid, Q_VARIABLE);
|
}
|
|
static u_long __q_ident(const char *name,
|
u_long flags, u_long node, u_long *qid_r)
|
{
|
struct clusterobj *cobj;
|
struct psos_queue *q;
|
struct service svc;
|
char short_name[5];
|
|
if (node)
|
return ERR_NODENO;
|
|
name = psos_trunc_name(short_name, name);
|
|
CANCEL_DEFER(svc);
|
cobj = cluster_findobj(&psos_queue_table, name);
|
CANCEL_RESTORE(svc);
|
if (cobj == NULL)
|
return ERR_OBJNF;
|
|
q = container_of(cobj, struct psos_queue, cobj);
|
if (((flags ^ q->flags) & Q_VARIABLE)) /* XXX: unsafe, but well... */
|
return (flags & Q_VARIABLE) ? ERR_NOTVARQ: ERR_VARQ;
|
|
*qid_r = mainheap_ref(q, u_long);
|
|
return SUCCESS;
|
}
|
|
u_long q_ident(const char *name, u_long node, u_long *qid_r)
|
{
|
return __q_ident(name, 0, node, qid_r);
|
}
|
|
u_long q_vident(const char *name, u_long node, u_long *qid_r)
|
{
|
return __q_ident(name, Q_VARIABLE, node, qid_r);
|
}
|
|
static u_long __q_send_inner(struct psos_queue *q, unsigned long flags,
|
u_long *buffer, u_long bytes)
|
{
|
struct psos_queue_wait *wait;
|
struct threadobj *thobj;
|
struct msgholder *msg;
|
u_long maxbytes;
|
|
thobj = syncobj_peek_grant(&q->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 ((q->flags & Q_LIMIT) && q->msgcount >= q->maxmsg)
|
return ERR_QFULL;
|
|
msg = xnmalloc(bytes + sizeof(*msg));
|
if (msg == NULL)
|
return ERR_NOMGB;
|
|
q->msgcount++;
|
msg->size = bytes;
|
holder_init(&msg->link);
|
|
if (bytes > 0)
|
memcpy(msg + 1, buffer, bytes);
|
|
if (flags & Q_JAMMED)
|
list_prepend(&msg->link, &q->msg_list);
|
else
|
list_append(&msg->link, &q->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)
|
syncobj_grant_to(&q->sobj, thobj);
|
|
return SUCCESS;
|
}
|
|
static u_long __q_send(u_long qid, u_long flags, u_long *buffer, u_long bytes)
|
{
|
struct syncstate syns;
|
struct psos_queue *q;
|
struct service svc;
|
int ret;
|
|
q = get_queue_from_id(qid, &ret);
|
if (q == NULL)
|
return ret;
|
|
CANCEL_DEFER(svc);
|
|
if (syncobj_lock(&q->sobj, &syns)) {
|
ret = ERR_OBJDEL;
|
goto out;
|
}
|
|
if (((flags ^ q->flags) & Q_VARIABLE)) {
|
ret = (flags & Q_VARIABLE) ? ERR_NOTVARQ: ERR_VARQ;
|
goto fail;
|
}
|
|
if (bytes > q->maxlen) {
|
ret = ERR_MSGSIZ;
|
goto fail;
|
}
|
|
ret = __q_send_inner(q, flags, buffer, bytes);
|
fail:
|
syncobj_unlock(&q->sobj, &syns);
|
out:
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
|
u_long q_send(u_long qid, u_long msgbuf[4])
|
{
|
return __q_send(qid, 0, msgbuf, sizeof(u_long[4]));
|
}
|
|
u_long q_vsend(u_long qid, void *msgbuf, u_long msglen)
|
{
|
return __q_send(qid, Q_VARIABLE, msgbuf, msglen);
|
}
|
|
u_long q_urgent(u_long qid, u_long msgbuf[4])
|
{
|
return __q_send(qid, Q_JAMMED, msgbuf, sizeof(u_long[4]));
|
}
|
|
u_long q_vurgent(u_long qid, void *msgbuf, u_long msglen)
|
{
|
return __q_send(qid, Q_VARIABLE | Q_JAMMED, msgbuf, msglen);
|
}
|
|
static u_long __q_broadcast(u_long qid, u_long flags,
|
u_long *buffer, u_long bytes, u_long *count_r)
|
{
|
struct syncstate syns;
|
struct psos_queue *q;
|
struct service svc;
|
int ret = SUCCESS;
|
|
q = get_queue_from_id(qid, &ret);
|
if (q == NULL)
|
return ret;
|
|
CANCEL_DEFER(svc);
|
|
if (syncobj_lock(&q->sobj, &syns)) {
|
ret = ERR_OBJDEL;
|
goto out;
|
}
|
|
if (((flags ^ q->flags) & Q_VARIABLE)) {
|
ret = (flags & Q_VARIABLE) ? ERR_NOTVARQ: ERR_VARQ;
|
goto fail;
|
}
|
|
if (bytes > q->maxlen) {
|
ret = ERR_MSGSIZ;
|
goto fail;
|
}
|
|
/* Release all pending tasks atomically. */
|
*count_r = 0;
|
while (syncobj_grant_wait_p(&q->sobj)) {
|
ret = __q_send_inner(q, flags, buffer, bytes);
|
if (ret)
|
break;
|
(*count_r)++;
|
}
|
fail:
|
syncobj_unlock(&q->sobj, &syns);
|
out:
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
|
u_long q_broadcast(u_long qid, u_long msgbuf[4], u_long *count_r)
|
{
|
return __q_broadcast(qid, 0, msgbuf, sizeof(u_long[4]), count_r);
|
}
|
|
u_long q_vbroadcast(u_long qid, void *msgbuf, u_long msglen, u_long *count_r)
|
{
|
return __q_broadcast(qid, Q_VARIABLE, msgbuf, msglen, count_r);
|
}
|
|
static u_long __q_receive(u_long qid, u_long flags, u_long timeout,
|
void *buffer, u_long msglen, u_long *msglen_r)
|
{
|
struct psos_queue_wait *wait = NULL;
|
struct timespec ts, *timespec;
|
struct msgholder *msg = NULL;
|
struct syncstate syns;
|
unsigned long nbytes;
|
struct psos_queue *q;
|
struct service svc;
|
int ret = SUCCESS;
|
|
q = get_queue_from_id(qid, &ret);
|
if (q == NULL)
|
return ret;
|
|
CANCEL_DEFER(svc);
|
|
if (syncobj_lock(&q->sobj, &syns)) {
|
ret = ERR_OBJDEL;
|
goto out;
|
}
|
|
if (((flags ^ q->flags) & Q_VARIABLE)) {
|
ret = (flags & Q_VARIABLE) ? ERR_NOTVARQ: ERR_VARQ;
|
goto fail;
|
}
|
retry:
|
if (!list_empty(&q->msg_list)) {
|
q->msgcount--;
|
msg = list_pop_entry(&q->msg_list, struct msgholder, link);
|
nbytes = msg->size;
|
if (nbytes > msglen)
|
nbytes = msglen;
|
if (nbytes > 0)
|
memcpy(buffer, msg + 1, nbytes);
|
xnfree(msg);
|
goto done;
|
}
|
|
if (flags & Q_NOWAIT) {
|
ret = ERR_NOMSG;
|
goto fail;
|
}
|
|
if (timeout != 0) {
|
timespec = &ts;
|
clockobj_ticks_to_timeout(&psos_clock, timeout, timespec);
|
} else
|
timespec = NULL;
|
|
wait = threadobj_prepare_wait(struct psos_queue_wait);
|
wait->ptr = __moff(buffer);
|
wait->size = msglen;
|
|
ret = syncobj_wait_grant(&q->sobj, timespec, &syns);
|
if (ret == -EIDRM) {
|
ret = ERR_QKILLD;
|
goto out;
|
}
|
|
if (ret == -ETIMEDOUT) {
|
ret = ERR_TIMEOUT;
|
goto fail;
|
}
|
nbytes = wait->size;
|
if (nbytes == -1UL) /* No direct copy? */
|
goto retry;
|
done:
|
if (msglen_r)
|
*msglen_r = nbytes;
|
fail:
|
syncobj_unlock(&q->sobj, &syns);
|
out:
|
if (wait)
|
threadobj_finish_wait();
|
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
|
u_long q_receive(u_long qid, u_long flags, u_long timeout, u_long msgbuf[4])
|
{
|
return __q_receive(qid, flags & ~Q_VARIABLE,
|
timeout, msgbuf, sizeof(u_long[4]), NULL);
|
}
|
|
u_long q_vreceive(u_long qid, u_long flags, u_long timeout,
|
void *msgbuf, u_long msglen, u_long *msglen_r)
|
{
|
return __q_receive(qid, flags | Q_VARIABLE,
|
timeout, msgbuf, msglen, msglen_r);
|
}
|
