uORB replace ORBMap with linked list

src/include/containers/List.hpp

@@ -40,37 +40,23 @@
 #pragma once
 
 template<class T>
-class __EXPORT ListNode
+class ListNode
 {
 public:
-	ListNode() : _sibling() {}
-	virtual ~ListNode() = default;
-
-	// no copy, assignment, move, move assignment
-	ListNode(const ListNode &) = delete;
-	ListNode &operator=(const ListNode &) = delete;
-	ListNode(ListNode &&) = delete;
-	ListNode &operator=(ListNode &&) = delete;
 
 	void setSibling(T sibling) { _sibling = sibling; }
-	const T getSibling() { return _sibling; }
+	const T getSibling() const { return _sibling; }
 
 protected:
-	T _sibling;
+
+	T _sibling{nullptr};
+
 };
 
 template<class T>
-class __EXPORT List
+class List
 {
 public:
-	List() : _head() {}
-	virtual ~List() = default;
-
-	// no copy, assignment, move, move assignment
-	List(const List &) = delete;
-	List &operator=(const List &) = delete;
-	List(List &&) = delete;
-	List &operator=(List &&) = delete;
 
 	void add(T newNode)
 	{
@@ -78,8 +64,9 @@ public:
 		_head = newNode;
 	}
 
-	const T getHead() { return _head; }
+	const T getHead() const { return _head; }
 
 protected:
-	T _head;
+
+	T _head{nullptr};
 };

src/lib/cdev/CDev.hpp

@@ -155,6 +155,13 @@ public:
 	 */
 	virtual int	poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup);
 
+	/**
+	 * Get the device name.
+	 *
+	 * @return the file system string of the device handle
+	 */
+	const char	*get_devname() const { return _devname; }
+
 protected:
 	/**
 	 * Pointer to the default cdev file operations table; useful for
@@ -239,13 +246,6 @@ protected:
 	 */
 	virtual int unregister_class_devname(const char *class_devname, unsigned class_instance);
 
-	/**
-	 * Get the device name.
-	 *
-	 * @return the file system string of the device handle
-	 */
-	const char	*get_devname() { return _devname; }
-
 	/**
 	 * Take the driver lock.
 	 *

src/modules/uORB/ORBMap.hpp

@@ -1,161 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2015 Mark Charlebois. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <string.h>
-#include <stdlib.h>
-
-
-namespace uORB
-{
-class DeviceNode;
-class ORBMap;
-}
-
-class uORB::ORBMap
-{
-public:
-	struct Node {
-		struct Node *next;
-		const char *node_name;
-		uORB::DeviceNode *node;
-	};
-
-	ORBMap() :
-		_top(nullptr),
-		_end(nullptr)
-	{ }
-	~ORBMap()
-	{
-		while (_top != nullptr) {
-			unlinkNext(_top);
-
-			if (_top->next == nullptr) {
-				free(_top);
-				_top = nullptr;
-				_end = nullptr;
-			}
-		}
-	}
-
-	/**
-	 * Insert an element with a unique name
-	 * @param node_name name of the node. This will not be copied, so the caller has to ensure
-	 *                  the pointer is valid until the node is removed from ORBMap
-	 * @param node
-	 */
-	void insert(const char *node_name, uORB::DeviceNode *node)
-	{
-		Node **p;
-
-		if (_top == nullptr) {
-			p = &_top;
-
-		} else {
-			p = &_end->next;
-		}
-
-		*p = (Node *)malloc(sizeof(Node));
-
-		if (_end) {
-			_end = _end->next;
-
-		} else {
-			_end = _top;
-		}
-
-		_end->next = nullptr;
-		_end->node_name = node_name;
-		_end->node = node;
-	}
-
-	bool find(const char *node_name)
-	{
-		Node *p = _top;
-
-		while (p) {
-			if (strcmp(p->node_name, node_name) == 0) {
-				return true;
-			}
-
-			p = p->next;
-		}
-
-		return false;
-	}
-
-	uORB::DeviceNode *get(const char *node_name)
-	{
-		Node *p = _top;
-
-		while (p) {
-			if (strcmp(p->node_name, node_name) == 0) {
-				return p->node;
-			}
-
-			p = p->next;
-		}
-
-		return nullptr;
-	}
-
-	Node *top() const
-	{
-		return _top;
-	}
-
-	bool empty() const
-	{
-		return !_top;
-	}
-
-private:
-	void unlinkNext(Node *a)
-	{
-		Node *b = a->next;
-
-		if (b != nullptr) {
-			if (_end == b) {
-				_end = a;
-			}
-
-			a->next = b->next;
-			free(b);
-		}
-	}
-
-	Node *_top;
-	Node *_end;
-};
-

src/modules/uORB/uORBDeviceMaster.cpp

@@ -43,24 +43,6 @@
 #include <px4_sem.hpp>
 #include <systemlib/px4_macros.h>
 
-#ifdef __PX4_NUTTX
-#define ITERATE_NODE_MAP() \
-	for (ORBMap::Node *node_iter = _node_map.top(); node_iter; node_iter = node_iter->next)
-#define INIT_NODE_MAP_VARS(node_obj, node_name_str) \
-	DeviceNode *node_obj = node_iter->node; \
-	const char *node_name_str = node_iter->node_name; \
-	UNUSED(node_name_str);
-
-#else
-#include <algorithm>
-#define ITERATE_NODE_MAP() \
-	for (const auto &node_iter : _node_map)
-#define INIT_NODE_MAP_VARS(node_obj, node_name_str) \
-	DeviceNode *node_obj = node_iter.second; \
-	const char *node_name_str = node_iter.first.c_str(); \
-	UNUSED(node_name_str);
-#endif
-
 uORB::DeviceMaster::DeviceMaster()
 {
 	px4_sem_init(&_lock, 0, 1);
@@ -121,7 +103,7 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in
 		}
 
 		/* construct the new node */
-		uORB::DeviceNode *node = new uORB::DeviceNode(meta, devpath, priority);
+		uORB::DeviceNode *node = new uORB::DeviceNode(meta, group_tries, devpath, priority);
 
 		/* if we didn't get a device, that's bad */
 		if (node == nullptr) {
@@ -139,7 +121,7 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in
 			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);
+				uORB::DeviceNode *existing_node = getDeviceNodeLocked(meta, group_tries);
 
 				if ((existing_node != nullptr) && !(existing_node->is_published())) {
 					/* nothing has been published yet, lets claim it */
@@ -156,11 +138,7 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in
 
 		} else {
 			// add to the node map;.
-#ifdef __PX4_NUTTX
+			_node_list.add(node);
-			_node_map.insert(devpath, node);
-#else
-			_node_map[std::string(devpath)] = node;
-#endif
 		}
 
 		group_tries++;
@@ -184,9 +162,8 @@ void uORB::DeviceMaster::printStatistics(bool reset)
 	bool had_print = false;
 
 	lock();
-	ITERATE_NODE_MAP() {
-		INIT_NODE_MAP_VARS(node, node_name)
 
+	for (DeviceNode *node = _node_list.getHead(); node != nullptr; node = node->getSibling()) {
 		if (node->print_statistics(reset)) {
 			had_print = true;
 		}
@@ -212,8 +189,8 @@ void uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node
 		}
 	}
 
-	ITERATE_NODE_MAP() {
+	for (DeviceNode *node = _node_list.getHead(); node != nullptr; node = node->getSibling()) {
-		INIT_NODE_MAP_VARS(node, node_name)
+
 		++num_topics;
 
 		//check if already added
@@ -255,8 +232,7 @@ void uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node
 		}
 
 		last_node->node = node;
-		int node_name_len = strlen(node_name);
+
-		last_node->instance = (uint8_t)(node_name[node_name_len - 1] - '0');
 		size_t name_length = strlen(last_node->node->get_meta()->o_name);
 
 		if (name_length > max_topic_name_length) {
@@ -286,7 +262,7 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters)
 
 	lock();
 
-	if (_node_map.empty()) {
+	if (_node_list.getHead() == nullptr) {
 		unlock();
 		PX4_INFO("no active topics");
 		return;
@@ -374,7 +350,7 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters)
 
 				if (!print_active_only || cur_node->pub_msg_delta > 0) {
 					PX4_INFO_RAW(CLEAR_LINE "%-*s %2i %4i %4i %5i %i\n", (int)max_topic_name_length,
-						     cur_node->node->get_meta()->o_name, (int)cur_node->instance,
+						     cur_node->node->get_meta()->o_name, (int)cur_node->node->get_instance(),
 						     (int)cur_node->node->subscriber_count(), cur_node->pub_msg_delta,
 						     (int)cur_node->lost_msg_delta, cur_node->node->get_queue_size());
 				}
@@ -403,39 +379,37 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters)
 uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath)
 {
 	lock();
-	uORB::DeviceNode *node = getDeviceNodeLocked(nodepath);
+
+	for (DeviceNode *node = _node_list.getHead(); node != nullptr; node = node->getSibling()) {
+		if (strcmp(node->get_devname(), nodepath) == 0) {
 			unlock();
+			return node;
+		}
+	}
+
+	unlock();
+
+	return nullptr;
+}
+
+uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const struct orb_metadata *meta, const uint8_t instance)
+{
+	lock();
+	uORB::DeviceNode *node = getDeviceNodeLocked(meta, instance);
+	unlock();
+
 	//We can safely return the node that can be used by any thread, because
 	//a DeviceNode never gets deleted.
 	return node;
 }
 
-
+uORB::DeviceNode *uORB::DeviceMaster::getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance)
-#ifdef __PX4_NUTTX
-uORB::DeviceNode *uORB::DeviceMaster::getDeviceNodeLocked(const char *nodepath)
 {
-	uORB::DeviceNode *rc = nullptr;
+	for (DeviceNode *node = _node_list.getHead(); node != nullptr; node = node->getSibling()) {
-
+		if ((strcmp(node->get_name(), meta->o_name) == 0) && (node->get_instance() == instance)) {
-	if (_node_map.find(nodepath)) {
+			return node;
-		rc = _node_map.get(nodepath);
+		}
 	}
 
-	return rc;
+	return nullptr;
 }
-
-#else
-
-uORB::DeviceNode *uORB::DeviceMaster::getDeviceNodeLocked(const char *nodepath)
-{
-	uORB::DeviceNode *rc = nullptr;
-	std::string np(nodepath);
-
-	auto iter = _node_map.find(np);
-
-	if (iter != _node_map.end()) {
-		rc = iter->second;
-	}
-
-	return rc;
-}
-#endif

src/modules/uORB/uORBDeviceMaster.hpp

@@ -45,17 +45,10 @@ class DeviceMaster;
 class Manager;
 }
 
-#ifdef __PX4_NUTTX
 #include <string.h>
 #include <stdlib.h>
-#include "ORBMap.hpp"
 
-#else
+#include <containers/List.hpp>
-
-#include <string>
-#include <map>
-
-#endif /* __PX4_NUTTX */
 
 /**
  * Master control device for ObjDev.
@@ -74,6 +67,7 @@ public:
 	 * @return node if exists, nullptr otherwise
 	 */
 	uORB::DeviceNode *getDeviceNode(const char *node_name);
+	uORB::DeviceNode *getDeviceNode(const struct orb_metadata *meta, const uint8_t instance);
 
 	/**
 	 * Print statistics for each existing topic.
@@ -97,7 +91,6 @@ private:
 
 	struct DeviceNodeStatisticsData {
 		DeviceNode *node;
-		uint8_t instance;
 		uint32_t last_lost_msg_count;
 		unsigned int last_pub_msg_count;
 		uint32_t lost_msg_delta;
@@ -115,13 +108,10 @@ private:
 	 * _lock must already be held when calling this.
 	 * @return node if exists, nullptr otherwise
 	 */
-	uORB::DeviceNode *getDeviceNodeLocked(const char *node_name);
+	uORB::DeviceNode *getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance);
+
+	List<uORB::DeviceNode *> _node_list;
 
-#ifdef __PX4_NUTTX
-	ORBMap _node_map;
-#else
-	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) */

src/modules/uORB/uORBDeviceNode.cpp

@@ -53,10 +53,12 @@ uORB::DeviceNode::SubscriberData *uORB::DeviceNode::filp_to_sd(cdev::file_t *fil
 	return (SubscriberData *)(filp->f_priv);
 }
 
-uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const char *path, int priority, unsigned int queue_size) :
+uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path,
+			     uint8_t priority, uint8_t queue_size) :
 	CDev(path),
 	_meta(meta),
-	_priority((uint8_t)priority),
+	_instance(instance),
+	_priority(priority),
 	_queue_size(queue_size)
 {
 }

src/modules/uORB/uORBDeviceNode.hpp

@@ -38,6 +38,8 @@
 
 #include <lib/cdev/CDev.hpp>
 
+#include <containers/List.hpp>
+
 namespace uORB
 {
 class DeviceNode;
@@ -48,10 +50,11 @@ class Manager;
 /**
  * Per-object device instance.
  */
-class uORB::DeviceNode : public cdev::CDev
+class uORB::DeviceNode : public cdev::CDev, public ListNode<uORB::DeviceNode *>
 {
 public:
-	DeviceNode(const struct orb_metadata *meta, const char *path, int priority, unsigned int queue_size = 1);
+	DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t priority,
+		   uint8_t queue_size = 1);
 	~DeviceNode();
 
 	// no copy, assignment, move, move assignment
@@ -175,11 +178,19 @@ public:
 	 */
 	bool print_statistics(bool reset);
 
-	unsigned int get_queue_size() const { return _queue_size; }
+	uint8_t get_queue_size() const { return _queue_size; }
+
 	int8_t subscriber_count() const { return _subscriber_count; }
+
 	uint32_t lost_message_count() const { return _lost_messages; }
-	unsigned int published_message_count() const { return _generation; }
+
-	const struct orb_metadata *get_meta() const { return _meta; }
+	unsigned published_message_count() const { return _generation; }
+
+	const orb_metadata *get_meta() const { return _meta; }
+
+	const char *get_name() const { return _meta->o_name; }
+
+	uint8_t get_instance() const { return _instance; }
 
 	int get_priority() const { return _priority; }
 	void set_priority(uint8_t priority) { _priority = priority; }
@@ -211,7 +222,8 @@ private:
 		{ if (update_interval) { update_interval->update_reported = update_reported_flag; } }
 	};
 
-	const struct orb_metadata *_meta; /**< object metadata information */
+	const orb_metadata *_meta; /**< object metadata information */
+	const uint8_t _instance; /**< orb multi instance identifier */
 	uint8_t     *_data{nullptr};   /**< allocated object buffer */
 	hrt_abstime   _last_update{0}; /**< time the object was last updated */
 	volatile unsigned   _generation{0};  /**< object generation count */
@@ -220,19 +232,14 @@ private:
 	uint8_t _queue_size; /**< maximum number of elements in the queue */
 	int8_t _subscriber_count{0};
 
-	inline static SubscriberData    *filp_to_sd(cdev::file_t *filp);
-
-#ifdef __PX4_NUTTX
-	pid_t     _publisher {0}; /**< if nonzero, current publisher. Only used inside the advertise call.
-					We allow one publisher to have an open file descriptor at the same time. */
-#else
 	px4_task_t _publisher{0}; /**< if nonzero, current publisher. Only used inside the advertise call.
 						We allow one publisher to have an open file descriptor at the same time. */
-#endif
 
 	// statistics
-	uint32_t _lost_messages = 0; ///< nr of lost messages for all subscribers. If two subscribers lose the same
+	uint32_t _lost_messages = 0; /**< nr of lost messages for all subscribers. If two subscribers lose the same
-	///message, it is counted as two.
+					message, it is counted as two. */
+
+	inline static SubscriberData    *filp_to_sd(cdev::file_t *filp);
 
 	/**
 	 * Perform a deferred update for a rate-limited subscriber.