POSIX: Enabled hrt queued work to be run

In STM32, the ISR handler calls hrt_call_invoke(). There is no
interrupt timer inthe POSIX port so a work item is put on the
high priority work queue to expire at the next event (in ticks)
or at the next max delay interval.

Counter wrapping is likely still not handled properly in this code.

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

src/platforms/posix/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,26 @@ __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)
+{
+	sem_wait(&_hrt_lock);
+}
+
+static void hrt_unlock(void)
+{
+	sem_post(&_hrt_lock);
+}
+
 /*
  * Get absolute time.
  */
@@ -119,7 +139,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;
 
@@ -127,6 +147,7 @@ void	hrt_cancel(struct hrt_call *entry)
 	 * being re-entered when the callout returns.
 	 */
 	entry->period = 0;
+	hrt_unlock();
 	// endif
 }
 
@@ -155,7 +176,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
@@ -163,6 +187,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)) {
@@ -186,6 +211,25 @@ 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)
+{
+	hrt_lock();
+
+	/* run any callouts that have met their deadline */
+	hrt_call_invoke();
+
+	/* and schedule the next interrupt */
+	hrt_call_reschedule();
+
+	hrt_unlock();
+}
+
 /**
  * Reschedule the next timer interrupt.
  *
@@ -197,7 +241,10 @@ 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.
 	 *
@@ -215,18 +262,25 @@ 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);
 
 		} 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
+        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
@@ -244,6 +298,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
 	entry->arg = arg;
 
 	hrt_call_enter(entry);
+	hrt_unlock();
 }
 
 /*
@@ -254,6 +309,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,
@@ -291,3 +347,50 @@ 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;
+
+	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) {
+			//lldbg("call %p: %p(%p)\n", call, call->callout, call->arg);
+			call->callout(call->arg);
+		}
+
+		/* 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);
+		}
+	}
+}
+

src/platforms/posix/px4_layer/px4_posix_impl.cpp

@@ -51,6 +51,8 @@ __BEGIN_DECLS
 
 long PX4_TICKS_PER_SEC = sysconf(_SC_CLK_TCK);
 
+extern void hrt_init(void);
+
 __END_DECLS
 
 extern struct wqueue_s gwork[NWORKERS];
@@ -78,6 +80,7 @@ void init(int argc, char *argv[], const char *app_name)
 			       work_lpthread,
 			       (char* const*)NULL);
 
+	hrt_init();
 }
 
 }

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

@@ -46,6 +46,11 @@
 
 px4::AppState HRTTest::appState;
 
+static void timer_expired(void *arg)
+{
+	printf("Test\n");
+}
+
 int HRTTest::main()
 {
 	appState.setRunning(true);
@@ -62,5 +67,18 @@ 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));
+	hrt_cancel(&t1);
+	printf("HRT_CALL %d\n", hrt_called(&t1));
+
 	return 0;
 }