From e74f7c1b02a76537d0d8489bbc5ea7e27c662cef Mon Sep 17 00:00:00 2001
From: Ville Juven <ville.juven@unikie.com>
Date: Tue, 7 May 2024 13:07:40 +0300
Subject: [PATCH] uORBManager.cpp: Use InxedexStack for global semaphore list

This optimizes one loop when reserving new signaling semaphore.
---
 platforms/common/uORB/uORBManager.cpp | 20 +++++++-------------
 platforms/common/uORB/uORBManager.hpp |  5 +++--
 2 files changed, 10 insertions(+), 15 deletions(-)

diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp
index 6a9327a4e481..18fe6098a545 100644
--- a/platforms/common/uORB/uORBManager.cpp
+++ b/platforms/common/uORB/uORBManager.cpp
@@ -626,29 +626,26 @@ void uORB::Manager::GlobalSemPool::init(void)
 
 void uORB::Manager::GlobalSemPool::free(int8_t i)
 {
+	IndexedStackHandle<SEM_LIST_T> sems(_global_sems);
+
 	lock();
 
-	_global_sem[i].free();
-	_global_sem[i].in_use = false;
+	sems.push_free(i);
 
 	unlock();
 }
 
 int8_t uORB::Manager::GlobalSemPool::reserve()
 {
+	IndexedStackHandle<SEM_LIST_T> sems(_global_sems);
+
 	lock();
 
 	// Find the first free lock
-	int8_t i;
-
-	for (i = 0; i < NUM_GLOBAL_SEMS; i++) {
-		if (!_global_sem[i].in_use) {
-			break;
-		}
-	}
+	int8_t i = sems.pop_free();
 
 	// Check that we got one
-	if (i ==  NUM_GLOBAL_SEMS) {
+	if (!sems.handle_valid(i)) {
 		PX4_ERR("Out of global locks");
 		unlock();
 		return -1;
@@ -657,9 +654,6 @@ int8_t uORB::Manager::GlobalSemPool::reserve()
 	// Make sure the semaphore is initialized properly for the new user
 	_global_sem[i].init();
 
-	// Mark this one as in use
-	_global_sem[i].in_use = true;
-
 	unlock();
 
 	return i;
diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp
index 07cbb5a31a39..96e95c7b04b5 100644
--- a/platforms/common/uORB/uORBManager.hpp
+++ b/platforms/common/uORB/uORBManager.hpp
@@ -51,6 +51,7 @@
 #endif /* CONFIG_ORB_COMMUNICATOR */
 
 #define NUM_GLOBAL_SEMS 40
+#define SEM_LIST_T GlobalLock, int8_t, NUM_GLOBAL_SEMS
 
 namespace uORB
 {
@@ -645,15 +646,14 @@ class Manager
 #if defined(__PX4_NUTTX)
 				sem_setprotocol(&_sem, SEM_PRIO_NONE);
 #endif
-				in_use = false;
 			}
 			void free() { px4_sem_destroy(&_sem); }
 			int take() { return px4_sem_wait(&_sem); }
 			int take_timedwait(struct timespec *abstime) { return px4_sem_timedwait(&_sem, abstime); }
 			void release() { px4_sem_post(&_sem); }
 			int value() { int value; px4_sem_getvalue(&_sem, &value); return value; }
-			bool in_use{false};
 
+			int8_t next; /* For linkage */
 		private:
 			px4_sem_t _sem; /* For signaling to the callback thread */
 		};
@@ -662,6 +662,7 @@ class Manager
 		void lock() { do {} while (px4_sem_wait(&_semLock) != 0); }
 		void unlock() { px4_sem_post(&_semLock); }
 
+		IndexedStack<SEM_LIST_T> _global_sems;
 		GlobalLock _global_sem[NUM_GLOBAL_SEMS];
 		px4_sem_t _semLock;
 	} g_sem_pool;