User-mode work queue logic should not disable interrupts

5 files changed, 389 insertions, 1 deletions

diff --git a/nuttx/sched/wqueue/Make.defs b/nuttx/sched/wqueue/Make.defs
index 4a375926a..54efbbd7d 100644
--- a/nuttx/sched/wqueue/Make.defs
+++ b/nuttx/sched/wqueue/Make.defs
@@ -37,7 +37,7 @@
 
 ifeq ($(CONFIG_SCHED_WORKQUEUE),y)
 
-CSRCS += kwork_queue.c kwork_cancel.c kwork_signal.c
+CSRCS += kwork_queue.c kwork_process.c kwork_cancel.c kwork_signal.c
 
 # Add high priority work queue files
 
diff --git a/nuttx/sched/wqueue/kwork_cancel.c b/nuttx/sched/wqueue/kwork_cancel.c
index e8a44acf2..589389c52 100644
--- a/nuttx/sched/wqueue/kwork_cancel.c
+++ b/nuttx/sched/wqueue/kwork_cancel.c
@@ -39,8 +39,11 @@
 
 #include <nuttx/config.h>
 
+#include <queue.h>
+#include <assert.h>
 #include <errno.h>
 
+#include <nuttx/arch.h>
 #include <nuttx/wqueue.h>
 
 #include "wqueue/wqueue.h"
@@ -68,6 +71,60 @@
  ****************************************************************************/
 
 /****************************************************************************
+ * Name: work_qcancel
+ *
+ * Description:
+ *   Cancel previously queued work.  This removes work from the work queue.
+ *   After work has been cancelled, it may be re-queue by calling work_queue()
+ *   again.
+ *
+ * Input parameters:
+ *   qid    - The work queue ID
+ *   work   - The previously queue work structure to cancel
+ *
+ * Returned Value:
+ *   Zero (OK) on success, a negated errno on failure.  This error may be
+ *   reported:
+ *
+ *   -ENOENT - There is no such work queued.
+ *   -EINVAL - An invalid work queue was specified
+ *
+ ****************************************************************************/
+
+static int work_qcancel(FAR struct wqueue_s *wqueue, FAR struct work_s *work)
+{
+  irqstate_t flags;
+  int ret = -ENOENT;
+
+  DEBUGASSERT(work != NULL);
+
+  /* Cancelling the work is simply a matter of removing the work structure
+   * from the work queue.  This must be done with interrupts disabled because
+   * new work is typically added to the work queue from interrupt handlers.
+   */
+
+  flags = irqsave();
+  if (work->worker != NULL)
+    {
+      /* A little test of the integrity of the work queue */
+
+      DEBUGASSERT(work->dq.flink || (FAR dq_entry_t *)work == wqueue->q.tail);
+      DEBUGASSERT(work->dq.blink || (FAR dq_entry_t *)work == wqueue->q.head);
+
+      /* Remove the entry from the work queue and make sure that it is
+       * mark as available (i.e., the worker field is nullified).
+       */
+
+      dq_rem((FAR dq_entry_t *)work, &wqueue->q);
+      work->worker = NULL;
+      ret = OK;
+    }
+
+  irqrestore(flags);
+  return ret;
+}
+
+/****************************************************************************
  * Public Functions
  ****************************************************************************/
 
diff --git a/nuttx/sched/wqueue/kwork_process.c b/nuttx/sched/wqueue/kwork_process.c
new file mode 100644
index 000000000..1e878dc43
--- /dev/null
+++ b/nuttx/sched/wqueue/kwork_process.c
@@ -0,0 +1,251 @@
+/****************************************************************************
+ * libc/wqueue/work_process.c
+ *
+ *   Copyright (C) 2009-2014 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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 <nuttx/config.h>
+
+#include <stdint.h>
+#include <unistd.h>
+#include <queue.h>
+
+#include <nuttx/clock.h>
+#include <nuttx/wqueue.h>
+
+#include <arch/irq.h>
+
+#ifdef CONFIG_SCHED_WORKQUEUE
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Use CLOCK_MONOTONIC if it is available.  CLOCK_REALTIME can cause bad
+ * delays if the time is changed.
+ */
+
+#ifdef CONFIG_CLOCK_MONOTONIC
+#  define WORK_CLOCK CLOCK_MONOTONIC
+#else
+#  define WORK_CLOCK CLOCK_REALTIME
+#endif
+
+#ifndef MIN
+#  define MIN(a,b) ((a) < (b) ? (a) : (b))
+#endif
+
+/****************************************************************************
+ * Private Type Declarations
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: work_process
+ *
+ * Description:
+ *   This is the logic that performs actions placed on any work list.  This
+ *   logic is the common underlying logic to all work queues.  This logic is
+ *   part of the internal implementation of each work queue; it should not
+ *   be called from application level logic.
+ *
+ * Input parameters:
+ *   wqueue - Describes the work queue to be processed
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+void work_process(FAR struct wqueue_s *wqueue)
+{
+  volatile FAR struct work_s *work;
+  worker_t  worker;
+  irqstate_t flags;
+  FAR void *arg;
+  uint32_t elapsed;
+  uint32_t remaining;
+  uint32_t stick;
+  uint32_t ctick;
+  uint32_t next;
+
+  /* Then process queued work.  We need to keep interrupts disabled while
+   * we process items in the work list.
+   */
+
+  next  = wqueue->delay;
+  flags = irqsave();
+
+  /* Get the time that we started this polling cycle in clock ticks. */
+
+  stick = clock_systimer();
+
+  /* And check each entry in the work queue.  Since we have disabled
+   * interrupts we know:  (1) we will not be suspended unless we do
+   * so ourselves, and (2) there will be no changes to the work queue
+   */
+
+  work = (FAR struct work_s *)wqueue->q.head;
+  while (work)
+    {
+      DEBUGASSERT(wqueue->wq_sem.count > 0);
+
+      /* Is this work ready?  It is ready if there is no delay or if
+       * the delay has elapsed. qtime is the time that the work was added
+       * to the work queue.  It will always be greater than or equal to
+       * zero.  Therefore a delay of zero will always execute immediately.
+       */
+
+      ctick   = clock_systimer();
+      elapsed = ctick - work->qtime;
+      if (elapsed >= work->delay)
+        {
+          /* Remove the ready-to-execute work from the list */
+
+          (void)dq_rem((struct dq_entry_s *)work, &wqueue->q);
+
+          /* Extract the work description from the entry (in case the work
+           * instance by the re-used after it has been de-queued).
+           */
+
+          worker = work->worker;
+
+          /* Check for a race condition where the work may be nullified
+           * before it is removed from the queue.
+           */
+
+          if (worker != NULL)
+            {
+              /* Extract the work argument (before re-enabling interrupts) */
+
+              arg = work->arg;
+
+              /* Mark the work as no longer being queued */
+
+              work->worker = NULL;
+
+              /* Do the work.  Re-enable interrupts while the work is being
+               * performed... we don't have any idea how long this will take!
+               */
+
+              irqrestore(flags);
+              worker(arg);
+
+              /* Now, unfortunately, since we re-enabled interrupts we don't
+               * know the state of the work list and we will have to start
+               * back at the head of the list.
+               */
+
+              flags = irqsave();
+              work  = (FAR struct work_s *)wqueue->q.head;
+            }
+          else
+            {
+              /* Cancelled.. Just move to the next work in the list with
+               * interrupts still disabled.
+               */
+
+              work = (FAR struct work_s *)work->dq.flink;
+            }
+        }
+      else
+        {
+          /* This one is not ready.. will it be ready before the next
+           * scheduled wakeup interval?
+           *
+           * NOTE that elapsed is relative to the the current time,
+           * not the time of beginning of this queue processing pass.
+           * So it may need an adjustment.
+           */
+
+          elapsed  += (ctick - stick);
+          remaining = elapsed - work->delay;
+          if (remaining < next)
+            {
+              /* Yes.. Then schedule to wake up when the work is ready */
+
+              next = remaining;
+            }
+
+          /* Then try the next in the list. */
+
+          work = (FAR struct work_s *)work->dq.flink;
+        }
+    }
+
+  /* Get the delay (in clock ticks) since we started the sampling */
+
+  elapsed = clock_systimer() - work->qtime;
+  if (elapsed <= wqueue->delay)
+    {
+      /* How must time would we need to delay to get to the end of the 
+       * sampling period?  The amount of time we delay should be the smaller
+       * of the time to the end of the sampling period and the time to the
+       * next work expiry.
+       */
+
+      remaining = wqueue->delay - elapsed;
+      next      = MIN(next, remaining);
+      if (next > 0)
+        {
+          /* Wait awhile to check the work list.  We will wait here until
+           * either the time elapses or until we are awakened by a signal.
+           * Interrupts will be re-enabled while we wait.
+           */
+
+          usleep(next * USEC_PER_TICK);
+        }
+    }
+
+  irqrestore(flags);
+}
+
+#endif /* CONFIG_SCHED_WORKQUEUE */
diff --git a/nuttx/sched/wqueue/kwork_queue.c b/nuttx/sched/wqueue/kwork_queue.c
index 9a979f947..e385d76b7 100644
--- a/nuttx/sched/wqueue/kwork_queue.c
+++ b/nuttx/sched/wqueue/kwork_queue.c
@@ -40,8 +40,12 @@
 #include <nuttx/config.h>
 
 #include <stdint.h>
+#include <queue.h>
+#include <assert.h>
 #include <errno.h>
 
+#include <nuttx/arch.h>
+#include <nuttx/clock.h>
 #include <nuttx/wqueue.h>
 
 #include "wqueue/wqueue.h"
@@ -69,6 +73,63 @@
  ****************************************************************************/
 
 /****************************************************************************
+ * Name: work_qqueue
+ *
+ * Description:
+ *   Queue work to be performed at a later time.  All queued work will be
+ *   performed on the worker thread of of execution (not the caller's).
+ *
+ *   The work structure is allocated by caller, but completely managed by
+ *   the work queue logic.  The caller should never modify the contents of
+ *   the work queue structure; the caller should not call work_queue()
+ *   again until either (1) the previous work has been performed and removed
+ *   from the queue, or (2) work_cancel() has been called to cancel the work
+ *   and remove it from the work queue.
+ *
+ * Input parameters:
+ *   qid    - The work queue ID (index)
+ *   work   - The work structure to queue
+ *   worker - The worker callback to be invoked.  The callback will invoked
+ *            on the worker thread of execution.
+ *   arg    - The argument that will be passed to the workder callback when
+ *            int is invoked.
+ *   delay  - Delay (in clock ticks) from the time queue until the worker
+ *            is invoked. Zero means to perform the work immediately.
+ *
+ * Returned Value:
+ *   Zero on success, a negated errno on failure
+ *
+ ****************************************************************************/
+
+static int work_qqueue(FAR struct wqueue_s *wqueue, FAR struct work_s *work,
+                       worker_t worker, FAR void *arg, uint32_t delay)
+{
+  irqstate_t flags;
+
+  DEBUGASSERT(work != NULL);
+
+  /* First, initialize the work structure */
+
+  work->worker = worker;           /* Work callback */
+  work->arg    = arg;              /* Callback argument */
+  work->delay  = delay;            /* Delay until work performed */
+
+  /* Now, time-tag that entry and put it in the work queue.  This must be
+   * done with interrupts disabled.  This permits this function to be called
+   * from with task logic or interrupt handlers.
+   */
+
+  flags        = irqsave();
+  work->qtime  = clock_systimer(); /* Time work queued */
+
+  dq_addlast((FAR dq_entry_t *)work, &wqueue->q);
+  kill(wqueue->pid[0], SIGWORK);   /* Wake up the worker thread */
+
+  irqrestore(flags);
+  return OK;
+}
+
+/****************************************************************************
  * Public Functions
  ****************************************************************************/
 
diff --git a/nuttx/sched/wqueue/wqueue.h b/nuttx/sched/wqueue/wqueue.h
index 0c95b8501..530775072 100644
--- a/nuttx/sched/wqueue/wqueue.h
+++ b/nuttx/sched/wqueue/wqueue.h
@@ -123,5 +123,24 @@ int work_hpstart(void);
 int work_lpstart(void);
 #endif
 
+/****************************************************************************
+ * Name: work_process
+ *
+ * Description:
+ *   This is the logic that performs actions placed on any work list.  This
+ *   logic is the common underlying logic to all work queues.  This logic is
+ *   part of the internal implementation of each work queue; it should not
+ *   be called from application level logic.
+ *
+ * Input parameters:
+ *   wqueue - Describes the work queue to be processed
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+void work_process(FAR struct wqueue_s *wqueue);
+
 #endif /* CONFIG_SCHED_WORKQUEUE */
 #endif /* __SCHED_WQUEUE_WQUEUE_H */