Linux: Added preliminary work queue support

Based on NuttX work queue code.

Not yet functional.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/platforms/linux/px4_layer/module.mk

@@ -38,6 +38,9 @@
 SRCS		 = 	\
 			px4_linux_impl.cpp \
 			px4_linux_tasks.c  \
+			work_thread.c \
+			work_queue.c \
+			work_cancel.c \
 			lib_crc32.c \
 			drv_hrt.c \
 			queue.c \
@@ -45,6 +48,7 @@ SRCS		 = 	\
 			dq_remfirst.c \
 			sq_addlast.c \
 			sq_remfirst.c \
-			sq_addafter.c
+			sq_addafter.c \
+			dq_rem.c 
 
 MAXOPTIMIZATION	 = -Os

src/platforms/linux/px4_layer/px4_linux_impl.cpp

@@ -56,60 +56,32 @@ long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK);
 
 __END_DECLS
 
+extern struct wqueue_s gwork[NWORKERS];
+
 namespace px4
 {
 
 void init(int argc, char *argv[], const char *app_name)
 {
-        struct param_info_s test_1 = {
-                "TEST_1",
-                PARAM_TYPE_INT32
-        };
-        test_1.val.i = 2;
-
-        struct param_info_s test_2 = {
-                "TEST_2",
-                PARAM_TYPE_INT32
-        };
-        test_2.val.i = 4;
-
-        struct param_info_s rc_x = {
-                "RC_X",
-                PARAM_TYPE_INT32
-        };
-        rc_x.val.i = 8;
-
-        struct param_info_s rc2_x = {
-                "RC2_X",
-                PARAM_TYPE_INT32
-        };
-        rc2_x.val.i = 16;
-
-        param_array[0] = test_1;
-        param_array[1] = test_2;
-        param_array[2] = rc_x;
-        param_array[3] = rc2_x;
-        param_info_base = (struct param_info_s *) &param_array[0];
-        param_info_limit = (struct param_info_s *) &param_array[4];     // needs to point at the end of the data,
-                                                                        // therefore number of params + 1
-
 	printf("App name: %s\n", app_name);
+
+	// Create high priority worker thread
+	g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high",
+			       SCHED_DEFAULT,
+			       SCHED_PRIORITY_MAX,
+			       2000,
+			       work_hpthread,
+			       (char* const*)NULL);
+
+	// Create low priority worker thread
+	g_work[LPWORK].pid = px4_task_spawn_cmd("wkr_low",
+			       SCHED_DEFAULT,
+			       SCHED_PRIORITY_MIN,
+			       2000,
+			       work_lpthread,
+			       (char* const*)NULL);
+
 }
 
 }
 
-
-
-
-int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay)
-{
-	printf("work_queue: UNIMPLEMENTED\n");
-	return PX4_OK;
-}
-
-int work_cancel(int qid, struct work_s *work)
-{
-	printf("work_cancel: UNIMPLEMENTED\n");
-	return PX4_OK;
-}
-

src/platforms/linux/px4_layer/work_cancel.c

@@ -37,15 +37,10 @@
  * Included Files
  ****************************************************************************/
 
-#include <nuttx/config.h>
+#include <px4_config.h>
-
+#include <px4_defines.h>
 #include <queue.h>
-#include <assert.h>
+#include <px4_workqueue.h>
-#include <errno.h>
-#include <debug.h>
-
-#include <nuttx/arch.h>
-#include <nuttx/wqueue.h>
 
 #ifdef CONFIG_SCHED_WORKQUEUE
 
@@ -90,25 +85,25 @@
  *
  ****************************************************************************/
 
-int work_cancel(int qid, FAR struct work_s *work)
+int work_cancel(int qid, struct work_s *work)
 {
-  FAR struct wqueue_s *wqueue = &g_work[qid];
+  struct wqueue_s *wqueue = &g_work[qid];
-  irqstate_t flags;
+  //irqstate_t flags;
 
-  DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
+  //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
 
   /* 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();
+  //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.flink ||(FAR dq_entry_t *)work == wqueue->q.tail);
-      DEBUGASSERT(work->dq.blink ||(FAR dq_entry_t *)work == wqueue->q.head);
+      //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 availalbe (i.e., the worker field is nullified).
@@ -118,8 +113,8 @@ int work_cancel(int qid, FAR struct work_s *work)
       work->worker = NULL;
     }
 
-  irqrestore(flags);
+  //irqrestore(flags);
-  return OK;
+  return PX4_OK;
 }
 
 #endif /* CONFIG_SCHED_WORKQUEUE */

src/platforms/linux/px4_layer/work_queue.c

@@ -37,17 +37,13 @@
  * Included Files
  ****************************************************************************/
 
-#include <nuttx/config.h>
+#include <px4_config.h>
+#include <px4_defines.h>
 
+#include <signal.h>
 #include <stdint.h>
 #include <queue.h>
-#include <assert.h>
+#include <px4_workqueue.h>
-#include <errno.h>
-#include <debug.h>
-
-#include <nuttx/arch.h>
-#include <nuttx/clock.h>
-#include <nuttx/wqueue.h>
 
 #ifdef CONFIG_SCHED_WORKQUEUE
 
@@ -104,13 +100,11 @@
  *
  ****************************************************************************/
 
-int work_queue(int qid, FAR struct work_s *work, worker_t worker,
+int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay)
-               FAR void *arg, uint32_t delay)
 {
-  FAR struct wqueue_s *wqueue = &g_work[qid];
+  struct wqueue_s *wqueue = &g_work[qid];
-  irqstate_t flags;
 
-  DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
+  //DEBUGASSERT(work != NULL && (unsigned)qid < NWORKERS);
 
   /* First, initialize the work structure */
 
@@ -123,14 +117,14 @@ int work_queue(int qid, FAR struct work_s *work, worker_t worker,
    * from with task logic or interrupt handlers.
    */
 
-  flags        = irqsave();
+  //flags        = irqsave();
   work->qtime  = clock_systimer(); /* Time work queued */
 
-  dq_addlast((FAR dq_entry_t *)work, &wqueue->q);
+  dq_addlast((dq_entry_t *)work, &wqueue->q);
-  kill(wqueue->pid, SIGWORK);      /* Wake up the worker thread */
+  pthread_kill(wqueue->pid, SIGUSR1);      /* Wake up the worker thread */
 
-  irqrestore(flags);
+  //irqrestore(flags);
-  return OK;
+  return PX4_OK;
 }
 
 #endif /* CONFIG_SCHED_WORKQUEUE */

src/platforms/linux/px4_layer/work_thread.c

@@ -37,19 +37,12 @@
  * Included Files
  ****************************************************************************/
 
-#include <nuttx/config.h>
+#include <px4_config.h>
-
+#include <px4_defines.h>
 #include <stdint.h>
 #include <unistd.h>
 #include <queue.h>
-#include <assert.h>
+#include <px4_workqueue.h>
-#include <errno.h>
-#include <debug.h>
-
-#include <nuttx/arch.h>
-#include <nuttx/wqueue.h>
-#include <nuttx/clock.h>
-#include <nuttx/kmalloc.h>
 
 #ifdef CONFIG_SCHED_WORKQUEUE
 
@@ -66,29 +59,8 @@
  ****************************************************************************/
 
 /* The state of each work queue. */
-
-#ifdef CONFIG_NUTTX_KERNEL
-
-  /* Play some games in the kernel mode build to assure that different
-   * naming is used for the global work queue data structures.  This may
-   * not be necessary but it safer.
-   *
-   * In this case g_work is #define'd to be either g_kernelwork or
-   * g_usrwork in include/nuttx/wqueue.h
-   */
-
-#  ifdef __KERNEL__
-struct wqueue_s g_kernelwork[NWORKERS];
-#  else
-struct wqueue_s g_usrwork[NWORKERS];
-#  endif
-
-#else /* CONFIG_NUTTX_KERNEL */
-
 struct wqueue_s g_work[NWORKERS];
 
-#endif /* CONFIG_NUTTX_KERNEL */
-
 /****************************************************************************
  * Private Variables
  ****************************************************************************/
@@ -115,7 +87,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
 {
   volatile FAR struct work_s *work;
   worker_t  worker;
-  irqstate_t flags;
+  //irqstate_t flags;
   FAR void *arg;
   uint32_t elapsed;
   uint32_t remaining;
@@ -125,8 +97,9 @@ static void work_process(FAR struct wqueue_s *wqueue)
    * we process items in the work list.
    */
 
-  next  = CONFIG_SCHED_WORKPERIOD / USEC_PER_TICK;
+  //next  = CONFIG_SCHED_WORKPERIOD / USEC_PER_TICK;
-  flags = irqsave();
+  next  = 100;
+  //flags = irqsave();
   work  = (FAR struct work_s *)wqueue->q.head;
   while (work)
     {
@@ -158,7 +131,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
            * performed... we don't have any idea how long that will take!
            */
 
-          irqrestore(flags);
+          //irqrestore(flags);
           worker(arg);
 
           /* Now, unfortunately, since we re-enabled interrupts we don't
@@ -166,7 +139,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
            * back at the head of the list.
            */
 
-          flags = irqsave();
+          //flags = irqsave();
           work  = (FAR struct work_s *)wqueue->q.head;
         }
       else
@@ -195,7 +168,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
    */
 
   usleep(next * USEC_PER_TICK);
-  irqrestore(flags);
+  //irqrestore(flags);
 }
 
 /****************************************************************************
@@ -258,7 +231,7 @@ int work_hpthread(int argc, char *argv[])
       work_process(&g_work[HPWORK]);
     }
 
-  return OK; /* To keep some compilers happy */
+  return PX4_OK; /* To keep some compilers happy */
 }
 
 #ifdef CONFIG_SCHED_LPWORK
@@ -276,7 +249,7 @@ int work_lpthread(int argc, char *argv[])
        * the IDLE thread (at a very, very low priority).
        */
 
-      sched_garbagecollection();
+      //sched_garbagecollection();
 
       /* Then process queued work.  We need to keep interrupts disabled while
        * we process items in the work list.
@@ -285,7 +258,7 @@ int work_lpthread(int argc, char *argv[])
       work_process(&g_work[LPWORK]);
     }
 
-  return OK; /* To keep some compilers happy */
+  return PX4_OK; /* To keep some compilers happy */
 }
 
 #endif /* CONFIG_SCHED_LPWORK */
@@ -306,9 +279,13 @@ int work_usrthread(int argc, char *argv[])
       work_process(&g_work[USRWORK]);
     }
 
-  return OK; /* To keep some compilers happy */
+  return PX4_OK; /* To keep some compilers happy */
 }
 
 #endif /* CONFIG_SCHED_USRWORK */
 
+int clock_systimer()
+{
+	return 1;
+}
 #endif /* CONFIG_SCHED_WORKQUEUE */

src/platforms/px4_config.h

@@ -44,6 +44,9 @@
 #include <px4_config.h>
 #elif defined (__PX4_LINUX)
 #define CONFIG_NFILE_STREAMS 1
+#define CONFIG_SCHED_WORKQUEUE 1
+#define CONFIG_SCHED_HPWORK 1
+#define CONFIG_SCHED_LPWORK 1
 #define CONFIG_ARCH_BOARD_LINUXTEST 1
 
 #define px4_errx(x, ...) errx(x, __VA_ARGS__)

src/platforms/px4_defines.h

@@ -123,7 +123,8 @@ __BEGIN_DECLS
 extern long PX4_TICKS_PER_SEC;
 __END_DECLS
 
-#define USEC2TICK(x) (PX4_TICKS_PER_SEC*(long)x/1000000L)
+#define USEC2TICK(x) (PX4_TICKS_PER_SEC*(long)(x)/1000000L)
+#define USEC_PER_TICK (1000000L/PX4_TICKS_PER_SEC)
 
 #define px4_statfs_buf_f_bavail_t unsigned long
 

src/platforms/px4_tasks.h

@@ -55,6 +55,7 @@ typedef int px4_task_t;
 
 #define SCHED_DEFAULT	SCHED_FIFO
 #define SCHED_PRIORITY_MAX sched_get_priority_max(SCHED_FIFO)
+#define SCHED_PRIORITY_MIN sched_get_priority_min(SCHED_FIFO)
 #define SCHED_PRIORITY_DEFAULT sched_get_priority_max(SCHED_FIFO)
 
 typedef pthread_t px4_task_t;

src/platforms/px4_workqueue.h

@@ -43,10 +43,22 @@
 #include <nuttx/clock.h>
 #elif defined(__PX4_LINUX)
 
+#include <stdint.h>
 #include <queue.h>
 
+__BEGIN_DECLS
+
+#define HPWORK 0
 #define LPWORK 1
-#define HPWORK 2
+#define NWORKERS 2
+
+struct wqueue_s
+{
+  pid_t             pid; /* The task ID of the worker thread */
+  struct dq_queue_s q;   /* The queue of pending work */
+};
+
+extern struct wqueue_s g_work[NWORKERS];
 
 /* Defines the work callback */
 
@@ -90,8 +102,7 @@ struct work_s
  *
  ****************************************************************************/
 
-int work_queue(int qid, FAR struct work_s *work, worker_t worker,
+int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_t delay);
-               FAR void *arg, uint32_t delay);
 
 /****************************************************************************
  * Name: work_cancel
@@ -110,7 +121,15 @@ int work_queue(int qid, FAR struct work_s *work, worker_t worker,
  *
  ****************************************************************************/
 
-int work_cancel(int qid, FAR struct work_s *work);
+int work_cancel(int qid, struct work_s *work);
+
+int clock_systimer(void);
+
+int work_hpthread(int argc, char *argv[]);
+int work_lpthread(int argc, char *argv[]);
+
+__END_DECLS
+
 #else 
 #error "Unknown target OS"
 #endif