From b6a17a653828106fd199c4ebb9f5b1ef279a8384 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 4 Aug 2020 11:09:41 -0400 Subject: [PATCH] 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 --- .../px4_work_queue/WorkItem.hpp | 8 +- .../px4_work_queue/WorkQueue.hpp | 8 +- platforms/posix/cmake/sitl_tests.cmake | 1 + src/include/containers/BlockingList.hpp | 12 +- .../containers/IntrusiveSortedList.hpp | 187 ++++++++++++ src/modules/uORB/uORBDeviceMaster.hpp | 6 +- src/modules/uORB/uORBDeviceNode.hpp | 5 +- src/systemcmds/tests/CMakeLists.txt | 1 + .../tests/test_IntrusiveSortedList.cpp | 285 ++++++++++++++++++ src/systemcmds/tests/tests_main.c | 1 + src/systemcmds/tests/tests_main.h | 1 + 11 files changed, 502 insertions(+), 13 deletions(-) create mode 100644 src/include/containers/IntrusiveSortedList.hpp create mode 100644 src/systemcmds/tests/test_IntrusiveSortedList.cpp diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp index 9b9d5a26b7..58a073a9b0 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp @@ -37,15 +37,18 @@ #include "WorkQueue.hpp" #include +#include #include #include #include #include +#include + namespace px4 { -class WorkItem : public ListNode, public IntrusiveQueueNode +class WorkItem : public IntrusiveSortedListNode, public IntrusiveQueueNode { 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) { diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp index 7366b91048..019ee6821e 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp @@ -48,7 +48,7 @@ namespace px4 class WorkItem; -class WorkQueue : public ListNode +class WorkQueue : public IntrusiveSortedListNode { 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(); } diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake index 130556162a..ea7c0c497c 100644 --- a/platforms/posix/cmake/sitl_tests.cmake +++ b/platforms/posix/cmake/sitl_tests.cmake @@ -17,6 +17,7 @@ set(tests hrt int IntrusiveQueue + IntrusiveSortedList List mathlib matrix diff --git a/src/include/containers/BlockingList.hpp b/src/include/containers/BlockingList.hpp index a44207748c..160e93573f 100644 --- a/src/include/containers/BlockingList.hpp +++ b/src/include/containers/BlockingList.hpp @@ -39,14 +39,14 @@ #pragma once -#include "List.hpp" +#include "IntrusiveSortedList.hpp" #include "LockGuard.hpp" #include #include template -class BlockingList : public List +class BlockingList : public IntrusiveSortedList { public: @@ -59,25 +59,25 @@ public: void add(T newNode) { LockGuard lg{_mutex}; - List::add(newNode); + IntrusiveSortedList::add(newNode); } bool remove(T removeNode) { LockGuard lg{_mutex}; - return List::remove(removeNode); + return IntrusiveSortedList::remove(removeNode); } size_t size() { LockGuard lg{_mutex}; - return List::size(); + return IntrusiveSortedList::size(); } void clear() { LockGuard lg{_mutex}; - List::clear(); + IntrusiveSortedList::clear(); } pthread_mutex_t &mutex() { return _mutex; } diff --git a/src/include/containers/IntrusiveSortedList.hpp b/src/include/containers/IntrusiveSortedList.hpp new file mode 100644 index 0000000000..1997812a50 --- /dev/null +++ b/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 + +template +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 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}; +}; diff --git a/src/modules/uORB/uORBDeviceMaster.hpp b/src/modules/uORB/uORBDeviceMaster.hpp index 5484e38ffd..b696e979e3 100644 --- a/src/modules/uORB/uORBDeviceMaster.hpp +++ b/src/modules/uORB/uORBDeviceMaster.hpp @@ -50,7 +50,7 @@ class Manager; #include #include -#include +#include #include 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 _node_list; + IntrusiveSortedList _node_list; AtomicBitset _node_exists[ORB_MULTI_MAX_INSTANCES]; px4_sem_t _lock; /**< lock to protect access to all class members (also for derived classes) */ diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index 5707d1a202..0eb34bc36a 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -38,6 +38,7 @@ #include +#include #include #include @@ -52,7 +53,7 @@ class SubscriptionCallback; /** * Per-object device instance. */ -class uORB::DeviceNode : public cdev::CDev, public ListNode +class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode { 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. diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index adf10d3e24..be1f31262e 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/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 diff --git a/src/systemcmds/tests/test_IntrusiveSortedList.cpp b/src/systemcmds/tests/test_IntrusiveSortedList.cpp new file mode 100644 index 0000000000..e549e3a7d0 --- /dev/null +++ b/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 +#include +#include +#include + +class testContainer : public IntrusiveSortedListNode +{ +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 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 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 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 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) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index ffd2ad2372..fbfa260eb8 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/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}, diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h index a4efd50bc8..8b3d49756c 100644 --- a/src/systemcmds/tests/tests_main.h +++ b/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[]);