From cf09b851b818b56d99ca25d63bf4ea4075e9abd6 Mon Sep 17 00:00:00 2001
From: Jukka Laitinen <jukkax@ssrc.tii.ae>
Date: Thu, 14 Dec 2023 15:45:27 +0200
Subject: [PATCH] Fix a bug in callback_thread

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
---
 platforms/common/uORB/SubscriptionCallback.hpp |  4 +++-
 platforms/common/uORB/uORBManager.cpp          | 12 +++++++++++-
 2 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/platforms/common/uORB/SubscriptionCallback.hpp b/platforms/common/uORB/SubscriptionCallback.hpp
index f8e900bc1a20..3c83b766cab7 100644
--- a/platforms/common/uORB/SubscriptionCallback.hpp
+++ b/platforms/common/uORB/SubscriptionCallback.hpp
@@ -128,7 +128,7 @@ class SubscriptionCallback : public SubscriptionInterval, public ListNode<Subscr
 		return DeviceNode::get_max_writer_priority(_subscription.get_node());
 	}
 
-	void do_call()
+	bool do_call()
 	{
 		bool dequeued = uORB::Manager::dequeueCallback(_subscription.get_node(), _cb_handle);
 
@@ -136,6 +136,8 @@ class SubscriptionCallback : public SubscriptionInterval, public ListNode<Subscr
 
 			call();
 		}
+
+		return dequeued;
 	}
 #endif
 
diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp
index a597ba9c83be..a46dd5f7533e 100644
--- a/platforms/common/uORB/uORBManager.cpp
+++ b/platforms/common/uORB/uORBManager.cpp
@@ -530,14 +530,24 @@ int
 uORB::Manager::callback_thread(int argc, char *argv[])
 {
 	while (true) {
+		/* Sleep here waiting for callbacks */
 		lockThread(per_process_lock);
 
+		/* Each publish has unlocked once, and each do_call() will
+		 * lock it back. Free the lock once here, so that after the
+		 * last call we won't lock in the last do_call()
+		*/
+		unlockThread(per_process_lock);
+
 		lock_cb_list();
 
 		for (auto sub : per_process_cb_list) {
 			adjust_callback_thread_priority(sub->priority());
-			sub->do_call();
 
+			/* Just in cast the callback thread has been starved,
+			 * run all the queued callbacks now
+			 */
+			while (sub->do_call()) {}
 		}
 
 		unlock_cb_list();