mq_receive/send: Return appropriate errnos and stop waiting if signal received.

git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@164 42af7a65-404d-4744-a932-0658087f49c3

9 files changed, 482 insertions, 215 deletions

diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog
index 790d4e795..476f87bbb 100644
--- a/nuttx/ChangeLog
+++ b/nuttx/ChangeLog
@@ -91,6 +91,8 @@
 
 0.2.3 2007-xx-xx Gregory Nutt <spudmonkey@racsa.co.cr>
 
+	* mq_receive and mq_send now return errno's appropriately
+	* mq_receive and mq_send are now correctly awakened by signals.
 	* Started m68322
 
 
diff --git a/nuttx/examples/ostest/mqueue.c b/nuttx/examples/ostest/mqueue.c
index 00b17e124..99cf1e585 100644
--- a/nuttx/examples/ostest/mqueue.c
+++ b/nuttx/examples/ostest/mqueue.c
@@ -37,13 +37,17 @@
  * Included Files
  **************************************************************************/
 
+#include <nuttx/config.h>
+
 #include <stdio.h>
+#include <unistd.h>
 #include <string.h>
 #include <ctype.h>
 #include <fcntl.h>
 #include <pthread.h>
 #include <mqueue.h>
 #include <sched.h>
+#include <errno.h>
 
 #include "ostest.h"
 
@@ -51,11 +55,18 @@
  * Private Definitions
  **************************************************************************/
 
-#define TEST_MESSAGE "This is a test and only a test"
+#define TEST_MESSAGE        "This is a test and only a test"
 #ifdef SDCC
-#define TEST_MSGLEN  (31)
+#define TEST_MSGLEN         (31)
 #else
-#define TEST_MSGLEN  (strlen(TEST_MESSAGE)+1)
+#define TEST_MSGLEN         (strlen(TEST_MESSAGE)+1)
+#endif
+
+#define TEST_SEND_NMSGS     (10)
+#ifndef CONFIG_DISABLE_SIGNALS
+# define TEST_RECEIVE_NMSGS (11)
+#else
+# define TEST_RECEIVE_NMSGS (10)
 #endif
 
 /**************************************************************************
@@ -121,9 +132,9 @@ static void *sender_thread(void *arg)
 
   memcpy(msg_buffer, TEST_MESSAGE, TEST_MSGLEN);
 
-  /* Perform the send 10 times */
+  /* Perform the send TEST_SEND_NMSGS times */
 
-  for (i = 0; i < 10; i++)
+  for (i = 0; i < TEST_SEND_NMSGS; i++)
     {
       status = mq_send(mqfd, msg_buffer, TEST_MSGLEN, 42);
       if (status < 0)
@@ -183,16 +194,27 @@ static void *receiver_thread(void *arg)
        pthread_exit((pthread_addr_t)1);
      }
 
-   /* Perform the receive 10 times */
+   /* Perform the receive TEST_RECEIVE_NMSGS times */
 
-   for (i = 0; i < 10; i++)
+   for (i = 0; i < TEST_RECEIVE_NMSGS; i++)
     {
       memset(msg_buffer, 0xaa, TEST_MSGLEN);
       nbytes = mq_receive(mqfd, msg_buffer, TEST_MSGLEN, 0);
       if (nbytes < 0)
         {
-          printf("receiver_thread: ERROR mq_receive failure on msg %d\n", i);
-          nerrors++;
+          /* mq_receive failed.  If the error is because of EINTR then
+           * it is not a failure.
+           */
+
+          if (*get_errno_ptr() != EINTR)
+            {
+              printf("receiver_thread: ERROR mq_receive failure on msg %d, errno=%d\n", i, *get_errno_ptr());
+              nerrors++;
+            }
+          else
+            {
+              printf("receiver_thread: mq_receive interrupted!\n", i);
+            }
         }
       else if (nbytes != TEST_MSGLEN)
         {
@@ -336,8 +358,26 @@ void mqueue_test(void)
       printf("mqueue_test: ERROR sender thread exited with %d errors\n", (int)result);
     }
 
+#ifndef CONFIG_DISABLE_SIGNALS
+  /* Wake up the receiver thread with a signal */
+
+  printf("mqueue_test: Killing receiver\n");
+  pthread_kill(receiver, 9);
+#endif
+
+  /* Wait a bit to see if the thread exits on its own */
+
+  usleep(500*1000);
+
+  /* Then cancel the thread and see if it did */
+
   printf("mqueue_test: Canceling receiver\n");
-  pthread_cancel(receiver);
+  status = pthread_cancel(receiver);
+  if (status == ESRCH)
+    {
+      printf("mqueue_test: receiver has already terminated\n");
+    }
+
   pthread_join(receiver, &result);
   if (result != (void*)0)
     {
diff --git a/nuttx/sched/Makefile b/nuttx/sched/Makefile
index a6ac894eb..1f6ec2384 100644
--- a/nuttx/sched/Makefile
+++ b/nuttx/sched/Makefile
@@ -71,7 +71,7 @@ SIGNAL_SRCS	= sig_initialize.c \
 		  sig_cleanup.c sig_received.c sig_deliver.c
 MQUEUE_SRCS	= mq_open.c mq_close.c mq_unlink.c mq_send.c mq_receive.c \
 		  mq_setattr.c mq_getattr.c mq_initialize.c mq_descreate.c \
-		  mq_findnamed.c mq_msgfree.c mq_msgqfree.c
+		  mq_findnamed.c mq_msgfree.c mq_msgqfree.c mq_waitirq.c
 ifneq ($(CONFIG_DISABLE_SIGNALS),y)
 MQUEUE_SRCS	+= mq_notify.c
 endif
diff --git a/nuttx/sched/mq_internal.h b/nuttx/sched/mq_internal.h
index a19e71bdf..a4e21d3c7 100644
--- a/nuttx/sched/mq_internal.h
+++ b/nuttx/sched/mq_internal.h
@@ -181,7 +181,7 @@ extern "C" {
 #define EXTERN extern
 #endif
 
-/* Functions defined in mq_initialized.c *******************/
+/* Functions defined in mq_initialize.c ********************/
 
 EXTERN void weak_function mq_initialize(void);
 EXTERN void        mq_desblockalloc(void);
@@ -191,6 +191,10 @@ EXTERN FAR msgq_t *mq_findnamed(const char *mq_name);
 EXTERN void        mq_msgfree(FAR mqmsg_t *mqmsg);
 EXTERN void        mq_msgqfree(FAR msgq_t *msgq);
 
+/* mq_waitirq.c ********************************************/
+
+EXTERN void        mq_waitirq(FAR _TCB *wtcb);
+
 #undef EXTERN
 #ifdef __cplusplus
 }
diff --git a/nuttx/sched/mq_receive.c b/nuttx/sched/mq_receive.c
index 65fcb81c2..aab7847f1 100644
--- a/nuttx/sched/mq_receive.c
+++ b/nuttx/sched/mq_receive.c
@@ -43,6 +43,7 @@
 #include <fcntl.h>          /* O_NONBLOCK */
 #include <string.h>
 #include <assert.h>
+#include <errno.h>
 #include <mqueue.h>
 #include <sched.h>
 #include <debug.h>
@@ -106,8 +107,17 @@
  *      priority.
  *
  * Return Value:
- *   Length of the selected message in bytes, otherwise -1
- *   (ERROR).
+ *   One success, the length of the selected message in bytes.is
+ *   returned.  On failure, -1 (ERROR) is returned and the errno
+ *   is set appropriately:
+ *
+ *   EAGAIN   The queue was empty, and the O_NONBLOCK flag was set
+ *            for the message queue description referred to by 'mqdes'.
+ *   EPERM    Message queue opened not opened for reading.
+ *   EMSGSIZE 'msglen' was less than the maxmsgsize attribute of the
+ *            message queue.
+ *   EINTR    The call was interrupted by a signal handler.
+ *   EINVAL   Invalid 'msg' or 'mqdes'
  *
  * Assumptions:
  *
@@ -125,110 +135,142 @@ int mq_receive(mqd_t mqdes, void *msg, size_t msglen, int *prio)
 
   /* Verify the input parameters */
 
-  sched_lock();
-  if (msg && mqdes && (mqdes->oflags & O_RDOK) != 0 &&
-      msglen >= (size_t)mqdes->msgq->maxmsgsize)
+  if (!msg || !mqdes)
     {
-      /* Get a pointer to the message queue */
+      *get_errno_ptr() = EINVAL;
+      return ERROR;
+    }
 
-      msgq = mqdes->msgq;
+  if ((mqdes->oflags & O_RDOK) == 0)
+    {
+      *get_errno_ptr() = EPERM;
+      return ERROR;
+    }
 
-      /* Several operations must be performed below:  We must determine if
-       * a message is pending and, if not, wait for the message.  Since
-       * messages can be sent from the interrupt level, there is a race
-       * condition that can only be eliminated by disabling interrupts!
-       */
+  if (msglen < (size_t)mqdes->msgq->maxmsgsize)
+    {
+      *get_errno_ptr() = EMSGSIZE;
+      return ERROR;
+    }
+
+  /* Get a pointer to the message queue */
+
+  sched_lock();
+  msgq = mqdes->msgq;
+
+  /* Several operations must be performed below:  We must determine if
+   * a message is pending and, if not, wait for the message.  Since
+   * messages can be sent from the interrupt level, there is a race
+   * condition that can only be eliminated by disabling interrupts!
+   */
 
-      saved_state = irqsave();
+  saved_state = irqsave();
 
-      /* Get the message from the head of the queue */
+  /* Get the message from the head of the queue */
 
-      while ((curr = (FAR mqmsg_t*)sq_remfirst(&msgq->msglist)) == NULL)
+  while ((curr = (FAR mqmsg_t*)sq_remfirst(&msgq->msglist)) == NULL)
+    {
+      /* Should we block until there the above condition has been
+       * satisfied?
+       */
+
+      if (!(mqdes->oflags & O_NONBLOCK))
         {
-          /* Should we block until there the above condition has been
-           * satisfied?
-           */
+          /* Block and try again */
 
-          if (!(mqdes->oflags & O_NONBLOCK))
-            {
-              /* Block and try again */
+          rtcb = (FAR _TCB*)g_readytorun.head;
+          rtcb->msgwaitq = msgq;
+          msgq->nwaitnotempty++;
 
-              rtcb = (FAR _TCB*)g_readytorun.head;
-              rtcb->msgwaitq = msgq;
-              msgq->nwaitnotempty++;
-              up_block_task(rtcb, TSTATE_WAIT_MQNOTEMPTY);
-            }
-          else
+          *get_errno_ptr() = OK;
+          up_block_task(rtcb, TSTATE_WAIT_MQNOTEMPTY);
+
+          /* When we resume at this point, either (1) the message queue
+           * is no longer empty, or (2) the wait has been interrupted by
+           * a signal.  We can detect the latter case be examining the
+           * errno value (should be EINTR).
+           */
+
+          if (*get_errno_ptr() != OK)
             {
               break;
             }
         }
-
-      /* If we got message, then decrement the number of messages in
-       * the queue while we are still in the critical section
-       */
-
-      if (curr)
+      else
         {
-          msgq->nmsgs--;
+          /* The queue was empty, and the O_NONBLOCK flag was set for the
+           * message queue description referred to by 'mqdes'.
+           */
+
+         *get_errno_ptr() = EAGAIN;
+          break;
         }
-      irqrestore(saved_state);
+    }
 
-      /* Check (again) if we got a message from the message queue*/
+  /* If we got message, then decrement the number of messages in
+   * the queue while we are still in the critical section
+   */
 
-      if (curr)
-        {
-          /* Get the length of the message (also the return value) */
+  if (curr)
+    {
+      msgq->nmsgs--;
+    }
+  irqrestore(saved_state);
 
-          ret = rcvmsglen = curr->msglen;
+  /* Check (again) if we got a message from the message queue*/
 
-          /* Copy the message into the caller's buffer */
+  if (curr)
+    {
+      /* Get the length of the message (also the return value) */
 
-          memcpy(msg, (const void*)curr->mail, rcvmsglen);
+      ret = rcvmsglen = curr->msglen;
 
-          /* Copy the message priority as well (if a buffer is provided) */
+      /* Copy the message into the caller's buffer */
 
-          if (prio)
-            {
-              *prio = curr->priority;
-            }
+      memcpy(msg, (const void*)curr->mail, rcvmsglen);
 
-          /* We are done with the message.  Deallocate it now. */
+      /* Copy the message priority as well (if a buffer is provided) */
 
-          mq_msgfree(curr);
+      if (prio)
+        {
+          *prio = curr->priority;
+        }
 
-          /* Check if any tasks are waiting for the MQ not full event. */
+      /* We are done with the message.  Deallocate it now. */
 
-          if (msgq->nwaitnotfull > 0)
+      mq_msgfree(curr);
+
+      /* Check if any tasks are waiting for the MQ not full event. */
+
+      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 _TCB*)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
+           */
+
+          if (!btcb)
+            {
+              PANIC(OSERR_MQNOTFULLCOUNT);
+            }
+          else
             {
-              /* 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 _TCB*)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
-               */
-
-              if (!btcb)
-                {
-                  PANIC(OSERR_MQNOTFULLCOUNT);
-                }
-              else
-                {
-                  btcb->msgwaitq = NULL;
-                  msgq->nwaitnotfull--;
-                  up_unblock_task(btcb);
-                }
-              irqrestore(saved_state);
+              btcb->msgwaitq = NULL;
+              msgq->nwaitnotfull--;
+              up_unblock_task(btcb);
             }
+          irqrestore(saved_state);
         }
     }
 
diff --git a/nuttx/sched/mq_send.c b/nuttx/sched/mq_send.c
index 96d577759..f8768c9a4 100644
--- a/nuttx/sched/mq_send.c
+++ b/nuttx/sched/mq_send.c
@@ -43,6 +43,7 @@
 #include  <fcntl.h>
 #include  <mqueue.h>
 #include  <string.h>
+#include  <errno.h>
 #include  <sched.h>
 #include  <debug.h>
 #include  <nuttx/arch.h>
@@ -197,7 +198,18 @@ FAR mqmsg_t *mq_msgalloc(void)
  *   prio - The priority of the message
  *
  * Return Value:
- *   None
+ *   On success, mq_send() returns0 (OK); on error, -1 (ERROR)
+ *   is returned, with errno set  to  indicate the error:
+ *
+ *   EAGAIN   The queue was empty, and the O_NONBLOCK flag was
+ *            set for the message queue description referred to
+ *            by mqdes.
+ *   EINVAL   Either msg or mqdes is NULL or the value of prio
+ *            is invalid.
+ *   EPERM    Message queue opened not opened for writing.
+ *   EMSGSIZE 'msglen' was greater than the maxmsgsize attribute
+ *            of the message queue.
+ *   EINTR    The call was interrupted by a signal handler.
  *
  * Assumptions/restrictions:
  *
@@ -216,192 +228,225 @@ int mq_send(mqd_t mqdes, const void *msg, size_t msglen, int prio)
 
   /* Verify the input parameters */
 
+  if (!msg || !mqdes || prio < 0 || prio > MQ_PRIO_MAX)
+    {
+      *get_errno_ptr() = EINVAL;
+      return ERROR;
+    }
+
+  if ((mqdes->oflags & O_WROK) == 0)
+    {
+      *get_errno_ptr() = EPERM;
+      return ERROR;
+    }
+
+  if (msglen < 0 || msglen > (size_t)mqdes->msgq->maxmsgsize)
+    {
+      *get_errno_ptr() = EMSGSIZE;
+      return ERROR;
+    }
+
+  /* Get a pointer to the message queue */
+
   sched_lock();
-  if (msg && mqdes && (mqdes->oflags & O_WROK) != 0 &&
-      msglen > 0 && msglen <= (size_t)mqdes->msgq->maxmsgsize &&
-      prio >= 0 && prio <= MQ_PRIO_MAX)
+  msgq = mqdes->msgq;
+
+  /* If we are sending a message from an interrupt handler, then
+   * try to get message structure unconditionally.
+   */
+
+  saved_state = irqsave();
+  if (up_interrupt_context())
     {
-      /* Get a pointer to the message queue */
+      curr = mq_msgalloc();
+    }
 
-      msgq = mqdes->msgq;
+  /* Otherwise, arbitrarily limit the number of messages in the
+   * queue to the value determined when the message queue was opened.
+   * This makes us more POSIX-like as well as prohibits one slow
+   * responding task from consuming all available memory.
+   */
 
-      /* If we are sending a message from an interrupt handler, then
-       * try to get message structure unconditionally.
+  else if (msgq->nmsgs >= msgq->maxmsgs)
+    {
+      /* Should we block until there is sufficient space in the
+       * message queue?
        */
 
-      saved_state = irqsave();
-      if (up_interrupt_context())
+      if ((mqdes->oflags & O_NONBLOCK) != 0)
         {
-          curr = mq_msgalloc();
+          /* No... We will return an error to the caller. */
+
+          *get_errno_ptr() = EAGAIN;
+          curr = NULL;
         }
 
-      /* Otherwise, arbitrarily limit the number of messages in the
-       * queue to the value determined when the message queue was opened.
-       * This makes us more POSIX-like as well as prohibits one slow
-       * responding task from consuming all available memory.
+      /* Yes... We will not return control until the message queue is
+       * available.
        */
 
-      else if (msgq->nmsgs >= msgq->maxmsgs)
+      else
         {
-          /* Should we block until there is sufficient space in the
-           * message queue?
+          boolean interrupted = FALSE;
+
+          /* Loop until there are fewer than max allowable messages in the 
+           * receiving message queue
            */
 
-          if ((mqdes->oflags & O_NONBLOCK) != 0)
+          while (msgq->nmsgs >= msgq->maxmsgs)
             {
-              /* No... We will return an error to the caller. */
+              /* Block until the message queue is no longer full.
+               * When we are unblocked, we will try again
+               */
 
-              curr = NULL;
-            }
+              rtcb = (FAR _TCB*)g_readytorun.head;
+              rtcb->msgwaitq = msgq;
+              (msgq->nwaitnotfull)++;
 
-          /* Yes... We will not return control until the message queue is
-           * available.
-           */
+              *get_errno_ptr() = OK;
+              up_block_task(rtcb, TSTATE_WAIT_MQNOTFULL);
 
-          else
-            {
-              /* Loop until there are fewer than max allowable messages in the 
-               * receiving message queue
+              /* When we resume at this point, either (1) the message queue
+               * is no longer empty, or (2) the wait has been interrupted by
+               * a signal.  We can detect the latter case be examining the
+               * errno value (should be EINTR).
                */
 
-              while (msgq->nmsgs >= msgq->maxmsgs)
+              if (*get_errno_ptr() != OK)
                 {
-                  /* Block until the message queue is no longer full.
-                   * When we are unblocked, we will try again
-                   */
-
-                  rtcb = (FAR _TCB*)g_readytorun.head;
-                  rtcb->msgwaitq = msgq;
-                  (msgq->nwaitnotfull)++;
-                  up_block_task(rtcb, TSTATE_WAIT_MQNOTFULL);
+                   interrupted = TRUE;
+                   break;
                 }
+            }
 
-              /* It should be okay to get add a message to the receiving
-               * message queue now.
-               */
+          /* If we were not interrupted, then it should be okay to add
+           * a message to the receiving message queue now.
+           */
 
+          if (!interrupted)
+            {
               curr = mq_msgalloc();
             }
         }
+    }
 
-      /* We are not in an interrupt handler and the receiving message queue
-       * is not full
-       */
+  /* We are not in an interrupt handler and the receiving message queue
+   * is not full
+   */
 
-      else
-        {
-          /* Just allocate a message */
+  else
+    {
+      /* Just allocate a message */
 
-          curr = mq_msgalloc();
-        }
-      irqrestore(saved_state);
+      curr = mq_msgalloc();
+    }
+  irqrestore(saved_state);
 
-      /* Check if we were able to get a message structure */
+  /* Check if we were able to get a message structure */
 
-      if (curr)
-        {
-          /* Construct the current message header info */
+  if (curr)
+    {
+      /* Construct the current message header info */
 
-          curr->priority = (ubyte)prio;
-          curr->msglen   = (ubyte)msglen;
+      curr->priority = (ubyte)prio;
+      curr->msglen   = (ubyte)msglen;
 
-          /* Copy the message data into the message */
+      /* Copy the message data into the message */
 
-          memcpy((void*)curr->mail, (const void*)msg, msglen);
+      memcpy((void*)curr->mail, (const void*)msg, msglen);
 
-          /* Insert the new message in the message queue */
+      /* Insert the new message in the message queue */
 
-          saved_state = irqsave();
+      saved_state = irqsave();
 
-          /* Search the message list to find the location to insert the new
-           * message. Each is list is maintained in ascending priority order.
-           */
+      /* Search the message list to find the location to insert the new
+       * message. Each is list is maintained in ascending priority order.
+       */
 
-          for (prev = NULL, next = (FAR mqmsg_t*)msgq->msglist.head;
-               next && prio <= next->priority;
-               prev = next, next = next->next);
+      for (prev = NULL, next = (FAR mqmsg_t*)msgq->msglist.head;
+           next && prio <= next->priority;
+           prev = next, next = next->next);
 
-          /* Add the message at the right place */
+      /* Add the message at the right place */
 
-          if (prev)
-            {
-              sq_addafter((FAR sq_entry_t*)prev, (FAR sq_entry_t*)curr,
-                          &msgq->msglist);
-            }
-          else
-            {
-              sq_addfirst((FAR sq_entry_t*)curr, &msgq->msglist);
-            }
+      if (prev)
+        {
+          sq_addafter((FAR sq_entry_t*)prev, (FAR sq_entry_t*)curr,
+                      &msgq->msglist);
+        }
+      else
+        {
+          sq_addfirst((FAR sq_entry_t*)curr, &msgq->msglist);
+        }
 
-          /* Increment the count of message in the queue */
+      /* Increment the count of message in the queue */
 
-          msgq->nmsgs++;
-          irqrestore(saved_state);
+      msgq->nmsgs++;
+      irqrestore(saved_state);
 
-          /* Check if we need to notify any tasks that are attached to the
-           * message queue
-           */
+      /* Check if we need to notify any tasks that are attached to the
+       * message queue
+       */
 
 #ifndef CONFIG_DISABLE_SIGNALS
-          if (msgq->ntmqdes)
-            {
-              /* Remove the message notification data from the message queue. */
+      if (msgq->ntmqdes)
+        {
+          /* Remove the message notification data from the message queue. */
 
 #ifdef CONFIG_CAN_PASS_STRUCTS
-              union sigval value      = msgq->ntvalue;
+          union sigval value      = msgq->ntvalue;
 #else
-              void *sival_ptr         = msgq->ntvalue.sival_ptr;
+          void *sival_ptr         = msgq->ntvalue.sival_ptr;
 #endif
-              int signo               = msgq->ntsigno;
-              int pid                 = msgq->ntpid;
+          int signo               = msgq->ntsigno;
+          int pid                 = msgq->ntpid;
 
-              /* Detach the notification */
+          /* Detach the notification */
 
-              msgq->ntpid             = INVALID_PROCESS_ID;
-              msgq->ntsigno           = 0;
-              msgq->ntvalue.sival_int = 0;
-              msgq->ntmqdes           = NULL;
+          msgq->ntpid             = INVALID_PROCESS_ID;
+          msgq->ntsigno           = 0;
+          msgq->ntvalue.sival_int = 0;
+          msgq->ntmqdes           = NULL;
 
-              /* Queue the signal -- What if this returns an error? */
+          /* Queue the signal -- What if this returns an error? */
 
 #ifdef CONFIG_CAN_PASS_STRUCTS
-              sig_mqnotempty(pid, signo, value);
+          sig_mqnotempty(pid, signo, value);
 #else
-              sig_mqnotempty(pid, signo, sival_ptr);
+          sig_mqnotempty(pid, signo, sival_ptr);
 #endif
-            }
+        }
 #endif
-          /* Check if any tasks are waiting for the MQ not empty event. */
+      /* Check if any tasks are waiting for the MQ not empty event. */
 
-          saved_state = irqsave();
-          if (msgq->nwaitnotempty > 0)
-            {
-              /* Find the highest priority task that is waiting for
-               * this queue to be non-empty in g_waitingformqnotempty
-               * list. sched_lock() should give us sufficent protection since
-               * interrupts should never cause a change in this list
-               */
+      saved_state = irqsave();
+      if (msgq->nwaitnotempty > 0)
+        {
+          /* Find the highest priority task that is waiting for
+           * this queue to be non-empty in g_waitingformqnotempty
+           * list. sched_lock() should give us sufficent protection since
+           * interrupts should never cause a change in this list
+           */
 
-              for (btcb = (FAR _TCB*)g_waitingformqnotempty.head;
-                   btcb && btcb->msgwaitq != msgq;
-                   btcb = btcb->flink);
+          for (btcb = (FAR _TCB*)g_waitingformqnotempty.head;
+               btcb && btcb->msgwaitq != msgq;
+               btcb = btcb->flink);
 
-              /* If one was found, unblock it */
+          /* If one was found, unblock it */
 
-              if (!btcb)
-                {
-                  PANIC(OSERR_MQNONEMPTYCOUNT);
-                }
-              else
-                {
-                  btcb->msgwaitq = NULL;
-                  msgq->nwaitnotempty--;
-                  up_unblock_task(btcb);
-                }
+          if (!btcb)
+            {
+              PANIC(OSERR_MQNONEMPTYCOUNT);
+            }
+          else
+            {
+              btcb->msgwaitq = NULL;
+              msgq->nwaitnotempty--;
+              up_unblock_task(btcb);
             }
-          irqrestore(saved_state);
-          ret = OK;
         }
+      irqrestore(saved_state);
+      ret = OK;
     }
 
   sched_unlock();
diff --git a/nuttx/sched/mq_waitirq.c b/nuttx/sched/mq_waitirq.c
new file mode 100644
index 000000000..be23e5b90
--- /dev/null
+++ b/nuttx/sched/mq_waitirq.c
@@ -0,0 +1,125 @@
+/************************************************************
+ * mq_waitirq.c
+ *
+ *   Copyright (C) 2007 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name Gregory Nutt nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************/
+
+/************************************************************
+ * Included Files
+ ************************************************************/
+
+#include <sys/types.h>
+#include <sched.h>
+#include <errno.h>
+#include <nuttx/arch.h>
+#include "sem_internal.h"
+
+/************************************************************
+ * Compilation Switches
+ ************************************************************/
+
+/************************************************************
+ * Definitions
+ ************************************************************/
+
+/************************************************************
+ * Private Type Declarations
+ ************************************************************/
+
+/************************************************************
+ * Global Variables
+ ************************************************************/
+
+/************************************************************
+ * Private Variables
+ ************************************************************/
+
+/************************************************************
+ * Private Functions
+ ************************************************************/
+
+/************************************************************
+ * Public Functions
+ ************************************************************/
+
+/************************************************************
+ * Function:  sem_waitirq
+ *
+ * Description:
+ *   This function is called when a signal is received by a
+ *   task that is waiting on a message queue -- either for a
+ *   queue to becoming not full (on mq_send) or not empty
+ *   (on mq_receive).
+ *
+ * Parameters:
+ *   wtcb - A pointer to the TCB of the task that is waiting
+ *      on a message queue, but has received a signal instead.
+ *
+ * Return Value:
+ *   None
+ *
+ * Assumptions:
+ *
+ ************************************************************/
+
+void mq_waitirq(FAR _TCB *wtcb)
+{
+  irqstate_t saved_state;
+
+  /* Disable interrupts.  This is necessary because an
+   * interrupt handler may attempt to send a message while we are
+   * doing this.
+   */
+
+  saved_state = irqsave();
+
+  /* It is possible that an interrupt/context switch beat us to the
+   * punch and already changed the task's state.
+   */
+
+  if (wtcb->task_state == TSTATE_WAIT_MQNOTEMPTY ||
+      wtcb->task_state == TSTATE_WAIT_MQNOTFULL)
+    {
+      /* Mark the errno value for the thread. */
+
+      wtcb->errno = EINTR;
+
+      /* Restart the the task. */
+
+      up_unblock_task(wtcb);
+    }
+
+  /* Interrupts may now be enabled. */
+
+  irqrestore(saved_state);
+}
+
diff --git a/nuttx/sched/sem_wait.c b/nuttx/sched/sem_wait.c
index cbf8fd019..b31a48fa8 100644
--- a/nuttx/sched/sem_wait.c
+++ b/nuttx/sched/sem_wait.c
@@ -156,11 +156,13 @@ int sem_wait(sem_t *sem)
 
           /* Add the TCB to the prioritized semaphore wait queue */
 
+          *get_errno_ptr() = 0;
           up_block_task(rtcb, TSTATE_WAIT_SEM);
 
           /* When we resume at this point, either (1) the semaphore has been
            * assigned to this thread of execution, or (2) the semaphore wait
-           * has been interrupted by a signal.
+           * has been interrupted by a signal.  We can detect the latter case
+           * be examining the errno value.
            */
 
           if (*get_errno_ptr() != EINTR)
diff --git a/nuttx/sched/sig_received.c b/nuttx/sched/sig_received.c
index 7a5e2e749..6cdf1dd9f 100644
--- a/nuttx/sched/sig_received.c
+++ b/nuttx/sched/sig_received.c
@@ -47,6 +47,7 @@
 #include "os_internal.h"
 #include "sem_internal.h"
 #include "sig_internal.h"
+#include "mq_internal.h"
 
 /************************************************************
  * Definitions
@@ -380,7 +381,13 @@ int sig_received(FAR _TCB *stcb, siginfo_t *info)
        * task must be unblocked when a signal is received.
        */
 
-      /* NOT YET IMPLEMENTED. */
+#ifndef CONFIG_DISABLE_MQUEUE
+     if (stcb->task_state == TSTATE_WAIT_MQNOTEMPTY ||
+         stcb->task_state == TSTATE_WAIT_MQNOTFULL)
+        {
+          mq_waitirq(stcb);
+        }
+#endif
    }
 
   return ret;