uORB advertise through uORBDeviceMaster directly

src/drivers/drv_orb_dev.h

@@ -45,29 +45,9 @@
 #include <sys/ioctl.h>
 #include <stdint.h>
 
-/*
- * ioctl() definitions
- */
-
-/** path to the uORB control device for pub/sub topics */
-#define TOPIC_MASTER_DEVICE_PATH	"/obj/_obj_"
-
-/** path to the uORB control device for parameter topics */
-#define PARAM_MASTER_DEVICE_PATH	"/param/_param_"
-
-/** maximum ogbject name length */
-#define ORB_MAXNAME		32
-
 #define _ORBIOCBASE		(0x2600)
 #define _ORBIOC(_n)		(_PX4_IOC(_ORBIOCBASE, _n))
 
-/*
- * IOCTLs for the uORB control device
- */
-
-/** Advertise a new topic described by *(uorb_metadata *)arg */
-#define ORBIOCADVERTISE		_ORBIOC(0)
-
 /*
  * IOCTLs for individual topics.
  */

src/modules/uORB/uORBDeviceMaster.cpp

@@ -61,122 +61,117 @@
 	UNUSED(node_name_str);
 #endif
 
-uORB::DeviceMaster::DeviceMaster() :
-	CDev(TOPIC_MASTER_DEVICE_PATH)
+uORB::DeviceMaster::DeviceMaster()
 {
+	px4_sem_init(&_lock, 0, 1);
 	_last_statistics_output = hrt_absolute_time();
 }
 
-int
-uORB::DeviceMaster::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
+uORB::DeviceMaster::~DeviceMaster()
 {
-	int ret;
+	px4_sem_destroy(&_lock);
+}
 
-	switch (cmd) {
-	case ORBIOCADVERTISE: {
-			const struct orb_advertdata *adv = (const struct orb_advertdata *)arg;
-			const struct orb_metadata *meta = adv->meta;
-			char nodepath[orb_maxpath];
+int
+uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, int priority)
+{
+	int ret = PX4_ERROR;
 
-			/* construct a path to the node - this also checks the node name */
-			ret = uORB::Utils::node_mkpath(nodepath, meta, adv->instance);
+	char nodepath[orb_maxpath];
 
-			if (ret != PX4_OK) {
-				return ret;
-			}
+	/* construct a path to the node - this also checks the node name */
+	ret = uORB::Utils::node_mkpath(nodepath, meta, instance);
 
-			ret = PX4_ERROR;
+	if (ret != PX4_OK) {
+		return ret;
+	}
 
-			/* try for topic groups */
-			const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1;
-			unsigned group_tries = 0;
+	ret = PX4_ERROR;
 
-			if (adv->instance) {
-				/* for an advertiser, this will be 0, but a for subscriber that requests a certain instance,
-				 * we do not want to start with 0, but with the instance the subscriber actually requests.
-				 */
-				group_tries = *adv->instance;
+	/* try for topic groups */
+	const unsigned max_group_tries = (instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1;
+	unsigned group_tries = 0;
 
-				if (group_tries >= max_group_tries) {
-					return -ENOMEM;
-				}
-			}
+	if (instance) {
+		/* for an advertiser, this will be 0, but a for subscriber that requests a certain instance,
+		 * we do not want to start with 0, but with the instance the subscriber actually requests.
+		 */
+		group_tries = *instance;
 
-			SmartLock smart_lock(_lock);
+		if (group_tries >= max_group_tries) {
+			return -ENOMEM;
+		}
+	}
 
-			do {
-				/* if path is modifyable change try index */
-				if (adv->instance != nullptr) {
-					/* replace the number at the end of the string */
-					nodepath[strlen(nodepath) - 1] = '0' + group_tries;
-					*(adv->instance) = group_tries;
-				}
+	SmartLock smart_lock(_lock);
 
-				/* driver wants a permanent copy of the path, so make one here */
-				const char *devpath = strdup(nodepath);
-
-				if (devpath == nullptr) {
-					return -ENOMEM;
-				}
-
-				/* construct the new node */
-				uORB::DeviceNode *node = new uORB::DeviceNode(meta, devpath, adv->priority);
-
-				/* if we didn't get a device, that's bad */
-				if (node == nullptr) {
-					free((void *)devpath);
-					return -ENOMEM;
-				}
-
-				/* initialise the node - this may fail if e.g. a node with this name already exists */
-				ret = node->init();
-
-				/* if init failed, discard the node and its name */
-				if (ret != PX4_OK) {
-					delete node;
-
-					if (ret == -EEXIST) {
-						/* if the node exists already, get the existing one and check if
-						 * something has been published yet. */
-						uORB::DeviceNode *existing_node = getDeviceNodeLocked(devpath);
-
-						if ((existing_node != nullptr) && !(existing_node->is_published())) {
-							/* nothing has been published yet, lets claim it */
-							existing_node->set_priority(adv->priority);
-							ret = PX4_OK;
-
-						} else {
-							/* otherwise: data has already been published, keep looking */
-						}
-					}
-
-					/* also discard the name now */
-					free((void *)devpath);
-
-				} else {
-					// add to the node map;.
-#ifdef __PX4_NUTTX
-					_node_map.insert(devpath, node);
-#else
-					_node_map[std::string(devpath)] = node;
-#endif
-				}
-
-				group_tries++;
-
-			} while (ret != PX4_OK && (group_tries < max_group_tries));
-
-			if (ret != PX4_OK && group_tries >= max_group_tries) {
-				ret = -ENOMEM;
-			}
-
-			return ret;
+	do {
+		/* if path is modifyable change try index */
+		if (instance != nullptr) {
+			/* replace the number at the end of the string */
+			nodepath[strlen(nodepath) - 1] = '0' + group_tries;
+			*instance = group_tries;
 		}
 
-	default:
-		/* give it to the superclass */
-		return CDev::ioctl(filp, cmd, arg);
+		/* driver wants a permanent copy of the path, so make one here */
+		const char *devpath = strdup(nodepath);
+
+		if (devpath == nullptr) {
+			return -ENOMEM;
+		}
+
+		/* construct the new node */
+		uORB::DeviceNode *node = new uORB::DeviceNode(meta, devpath, priority);
+
+		/* if we didn't get a device, that's bad */
+		if (node == nullptr) {
+			free((void *)devpath);
+			return -ENOMEM;
+		}
+
+		/* initialise the node - this may fail if e.g. a node with this name already exists */
+		ret = node->init();
+
+		/* if init failed, discard the node and its name */
+		if (ret != PX4_OK) {
+			delete node;
+
+			if (ret == -EEXIST) {
+				/* if the node exists already, get the existing one and check if
+				 * something has been published yet. */
+				uORB::DeviceNode *existing_node = getDeviceNodeLocked(devpath);
+
+				if ((existing_node != nullptr) && !(existing_node->is_published())) {
+					/* nothing has been published yet, lets claim it */
+					existing_node->set_priority(priority);
+					ret = PX4_OK;
+
+				} else {
+					/* otherwise: data has already been published, keep looking */
+				}
+			}
+
+			/* also discard the name now */
+			free((void *)devpath);
+
+		} else {
+			// add to the node map;.
+#ifdef __PX4_NUTTX
+			_node_map.insert(devpath, node);
+#else
+			_node_map[std::string(devpath)] = node;
+#endif
+		}
+
+		group_tries++;
+
+	} while (ret != PX4_OK && (group_tries < max_group_tries));
+
+	if (ret != PX4_OK && group_tries >= max_group_tries) {
+		ret = -ENOMEM;
 	}
+
+	return ret;
 }
 
 void uORB::DeviceMaster::printStatistics(bool reset)
@@ -207,7 +202,7 @@ void uORB::DeviceMaster::printStatistics(bool reset)
 void uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node, int &num_topics,
 		size_t &max_topic_name_length, char **topic_filter, int num_filters)
 {
-	DeviceNodeStatisticsData *cur_node;
+	DeviceNodeStatisticsData *cur_node = nullptr;
 	num_topics = 0;
 	DeviceNodeStatisticsData *last_node = *first_node;
 
@@ -277,7 +272,6 @@ void uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node
 
 void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters)
 {
-
 	bool print_active_only = true;
 
 	if (topic_filter && num_filters > 0) {

src/modules/uORB/uORBDeviceMaster.hpp

@@ -34,8 +34,9 @@
 #pragma once
 
 #include <stdint.h>
+
 #include "uORBCommon.hpp"
-#include <lib/cdev/CDev.hpp>
+#include <px4_posix.h>
 
 namespace uORB
 {
@@ -62,10 +63,11 @@ class Manager;
  * Used primarily to create new objects via the ORBIOCCREATE
  * ioctl.
  */
-class uORB::DeviceMaster : public cdev::CDev
+class uORB::DeviceMaster
 {
 public:
-	virtual int   ioctl(cdev::file_t *filp, int cmd, unsigned long arg);
+
+	int advertise(const struct orb_metadata *meta, int *instance, int priority);
 
 	/**
 	 * Public interface for getDeviceNodeLocked(). Takes care of synchronization.
@@ -91,7 +93,7 @@ public:
 private:
 	// Private constructor, uORB::Manager takes care of its creation
 	DeviceMaster();
-	virtual ~DeviceMaster() = default;
+	~DeviceMaster();
 
 	struct DeviceNodeStatisticsData {
 		DeviceNode *node;
@@ -121,4 +123,9 @@ private:
 	std::map<std::string, uORB::DeviceNode *> _node_map;
 #endif
 	hrt_abstime       _last_statistics_output;
+
+	px4_sem_t	_lock; /**< lock to protect access to all class members (also for derived classes) */
+
+	void		lock() { do {} while (px4_sem_wait(&_lock) != 0); }
+	void		unlock() { px4_sem_post(&_lock); }
 };

src/modules/uORB/uORBDeviceNode.cpp

@@ -397,8 +397,8 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v
 	uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle;
 	int ret;
 
-	/* check if the device handle is initialized */
-	if ((devnode == nullptr) || (meta == nullptr)) {
+	/* check if the device handle is initialized and data is valid */
+	if ((devnode == nullptr) || (meta == nullptr) || (data == nullptr)) {
 		errno = EFAULT;
 		return PX4_ERROR;
 	}

src/modules/uORB/uORBDeviceNode.hpp

@@ -36,6 +36,8 @@
 #include "uORBCommon.hpp"
 #include "uORBDeviceMaster.hpp"
 
+#include <lib/cdev/CDev.hpp>
+
 namespace uORB
 {
 class DeviceNode;

src/modules/uORB/uORBManager.cpp

@@ -44,12 +44,8 @@
 #include "uORBUtils.hpp"
 #include "uORBManager.hpp"
 
-
-//=========================  Static initializations =================
 uORB::Manager *uORB::Manager::_Instance = nullptr;
 
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
 bool uORB::Manager::initialize()
 {
 	if (_Instance == nullptr) {
@@ -59,8 +55,6 @@ bool uORB::Manager::initialize()
 	return _Instance != nullptr;
 }
 
-//-----------------------------------------------------------------------------
-//-----------------------------------------------------------------------------
 uORB::Manager::Manager()
 {
 #ifdef ORB_USE_PUBLISHER_RULES
@@ -89,17 +83,7 @@ uORB::DeviceMaster *uORB::Manager::get_device_master()
 	if (!_device_master) {
 		_device_master = new DeviceMaster();
 
-		if (_device_master) {
-			int ret = _device_master->init();
-
-			if (ret != PX4_OK) {
-				PX4_ERR("Initialization of DeviceMaster failed (%i)", ret);
-				errno = -ret;
-				delete _device_master;
-				_device_master = nullptr;
-			}
-
-		} else {
+		if (_device_master == nullptr) {
 			PX4_ERR("Failed to allocate DeviceMaster");
 			errno = ENOMEM;
 		}
@@ -188,7 +172,7 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
 #endif /* ORB_USE_PUBLISHER_RULES */
 
 	/* open the node as an advertiser */
-	int fd = node_open(meta, data, true, instance, priority);
+	int fd = node_open(meta, true, instance, priority);
 
 	if (fd == PX4_ERROR) {
 		PX4_ERR("%s advertise failed", meta->o_name);
@@ -246,13 +230,13 @@ int uORB::Manager::orb_unadvertise(orb_advert_t handle)
 
 int uORB::Manager::orb_subscribe(const struct orb_metadata *meta)
 {
-	return node_open(meta, nullptr, false);
+	return node_open(meta, false);
 }
 
 int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
 {
 	int inst = instance;
-	return node_open(meta, nullptr, false, &inst);
+	return node_open(meta, false, &inst);
 }
 
 int uORB::Manager::orb_unsubscribe(int fd)
@@ -322,42 +306,27 @@ int uORB::Manager::orb_get_interval(int handle, unsigned *interval)
 
 int uORB::Manager::node_advertise(const struct orb_metadata *meta, int *instance, int priority)
 {
-	int fd = -1;
 	int ret = PX4_ERROR;
 
 	/* fill advertiser data */
-	const struct orb_advertdata adv = { meta, instance, priority };
 
-	/* open the control device */
-	fd = px4_open(TOPIC_MASTER_DEVICE_PATH, 0);
-
-	if (fd < 0) {
-		goto out;
+	if (get_device_master()) {
+		ret = _device_master->advertise(meta, instance, priority);
 	}
 
-	/* advertise the object */
-	ret = px4_ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv);
-
 	/* it's PX4_OK if it already exists */
 	if ((PX4_OK != ret) && (EEXIST == errno)) {
 		ret = PX4_OK;
 	}
 
-out:
-
-	if (fd >= 0) {
-		px4_close(fd);
-	}
-
 	return ret;
 }
 
-int uORB::Manager::node_open(const struct orb_metadata *meta, const void *data, bool advertiser, int *instance,
-			     int priority)
+int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance, int priority)
 {
 	char path[orb_maxpath];
 	int fd = -1;
-	int ret = -1;
+	int ret = PX4_ERROR;
 
 	/*
 	 * If meta is null, the object was not defined, i.e. it is not
@@ -368,14 +337,6 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, const void *data,
 		return PX4_ERROR;
 	}
 
-	/*
-	 * Advertiser must publish an initial value.
-	 */
-	if (advertiser && (data == nullptr)) {
-		errno = EINVAL;
-		return PX4_ERROR;
-	}
-
 	/* if we have an instance and are an advertiser, we will generate a new node and set the instance,
 	 * so we do not need to open here */
 	if (!instance || !advertiser) {

src/modules/uORB/uORBManager.hpp

@@ -398,7 +398,7 @@ private: // class methods
 	 * Handles creation of the object and the initial publication for
 	 * advertisers.
 	 */
-	int node_open(const struct orb_metadata *meta, const void *data, bool advertiser, int *instance = nullptr,
+	int node_open(const struct orb_metadata *meta, bool advertiser, int *instance = nullptr,
 		      int priority = ORB_PRIO_DEFAULT);
 
 private: // data members