diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp index a2eddbef583..76d34ad2f64 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp @@ -38,6 +38,7 @@ #include #include +#include #include #include #include @@ -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}; diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp index f2a66bb8983..0c269aa6dc1 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp @@ -43,6 +43,8 @@ #include #include +#include + 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 _work_items; px4::atomic_bool _should_exit{false}; + pthread_t _runner_tid{}; #if defined(ENABLE_LOCKSTEP_SCHEDULER) int _lockstep_component {-1}; diff --git a/platforms/common/px4_work_queue/WorkItem.cpp b/platforms/common/px4_work_queue/WorkItem.cpp index ffc0dca2761..a84b4f34aab 100644 --- a/platforms/common/px4_work_queue/WorkItem.cpp +++ b/platforms/common/px4_work_queue/WorkItem.cpp @@ -37,8 +37,11 @@ #include #include +#include #include +#include + 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); } } diff --git a/platforms/common/px4_work_queue/WorkQueue.cpp b/platforms/common/px4_work_queue/WorkQueue.cpp index c8aa15995ba..bd85be267da 100644 --- a/platforms/common/px4_work_queue/WorkQueue.cpp +++ b/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 } diff --git a/platforms/posix/src/px4/common/drv_hrt.cpp b/platforms/posix/src/px4/common/drv_hrt.cpp index dfcb910a676..8c8cd32e92f 100644 --- a/platforms/posix/src/px4/common/drv_hrt.cpp +++ b/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 */