From 1d66f2cf836f5b30350ee0d89e87e1ce92e6aa4e Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sat, 9 Apr 2022 18:45:49 -0400
Subject: [PATCH] posix: HRT hrt_lock() sem_wait try again if error returned

---
 platforms/common/work_queue/hrt_queue.c       | 2 +-
 platforms/common/work_queue/hrt_thread.c      | 2 +-
 platforms/common/work_queue/hrt_work_cancel.c | 2 +-
 platforms/posix/src/px4/common/drv_hrt.cpp    | 5 +++--
 4 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/platforms/common/work_queue/hrt_queue.c b/platforms/common/work_queue/hrt_queue.c
index 74a4a8e17b..98c96d4161 100644
--- a/platforms/common/work_queue/hrt_queue.c
+++ b/platforms/common/work_queue/hrt_queue.c
@@ -123,7 +123,7 @@ int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t del
 	work->qtime  = hrt_absolute_time(); /* Time work queued */
 	//PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime);
 
-	dq_addlast((dq_entry_t *)work, &wqueue->q);
+	dq_addlast(&work->dq, &wqueue->q);
 
 	if (px4_getpid() != wqueue->pid) { /* only need to wake up if called from a different thread */
 #ifdef __PX4_QURT
diff --git a/platforms/common/work_queue/hrt_thread.c b/platforms/common/work_queue/hrt_thread.c
index 26a7e509dd..9465f0d9f2 100644
--- a/platforms/common/work_queue/hrt_thread.c
+++ b/platforms/common/work_queue/hrt_thread.c
@@ -155,7 +155,7 @@ static void hrt_work_process()
 		if (elapsed >= work->delay) {
 			/* Remove the ready-to-execute work from the list */
 
-			(void)dq_rem((struct dq_entry_s *) & (work->dq), &(wqueue->q));
+			(void)dq_rem((dq_entry_t *)&work->dq, &wqueue->q);
 			//PX4_INFO("Dequeued work=%p", work);
 
 			/* Extract the work description from the entry (in case the work
diff --git a/platforms/common/work_queue/hrt_work_cancel.c b/platforms/common/work_queue/hrt_work_cancel.c
index e5f9f5eabf..2fdea7ee9e 100644
--- a/platforms/common/work_queue/hrt_work_cancel.c
+++ b/platforms/common/work_queue/hrt_work_cancel.c
@@ -103,7 +103,7 @@ void hrt_work_cancel(struct work_s *work)
 		 * mark as availalbe (i.e., the worker field is nullified).
 		 */
 
-		dq_rem((dq_entry_t *)work, &wqueue->q);
+		dq_rem(&work->dq, &wqueue->q);
 		work->worker = NULL;
 	}
 
diff --git a/platforms/posix/src/px4/common/drv_hrt.cpp b/platforms/posix/src/px4/common/drv_hrt.cpp
index a75c953f85..6253e8171e 100644
--- a/platforms/posix/src/px4/common/drv_hrt.cpp
+++ b/platforms/posix/src/px4/common/drv_hrt.cpp
@@ -95,7 +95,8 @@ hrt_abstime hrt_absolute_time_offset()
 
 static void hrt_lock()
 {
-	px4_sem_wait(&_hrt_lock);
+	// loop as the wait may be interrupted by a signal
+	do {} while (px4_sem_wait(&_hrt_lock) != 0);
 }
 
 static void hrt_unlock()
@@ -333,7 +334,7 @@ hrt_call_reschedule()
 	// Remove the existing expiry and update with the new expiry
 	hrt_work_cancel(&_hrt_work);
 
-	hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, nullptr, delay);
+	hrt_work_queue(&_hrt_work, &hrt_tim_isr, nullptr, delay);
 }
 
 static void