int poll(FAR struct pollfd *fds, nfds_t nfds, int timeout)
{
WDOG_ID wdog;
sem_t sem;
int count = 0;
int ret;
sem_init(&sem, 0, 0);
ret = poll_setup(fds, nfds, &sem);
if (ret >= 0)
{
if (timeout >= 0)
{
/* Wait for the poll event with a timeout. Note that the
* millisecond timeout has to be converted to system clock
* ticks for wd_start
*/
wdog = wd_create();
wd_start(wdog, MSEC2TICK(timeout), poll_timeout, 1, (uint32_t)&sem);
poll_semtake(&sem);
wd_delete(wdog);
}
else
{
/* Wait for the poll event with no timeout */
poll_semtake(&sem);
}
/* Teardown the poll operation and get the count of events */
ret = poll_teardown(fds, nfds, &count);
}
sem_destroy(&sem);
/* Check for errors */
if (ret < 0)
{
set_errno(-ret);
return ERROR;
}
return count;
}
void uip_grpfree(FAR struct uip_driver_s *dev, FAR struct igmp_group_s *group)
{
uip_lock_t flags;
grplldbg("Free: %p flags: %02x\n", group, group->flags);
/* Cancel the wdog */
flags = uip_lock();
wd_cancel(group->wdog);
/* Remove the group structure from the group list in the device structure */
sq_rem((FAR sq_entry_t*)group, &dev->grplist);
/* Destroy the wait semapore */
(void)sem_destroy(&group->sem);
/* Destroy the wdog */
wd_delete(group->wdog);
/* Then release the group structure resources. Check first if this is one
* of the pre-allocated group structures that we will retain in a free list.
*/
#if CONFIG_PREALLOC_IGMPGROUPS > 0
if (IS_PREALLOCATED(group->flags))
{
grplldbg("Put back on free list\n");
sq_addlast((FAR sq_entry_t*)group, &g_freelist);
uip_unlock(flags);
}
else
#endif
{
/* No.. deallocate the group structure. Use sched_free() just in case
* this function is executing within an interrupt handler.
*/
uip_unlock(flags);
grplldbg("Call sched_free()\n");
sched_free(group);
}
}
void wd_recover(FAR struct tcb_s *tcb)
{
irqstate_t flags;
/* The task is being deleted. If it is waiting for any timed event, then
* tcb->waitdog will be non-NULL. Cancel the watchdog now so that no
* events occur after the watchdog expires. Obviously there are lots of
* race conditions here so this will most certainly have to be revisited in
* the future.
*/
flags = irqsave();
if (tcb->waitdog)
{
(void)wd_cancel(tcb->waitdog);
(void)wd_delete(tcb->waitdog);
tcb->waitdog = NULL;
}
irqrestore(flags);
}
int pthread_cond_timedwait(FAR pthread_cond_t *cond, FAR pthread_mutex_t *mutex,
FAR const struct timespec *abstime)
{
FAR struct tcb_s *rtcb = this_task();
int ticks;
int mypid = (int)getpid();
irqstate_t int_state;
int ret = OK;
int status;
sdbg("cond=0x%p mutex=0x%p abstime=0x%p\n", cond, mutex, abstime);
DEBUGASSERT(rtcb->waitdog == NULL);
/* Make sure that non-NULL references were provided. */
if (!cond || !mutex)
{
ret = EINVAL;
}
/* Make sure that the caller holds the mutex */
else if (mutex->pid != mypid)
{
ret = EPERM;
}
/* If no wait time is provided, this function degenerates to
* the same behavior as pthread_cond_wait().
*/
else if (!abstime)
{
ret = pthread_cond_wait(cond, mutex);
}
else
{
/* Create a watchdog */
rtcb->waitdog = wd_create();
if (!rtcb->waitdog)
{
ret = EINVAL;
}
else
{
sdbg("Give up mutex...\n");
/* We must disable pre-emption and interrupts here so that
* the time stays valid until the wait begins. This adds
* complexity because we assure that interrupts and
* pre-emption are re-enabled correctly.
*/
sched_lock();
int_state = irqsave();
/* Convert the timespec to clock ticks. We must disable pre-emption
* here so that this time stays valid until the wait begins.
*/
ret = clock_abstime2ticks(CLOCK_REALTIME, abstime, &ticks);
if (ret)
{
/* Restore interrupts (pre-emption will be enabled when
* we fall through the if/then/else
*/
irqrestore(int_state);
}
else
{
/* Check the absolute time to wait. If it is now or in the past, then
* just return with the timedout condition.
*/
if (ticks <= 0)
{
/* Restore interrupts and indicate that we have already timed out.
* (pre-emption will be enabled when we fall through the
* if/then/else
*/
irqrestore(int_state);
ret = ETIMEDOUT;
}
else
{
/* Give up the mutex */
mutex->pid = -1;
ret = pthread_givesemaphore((FAR sem_t *)&mutex->sem);
if (ret)
{
/* Restore interrupts (pre-emption will be enabled when
* we fall through the if/then/else)
*/
irqrestore(int_state);
}
else
{
/* Start the watchdog */
wd_start(rtcb->waitdog, ticks, (wdentry_t)pthread_condtimedout,
2, (uint32_t)mypid, (uint32_t)SIGCONDTIMEDOUT);
/* Take the condition semaphore. Do not restore interrupts
* until we return from the wait. This is necessary to
* make sure that the watchdog timer and the condition wait
* are started atomically.
*/
status = sem_wait((FAR sem_t *)&cond->sem);
/* Did we get the condition semaphore. */
if (status != OK)
{
/* NO.. Handle the special case where the semaphore wait was
* awakened by the receipt of a signal -- presumably the
* signal posted by pthread_condtimedout().
*/
if (get_errno() == EINTR)
{
sdbg("Timedout!\n");
ret = ETIMEDOUT;
}
else
{
ret = EINVAL;
}
}
/* The interrupts stay disabled until after we sample the errno.
* This is because when debug is enabled and the console is used
* for debug output, then the errno can be altered by interrupt
* handling! (bad)
*/
irqrestore(int_state);
}
/* Reacquire the mutex (retaining the ret). */
sdbg("Re-locking...\n");
status = pthread_takesemaphore((FAR sem_t *)&mutex->sem);
if (!status)
{
mutex->pid = mypid;
}
else if (!ret)
{
ret = status;
}
}
/* Re-enable pre-emption (It is expected that interrupts
* have already been re-enabled in the above logic)
*/
sched_unlock();
}
/* We no longer need the watchdog */
wd_delete(rtcb->waitdog);
rtcb->waitdog = NULL;
}
}
sdbg("Returning %d\n", ret);
return ret;
}
int sigtimedwait(FAR const sigset_t *set, FAR struct siginfo *info,
FAR const struct timespec *timeout)
{
FAR struct tcb_s *rtcb = (FAR struct tcb_s*)g_readytorun.head;
sigset_t intersection;
FAR sigpendq_t *sigpend;
irqstate_t saved_state;
int32_t waitticks;
int ret = ERROR;
DEBUGASSERT(rtcb->waitdog == NULL);
sched_lock(); /* Not necessary */
/* Several operations must be performed below: We must determine if any
* signal is pending and, if not, wait for the signal. Since signals can
* be posted from the interrupt level, there is a race condition that
* can only be eliminated by disabling interrupts!
*/
saved_state = irqsave();
/* Check if there is a pending signal corresponding to one of the
* signals in the pending signal set argument.
*/
intersection = *set & sig_pendingset(rtcb);
if (intersection != NULL_SIGNAL_SET)
{
/* One or more of the signals in intersections is sufficient to cause
* us to not wait. Pick the lowest numbered signal and mark it not
* pending.
*/
sigpend = sig_removependingsignal(rtcb, sig_lowest(&intersection));
ASSERT(sigpend);
/* Return the signal info to the caller if so requested */
if (info)
{
memcpy(info, &sigpend->info, sizeof(struct siginfo));
}
/* Then dispose of the pending signal structure properly */
sig_releasependingsignal(sigpend);
irqrestore(saved_state);
/* The return value is the number of the signal that awakened us */
ret = sigpend->info.si_signo;
}
/* We will have to wait for a signal to be posted to this task. */
else
{
/* Save the set of pending signals to wait for */
rtcb->sigwaitmask = *set;
/* Check if we should wait for the timeout */
if (timeout)
{
/* Convert the timespec to system clock ticks, making sure that
* the resulting delay is greater than or equal to the requested
* time in nanoseconds.
*/
#ifdef CONFIG_HAVE_LONG_LONG
uint64_t waitticks64 = ((uint64_t)timeout->tv_sec * NSEC_PER_SEC +
(uint64_t)timeout->tv_nsec + NSEC_PER_TICK - 1) /
NSEC_PER_TICK;
DEBUGASSERT(waitticks64 <= UINT32_MAX);
waitticks = (uint32_t)waitticks64;
#else
uint32_t waitmsec;
DEBUGASSERT(timeout->tv_sec < UINT32_MAX / MSEC_PER_SEC);
waitmsec = timeout->tv_sec * MSEC_PER_SEC +
(timeout->tv_nsec + NSEC_PER_MSEC - 1) / NSEC_PER_MSEC;
waitticks = MSEC2TICK(waitmsec);
#endif
/* Create a watchdog */
rtcb->waitdog = wd_create();
DEBUGASSERT(rtcb->waitdog);
if (rtcb->waitdog)
{
/* This little bit of nonsense is necessary for some
* processors where sizeof(pointer) < sizeof(uint32_t).
* see wdog.h.
*/
wdparm_t wdparm;
wdparm.pvarg = (FAR void *)rtcb;
/* Start the watchdog */
wd_start(rtcb->waitdog, waitticks, (wdentry_t)sig_timeout, 1,
wdparm.dwarg);
/* Now wait for either the signal or the watchdog */
up_block_task(rtcb, TSTATE_WAIT_SIG);
/* We no longer need the watchdog */
wd_delete(rtcb->waitdog);
rtcb->waitdog = NULL;
}
/* REVISIT: And do what if there are no watchdog timers? The wait
* will fail and we will return something bogus.
*/
}
/* No timeout, just wait */
else
{
/* And wait until one of the unblocked signals is posted */
up_block_task(rtcb, TSTATE_WAIT_SIG);
}
/* We are running again, clear the sigwaitmask */
rtcb->sigwaitmask = NULL_SIGNAL_SET;
/* When we awaken, the cause will be in the TCB. Get the signal number
* or timeout) that awakened us.
*/
if (GOOD_SIGNO(rtcb->sigunbinfo.si_signo))
{
/* We were awakened by a signal... but is it one of the signals that
* we were waiting for?
*/
if (sigismember(set, rtcb->sigunbinfo.si_signo))
{
/* Yes.. the return value is the number of the signal that
* awakened us.
*/
ret = rtcb->sigunbinfo.si_signo;
}
else
{
/* No... then set EINTR and report an error */
set_errno(EINTR);
ret = ERROR;
}
}
else
{
/* Otherwise, we must have been awakened by the timeout. Set EGAIN
* and return an error.
*/
DEBUGASSERT(rtcb->sigunbinfo.si_signo == SIG_WAIT_TIMEOUT);
set_errno(EAGAIN);
ret = ERROR;
}
/* Return the signal info to the caller if so requested */
if (info)
{
memcpy(info, &rtcb->sigunbinfo, sizeof(struct siginfo));
}
irqrestore(saved_state);
}
sched_unlock();
return ret;
}
int sem_timedwait(FAR sem_t *sem, FAR const struct timespec *abstime)
{
FAR struct tcb_s *rtcb = (FAR struct tcb_s *)g_readytorun.head;
irqstate_t flags;
int ticks;
int err;
int ret = ERROR;
DEBUGASSERT(up_interrupt_context() == false && rtcb->waitdog == NULL);
/* Verify the input parameters and, in case of an error, set
* errno appropriately.
*/
#ifdef CONFIG_DEBUG
if (!abstime || !sem)
{
err = EINVAL;
goto errout;
}
#endif
/* Create a watchdog. We will not actually need this watchdog
* unless the the semaphore is unavailable, but we will reserve it up
* front before we enter the following critical section.
*/
rtcb->waitdog = wd_create();
if (!rtcb->waitdog)
{
err = ENOMEM;
goto errout;
}
/* We will disable interrupts until we have completed the semaphore
* wait. We need to do this (as opposed to just disabling pre-emption)
* because there could be interrupt handlers that are asynchronoulsy
* posting semaphores and to prevent race conditions with watchdog
* timeout. This is not too bad because interrupts will be re-
* enabled while we are blocked waiting for the semaphore.
*/
flags = irqsave();
/* Try to take the semaphore without waiting. */
ret = sem_trywait(sem);
if (ret == 0)
{
/* We got it! */
irqrestore(flags);
wd_delete(rtcb->waitdog);
rtcb->waitdog = NULL;
return OK;
}
/* We will have to wait for the semphore. Make sure that we were provided
* with a valid timeout.
*/
if (abstime->tv_sec < 0 || abstime->tv_nsec > 1000000000)
{
err = EINVAL;
goto errout_disabled;
}
/* Convert the timespec to clock ticks. We must have interrupts
* disabled here so that this time stays valid until the wait begins.
*/
err = clock_abstime2ticks(CLOCK_REALTIME, abstime, &ticks);
/* If the time has already expired return immediately. */
if (err == OK && ticks <= 0)
{
err = ETIMEDOUT;
goto errout_disabled;
}
/* Handle any time-related errors */
if (err != OK)
{
goto errout_disabled;
}
/* Start the watchdog */
err = OK;
wd_start(rtcb->waitdog, ticks, (wdentry_t)sem_timeout, 1, getpid());
/* Now perform the blocking wait */
ret = sem_wait(sem);
/* Stop the watchdog timer */
wd_cancel(rtcb->waitdog);
/* We can now restore interrupts and delete the watchdog */
irqrestore(flags);
wd_delete(rtcb->waitdog);
rtcb->waitdog = NULL;
/* We are either returning success or an error detected by sem_wait()
* or the timeout detected by sem_timeout(). The 'errno' value has
* been set appropriately by sem_wait() or sem_timeout() in those
* cases.
*/
return ret;
/* Error exits */
errout_disabled:
irqrestore(flags);
wd_delete(rtcb->waitdog);
rtcb->waitdog = NULL;
errout:
set_errno(err);
return ERROR;
}
ssize_t mq_timedreceive(mqd_t mqdes, void *msg, size_t msglen,
int *prio, const struct timespec *abstime)
{
FAR struct tcb_s *rtcb = (FAR struct tcb_s *)g_readytorun.head;
FAR mqmsg_t *mqmsg;
irqstate_t saved_state;
int ret = ERROR;
DEBUGASSERT(up_interrupt_context() == false && rtcb->waitdog == NULL);
/* Verify the input parameters and, in case of an error, set
* errno appropriately.
*/
if (mq_verifyreceive(mqdes, msg, msglen) != OK)
{
return ERROR;
}
if (!abstime || abstime->tv_sec < 0 || abstime->tv_nsec > 1000000000)
{
set_errno(EINVAL);
return ERROR;
}
/* Create a watchdog. We will not actually need this watchdog
* unless the queue is not empty, but we will reserve it up front
* before we enter the following critical section.
*/
rtcb->waitdog = wd_create();
if (!rtcb->waitdog)
{
set_errno(EINVAL);
return ERROR;
}
/* Get the next mesage from the message queue. We will disable
* pre-emption until we have completed the message received. This
* is not too bad because if the receipt takes a long time, it will
* be because we are blocked waiting for a message and pre-emption
* will be re-enabled while we are blocked
*/
sched_lock();
/* Furthermore, mq_waitreceive() expects to have interrupts disabled
* because messages can be sent from interrupt level.
*/
saved_state = irqsave();
/* Check if the message queue is empty. If it is NOT empty, then we
* will not need to start timer.
*/
if (mqdes->msgq->msglist.head == NULL)
{
int ticks;
/* Convert the timespec to clock ticks. We must have interrupts
* disabled here so that this time stays valid until the wait begins.
*/
int 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)
{
set_errno(result);
irqrestore(saved_state);
sched_unlock();
wd_delete(rtcb->waitdog);
rtcb->waitdog = NULL;
return ERROR;
}
/* Start the watchdog */
wd_start(rtcb->waitdog, ticks, (wdentry_t)mq_rcvtimeout, 1, getpid());
}
/* Get the message from the message queue */
mqmsg = mq_waitreceive(mqdes);
/* Stop the watchdog timer (this is not harmful in the case where
* it was never started)
*/
wd_cancel(rtcb->waitdog);
/* We can now restore interrupts */
irqrestore(saved_state);
/* Check if we got a message from the message queue. We might
* not have a message if:
*
* - The message queue is empty and O_NONBLOCK is set in the mqdes
* - The wait was interrupted by a signal
* - The watchdog timeout expired
*/
if (mqmsg)
{
ret = mq_doreceive(mqdes, mqmsg, msg, prio);
}
sched_unlock();
wd_delete(rtcb->waitdog);
rtcb->waitdog = NULL;
return ret;
}
int sem_tickwait(FAR sem_t *sem, systime_t start, uint32_t delay)
{
FAR struct tcb_s *rtcb = (FAR struct tcb_s *)g_readytorun.head;
irqstate_t flags;
systime_t elapsed;
int ret;
DEBUGASSERT(sem != NULL && up_interrupt_context() == false &&
rtcb->waitdog == NULL);
/* Create a watchdog. We will not actually need this watchdog
* unless the semaphore is unavailable, but we will reserve it up
* front before we enter the following critical section.
*/
rtcb->waitdog = wd_create();
if (!rtcb->waitdog)
{
return -ENOMEM;
}
/* We will disable interrupts until we have completed the semaphore
* wait. We need to do this (as opposed to just disabling pre-emption)
* because there could be interrupt handlers that are asynchronously
* posting semaphores and to prevent race conditions with watchdog
* timeout. This is not too bad because interrupts will be re-
* enabled while we are blocked waiting for the semaphore.
*/
flags = enter_critical_section();
/* Try to take the semaphore without waiting. */
ret = sem_trywait(sem);
if (ret == OK)
{
/* We got it! */
goto success_with_irqdisabled;
}
/* We will have to wait for the semaphore. Make sure that we were provided
* with a valid timeout.
*/
if (delay == 0)
{
/* Return the errno from sem_trywait() */
ret = -get_errno();
goto errout_with_irqdisabled;
}
/* Adjust the delay for any time since the delay was calculated */
elapsed = clock_systimer() - start;
if (/* elapsed >= (UINT32_MAX / 2) || */ elapsed >= delay)
{
ret = -ETIMEDOUT;
goto errout_with_irqdisabled;
}
delay -= elapsed;
/* Start the watchdog with interrupts still disabled */
(void)wd_start(rtcb->waitdog, delay, (wdentry_t)sem_timeout, 1, getpid());
/* Now perform the blocking wait */
ret = sem_wait(sem);
if (ret < 0)
{
/* Return the errno from sem_wait() */
ret = -get_errno();
goto errout_with_irqdisabled;
}
/* Stop the watchdog timer */
wd_cancel(rtcb->waitdog);
/* We can now restore interrupts and delete the watchdog */
/* Success exits */
success_with_irqdisabled:
/* Error exits */
errout_with_irqdisabled:
leave_critical_section(flags);
wd_delete(rtcb->waitdog);
rtcb->waitdog = NULL;
return ret;
}
int mq_timedsend(mqd_t mqdes, const char *msg, size_t msglen, int prio,
const struct timespec *abstime)
{
WDOG_ID wdog;
FAR msgq_t *msgq;
FAR mqmsg_t *mqmsg = NULL;
irqstate_t saved_state;
int ret = ERROR;
DEBUGASSERT(up_interrupt_context() == false);
/* Verify the input parameters -- setting errno appropriately
* on any failures to verify.
*/
if (mq_verifysend(mqdes, msg, msglen, prio) != OK)
{
return ERROR;
}
if (!abstime || abstime->tv_sec < 0 || abstime->tv_nsec > 1000000000)
{
set_errno(EINVAL);
return ERROR;
}
/* Get a pointer to the message queue */
msgq = mqdes->msgq;
/* 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.
*/
wdog = wd_create();
if (!wdog)
{
set_errno(EINVAL);
return ERROR;
}
/* Allocate a message structure:
* - If we are called from an interrupt handler, or
* - If the message queue is not full, or
*/
sched_lock();
saved_state = irqsave();
if (up_interrupt_context() || /* In an interrupt handler */
msgq->nmsgs < msgq->maxmsgs) /* OR Message queue not full */
{
/* Allocate the message */
irqrestore(saved_state);
mqmsg = mq_msgalloc();
}
else
{
int ticks;
/* We are not in an interupt 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.
*/
int 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)
{
set_errno(result);
ret = ERROR;
}
/* Start the watchdog and begin the wait for MQ not full */
if (result == OK)
{
/* Start the watchdog */
wd_start(wdog, 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(wdog);
}
/* 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.
*/
if (ret == OK)
{
mqmsg = mq_msgalloc();
}
}
/* Check if we were able to get a message structure -- this can fail
* either because we cannot send the message (and didn't bother trying
* to allocate it) or because the allocation failed.
*/
if (mqmsg)
{
/* Yes, peform the message send. */
ret = mq_dosend(mqdes, mqmsg, msg, msglen, prio);
}
sched_unlock();
wd_delete(wdog);
return ret;
}
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;
}
