static inline int __rt_comedi_wait(RTIME until, unsigned int *cbmask, int waitmode)
{
if (cbmask) {
long retval;
RT_TASK *task = _rt_whoami();
switch (waitmode) {
case WAIT:
rt_printk("COMEDI WAIT WARNING: REACHED AND SKIPPED.\n");
rt_task_suspend_timed(task, nano2count(100000));
break;
case WAITIF:
rt_task_suspend_if(task);
break;
case WAITUNTIL:
rt_task_suspend_until(task, until);
break;
default: // useless, just to avoid compiler warnings
return RTE_PERM;
}
retval = 0;
task->resumsg = CBMASK;
if (KSPACE(cbmask)) {
cbmask[0] = (unsigned int)task->resumsg;
} else {
rt_put_user((unsigned int)task->resumsg, cbmask);
}
task->resumsg = 0;
return retval;
}
return RTE_PERM;
}
RTAI_SYSCALL_MODE int _rt_msg_evdrp(RT_MSGQ *mq, void *msg, int msg_size, int *msgpri, int space)
{
int size;
RT_MSG *msg_ptr;
void *p;
size = min((msg_ptr = mq->firstmsg)->hdr.size, msg_size);
if (space)
{
memcpy(msg, (p = msg_ptr->hdr.malloc) ? p : msg_ptr->msg, size);
if (msgpri)
{
*msgpri = msg_ptr->hdr.priority;
}
}
else
{
rt_copy_to_user(msg, (p = msg_ptr->hdr.malloc) ? p : msg_ptr->msg, size);
if (msgpri)
{
rt_put_user(msg_ptr->hdr.priority, msgpri);
}
}
return 0;
}
static inline int __rt_comedi_command_data_wread(void *dev, unsigned int subdev, long nchans, lsampl_t *data, RTIME until, unsigned int *cbmaskarg, int waitmode)
{
unsigned int cbmask, mask;
long retval, kspace;
if ((kspace = KSPACE(cbmaskarg))) {
mask = cbmaskarg[0];
} else {
rt_get_user(mask, cbmaskarg);
}
switch (waitmode) {
case WAIT:
retval = rt_comedi_wait(&cbmask);
break;
case WAITIF:
retval = rt_comedi_wait_if(&cbmask);
break;
case WAITUNTIL:
retval = _rt_comedi_wait_until(&cbmask, until);
break;
default: // useless, just to avoid compiler warnings
return RTE_PERM;
}
if (!retval && (mask & cbmask)) {
if (kspace) {
cbmaskarg[0] = cbmask;
} else {
rt_put_user(cbmask, cbmaskarg);
}
return rt_comedi_command_data_read(dev, subdev, nchans, data);
}
return retval;
}
static int _receive(RT_MSGQ *mq, void *msg, int msg_size, int *msgpri, int space)
{
int size;
RT_MSG *msg_ptr;
void *p;
size = min((msg_ptr = mq->firstmsg)->hdr.size, msg_size);
if (space)
{
memcpy(msg, (p = msg_ptr->hdr.malloc) ? p : msg_ptr->msg, size);
if (msgpri)
{
*msgpri = msg_ptr->hdr.priority;
}
}
else
{
rt_copy_to_user(msg, (p = msg_ptr->hdr.malloc) ? p : msg_ptr->msg, size);
if (msgpri)
{
rt_put_user(msg_ptr->hdr.priority, msgpri);
}
}
if (msg_ptr->hdr.broadcast)
{
if (!--msg_ptr->hdr.broadcast)
{
rt_sem_wait_barrier(&mq->broadcast);
goto relslot;
}
else
{
rt_sem_signal(&mq->received);
rt_sem_signal(&mq->receivers);
rt_sem_wait_barrier(&mq->broadcast);
}
}
else
{
unsigned long flags;
relslot: flags = rt_spin_lock_irqsave(&mq->lock);
mq->firstmsg = msg_ptr->hdr.next;
mq->slots[--mq->slot] = msg_ptr;
rt_spin_unlock_irqrestore(flags, &mq->lock);
rt_sem_signal(&mq->freslots);
rt_sem_signal(&mq->receivers);
if (p)
{
rt_free(p);
}
}
return msg_size - size;
}