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

@@ -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;
-
-			ts.tv_sec = timeout/1000;
-			ts.tv_nsec = (timeout % 1000)*1000000;
+			// Use a work queue task
+			work_s _hpwork;
 
-			// 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;

src/platforms/posix/px4_layer/hrt_work.h → 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");

src/platforms/qurt/px4_layer/hrt_work.h → 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);