ssize_t mq_doreceive(mqd_t mqdes, mqmsg_t *mqmsg, void *ubuffer, int *prio)
{
FAR struct tcb_s *btcb;
irqstate_t saved_state;
FAR msgq_t *msgq;
ssize_t rcvmsglen;
/* Get the length of the message (also the return value) */
rcvmsglen = mqmsg->msglen;
/* Copy the message into the caller's buffer */
memcpy(ubuffer, (const void*)mqmsg->mail, rcvmsglen);
/* Copy the message priority as well (if a buffer is provided) */
if (prio)
{
*prio = mqmsg->priority;
}
/* We are done with the message. Deallocate it now. */
mq_msgfree(mqmsg);
/* Check if any tasks are waiting for the MQ not full event. */
msgq = mqdes->msgq;
if (msgq->nwaitnotfull > 0)
{
/* Find the highest priority task that is waiting for
* this queue to be not-full in g_waitingformqnotfull list.
* This must be performed in a critical section because
* messages can be sent from interrupt handlers.
*/
saved_state = irqsave();
for (btcb = (FAR struct tcb_s*)g_waitingformqnotfull.head;
btcb && btcb->msgwaitq != msgq;
btcb = btcb->flink);
/* If one was found, unblock it. NOTE: There is a race
* condition here: the queue might be full again by the
* time the task is unblocked
*/
ASSERT(btcb);
btcb->msgwaitq = NULL;
msgq->nwaitnotfull--;
up_unblock_task(btcb);
irqrestore(saved_state);
}
/* Return the length of the message transferred to the user buffer */
return rcvmsglen;
}
void mq_msgqfree(FAR msgq_t *msgq)
{
FAR mqmsg_t *curr;
FAR mqmsg_t *next;
/* Deallocate any stranded messages in the message queue. */
curr = (FAR mqmsg_t*)msgq->msglist.head;
while (curr)
{
/* Deallocate the message structure. */
next = curr->next;
mq_msgfree(curr);
curr = next;
}
/* Then deallocate the message queue itself */
sched_free(msgq);
}
int mq_timedsend(mqd_t mqdes, FAR const char *msg, size_t msglen, int prio,
FAR const struct timespec *abstime)
{
FAR struct tcb_s *rtcb = this_task();
FAR struct mqueue_inode_s *msgq;
FAR struct mqueue_msg_s *mqmsg = NULL;
irqstate_t saved_state;
int ticks;
int result;
int ret = ERROR;
DEBUGASSERT(up_interrupt_context() == false && rtcb->waitdog == NULL);
/* Verify the input parameters -- setting errno appropriately
* on any failures to verify.
*/
if (mq_verifysend(mqdes, msg, msglen, prio) != OK)
{
/* mq_verifysend() will set the errno appropriately */
return ERROR;
}
/* Pre-allocate a message structure */
mqmsg = mq_msgalloc();
if (!mqmsg)
{
/* Failed to allocate the message */
set_errno(ENOMEM);
return ERROR;
}
/* Get a pointer to the message queue */
sched_lock();
msgq = mqdes->msgq;
/* OpenGroup.org: "Under no circumstance shall the operation fail with a
* timeout if there is sufficient room in the queue to add the message
* immediately. The validity of the abstime parameter need not be checked
* when there is sufficient room in the queue."
*
* Also ignore the time value if for some crazy reason we were called from
* an interrupt handler. This probably really should be an assertion.
*
* NOTE: There is a race condition here: What if a message is added by
* interrupt related logic so that queue again becomes non-empty. That
* is handled because mq_dosend() will permit the maxmsgs limit to be
* exceeded in that case.
*/
if (msgq->nmsgs < msgq->maxmsgs || up_interrupt_context())
{
/* Do the send with no further checks (possibly exceeding maxmsgs)
* Currently mq_dosend() always returns OK.
*/
ret = mq_dosend(mqdes, mqmsg, msg, msglen, prio);
sched_unlock();
return ret;
}
/* The message queue is full... We are going to wait. Now we must have a
* valid time value.
*/
if (!abstime || abstime->tv_nsec < 0 || abstime->tv_nsec >= 1000000000)
{
result = EINVAL;
goto errout_with_mqmsg;
}
/* Create a watchdog. We will not actually need this watchdog
* unless the queue is full, but we will reserve it up front
* before we enter the following critical section.
*/
rtcb->waitdog = wd_create();
if (!rtcb->waitdog)
{
result = EINVAL;
goto errout_with_mqmsg;
}
/* We are not in an interrupt handler and the message queue is full.
* Set up a timed wait for the message queue to become non-full.
*
* Convert the timespec to clock ticks. We must have interrupts
* disabled here so that this time stays valid until the wait begins.
*/
saved_state = irqsave();
result = clock_abstime2ticks(CLOCK_REALTIME, abstime, &ticks);
/* If the time has already expired and the message queue is empty,
* return immediately.
*/
if (result == OK && ticks <= 0)
{
result = ETIMEDOUT;
}
/* Handle any time-related errors */
if (result != OK)
{
goto errout_with_irqsave;
}
/* Start the watchdog and begin the wait for MQ not full */
wd_start(rtcb->waitdog, ticks, (wdentry_t)mq_sndtimeout, 1, getpid());
/* And wait for the message queue to be non-empty */
ret = mq_waitsend(mqdes);
/* This may return with an error and errno set to either EINTR
* or ETIMEOUT. Cancel the watchdog timer in any event.
*/
wd_cancel(rtcb->waitdog);
/* Check if mq_waitsend() failed */
if (ret < 0)
{
/* mq_waitsend() will set the errno, but the error exit will reset it */
result = get_errno();
goto errout_with_irqsave;
}
/* That is the end of the atomic operations */
irqrestore(saved_state);
/* If any of the above failed, set the errno. Otherwise, there should
* be space for another message in the message queue. NOW we can allocate
* the message structure.
*
* Currently mq_dosend() always returns OK.
*/
ret = mq_dosend(mqdes, mqmsg, msg, msglen, prio);
sched_unlock();
wd_delete(rtcb->waitdog);
rtcb->waitdog = NULL;
return ret;
/* Exit here with (1) the scheduler locked, (2) a message allocated, (3) a
* wdog allocated, and (4) interrupts disabled. The error code is in
* 'result'
*/
errout_with_irqsave:
irqrestore(saved_state);
wd_delete(rtcb->waitdog);
rtcb->waitdog = NULL;
/* Exit here with (1) the scheduler locked and 2) a message allocated. The
* error code is in 'result'
*/
errout_with_mqmsg:
mq_msgfree(mqmsg);
sched_unlock();
set_errno(result);
return ERROR;
}