QuRT: updated task support based on posix fixes

The posix layer implementations should work on QuRT.

QuRT needs to provide a way for getting the current time.

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

makefiles/qurt/config_qurt_default.mk

@@ -72,7 +72,7 @@ MODULES		+= platforms/posix/drivers/barosim
 # Unit tests
 #
 MODULES		+= platforms/qurt/tests/hello
-#MODULES		+= platforms/posix/tests/vcdev_test
+MODULES		+= platforms/posix/tests/vcdev_test
 #MODULES		+= platforms/posix/tests/hrt_test
-#MODULES		+= platforms/posix/tests/wqueue
+MODULES		+= platforms/posix/tests/wqueue
 

src/platforms/px4_time.h

@@ -10,10 +10,13 @@
 
 #elif defined(__PX4_QURT)
 
+#include <sys/timespec.h>
+
 #define CLOCK_REALTIME 1
 
 __BEGIN_DECLS
 
+#if 0
 #if !defined(__cplusplus)
 struct timespec
 {
@@ -21,6 +24,7 @@ struct timespec
 	long tv_nsec;
 };
 #endif
+#endif
 int px4_clock_gettime(clockid_t clk_id, struct timespec *tp);
 int px4_clock_settime(clockid_t clk_id, struct timespec *tp);
 

src/platforms/qurt/main.cpp

@@ -52,6 +52,9 @@
 #include "px4_middleware.h"
 
 static const char *commands = 
+"x\n"
+"hello start"
+#if 0
 "uorb start\n"
 "simulator start -s\n"
 "barosim start\n"
@@ -68,7 +71,9 @@ static const char *commands =
 "hil mode_pwm\n"
 "commander start\n"
 "list_devices\n"
-"list_topics\n";
+"list_topics\n"
+#endif
+;
 
 
 static void run_cmd(const vector<string> &appargs) {
@@ -148,8 +153,13 @@ static void process_commands(const char *cmds)
 	}
 }
 
+namespace px4 {
+extern void init_once(void);
+};
+
 int main(int argc, char **argv)
 {
+	px4::init_once();
 	px4::init(argc, argv, "mainapp");
 	process_commands(commands);
 	for (;;) {}

src/platforms/qurt/px4_layer/drv_hrt.c

@@ -37,9 +37,12 @@
  * High-resolution timer with callouts and timekeeping.
  */
 
+#include <px4_workqueue.h>
 #include <drivers/drv_hrt.h>
+#include <semaphore.h>
 #include <time.h>
 #include <string.h>
+#include <stdio.h>
 
 static struct sq_queue_s	callout_queue;
 
@@ -51,9 +54,28 @@ __EXPORT uint32_t	latency_counters[LATENCY_BUCKET_COUNT + 1];
 
 static void		hrt_call_reschedule(void);
 
+// Intervals in ms
 #define HRT_INTERVAL_MIN	50
 #define HRT_INTERVAL_MAX	50000
 
+static sem_t 	_hrt_lock;
+static struct work_s	_hrt_work;
+
+static void
+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);
+}
+
 /*
  * Get absolute time.
  */
@@ -61,7 +83,7 @@ hrt_abstime hrt_absolute_time(void)
 {
 	struct timespec ts;
 
-	// FIXME - not supported in QURT
+	// FIXME - clock_gettime unsupported in QuRT
 	//clock_gettime(CLOCK_MONOTONIC, &ts);
 	return ts_to_abstime(&ts);
 }
@@ -120,7 +142,7 @@ bool	hrt_called(struct hrt_call *entry)
  */
 void	hrt_cancel(struct hrt_call *entry)
 {
-	// FIXME - need a lock
+	hrt_lock();
 	sq_rem(&entry->link, &callout_queue);
 	entry->deadline = 0;
 
@@ -128,6 +150,7 @@ void	hrt_cancel(struct hrt_call *entry)
 	 * being re-entered when the callout returns.
 	 */
 	entry->period = 0;
+	hrt_unlock();
 	// endif
 }
 
@@ -156,7 +179,10 @@ void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
  */
 void	hrt_init(void)
 {
+	//printf("hrt_init\n");
 	sq_init(&callout_queue);
+	sem_init(&_hrt_lock, 0, 1);
+	memset(&_hrt_work, 0, sizeof(_hrt_work));
 }
 
 static void
@@ -164,6 +190,7 @@ hrt_call_enter(struct hrt_call *entry)
 {
 	struct hrt_call	*call, *next;
 
+	//printf("hrt_call_enter\n");
 	call = (struct hrt_call *)sq_peek(&callout_queue);
 
 	if ((call == NULL) || (entry->deadline < call->deadline)) {
@@ -187,6 +214,27 @@ hrt_call_enter(struct hrt_call *entry)
 	//lldbg("scheduled\n");
 }
 
+/**
+ * Timer interrupt handler
+ *
+ * This routine simulates a timer interrupt handler
+ */
+static void 
+hrt_tim_isr(void *p)
+{
+
+	//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();
+
+	hrt_unlock();
+}
+
 /**
  * Reschedule the next timer interrupt.
  *
@@ -198,6 +246,9 @@ hrt_call_reschedule()
 	hrt_abstime	now = hrt_absolute_time();
 	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
 	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
+	uint32_t	ticks = USEC2TICK(HRT_INTERVAL_MAX*1000);
+
+	//printf("hrt_call_reschedule\n");
 	
 	/*
 	 * Determine what the next deadline will be.
@@ -216,18 +267,26 @@ hrt_call_reschedule()
 		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
 			//lldbg("pre-expired\n");
 			/* set a minimal deadline so that we call ASAP */
-			deadline = now + HRT_INTERVAL_MIN;
+			ticks = USEC2TICK(HRT_INTERVAL_MIN*1000);
 
 		} else if (next->deadline < deadline) {
 			//lldbg("due soon\n");
-			deadline = next->deadline;
+			ticks = USEC2TICK((next->deadline - now)*1000);
 		}
 	}
+
+	// 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);
 }
 
 static void
 hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
 {
+	//printf("hrt_call_internal\n");
+	hrt_lock();
+	//printf("hrt_call_internal after lock\n");
 	/* if the entry is currently queued, remove it */
 	/* note that we are using a potentially uninitialised
 	   entry->link here, but it is safe as sq_rem() doesn't
@@ -245,6 +304,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
 	entry->arg = arg;
 
 	hrt_call_enter(entry);
+	hrt_unlock();
 }
 
 /*
@@ -255,6 +315,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
  */
 void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
 {
+	//printf("hrt_call_after\n");
 	hrt_call_internal(entry,
 			  hrt_absolute_time() + delay,
 			  0,
@@ -292,3 +353,57 @@ void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callo
 void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
 #endif
 
+static void
+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();
+
+		call = (struct hrt_call *)sq_peek(&callout_queue);
+
+		if (call == NULL)
+			break;
+
+		if (call->deadline > now)
+			break;
+
+		sq_rem(&call->link, &callout_queue);
+		//lldbg("call pop\n");
+
+		/* save the intended deadline for periodic calls */
+		deadline = call->deadline;
+
+		/* zero the deadline, as the call has occurred */
+		call->deadline = 0;
+
+		/* 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 */
+		if (call->period != 0) {
+			// re-check call->deadline to allow for
+			// callouts to re-schedule themselves
+			// using hrt_call_delay()
+			if (call->deadline <= now) {
+				call->deadline = deadline + call->period;
+			}
+
+			hrt_call_enter(call);
+		}
+	}
+	hrt_unlock();
+}
+

src/platforms/qurt/px4_layer/px4_qurt_impl.cpp

@@ -56,6 +56,8 @@ long PX4_TICKS_PER_SEC = 1000;
 void usleep(useconds_t usec) {}
 unsigned int sleep(unsigned int sec) { return 0; }
 
+extern void hrt_init(void);
+
 __END_DECLS
 
 extern struct wqueue_s gwork[NWORKERS];
@@ -63,6 +65,14 @@ 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);

src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp

@@ -79,17 +79,15 @@ typedef struct
 
 static void entry_adapter ( void *ptr )
 {
+	printf("entry_adapter\n");
 	pthdata_t *data = (pthdata_t *) ptr;
 
-	printf("TEST3\n");
+	data->entry(data->argc, data->argv);
-#if 0
+	free(ptr);
-	//data->entry(data->argc, data->argv);
+	printf("after entry\n");
-	printf("TEST4\n");
 	printf("Before px4_task_exit\n");
 	px4_task_exit(0); 
-	//free(ptr);
 	printf("After px4_task_exit\n");
-#endif
 } 
 
 void
@@ -106,21 +104,17 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
 	unsigned int len = 0;
 	unsigned long offset;
 	unsigned long structsize;
-	char * p;
+	char * p = (char *)argv;
 
-	//printf("arg %d %s %s %p\n", argc, argv[0], argv[1], argv[2]);
-	printf("arg %d %p\n", argc, argv);
 	// Calculate argc
-	if (argv) {
+	while (p != (char *)0) {
-		for(;;) {
+		p = argv[argc];
-			p = argv[argc];
+		if (p == (char *)0)
-			if (p == (char *)0)
+			break;
-				break;
+		++argc;
-			printf("arg %d %s\n", argc, argv[argc]);
+		len += strlen(p)+1;
-			++argc;
-			len += strlen(p)+1;
-		}
 	}
+	printf("arg %d %p\n", argc, argv);
         structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *);
 	pthdata_t *taskdata;
     

src/platforms/qurt/px4_layer/work_lock.h

@@ -0,0 +1,53 @@
+/****************************************************************************
+ *
+ * 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);
+inline void work_lock(int id)
+{
+	//printf("work_lock %d\n", id);
+	sem_wait(&_work_lock[id]);
+}
+
+inline void work_unlock(int id);
+inline void work_unlock(int id)
+{
+	//printf("work_unlock %d\n", id);
+	sem_post(&_work_lock[id]);
+}
+

src/platforms/qurt/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, SIGALRM);      /* Wake up the worker thread */
 
-  //irqrestore(flags);
+  work_unlock(qid);
   return PX4_OK;
 }
 

src/platforms/qurt/px4_layer/work_thread.c

@@ -39,12 +39,14 @@
 
 #include <px4_config.h>
 #include <px4_defines.h>
+#include <px4_time.h>
 #include <stdint.h>
 #include <stdio.h>
 #include <unistd.h>
 #include <queue.h>
 #include <px4_workqueue.h>
 #include <drivers/drv_hrt.h>
+#include "work_lock.h"
 
 #ifdef CONFIG_SCHED_WORKQUEUE
 
@@ -66,6 +68,7 @@ struct wqueue_s g_work[NWORKERS];
 /****************************************************************************
  * Private Variables
  ****************************************************************************/
+sem_t _work_lock[NWORKERS];
 
 /****************************************************************************
  * Private Functions
@@ -85,13 +88,12 @@ 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;
-  uint32_t elapsed;
+  uint64_t elapsed;
   uint32_t remaining;
   uint32_t next;
 
@@ -100,7 +102,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)
     {
@@ -110,8 +114,8 @@ static void work_process(FAR struct wqueue_s *wqueue)
        * zero.  Therefore a delay of zero will always execute immediately.
        */
 
-      elapsed = clock_systimer() - work->qtime;
+      elapsed = USEC2TICK(clock_systimer() - work->qtime);
-      //printf("work_process: elapsed=%d delay=%d\n", elapsed, work->delay);
+      //printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay);
       if (elapsed >= work->delay)
         {
           /* Remove the ready-to-execute work from the list */
@@ -133,7 +137,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 +149,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
@@ -155,7 +159,7 @@ static void work_process(FAR struct wqueue_s *wqueue)
            */
 
           /* Here: elapsed < work->delay */
-          remaining = work->delay - elapsed;
+          remaining = USEC_PER_TICK*(work->delay - elapsed);
           if (remaining < next)
             {
               /* Yes.. Then schedule to wake up when the work is ready */
@@ -172,15 +176,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);
 
-  //FIXME - DSPAL doesn't support usleep
+  usleep(next);
-  //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
  *
@@ -235,7 +264,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 */
@@ -262,7 +291,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 */
@@ -283,7 +312,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 */
@@ -293,6 +322,7 @@ int work_usrthread(int argc, char *argv[])
 
 uint32_t clock_systimer()
 {
+	//printf("clock_systimer: %0lx\n", hrt_absolute_time());
 	return (0x00000000ffffffff & hrt_absolute_time());
 }
 #endif /* CONFIG_SCHED_WORKQUEUE */