fix(WorkQueue): fix lifetime race between Run() and destruction

Two related fixes for HRT callout queue corruption under lockstep SIH:

1. WorkQueue::Run() popped a WorkItem, released the work-queue lock,
   then called work->Run(). If another thread called Deinit on the
   item in that window, wq->Remove was a no-op (item already popped)
   and Deinit returned while Run() was still executing. Run() could
   then call ScheduleDelayed → hrt_call_after on an object that the
   caller of Deinit has since destructed and reused via placement new
   — the hrt_call metadata ends up living inside another object's
   fields, which later overwrite the flink pointer and tear the hrt
   callout queue.

   Fix: WorkItem carries a _run_in_progress atomic flag set by
   WorkQueue::Run around work->Run(). Deinit spins on that flag after
   wq->Remove so it cannot return while a Run() is still executing.
   WorkQueue captures its worker tid in the constructor (the ctor
   runs on the worker thread) so Deinit can skip the self-wait when
   called from inside Run() itself (e.g. should_exit() paths that
   invoke ScheduleClear).

2. _hrt_lock was a px4_sem_t and hrt_call_invoke unlocked around the
   callback so callbacks could re-enter hrt_call_*. That also exposed
   the queue to other threads mid-invocation. Switch _hrt_lock to a
   PTHREAD_MUTEX_RECURSIVE pthread_mutex and hold it across the
   callback — matches NuttX's enter_critical_section() nesting
   semantics, lets callbacks reschedule themselves safely, and
   prevents concurrent queue manipulation.

Together these eliminate the HRT queue tearing observed in the
SIH-at-20x MAVSDK integration soak (torn flink chains with the tail
unreachable from the head; orphan nodes pointing into reused
memory).

platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp

@@ -38,6 +38,7 @@
 
 #include <containers/IntrusiveQueue.hpp>
 #include <containers/IntrusiveSortedList.hpp>
+#include <px4_platform_common/atomic.h>
 #include <px4_platform_common/defines.h>
 #include <drivers/drv_hrt.h>
 #include <lib/mathlib/mathlib.h>
@@ -131,6 +132,11 @@ protected:
 	const char 	*_item_name;
 	uint32_t	_run_count{0};
 
+	// True while a WorkQueue worker is inside Run() for this item.
+	// Set before Run(), cleared after. Used by Deinit to wait out any
+	// in-flight Run() before the owning object's memory can be reused.
+	px4::atomic_bool	_run_in_progress{false};
+
 private:
 
 	WorkQueue	*_wq{nullptr};

platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp

@@ -43,6 +43,8 @@
 #include <px4_platform_common/sem.h>
 #include <px4_platform_common/tasks.h>
 
+#include <pthread.h>
+
 namespace px4
 {
 
@@ -69,7 +71,12 @@ public:
 
 	void Run();
 
-	void request_stop() { _should_exit.store(true); }
+	// Thread id of the worker that executes Run(). Captured once on the
+	// worker thread itself (WorkQueue is constructed on its own worker).
+	// Used by WorkItem::Deinit to avoid self-wait when called from inside Run().
+	pthread_t runner_tid() const { return _runner_tid; }
+
+	void request_stop();
 
 	void print_status(bool last = false);
 
@@ -100,6 +107,7 @@ private:
 	const wq_config_t		&_config;
 	BlockingList<WorkItem *>	_work_items;
 	px4::atomic_bool		_should_exit{false};
+	pthread_t			_runner_tid{};
 
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 	int _lockstep_component {-1};

platforms/common/px4_work_queue/WorkItem.cpp

@@ -37,8 +37,11 @@
 #include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
 
 #include <px4_platform_common/log.h>
+#include <px4_platform_common/posix.h>
 #include <drivers/drv_hrt.h>
 
+#include <pthread.h>
+
 namespace px4
 {
 
@@ -93,6 +96,22 @@ void WorkItem::Deinit()
 		// remove any queued work
 		wq_temp->Remove(this);
 
+		// If Run() is already executing on the worker thread at this
+		// moment, Remove was a no-op and Run() will still read/write
+		// `this`. Wait for it to return before letting the owning
+		// object's memory be torn down — otherwise a late
+		// ScheduleDelayed from Run() writes into freed/reused memory
+		// and corrupts the hrt callout queue.
+		//
+		// Skip the wait if we're being called from inside Run() on the
+		// worker thread itself (e.g. should_exit() → ScheduleClear),
+		// which would deadlock against our own flag.
+		if (!pthread_equal(pthread_self(), wq_temp->runner_tid())) {
+			while (_run_in_progress.load()) {
+				px4_usleep(100);
+			}
+		}
+
 		wq_temp->Detach(this);
 	}
 }

platforms/common/px4_work_queue/WorkQueue.cpp

@@ -47,6 +47,12 @@ namespace px4
 WorkQueue::WorkQueue(const wq_config_t &config) :
 	_config(config)
 {
+	// WorkQueue is constructed on its own worker thread (see
+	// WorkQueueRunner in WorkQueueManager.cpp), so pthread_self() here
+	// captures the worker's tid for later self-wait checks in
+	// WorkItem::Deinit.
+	_runner_tid = pthread_self();
+
 	// set the threads name
 #ifdef __PX4_DARWIN
 	pthread_setname_np(_config.name);
@@ -144,6 +150,17 @@ void WorkQueue::Add(WorkItem *item)
 	SignalWorkerThread();
 }
 
+void WorkQueue::request_stop()
+{
+	_should_exit.store(true);
+	// Wake the worker so Run() re-checks should_exit() and returns. Without
+	// this, a queue parked in px4_sem_wait() (no pending work) never notices
+	// the stop request — e.g. when WorkQueueManagerStop() asks every queue to
+	// stop while WorkItems are still attached (so the Detach self-stop path
+	// never ran). The worker would block forever and shutdown would hang.
+	SignalWorkerThread();
+}
+
 void WorkQueue::SignalWorkerThread()
 {
 	int sem_val;
@@ -183,10 +200,17 @@ void WorkQueue::Run()
 		while (!_q.empty()) {
 			WorkItem *work = _q.pop();
 
+			// Mark as in-flight so ~WorkItem / Deinit on another thread
+			// can wait for us to return before the item's memory is torn
+			// down. Without this, a Run() executing after Deinit has
+			// popped the item will call Schedule* on dead memory,
+			// corrupting the hrt callout queue.
+			work->_run_in_progress.store(true);
+
 			work_unlock(); // unlock work queue to run (item may requeue itself)
 			work->RunPreamble();
 			work->Run();
-			// Note: after Run() we cannot access work anymore, as it might have been deleted
+			work->_run_in_progress.store(false);
 			work_lock(); // re-lock
 		}
 

platforms/posix/src/px4/common/drv_hrt.cpp

@@ -81,7 +81,14 @@ const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
 const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
 __EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
 
-static px4_sem_t 	_hrt_lock;
+// Recursive mutex so hrt_call_invoke() can hold it across callbacks that may
+// re-enter hrt_call_* on the same thread (e.g. a callout that reschedules
+// itself). On NuttX this is implicit: the equivalent code there uses
+// enter_critical_section(), which is a nestable IRQ-disable counter. On POSIX
+// we need PTHREAD_MUTEX_RECURSIVE to get matching semantics — without it we'd
+// have to unlock around callbacks, which leaks the queue to other threads and
+// causes list corruption under load.
+static pthread_mutex_t	_hrt_lock;
 static struct work_s	_hrt_work;
 
 static void hrt_latency_update();
@@ -91,13 +98,12 @@ static void hrt_call_invoke();
 
 static void hrt_lock()
 {
-	// loop as the wait may be interrupted by a signal
-	do {} while (px4_sem_wait(&_hrt_lock) != 0);
+	pthread_mutex_lock(&_hrt_lock);
 }
 
 static void hrt_unlock()
 {
-	px4_sem_post(&_hrt_lock);
+	pthread_mutex_unlock(&_hrt_lock);
 }
 
 /*
@@ -201,12 +207,16 @@ void	hrt_init()
 {
 	sq_init(&callout_queue);
 
-	int sem_ret = px4_sem_init(&_hrt_lock, 0, 1);
+	pthread_mutexattr_t attr;
+	pthread_mutexattr_init(&attr);
+	pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
 
-	if (sem_ret) {
-		PX4_ERR("SEM INIT FAIL: %s", strerror(errno));
+	if (pthread_mutex_init(&_hrt_lock, &attr) != 0) {
+		PX4_ERR("hrt mutex init failed: %s", strerror(errno));
 	}
 
+	pthread_mutexattr_destroy(&attr);
+
 	memset(&_hrt_work, 0, sizeof(_hrt_work));
 }
 
@@ -428,13 +438,15 @@ hrt_call_invoke()
 		void *arg = call->arg;
 
 		if (callout) {
-			// Unlock so we don't deadlock in callback
-			hrt_unlock();
-
-			//PX4_INFO("call %p: %p(%p)", call, callout, arg);
+			// We hold _hrt_lock across the callout. Previously we unlocked
+			// here to let callbacks re-enter hrt_call_*, but that exposes the
+			// callout queue to other threads while the current entry is mid-
+			// invocation (removed from the queue but about to be re-inserted
+			// by the periodic path below). Under load that caused list
+			// corruption — entries orphaned with stale flink, queue torn.
+			// _hrt_lock is now a recursive mutex, so same-thread re-entry
+			// from the callback is safe.
 			callout(arg);
-
-			hrt_lock();
 		}
 
 		/* if the callout has a non-zero period, it has to be re-entered */