orb_poll: Check first if there is need to register poll

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

platforms/common/uORB/uORBManager.cpp

@@ -378,16 +378,10 @@ int uORB::Manager::orb_get_interval(orb_sub_t handle, unsigned *interval)
 int uORB::Manager::orb_poll(orb_poll_struct_t *fds, unsigned int nfds, int timeout)
 {
 	SubscriptionPollable *sub;
+	int8_t lock_idx = -1;
 
-	// Get a poll semaphore from the global pool
-	int8_t lock_idx = g_sem_pool.reserve();
+	// No need to register poll if any orb is updated already
 
-	if (lock_idx < 0) {
-		PX4_ERR("Out of thread locks");
-		return -1;
-	}
-
-	// Any orb updated already?
 	bool err = false;
 	int count = 0;
 
@@ -396,10 +390,8 @@ int uORB::Manager::orb_poll(orb_poll_struct_t *fds, unsigned int nfds, int timeo
 
 		if ((fds[i].events & POLLIN) == POLLIN) {
 			sub = static_cast<SubscriptionPollable *>(fds[i].fd);
-			sub->registerPoll(lock_idx);
 
 			if (sub->updated()) {
-				fds[i].revents = POLLIN;
 				count++;
 			}
 		}
@@ -409,6 +401,22 @@ int uORB::Manager::orb_poll(orb_poll_struct_t *fds, unsigned int nfds, int timeo
 	// If some orb was updated after registration, but not yet refelected in "updated", the semaphore is already released. So there is no race in here.
 
 	if (count == 0) {
+		// Poll for all the requested POLLIN events
+
+		// Get a poll semaphore from the global pool
+		lock_idx = g_sem_pool.reserve();
+
+		if (lock_idx < 0) {
+			PX4_ERR("Out of thread locks");
+			return -1;
+		}
+
+		for (unsigned i = 0; i < nfds; i++) {
+			if ((fds[i].events & POLLIN) == POLLIN) {
+				sub = static_cast<SubscriptionPollable *>(fds[i].fd);
+				sub->registerPoll(lock_idx);
+			}
+		}
 
 		// First advertiser will wake us up, or it might have happened already
 		// during registration above
@@ -452,8 +460,10 @@ int uORB::Manager::orb_poll(orb_poll_struct_t *fds, unsigned int nfds, int timeo
 		}
 	}
 
-	// release the semaphore
-	g_sem_pool.free(lock_idx);
+	// Release the semaphore
+	if (lock_idx >= 0) {
+		g_sem_pool.free(lock_idx);
+	}
 
 	return err ? -1 : count;
 }