uORB: Automatically adjust orb_callback thread priority according to …

…the publisher priority

Signed-off-by: Jukka Laitinen <[email protected]>

platforms/common/uORB/SubscriptionCallback.hpp

@@ -123,6 +123,11 @@ class SubscriptionCallback : public SubscriptionInterval, public ListNode<Subscr
 	virtual void call() = 0;
 
 #ifndef CONFIG_BUILD_FLAT
+	int priority()
+	{
+		return DeviceNode::get_max_writer_priority(_subscription.get_node());
+	}
+
 	void do_call()
 	{
 		bool dequeued = uORB::Manager::dequeueCallback(_subscription.get_node(), _cb_handle);

platforms/common/uORB/uORBDeviceNode.cpp

@@ -596,6 +596,15 @@ uORB::DeviceNode::write(const char *buffer, const orb_metadata *meta, orb_advert
 
 	memcpy(((uint8_t *)node_data(handle)) + o_size * (generation % _queue_size), buffer, o_size);
 
+#ifndef CONFIG_BUILD_FLAT
+
+	/* At the first write, store publisher priority for subscriber callback thread adjustment */
+	if (_data_valid == false) {
+		set_max_writer_priority();
+	}
+
+#endif
+
 	/* Mark at least one data has been published */
 	_data_valid = true;
 
@@ -925,3 +934,17 @@ uORB::DeviceNode::_unregister_callback(uorb_cb_handle_t &cb_handle)
 
 	return ret;
 }
+
+#ifndef CONFIG_BUILD_FLAT
+void
+uORB::DeviceNode::set_max_writer_priority()
+{
+	sched_param param;
+	int policy;
+	int schedparam_ret = pthread_getschedparam(pthread_self(), &policy, &param);
+
+	if (schedparam_ret == 0 && param.sched_priority > _max_writer_priority) {
+		_max_writer_priority = param.sched_priority;
+	}
+}
+#endif

platforms/common/uORB/uORBDeviceNode.hpp

@@ -258,6 +258,13 @@ class DeviceNode
 #endif
 	}
 
+#ifndef CONFIG_BUILD_FLAT
+	static int get_max_writer_priority(orb_advert_t &node_handle)
+	{
+		return node(node_handle)->_max_writer_priority;
+	}
+#endif
+
 private:
 	friend uORBTest::UnitTest;
 
@@ -268,15 +275,16 @@ class DeviceNode
 	px4::atomic<unsigned>  _generation{0};  /**< object generation count */
 
 	struct EventWaitItem {
+		hrt_abstime last_update;
 #ifdef CONFIG_BUILD_FLAT
 		class SubscriptionCallback *subscriber;
-#else
-		bool cb_triggered;
 #endif
-		hrt_abstime last_update;
 		uint32_t interval_us;
+		uorb_cb_handle_t next;
 		int8_t lock;
-		uorb_cb_handle_t next; // List ptr
+#ifndef CONFIG_BUILD_FLAT
+		bool cb_triggered;
+#endif
 	};
 
 	IndexedStack<CB_LIST_T> _callbacks;
@@ -356,14 +364,19 @@ class DeviceNode
 	void *_data{nullptr};
 #else
 
+	/**
+	 * Get current thread priority and set the _max_writer_priority
+	 */
+	void set_max_writer_priority();
+
 	/**
 	 * Mutex for protecting node's internal data
 	 */
 
 	void		lock() { do {} while (px4_sem_wait(&_lock) != 0); }
 	void		unlock() { px4_sem_post(&_lock); }
 	px4_sem_t	_lock; /**< lock to protect access to all class members */
-
+	int _max_writer_priority{0};
 	char _devname[NAME_MAX + 1];
 #endif
 };

platforms/common/uORB/uORBManager.cpp

@@ -66,6 +66,7 @@ uORB::Manager *uORB::Manager::_Instance = nullptr;
 int8_t uORB::Manager::per_process_lock = -1;
 pid_t uORB::Manager::per_process_cb_thread = -1;
 List<class uORB::SubscriptionCallback *> uORB::Manager::per_process_cb_list;
+int uORB::Manager::per_process_cb_priority = SCHED_PRIORITY_DEFAULT;
 pthread_mutex_t uORB::Manager::per_process_cb_list_mutex = PTHREAD_MUTEX_INITIALIZER;
 #endif
 
@@ -472,7 +473,7 @@ uORB::Manager::launchCallbackThread()
 	if (per_process_cb_thread == -1) {
 		per_process_cb_thread = px4_task_spawn_cmd("orb_callback",
 					SCHED_DEFAULT,
-					SCHED_PRIORITY_MAX - 1,
+					per_process_cb_priority,
 					PX4_STACK_ADJUSTED(1024),
 					callback_thread,
 					nullptr);
@@ -487,6 +488,30 @@ uORB::Manager::launchCallbackThread()
 	return per_process_lock;
 }
 
+void
+uORB::Manager::adjust_callback_thread_priority(int priority)
+{
+	if (priority > per_process_cb_priority) {
+		sched_param param;
+		int policy;
+		int schedparam_ret = pthread_getschedparam(per_process_cb_thread, &policy, &param);
+
+		if (schedparam_ret) {
+			PX4_ERR("getschedparam fail\n");
+
+		} else {
+			param.sched_priority = priority;
+
+			if (pthread_setschedparam(per_process_cb_thread, policy, &param)) {
+				PX4_ERR("setschedparam fail\n");
+
+			} else {
+				per_process_cb_priority = priority;
+			}
+		}
+	}
+}
+
 int
 uORB::Manager::callback_thread(int argc, char *argv[])
 {
@@ -496,7 +521,9 @@ uORB::Manager::callback_thread(int argc, char *argv[])
 		pthread_mutex_lock(&per_process_cb_list_mutex);
 
 		for (auto sub : per_process_cb_list) {
+			adjust_callback_thread_priority(sub->priority());
 			sub->do_call();
+
 		}
 
 		pthread_mutex_unlock(&per_process_cb_list_mutex);

platforms/common/uORB/uORBManager.hpp

@@ -466,6 +466,8 @@ class Manager
 
 	static void cleanup();
 	static int callback_thread(int argc, char *argv[]);
+	static void adjust_callback_thread_priority(int priority);
+
 	static int8_t launchCallbackThread();
 
 private: // data members
@@ -670,6 +672,7 @@ class Manager
 #ifndef CONFIG_BUILD_FLAT
 	static int8_t per_process_lock;
 	static pid_t per_process_cb_thread;
+	static int per_process_cb_priority;
 	static List<class SubscriptionCallback *> per_process_cb_list;
 	static pthread_mutex_t per_process_cb_list_mutex;
 #endif