POSIX: fixed hrt call and workqueue implementation

The HRT call processing normally happens via HW timer interrupt
handler. Since the POSIX port has no ISR handling, the HP work
queue is used.

Instead of irq_save() and irq_restore() calls to disable/enable
interrupts, a mutex is used to protect each queue.

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

src/platforms/posix/main.cpp

@@ -43,6 +43,10 @@
 #include <sstream>
 #include <vector>
 
+namespace px4 {
+void init_once(void);
+}
+
 using namespace std;
 
 typedef int (*px4_main_t)(int argc, char *argv[]);
@@ -94,6 +98,8 @@ int main(int argc, char **argv)
 
 	string mystr;
 	
+	px4::init_once();
+
 	px4::init(argc, argv, "mainapp");
 
 	while(1) {

src/platforms/posix/px4_layer/drv_hrt.c

@@ -66,11 +66,13 @@ hrt_call_invoke(void);
 
 static void hrt_lock(void)
 {
+	//printf("hrt_lock\n");
 	sem_wait(&_hrt_lock);
 }
 
 static void hrt_unlock(void)
 {
+	//printf("hrt_unlock\n");
 	sem_post(&_hrt_lock);
 }
 
@@ -219,11 +221,13 @@ hrt_call_enter(struct hrt_call *entry)
 static void 
 hrt_tim_isr(void *p)
 {
-	hrt_lock();
 
+	//printf("hrt_tim_isr\n");
 	/* run any callouts that have met their deadline */
 	hrt_call_invoke();
 
+	hrt_lock();
+
 	/* and schedule the next interrupt */
 	hrt_call_reschedule();
 
@@ -262,7 +266,7 @@ hrt_call_reschedule()
 		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
 			//lldbg("pre-expired\n");
 			/* set a minimal deadline so that we call ASAP */
-			ticks = USEC2TICK(HRT_INTERVAL_MIN);
+			ticks = USEC2TICK(HRT_INTERVAL_MIN*1000);
 
 		} else if (next->deadline < deadline) {
 			//lldbg("due soon\n");
@@ -272,6 +276,7 @@ hrt_call_reschedule()
 
 	// There is no timer ISR, so simulate one by putting an event on the 
 	// high priority work queue
+	//printf("ticks = %u\n", ticks);
         work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks);
 }
 
@@ -353,6 +358,7 @@ hrt_call_invoke(void)
 	struct hrt_call	*call;
 	hrt_abstime deadline;
 
+	hrt_lock();
 	while (true) {
 		/* get the current time */
 		hrt_abstime now = hrt_absolute_time();
@@ -376,8 +382,13 @@ hrt_call_invoke(void)
 
 		/* invoke the callout (if there is one) */
 		if (call->callout) {
+			// Unlock so we don't deadlock in callback
+			hrt_unlock();
+
 			//lldbg("call %p: %p(%p)\n", call, call->callout, call->arg);
 			call->callout(call->arg);
+
+			hrt_lock();
 		}
 
 		/* if the callout has a non-zero period, it has to be re-entered */
@@ -392,5 +403,6 @@ hrt_call_invoke(void)
 			hrt_call_enter(call);
 		}
 	}
+	hrt_unlock();
 }
 

src/platforms/posix/px4_layer/px4_posix_impl.cpp

@@ -55,32 +55,20 @@ extern void hrt_init(void);
 
 __END_DECLS
 
-extern struct wqueue_s gwork[NWORKERS];
-
 namespace px4
 {
 
+void init_once(void);
+
+void init_once(void)
+{
+	work_queues_init();
+	hrt_init();
+}
+
 void init(int argc, char *argv[], const char *app_name)
 {
 	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);
-
-	hrt_init();
 }
 
 }

src/platforms/posix/px4_layer/work_lock.h

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2015 Mark Charlebois. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+
+#include <semaphore.h>
+#include <stdio.h>
+
+#pragma once
+extern sem_t _work_lock[];
+
+inline void work_lock(int id)
+{
+	//printf("work_lock %d\n", id);
+	sem_wait(&_work_lock[id]);
+}
+
+inline void work_unlock(int id)
+{
+	//printf("work_unlock %d\n", id);
+	sem_post(&_work_lock[id]);
+}
+

src/platforms/posix/px4_layer/work_queue.c

@@ -43,7 +43,10 @@
 #include <signal.h>
 #include <stdint.h>
 #include <queue.h>
+#include <stdio.h>
+#include <semaphore.h>
 #include <px4_workqueue.h>
+#include "work_lock.h"
 
 #ifdef CONFIG_SCHED_WORKQUEUE
 
@@ -117,13 +120,13 @@ int work_queue(int qid, struct work_s *work, worker_t worker, void *arg, uint32_
    * from with task logic or interrupt handlers.
    */
 
-  //flags        = irqsave();
+  work_lock(qid);
   work->qtime  = clock_systimer(); /* Time work queued */
 
   dq_addlast((dq_entry_t *)work, &wqueue->q);
   px4_task_kill(wqueue->pid, SIGCONT);      /* Wake up the worker thread */
 
-  //irqrestore(flags);
+  work_unlock(qid);
   return PX4_OK;
 }
 

src/platforms/posix/px4_layer/work_thread.c

@@ -45,6 +45,7 @@
 #include <queue.h>
 #include <px4_workqueue.h>
 #include <drivers/drv_hrt.h>
+#include "work_lock.h"
 
 #ifdef CONFIG_SCHED_WORKQUEUE
 
@@ -66,6 +67,7 @@ struct wqueue_s g_work[NWORKERS];
 /****************************************************************************
  * Private Variables
  ****************************************************************************/
+sem_t _work_lock[NWORKERS];
 
 /****************************************************************************
  * Private Functions
@@ -85,11 +87,10 @@ struct wqueue_s g_work[NWORKERS];
  *
  ****************************************************************************/
 
-static void work_process(FAR struct wqueue_s *wqueue)
+static void work_process(FAR struct wqueue_s *wqueue, int lock_id)
 {
   volatile FAR struct work_s *work;
   worker_t  worker;
-  //irqstate_t flags;
   FAR void *arg;
   uint64_t elapsed;
   uint32_t remaining;
@@ -100,7 +101,9 @@ static void work_process(FAR struct wqueue_s *wqueue)
    */
 
   next  = CONFIG_SCHED_WORKPERIOD;
-  //flags = irqsave();
+
+  work_lock(lock_id);
+
   work  = (FAR struct work_s *)wqueue->q.head;
   while (work)
     {
@@ -133,7 +136,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
            * performed... we don't have any idea how long that will take!
            */
 
-          //irqrestore(flags);
+          work_unlock(lock_id);
 	  if (!worker) {
              printf("MESSED UP: worker = 0\n");
           }
@@ -145,7 +148,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
            * back at the head of the list.
            */
 
-          //flags = irqsave();
+          work_lock(lock_id);
           work  = (FAR struct work_s *)wqueue->q.head;
         }
       else
@@ -172,14 +175,40 @@ static void work_process(FAR struct wqueue_s *wqueue)
   /* Wait awhile to check the work list.  We will wait here until either
    * the time elapses or until we are awakened by a signal.
    */
+  work_unlock(lock_id);
 
   usleep(next);
-  //irqrestore(flags);
 }
 
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
+void work_queues_init(void)
+{
+	sem_init(&_work_lock[HPWORK], 0, 1);
+	sem_init(&_work_lock[LPWORK], 0, 1);
+#ifdef CONFIG_SCHED_USRWORK
+	sem_init(&_work_lock[USRWORK], 0, 1);
+#endif
+
+	// Create high priority worker thread
+	g_work[HPWORK].pid = px4_task_spawn_cmd("wkr_high",
+			       SCHED_DEFAULT,
+			       SCHED_PRIORITY_MAX-1,
+			       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);
+
+}
+
 /****************************************************************************
  * Name: work_hpthread, work_lpthread, and work_usrthread
  *
@@ -234,7 +263,7 @@ int work_hpthread(int argc, char *argv[])
        * we process items in the work list.
        */
 
-      work_process(&g_work[HPWORK]);
+      work_process(&g_work[HPWORK], HPWORK);
     }
 
   return PX4_OK; /* To keep some compilers happy */
@@ -261,7 +290,7 @@ int work_lpthread(int argc, char *argv[])
        * we process items in the work list.
        */
 
-      work_process(&g_work[LPWORK]);
+      work_process(&g_work[LPWORK], LPWORK);
     }
 
   return PX4_OK; /* To keep some compilers happy */
@@ -282,7 +311,7 @@ int work_usrthread(int argc, char *argv[])
        * we process items in the work list.
        */
 
-      work_process(&g_work[USRWORK]);
+      work_process(&g_work[USRWORK], USRWORK);
     }
 
   return PX4_OK; /* To keep some compilers happy */

src/platforms/posix/tests/hrt_test/hrt_test.cpp

@@ -46,9 +46,17 @@
 
 px4::AppState HRTTest::appState;
 
+static struct hrt_call t1;
+static int update_interval = 1;
+
 static void timer_expired(void *arg)
 {
+	static int i = 0;
 	printf("Test\n");
+	if (i < 5) {
+		i++;
+		hrt_call_after(&t1, update_interval, timer_expired, (void *)0);
+	}
 }
 
 int HRTTest::main()
@@ -67,18 +75,15 @@ int HRTTest::main()
 	printf("Elapsed time %llu in 1 sec (sleep)\n", (unsigned long long)elt);
 	printf("Start time %llu\n", (unsigned long long)t);
 
-	struct hrt_call t1;
-	int update_interval = 1;
-
 	memset(&t1, 0, sizeof(t1));
 
 	printf("HRT_CALL %d\n", hrt_called(&t1));
 	
 	hrt_call_after(&t1, update_interval, timer_expired, (void *)0);
 	sleep(2);
-	printf("HRT_CALL %d\n", hrt_called(&t1));
+	printf("HRT_CALL - %d\n", hrt_called(&t1));
 	hrt_cancel(&t1);
-	printf("HRT_CALL %d\n", hrt_called(&t1));
+	printf("HRT_CALL + %d\n", hrt_called(&t1));
 
 	return 0;
 }

src/platforms/posix/tests/hrt_test/hrt_test_main.cpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file hello_main.cpp
+ * @file hrt_test_main.cpp
  * Example for Linux
  *
  * @author Mark Charlebois <charlebm@gmail.com>
@@ -44,9 +44,9 @@
 
 int PX4_MAIN(int argc, char **argv)
 {
-	px4::init(argc, argv, "hello");
+	px4::init(argc, argv, "hrt_test");
 
-	printf("hello\n");
+	printf("starting\n");
 	HRTTest test;
 	test.main();
 

src/platforms/px4_workqueue.h

@@ -73,6 +73,15 @@ struct work_s
   uint32_t  delay;       /* Delay until work performed */
 };
 
+/****************************************************************************
+ * Name: work_queues_init()
+ *
+ * Description:
+ *   Initialize the work queues.
+ *
+ ****************************************************************************/
+void work_queues_init(void);
+
 /****************************************************************************
  * Name: work_queue
  *