uORB: Fix uORB callback register/unregister issues

In memory protected modes there were the following issues:
1. It is wrong to self-unregister the callback in uORBDeviceNode. The device
   node can't assume that the callback thread is killed if the callbacks
   haven't been handled timely. It might also be temporarily blocked e.g. during boot.
   If the device node unregisters the callback and the callback thread gets executed later,
   the cb_handle would point to an item within the device node that is not any more in use.
2. While registering a callback, it has to be registered to both callback thread and the
   device node before letting callback thread handle them.

Fix these issues as follows:
1. Don't let the device node unregister the callback by itself
2. Keep the callback list locked for the full registration of the callback

In addition, make a minor clean up for the uORB::DeviceNode::_unregister_callback.
Get a pointer to the callback item before removing it from the node's cb list. The original
code happened to work since the callback handle is just an idex to statically allocated items,
but it looked weird.

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

platforms/common/uORB/uORBDeviceNode.cpp

@@ -576,9 +576,6 @@ uORB::DeviceNode::write(const char *buffer, const orb_metadata *meta, orb_advert
 	/* If data size has changed, re-map the data */
 	size_t data_size = _queue_size * o_size;
 
-	/* Remove single unresponsive entry at a time (if any) */
-	uorb_cb_handle_t remove_cb = UORB_INVALID_CB_HANDLE;
-
 	/* Perform an atomic copy. */
 	ATOMIC_ENTER;
 
@@ -626,8 +623,11 @@ uORB::DeviceNode::write(const char *buffer, const orb_metadata *meta, orb_advert
 				__atomic_fetch_add(&item->cb_triggered, 1, __ATOMIC_SEQ_CST);
 				Manager::unlockThread(item->lock);
 
-			} else {
-				remove_cb = cb;
+			} else if (item->cb_triggered == CB_ALIVE_MAX_VALUE) {
+				// Callbacks are not being handled? Post once more and print an error
+				__atomic_fetch_add(&item->cb_triggered, 1, __ATOMIC_SEQ_CST);
+				Manager::unlockThread(item->lock);
+				PX4_ERR("CB triggered from %s is not being handled?\n", get_name());
 			}
 
 #endif
@@ -638,11 +638,6 @@ uORB::DeviceNode::write(const char *buffer, const orb_metadata *meta, orb_advert
 
 	ATOMIC_LEAVE;
 
-	if (callbacks.handle_valid(remove_cb)) {
-		PX4_ERR("Removing subscriber due to inactivity\n");
-		unregister_callback(handle, remove_cb);
-	}
-
 	return o_size;
 }
 
@@ -908,6 +903,9 @@ int
 uORB::DeviceNode::_unregister_callback(uorb_cb_handle_t &cb_handle)
 {
 	IndexedStackHandle<CB_LIST_T> callbacks(_callbacks);
+#ifndef CONFIG_BUILD_FLAT
+	EventWaitItem *item = callbacks.peek(cb_handle);
+#endif
 	int ret = 0;
 
 	ATOMIC_ENTER;
@@ -919,7 +917,6 @@ uORB::DeviceNode::_unregister_callback(uorb_cb_handle_t &cb_handle)
 
 	} else {
 #ifndef CONFIG_BUILD_FLAT
-		EventWaitItem *item = callbacks.peek(cb_handle);
 		ret = item->cb_triggered;
 #endif
 		callbacks.push_free(cb_handle);

platforms/common/uORB/uORBManager.cpp

@@ -557,7 +557,8 @@ uORB::Manager::registerCallback(orb_advert_t &node_handle, SubscriptionCallback
 
 		lock_cb_list();
 		per_process_cb_list.add(callback_sub);
-		unlock_cb_list();
+
+		// keep the cb list locked; this prevents the callback thread from trying to access the callback item before it is registered to the device node
 #endif
 		ret = uORB::DeviceNode::register_callback(node_handle, callback_sub, cb_lock, last_update,
 
@@ -566,11 +567,10 @@ uORB::Manager::registerCallback(orb_advert_t &node_handle, SubscriptionCallback
 	}
 
 	if (!ret) {
-		lock_cb_list();
 		per_process_cb_list.remove(callback_sub);
-		unlock_cb_list();
 	}
 
+	unlock_cb_list();
 #endif
 
 	return ret;