diff --git a/platforms/common/uORB/SubscriptionCallback.hpp b/platforms/common/uORB/SubscriptionCallback.hpp
index 753aa5af713e..1d7eb3d57fd9 100644
--- a/platforms/common/uORB/SubscriptionCallback.hpp
+++ b/platforms/common/uORB/SubscriptionCallback.hpp
@@ -42,6 +42,14 @@
 #include <px4_platform_common/px4_work_queue/WorkItem.hpp>
 #include <px4_platform_common/posix.h>
 
+#ifdef CONFIG_BUILD_FLAT
+#define CB_LOCK()
+#define CB_UNLOCK()
+#else
+#define CB_LOCK() lock()
+#define CB_UNLOCK() unlock()
+#endif
+
 namespace uORB
 {
 
@@ -59,19 +67,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()
 	{
+		CB_LOCK();
+
 		if (!registered()) {
 			if (!orb_advert_valid(_subscription.get_node())) {
 				// force topic creation
 				if (!_subscription.subscribe(true)) {
+					unlock();
 					return false;
 				}
 			}
@@ -81,16 +94,21 @@ class SubscriptionCallback : public SubscriptionInterval
 			}
 		}
 
+		CB_UNLOCK();
 		return registered();
 	}
 
 	void unregisterCallback()
 	{
+		CB_LOCK();
+
 		if (registered()) {
 			uorb_cb_handle_t handle = _cb_handle;
 			_cb_handle = UORB_INVALID_CB_HANDLE;
 			DeviceNode::unregister_callback(_subscription.get_node(), handle);
 		}
+
+		CB_UNLOCK();
 	}
 
 	/**
@@ -126,8 +144,28 @@ class SubscriptionCallback : public SubscriptionInterval
 
 	virtual void call() = 0;
 
+	void do_call()
+	{
+		CB_LOCK();
+
+		// Run the callback if it is still valid
+		if (registered()) {
+			call();
+		}
+
+		CB_UNLOCK();
+	}
+
 	bool registered() const { return uorb_cb_handle_valid(_cb_handle); }
 
+private:
+#ifndef CONFIG_BUILD_FLAT
+	/* 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 */
+#endif
 protected:
 
 	uorb_cb_handle_t _cb_handle{UORB_INVALID_CB_HANDLE};
diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp
index b42d086adadd..b60302f2107f 100644
--- a/platforms/common/uORB/uORBManager.cpp
+++ b/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);