From da2ac877f829db1339948bcadd29bcbc6a9970eb Mon Sep 17 00:00:00 2001
From: Mark Charlebois <charlebm@gmail.com>
Date: Mon, 29 Jun 2015 19:08:06 -0700
Subject: [PATCH] POSIX: Changed px4_poll to use hrt_work queue

QuRT's pthread_cancel implementation is lacking, and causes px4_poll to
always wait for the maximumn timeout. A cleaner implementation is provided
that uses the HRT work queue for posix targets.

In the future the posix code should be rtefactiored so that qurt (and other)
implementations that are duplicated, use the posix implementation.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
---
 src/drivers/device/vdev_posix.cpp             | 22 +++++--------------
 .../posix/{px4_layer => include}/hrt_work.h   |  5 +----
 .../qurt/{px4_layer => include}/hrt_work.h    |  2 +-
 3 files changed, 8 insertions(+), 21 deletions(-)
 rename src/platforms/posix/{px4_layer => include}/hrt_work.h (95%)
 rename src/platforms/qurt/{px4_layer => include}/hrt_work.h (99%)

diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp
index 33aaa1647f..f9a6ebc559 100644
--- a/src/drivers/device/vdev_posix.cpp
+++ b/src/drivers/device/vdev_posix.cpp
@@ -43,6 +43,7 @@
 #include "device.h"
 #include "vfile.h"
 
+#include <hrt_work.h>
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>
@@ -61,7 +62,7 @@ struct timerData {
 	~timerData() {}
 };
 
-static void *timer_handler(void *data)
+static void timer_cb(void *data)
 {
 	struct timerData *td = (struct timerData *)data;
 
@@ -72,7 +73,6 @@ static void *timer_handler(void *data)
 	sem_post(&(td->sem));
 
 	PX4_DEBUG("timer_handler: Timer expired");
-	return 0;
 }
 
 #define PX4_MAX_FD 200
@@ -239,25 +239,16 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
 	{
 		if (timeout >= 0)
 		{
-			pthread_t pt;
-			void *res;
+			// Use a work queue task
+			work_s _hpwork;
 
-			ts.tv_sec = timeout/1000;
-			ts.tv_nsec = (timeout % 1000)*1000000;
-
-			// Create a timer to unblock
 			struct timerData td(sem, ts);
-			int rv = pthread_create(&pt, NULL, timer_handler, (void *)&td);
-			if (rv != 0) {
-				count = -1;
-				goto cleanup;
-			}
+			hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&td, 1000*timeout);
 			sem_wait(&sem);
 
 			// Make sure timer thread is killed before sem goes
 			// out of scope
-			(void)pthread_cancel(pt);
-			(void)pthread_join(pt, &res);
+			hrt_work_cancel(&_hpwork);
         	}
 		else
 		{
@@ -283,7 +274,6 @@ int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
 		}
 	}
 
-cleanup:
 	sem_destroy(&sem);
 
 	return count;
diff --git a/src/platforms/posix/px4_layer/hrt_work.h b/src/platforms/posix/include/hrt_work.h
similarity index 95%
rename from src/platforms/posix/px4_layer/hrt_work.h
rename to src/platforms/posix/include/hrt_work.h
index d926a6d250..39e53f95d2 100644
--- a/src/platforms/posix/px4_layer/hrt_work.h
+++ b/src/platforms/posix/include/hrt_work.h
@@ -43,12 +43,9 @@ extern sem_t _hrt_work_lock;
 extern struct wqueue_s g_hrt_work;
 
 void hrt_work_queue_init(void);
-int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay);
+int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay);
 void hrt_work_cancel(struct work_s *work);
 
-//inline void hrt_work_lock(void);
-//inline void hrt_work_unlock(void);
-
 static inline void hrt_work_lock()
 {
 	//PX4_INFO("hrt_work_lock");
diff --git a/src/platforms/qurt/px4_layer/hrt_work.h b/src/platforms/qurt/include/hrt_work.h
similarity index 99%
rename from src/platforms/qurt/px4_layer/hrt_work.h
rename to src/platforms/qurt/include/hrt_work.h
index 566684eb86..92b079ac6b 100644
--- a/src/platforms/qurt/px4_layer/hrt_work.h
+++ b/src/platforms/qurt/include/hrt_work.h
@@ -43,7 +43,7 @@ extern sem_t _hrt_work_lock;
 extern struct wqueue_s g_hrt_work;
 
 void hrt_work_queue_init(void);
-int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t delay);
+int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay);
 void hrt_work_cancel(struct work_s *work);
 
 inline void hrt_work_lock(void);