From 25aded36ec9337e274062eb7b4a46eb104ada021 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 16 Oct 2019 11:48:14 +0200 Subject: [PATCH] WorkQueue: avoid potential semaphore counter overflow This could happen in the following cases: - IRQ/publisher rate is faster than the processing rate, and therefore WorkQueue::Add is called at a higher rate - a long-running or stuck task that blocks the work queue a long time Both cases are not expected to happen under 'normal' circumstances (if the system runs as expected). --- .../px4_work_queue/WorkQueue.hpp | 2 ++ platforms/common/px4_work_queue/WorkQueue.cpp | 17 ++++++++++++----- 2 files changed, 14 insertions(+), 5 deletions(-) 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 6019c890ee..da3f65de57 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 @@ -76,6 +76,8 @@ private: bool should_exit() const { return _should_exit.load(); } + inline void signal_worker_thread(); + #ifdef __PX4_NUTTX // In NuttX work can be enqueued from an ISR void work_lock() { _flags = enter_critical_section(); } diff --git a/platforms/common/px4_work_queue/WorkQueue.cpp b/platforms/common/px4_work_queue/WorkQueue.cpp index 22a8d79505..1d9e7aa242 100644 --- a/platforms/common/px4_work_queue/WorkQueue.cpp +++ b/platforms/common/px4_work_queue/WorkQueue.cpp @@ -99,9 +99,7 @@ WorkQueue::Detach(WorkItem *item) PX4_DEBUG("stopping: %s, last active WorkItem closing", _config.name); request_stop(); - - // Wake up the worker thread - px4_sem_post(&_process_lock); + signal_worker_thread(); } work_unlock(); @@ -114,8 +112,17 @@ WorkQueue::Add(WorkItem *item) _q.push(item); work_unlock(); - // Wake up the worker thread - px4_sem_post(&_process_lock); + signal_worker_thread(); +} + +void +WorkQueue::signal_worker_thread() +{ + int sem_val; + + if (px4_sem_getvalue(&_process_lock, &sem_val) == 0 && sem_val <= 0) { + px4_sem_post(&_process_lock); + } } void