SubscriptionCallback: Add lock protecting callback during its execution

There is a potential problem with sub->call() and sub->[un-]register(),
when the callback is executing, if someone drops the callback registration,
it will result in a resource leak and a crash.

This does not happen in flat mode, as the uORB::DeviceNode::lock protects
both, but when a callback thread is used, there is a race condition which
is fixed by this patch.

platforms/common/uORB/SubscriptionCallback.hpp

@@ -59,19 +59,24 @@ class SubscriptionCallback : public SubscriptionInterval
 	SubscriptionCallback(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) :
 		SubscriptionInterval(meta, interval_us, instance)
 	{
+		px4_sem_init(&_lock, 1, 1);
 	}
 
 	virtual ~SubscriptionCallback()
 	{
 		unregisterCallback();
+		px4_sem_destroy(&_lock);
 	};
 
 	bool registerCallback()
 	{
+		lock();
+
 		if (!registered()) {
 			if (!orb_advert_valid(_subscription.get_node())) {
 				// force topic creation
 				if (!_subscription.subscribe(true)) {
+					unlock();
 					return false;
 				}
 			}
@@ -81,16 +86,21 @@ class SubscriptionCallback : public SubscriptionInterval
 			}
 		}
 
+		unlock();
 		return registered();
 	}
 
 	void unregisterCallback()
 	{
+		lock();
+
 		if (registered()) {
 			uorb_cb_handle_t handle = _cb_handle;
 			_cb_handle = UORB_INVALID_CB_HANDLE;
 			DeviceNode::unregister_callback(_subscription.get_node(), handle);
 		}
+
+		unlock();
 	}
 
 	/**
@@ -126,8 +136,16 @@ class SubscriptionCallback : public SubscriptionInterval
 
 	virtual void call() = 0;
 
+	void do_call() { lock(); call(); unlock(); }
+
 	bool registered() const { return uorb_cb_handle_valid(_cb_handle); }
 
+private:
+	/* Make sure the callback remains valid during callback execution */
+
+	void lock() { do {} while (px4_sem_wait(&_lock) != 0); }
+	void unlock() { px4_sem_post(&_lock); }
+	px4_sem_t _lock; /* Lock to protect registered callback */
 protected:
 
 	uorb_cb_handle_t _cb_handle{UORB_INVALID_CB_HANDLE};

platforms/common/uORB/uORBManager.cpp

@@ -498,11 +498,7 @@ uORB::Manager::callback_thread(int argc, char *argv[])
 			break;
 		}
 
-		if (!sub->registered()) {
-			continue;
-		}
-
-		sub->call();
+		sub->do_call();
 	}
 
 	Manager::freeThreadLock(per_process_lock);