new IntrusiveSortedList container used for uORB, WorkQueues, and WorkItems

- new intrusive linked list container (c++ template) that sorts on insertion
 - primarily for convenience inspecting things in the system like uORB or WorkQueues
 - uorb status or top sorted alphabetically 
 - work_queue status threads sorted by priority, then items sorted alphabetically within each

platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp

@@ -37,15 +37,18 @@
 #include "WorkQueue.hpp"
 
 #include <containers/IntrusiveQueue.hpp>
+#include <containers/IntrusiveSortedList.hpp>
 #include <px4_platform_common/defines.h>
 #include <drivers/drv_hrt.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
 
+#include <string.h>
+
 namespace px4
 {
 
-class WorkItem : public ListNode<WorkItem *>, public IntrusiveQueueNode<WorkItem *>
+class WorkItem : public IntrusiveSortedListNode<WorkItem *>, public IntrusiveQueueNode<WorkItem *>
 {
 public:
 
@@ -57,6 +60,9 @@ public:
 	WorkItem(WorkItem &&) = delete;
 	WorkItem &operator=(WorkItem &&) = delete;
 
+	// WorkItems sorted by name
+	bool operator<=(const WorkItem &rhs) const { return (strcmp(ItemName(), rhs.ItemName()) <= 0); }
+
 	inline void ScheduleNow()
 	{
 		if (_wq != nullptr) {

platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp

@@ -48,7 +48,7 @@ namespace px4
 
 class WorkItem;
 
-class WorkQueue : public ListNode<WorkQueue *>
+class WorkQueue : public IntrusiveSortedListNode<WorkQueue *>
 {
 public:
 	explicit WorkQueue(const wq_config_t &wq_config);
@@ -56,7 +56,8 @@ public:
 
 	~WorkQueue();
 
-	const char *get_name() { return _config.name; }
+	const wq_config_t &get_config() const { return _config; }
+	const char *get_name() const { return _config.name; }
 
 	bool Attach(WorkItem *item);
 	void Detach(WorkItem *item);
@@ -72,6 +73,9 @@ public:
 
 	void print_status(bool last = false);
 
+	// WorkQueues sorted numerically by relative priority (-1 to -255)
+	bool operator<=(const WorkQueue &rhs) const { return _config.relative_priority >= rhs.get_config().relative_priority; }
+
 private:
 
 	bool should_exit() const { return _should_exit.load(); }

platforms/posix/cmake/sitl_tests.cmake

@@ -17,6 +17,7 @@ set(tests
 	hrt
 	int
 	IntrusiveQueue
+	IntrusiveSortedList
 	List
 	mathlib
 	matrix

src/include/containers/BlockingList.hpp

@@ -39,14 +39,14 @@
 
 #pragma once
 
-#include "List.hpp"
+#include "IntrusiveSortedList.hpp"
 #include "LockGuard.hpp"
 
 #include <pthread.h>
 #include <stdlib.h>
 
 template<class T>
-class BlockingList : public List<T>
+class BlockingList : public IntrusiveSortedList<T>
 {
 public:
 
@@ -59,25 +59,25 @@ public:
 	void add(T newNode)
 	{
 		LockGuard lg{_mutex};
-		List<T>::add(newNode);
+		IntrusiveSortedList<T>::add(newNode);
 	}
 
 	bool remove(T removeNode)
 	{
 		LockGuard lg{_mutex};
-		return List<T>::remove(removeNode);
+		return IntrusiveSortedList<T>::remove(removeNode);
 	}
 
 	size_t size()
 	{
 		LockGuard lg{_mutex};
-		return List<T>::size();
+		return IntrusiveSortedList<T>::size();
 	}
 
 	void clear()
 	{
 		LockGuard lg{_mutex};
-		List<T>::clear();
+		IntrusiveSortedList<T>::clear();
 	}
 
 	pthread_mutex_t &mutex() { return _mutex; }

src/include/containers/IntrusiveSortedList.hpp

@@ -0,0 +1,187 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 PX4 Development Team. 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file IntrusiveSortedList.hpp
+ *
+ * An intrusive linked list where nodes are sorted on insertion.
+ */
+
+#pragma once
+
+#include <stdlib.h>
+
+template<class T>
+class IntrusiveSortedListNode
+{
+public:
+	void setSortedSibling(T sibling) { _sorted_list_node_sibling = sibling; }
+	const T getSortedSibling() const { return _sorted_list_node_sibling; }
+protected:
+	T _sorted_list_node_sibling{nullptr};
+};
+
+template<class T>
+class IntrusiveSortedList
+{
+public:
+
+	void add(T newNode)
+	{
+		if (_head == nullptr) {
+			// list is empty, add as head
+			_head = newNode;
+			return;
+
+		} else {
+			if (*newNode <= *_head) {
+				newNode->setSortedSibling(_head);
+				_head = newNode;
+				return;
+			}
+
+			// find last node and add to end
+			T node = _head;
+
+			while (node != nullptr && node->getSortedSibling() != nullptr) {
+
+				if (*newNode <= *node->getSortedSibling()) {
+					// insert newNode
+					newNode->setSortedSibling(node->getSortedSibling());
+					node->setSortedSibling(newNode);
+					return;
+				}
+
+				node = node->getSortedSibling();
+			}
+
+			// reached the end, add
+			node->setSortedSibling(newNode);
+		}
+	}
+
+	bool remove(T removeNode)
+	{
+		if (removeNode == nullptr) {
+			return false;
+		}
+
+		// base case
+		if (removeNode == _head) {
+			if (_head != nullptr) {
+				_head = _head->getSortedSibling();
+			}
+
+			removeNode->setSortedSibling(nullptr);
+
+			return true;
+		}
+
+		for (T node = _head; node != nullptr; node = node->getSortedSibling()) {
+			// is sibling the node to remove?
+			if (node->getSortedSibling() == removeNode) {
+				// replace sibling
+				if (node->getSortedSibling() != nullptr) {
+					node->setSortedSibling(node->getSortedSibling()->getSortedSibling());
+
+				} else {
+					node->setSortedSibling(nullptr);
+				}
+
+				removeNode->setSortedSibling(nullptr);
+
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+	struct Iterator {
+		T node;
+		explicit Iterator(T v) : node(v) {}
+
+		operator T() const { return node; }
+		operator T &() { return node; }
+		T operator* () const { return node; }
+		Iterator &operator++ ()
+		{
+			if (node) {
+				node = node->getSortedSibling();
+			};
+
+			return *this;
+		}
+	};
+
+	Iterator begin() { return Iterator(_head); }
+	Iterator end() { return Iterator(nullptr); }
+
+	bool empty() const { return _head == nullptr; }
+
+	size_t size() const
+	{
+		size_t sz = 0;
+
+		for (T node = _head; node != nullptr; node = node->getSortedSibling()) {
+			sz++;
+		}
+
+		return sz;
+	}
+
+	void deleteNode(T node)
+	{
+		if (remove(node)) {
+			// only delete if node was successfully removed
+			delete node;
+		}
+	}
+
+	void clear()
+	{
+		T node = _head;
+
+		while (node != nullptr) {
+			T next = node->getSortedSibling();
+			delete node;
+			node = next;
+		}
+
+		_head = nullptr;
+	}
+
+protected:
+
+	T _head{nullptr};
+};

src/modules/uORB/uORBDeviceMaster.hpp

@@ -50,7 +50,7 @@ class Manager;
 #include <string.h>
 #include <stdlib.h>
 
-#include <containers/List.hpp>
+#include <containers/IntrusiveSortedList.hpp>
 #include <px4_platform_common/atomic_bitset.h>
 
 using px4::AtomicBitset;
@@ -85,7 +85,7 @@ public:
 	 * Continuously print statistics, like the unix top command for processes.
 	 * Exited when the user presses the enter key.
 	 * @param topic_filter list of topic filters: if set, each string can be a substring for topics to match.
-	 *        Or it can be '-a', which means to print all topics instead of only currently publishing ones.
+	 *        Or it can be '-a', which means to print all topics instead of only ones currently publishing with subscribers.
 	 * @param num_filters
 	 */
 	void showTop(char **topic_filter, int num_filters);
@@ -114,7 +114,7 @@ private:
 	 */
 	uORB::DeviceNode *getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance);
 
-	List<uORB::DeviceNode *> _node_list;
+	IntrusiveSortedList<uORB::DeviceNode *> _node_list;
 	AtomicBitset<ORB_TOPICS_COUNT> _node_exists[ORB_MULTI_MAX_INSTANCES];
 
 	px4_sem_t	_lock; /**< lock to protect access to all class members (also for derived classes) */

src/modules/uORB/uORBDeviceNode.hpp

@@ -38,6 +38,7 @@
 
 #include <lib/cdev/CDev.hpp>
 
+#include <containers/IntrusiveSortedList.hpp>
 #include <containers/List.hpp>
 #include <px4_platform_common/atomic.h>
 
@@ -52,7 +53,7 @@ class SubscriptionCallback;
 /**
  * Per-object device instance.
  */
-class uORB::DeviceNode : public cdev::CDev, public ListNode<uORB::DeviceNode *>
+class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode<uORB::DeviceNode *>
 {
 public:
 	DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, ORB_PRIO priority,
@@ -65,6 +66,8 @@ public:
 	DeviceNode(DeviceNode &&) = delete;
 	DeviceNode &operator=(DeviceNode &&) = delete;
 
+	bool operator<=(const DeviceNode &rhs) const { return (strcmp(get_devname(), rhs.get_devname()) <= 0); }
+
 	/**
 	 * Method to create a subscriber instance and return the struct
 	 * pointing to the subscriber as a file pointer.

src/systemcmds/tests/CMakeLists.txt

@@ -50,6 +50,7 @@ set(srcs
 	test_jig_voltages.cpp
 	test_led.c
 	test_List.cpp
+	test_IntrusiveSortedList.cpp
 	test_mathlib.cpp
 	test_matrix.cpp
 	test_microbench_atomic.cpp

src/systemcmds/tests/test_IntrusiveSortedList.cpp

@@ -0,0 +1,285 @@
+/****************************************************************************
+ *
+ *  Copyright (C) 2020 PX4 Development Team. 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_IntrusiveSortedList.cpp
+ * Tests the IntrusiveSortedList container.
+ */
+
+#include <unit_test.h>
+#include <containers/IntrusiveSortedList.hpp>
+#include <float.h>
+#include <math.h>
+
+class testContainer : public IntrusiveSortedListNode<testContainer *>
+{
+public:
+	int i{0};
+
+	// sorted numerically
+	bool operator<=(const testContainer &rhs) const { return i <= rhs.i; }
+};
+
+class IntrusiveSortedListTest : public UnitTest
+{
+public:
+	virtual bool run_tests();
+
+	bool test_add();
+	bool test_remove();
+	bool test_range_based_for();
+	bool test_reinsert();
+
+};
+
+bool IntrusiveSortedListTest::run_tests()
+{
+	ut_run_test(test_add);
+	ut_run_test(test_remove);
+	ut_run_test(test_range_based_for);
+	ut_run_test(test_reinsert);
+
+	return (_tests_failed == 0);
+}
+
+bool IntrusiveSortedListTest::test_add()
+{
+	IntrusiveSortedList<testContainer *> list1;
+
+	// size should be 0 initially
+	ut_compare("size initially 0", list1.size(), 0);
+	ut_assert_true(list1.empty());
+
+	// insert 100
+	for (int i = 0; i < 100; i++) {
+		testContainer *t = new testContainer();
+		t->i = i;
+		list1.add(t);
+
+		ut_compare("size increasing with i", list1.size(), i + 1);
+		ut_assert_true(!list1.empty());
+	}
+
+	// verify full size (100)
+	ut_assert_true(list1.size() == 100);
+
+	int i = 0;
+
+	for (auto t : list1) {
+		// verify all elements were inserted in order
+		ut_compare("stored i", i, t->i);
+		i++;
+	}
+
+	// delete all elements
+	list1.clear();
+
+	// verify list has been cleared
+	ut_assert_true(list1.empty());
+	ut_compare("size 0", list1.size(), 0);
+
+	return true;
+}
+
+bool IntrusiveSortedListTest::test_remove()
+{
+	IntrusiveSortedList<testContainer *> list1;
+
+	// size should be 0 initially
+	ut_compare("size initially 0", list1.size(), 0);
+	ut_assert_true(list1.empty());
+
+	// insert 100
+	for (int i = 0; i < 100; i++) {
+		testContainer *t = new testContainer();
+		t->i = i;
+		list1.add(t);
+
+		ut_compare("size increasing with i", list1.size(), i + 1);
+		ut_assert_true(!list1.empty());
+	}
+
+	// verify full size (100)
+	ut_assert_true(list1.size() == 100);
+
+	// test removing elements
+	for (int remove_i = 0; remove_i < 100; remove_i++) {
+
+		// find node with i == remove_i
+		testContainer *removed = nullptr;
+
+		for (auto t : list1) {
+			if (t->i == remove_i) {
+				ut_assert_true(list1.remove(t));
+				removed = t;
+			}
+		}
+
+		delete removed;
+
+		// iterate list again to verify removal
+		for (auto t : list1) {
+			ut_assert_true(t->i != remove_i);
+		}
+
+		ut_assert_true(list1.size() == 100 - remove_i - 1);
+	}
+
+	// list should now be empty
+	ut_assert_true(list1.empty());
+	ut_compare("size 0", list1.size(), 0);
+
+	// delete all elements (should be safe on empty list)
+	list1.clear();
+
+	// verify list has been cleared
+	ut_assert_true(list1.empty());
+	ut_compare("size 0", list1.size(), 0);
+
+	return true;
+}
+
+bool IntrusiveSortedListTest::test_range_based_for()
+{
+	IntrusiveSortedList<testContainer *> list1;
+
+	// size should be 0 initially
+	ut_compare("size initially 0", list1.size(), 0);
+	ut_assert_true(list1.empty());
+
+	// insert 100 elements in order
+	for (int i = 99; i >= 0; i--) {
+		testContainer *t = new testContainer();
+		t->i = i;
+
+		list1.add(t);
+
+		ut_assert_true(!list1.empty());
+	}
+
+	// verify all elements are in order
+	int i = 0;
+
+	for (auto t1 : list1) {
+		ut_compare("check count", i, t1->i);
+		i++;
+	}
+
+	// verify full size (100)
+	ut_compare("size check", list1.size(), 100);
+
+	// delete all elements
+	list1.clear();
+
+	// verify list has been cleared
+	ut_assert_true(list1.empty());
+	ut_compare("size check", list1.size(), 0);
+
+	return true;
+}
+
+bool IntrusiveSortedListTest::test_reinsert()
+{
+	IntrusiveSortedList<testContainer *> l1;
+
+	// size should be 0 initially
+	ut_compare("size initially 0", l1.size(), 0);
+	ut_assert_true(l1.empty());
+
+	// insert 100
+	for (int i = 0; i < 100; i++) {
+		testContainer *t = new testContainer();
+		t->i = i;
+		l1.add(t);
+
+		ut_compare("size increasing with i", l1.size(), i + 1);
+		ut_assert_true(!l1.empty());
+	}
+
+	// verify full size (100)
+	ut_assert_true(l1.size() == 100);
+	ut_assert_false(l1.empty());
+
+	// test removing elements
+	for (int remove_i = 0; remove_i < 100; remove_i++) {
+
+		ut_assert_false(l1.empty());
+
+		// find node with i == remove_i
+		testContainer *removed = nullptr;
+
+		for (auto t : l1) {
+			if (t->i == remove_i) {
+				ut_assert_true(l1.remove(t));
+				removed = t;
+			}
+		}
+
+		// l1 shouldn't be empty until the very last iteration
+		ut_assert_false(l1.empty());
+
+		// iterate list again to verify removal
+		for (auto t : l1) {
+			ut_assert_true(t->i != remove_i);
+		}
+
+		// size temporarily reduced by 1
+		ut_assert_true(l1.size() == 100 - 1);
+
+		// now re-insert the removed node
+		const size_t sz1 = l1.size();
+		l1.add(removed);
+		const size_t sz2 = l1.size();
+
+		// verify the size increase
+		ut_assert_true(sz2 > sz1);
+
+		// size restored to 100
+		ut_assert_true(l1.size() == 100);
+	}
+
+	// queue shouldn't be empty
+	ut_assert_false(l1.empty());
+	ut_compare("size still 100", l1.size(), 100);
+
+	// delete all elements
+	l1.clear();
+
+	// verify list has been cleared
+	ut_assert_true(l1.empty());
+	ut_compare("size 0", l1.size(), 0);
+
+	return true;
+}
+
+ut_declare_test_c(test_IntrusiveSortedList, IntrusiveSortedListTest)

src/systemcmds/tests/tests_main.c

@@ -93,6 +93,7 @@ const struct {
 	{"i2c_spi_cli",		test_i2c_spi_cli,		0},
 	{"IntrusiveQueue",	test_IntrusiveQueue,	0},
 	{"jig_voltages",	test_jig_voltages,	OPT_NOALLTEST},
+	{"IntrusiveSortedList",	test_IntrusiveSortedList, 0},
 	{"List",		test_List,		0},
 	{"mathlib",		test_mathlib,		0},
 	{"matrix",		test_matrix,		0},

src/systemcmds/tests/tests_main.h

@@ -60,6 +60,7 @@ extern int test_i2c_spi_cli(int argc, char *argv[]);
 extern int test_IntrusiveQueue(int argc, char *argv[]);
 extern int test_jig_voltages(int argc, char *argv[]);
 extern int test_led(int argc, char *argv[]);
+extern int test_IntrusiveSortedList(int argc, char *argv[]);
 extern int test_List(int argc, char *argv[]);
 extern int test_mathlib(int argc, char *argv[]);
 extern int test_matrix(int argc, char *argv[]);