uORB performance updates

Move some logic from Subscriber into uORBManager. This reduces calls from the
modules to the uORB manager, improving performance in protected build.

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>

platforms/common/uORB/Subscription.hpp

@@ -136,19 +136,40 @@ public:
 	/**
 	 * Check if there is a new update.
 	 */
-	bool updated() { return advertised() && Manager::updates_available(_node, _last_generation); }
+	bool updated()
+	{
+		if (!valid()) {
+			subscribe();
+		}
+
+		return valid() ? Manager::updates_available(_node, _last_generation) : false;
+	}
 
 	/**
 	 * Update the struct
 	 * @param dst The uORB message struct we are updating.
 	 */
-	bool update(void *dst) { return updated() && Manager::orb_data_copy(_node, dst, _last_generation); }
+	bool update(void *dst)
+	{
+		if (!valid()) {
+			subscribe();
+		}
+
+		return valid() ? Manager::orb_data_copy(_node, dst, _last_generation, true) : false;
+	}
 
 	/**
 	 * Copy the struct
 	 * @param dst The uORB message struct we are updating.
 	 */
-	bool copy(void *dst) { return advertised() && Manager::orb_data_copy(_node, dst, _last_generation); }
+	bool copy(void *dst)
+	{
+		if (!valid()) {
+			subscribe();
+		}
+
+		return valid() ? Manager::orb_data_copy(_node, dst, _last_generation, false) : false;
+	}
 
 	/**
 	 * Change subscription instance

platforms/common/uORB/uORBManager.cpp

@@ -172,7 +172,7 @@ int	uORB::Manager::orb_ioctl(unsigned int cmd, unsigned long arg)
 
 	case ORBIOCDEVDATACOPY: {
 			orbiocdevdatacopy_t *data = (orbiocdevdatacopy_t *)arg;
-			data->ret = uORB::Manager::orb_data_copy(data->handle, data->dst, data->generation);
+			data->ret = uORB::Manager::orb_data_copy(data->handle, data->dst, data->generation, data->only_if_updated);
 		}
 		break;
 
@@ -479,8 +479,16 @@ void uORB::Manager::orb_remove_internal_subscriber(void *node_handle)
 
 uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) { return static_cast<const DeviceNode *>(node_handle)->get_queue_size(); }
 
-bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation)
+bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated)
 {
+	if (!is_advertised(node_handle)) {
+		return false;
+	}
+
+	if (only_if_updated && !static_cast<const uORB::DeviceNode *>(node_handle)->updates_available(generation)) {
+		return false;
+	}
+
 	return static_cast<DeviceNode *>(node_handle)->copy(dst, generation);
 }
 
@@ -509,7 +517,8 @@ uint8_t uORB::Manager::orb_get_instance(const void *node_handle)
 #if !defined(CONFIG_BUILD_FLAT)
 unsigned uORB::Manager::updates_available(const void *node_handle, unsigned last_generation)
 {
-	return static_cast<const uORB::DeviceNode *>(node_handle)->updates_available(last_generation);
+	return is_advertised(node_handle) ? static_cast<const uORB::DeviceNode *>(node_handle)->updates_available(
+		       last_generation) : 0;
 }
 
 bool uORB::Manager::is_advertised(const void *node_handle)

platforms/common/uORB/uORBManager.hpp

@@ -117,6 +117,7 @@ typedef struct {
 	void *handle;
 	void *dst;
 	unsigned generation;
+	bool only_if_updated;
 	bool ret;
 } orbiocdevdatacopy_t;
 
@@ -450,7 +451,7 @@ public:
 
 	static uint8_t orb_get_queue_size(const void *node_handle);
 
-	static bool orb_data_copy(void *node_handle, void *dst, unsigned &generation);
+	static bool orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated);
 
 	static bool register_callback(void *node_handle, SubscriptionCallback *callback_sub);
 
@@ -460,9 +461,10 @@ public:
 
 #if defined(CONFIG_BUILD_FLAT)
 	/* These are optimized by inlining in NuttX Flat build */
-	static unsigned updates_available(const void *node_handle, unsigned last_generation) { return static_cast<const DeviceNode *>(node_handle)->updates_available(last_generation); }
+	static unsigned updates_available(const void *node_handle, unsigned last_generation) { return is_advertised(node_handle) ? static_cast<const DeviceNode *>(node_handle)->updates_available(last_generation) : 0; }
 
 	static bool is_advertised(const void *node_handle) { return static_cast<const DeviceNode *>(node_handle)->is_advertised(); }
+
 #else
 	static unsigned updates_available(const void *node_handle, unsigned last_generation);
 

platforms/common/uORB/uORBManagerUsr.cpp

@@ -303,9 +303,9 @@ uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle)
 	return data.size;
 }
 
-bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation)
+bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated)
 {
-	orbiocdevdatacopy_t data = {node_handle, dst, generation, false};
+	orbiocdevdatacopy_t data = {node_handle, dst, generation, only_if_updated, false};
 	boardctl(ORBIOCDEVDATACOPY, reinterpret_cast<unsigned long>(&data));
 	generation = data.generation;