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 d1ad917cdd..f2a66bb898 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 @@ -96,6 +96,7 @@ private: IntrusiveQueue _q; px4_sem_t _process_lock; + px4_sem_t _exit_lock; const wq_config_t &_config; BlockingList _work_items; px4::atomic_bool _should_exit{false}; diff --git a/platforms/common/px4_work_queue/WorkQueue.cpp b/platforms/common/px4_work_queue/WorkQueue.cpp index 48b88c7290..fa32017912 100644 --- a/platforms/common/px4_work_queue/WorkQueue.cpp +++ b/platforms/common/px4_work_queue/WorkQueue.cpp @@ -59,11 +59,20 @@ WorkQueue::WorkQueue(const wq_config_t &config) : px4_sem_init(&_process_lock, 0, 0); px4_sem_setprotocol(&_process_lock, SEM_PRIO_NONE); + + px4_sem_init(&_exit_lock, 0, 1); + px4_sem_setprotocol(&_exit_lock, SEM_PRIO_NONE); } WorkQueue::~WorkQueue() { + work_lock(); + + // Synchronize with ::Detach + px4_sem_wait(&_exit_lock); + px4_sem_destroy(&_exit_lock); + px4_sem_destroy(&_process_lock); work_unlock(); @@ -83,11 +92,14 @@ bool WorkQueue::Attach(WorkItem *item) } work_unlock(); + return false; } void WorkQueue::Detach(WorkItem *item) { + bool exiting = false; + work_lock(); _work_items.remove(item); @@ -96,11 +108,21 @@ void WorkQueue::Detach(WorkItem *item) // shutdown, no active WorkItems PX4_DEBUG("stopping: %s, last active WorkItem closing", _config.name); + // Deletion of this work queue might happen right after request_stop or + // SignalWorkerThread. Use a separate lock to prevent premature deletion + px4_sem_wait(&_exit_lock); + exiting = true; request_stop(); SignalWorkerThread(); } work_unlock(); + + // In case someone is deleting this wq already, signal + // that it is now allowed + if (exiting) { + px4_sem_post(&_exit_lock); + } } void WorkQueue::Add(WorkItem *item)