From 8cee5f78ca6951598ec49b9859c42b2004692ca3 Mon Sep 17 00:00:00 2001 From: Martin Mayer Date: Tue, 3 Jan 2023 19:46:58 +0100 Subject: [PATCH 01/17] added priority inheritance mutex + tests Signed-off-by: Martin Mayer --- CMakeLists.txt | 4 ++ include/rcpputils/mutex.hpp | 61 ++++++++++++++++++++ src/mutex.cpp | 80 ++++++++++++++++++++++++++ test/test_mutex.cpp | 108 ++++++++++++++++++++++++++++++++++++ 4 files changed, 253 insertions(+) create mode 100644 include/rcpputils/mutex.hpp create mode 100644 src/mutex.cpp create mode 100644 test/test_mutex.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b2a67720..6613becd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,6 +27,7 @@ add_library(${PROJECT_NAME} src/asserts.cpp src/filesystem_helper.cpp src/find_library.cpp + src/mutex.cpp src/env.cpp src/shared_library.cpp) target_include_directories(${PROJECT_NAME} PUBLIC @@ -73,6 +74,9 @@ if(BUILD_TESTING) ament_add_gtest(test_join test/test_join.cpp) target_link_libraries(test_join ${PROJECT_NAME}) + ament_add_gtest(test_mutex test/test_mutex.cpp) + target_link_libraries(test_mutex ${PROJECT_NAME}) + ament_add_gtest(test_time test/test_time.cpp) target_link_libraries(test_time ${PROJECT_NAME}) ament_target_dependencies(test_time rcutils) diff --git a/include/rcpputils/mutex.hpp b/include/rcpputils/mutex.hpp new file mode 100644 index 00000000..dab8da14 --- /dev/null +++ b/include/rcpputils/mutex.hpp @@ -0,0 +1,61 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCPPUTILS__MUTEX_HPP_ +#define RCPPUTILS__MUTEX_HPP_ + +#include + +#include "rcpputils/visibility_control.hpp" + +namespace rcpputils +{ + +/** + * Mutex with priority inheritance on systems that support it. + * This class derives from std::mutex to be fully compatible with standard C++. + * This implementation is a workaround that is needed until the C++ standard library offers the same mutex functionality. + **/ +class PIMutex : public std::mutex +{ +public: + /// Creates a mutex with priority inheritance enabled + RCPPUTILS_PUBLIC + PIMutex(); + + /// Releases the mutex + RCPPUTILS_PUBLIC + ~PIMutex(); +}; + +/** + * Recursive mutex with priority inheritance on systems that support it. + * This class derives from std::recursive_mutex to be fully compatible with standard C++. + * This implementation is a workaround that is needed until the C++ standard library offers the same mutex functionality. + **/ +class RecursivePIMutex : public std::recursive_mutex +{ +public: + /// Creates a recursive mutex with priority inheritance enabled + RCPPUTILS_PUBLIC + RecursivePIMutex(); + + /// Releases the mutex + RCPPUTILS_PUBLIC + ~RecursivePIMutex(); +}; + +} // namespace rcpputils + +#endif // RCPPUTILS__MUTEX_HPP_ diff --git a/src/mutex.cpp b/src/mutex.cpp new file mode 100644 index 00000000..44cbcfe1 --- /dev/null +++ b/src/mutex.cpp @@ -0,0 +1,80 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rcpputils/mutex.hpp" + +#ifndef _WIN32 +#include +#endif + +namespace rcpputils +{ + +#ifndef _WIN32 +PIMutex::PIMutex() +{ + // Destroy the underlying mutex + pthread_mutex_destroy(native_handle()); + + // Create mutex attribute with desired settings + pthread_mutexattr_t attr; + pthread_mutexattr_init(&attr); + pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT); + + // Initialize the underlying mutex + if (pthread_mutex_init(native_handle(), &attr) != 0) { + throw std::runtime_error("Mutex initialization failed."); + } + + // The attribute object isn't needed any more + pthread_mutexattr_destroy(&attr); +} + +RecursivePIMutex::RecursivePIMutex() +{ + // Destroy the underlying mutex + pthread_mutex_destroy(native_handle()); + + // Create mutex attribute with desired settings + pthread_mutexattr_t attr; + pthread_mutexattr_init(&attr); + pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT); + pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE); + + // Initialize the underlying mutex + if (pthread_mutex_init(native_handle(), &attr) != 0) { + throw std::runtime_error("Recursive mutex initialization failed."); + } + + // The attribute object isn't needed any more + pthread_mutexattr_destroy(&attr); +} +#else +PIMutex::PIMutex() +{ +} + +RecursivePIMutex::RecursivePIMutex() +{ +} +#endif // _WIN32 + +PIMutex::~PIMutex() +{ +} + +RecursivePIMutex::~RecursivePIMutex() +{ +} +} // namespace rcpputils diff --git a/test/test_mutex.cpp b/test/test_mutex.cpp new file mode 100644 index 00000000..78f23c4b --- /dev/null +++ b/test/test_mutex.cpp @@ -0,0 +1,108 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "rcpputils/mutex.hpp" + +using namespace std::chrono_literals; + +TEST(test_mutex, pimutex_trylock) { + rcpputils::PIMutex mutex; + mutex.lock(); + + EXPECT_FALSE(mutex.try_lock()); + + mutex.unlock(); +} + +TEST(test_mutex, rpimutex_trylock) { + rcpputils::RecursivePIMutex rmutex; + rmutex.lock(); + + EXPECT_TRUE(rmutex.try_lock()); + + rmutex.unlock(); + rmutex.unlock(); +} + +TEST(test_mutex, pimutex_trylocktwice) { + rcpputils::PIMutex mutex; + + EXPECT_TRUE(mutex.try_lock()); + EXPECT_FALSE(mutex.try_lock()); + + mutex.unlock(); +} + +TEST(test_mutex, rpimutex_trylocktwice) { + rcpputils::RecursivePIMutex rmutex; + + EXPECT_TRUE(rmutex.try_lock()); + EXPECT_TRUE(rmutex.try_lock()); + + rmutex.unlock(); + rmutex.unlock(); +} + +TEST(test_mutex, rpimutex_trylockthread) { + rcpputils::RecursivePIMutex test_rmutex; + std::atomic result {-1}; + test_rmutex.lock(); + + std::thread test_thread([&result, &test_rmutex]() { + result = test_rmutex.try_lock(); + }); + test_thread.join(); + EXPECT_FALSE(result); + + test_rmutex.unlock(); +} + +TEST(test_mutex, pimutex_lockthread) { + rcpputils::PIMutex test_mutex; + test_mutex.lock(); + std::atomic result {-1}; + + std::thread* test_thread = new std::thread([&result, &test_mutex]() { + result = 0; + test_mutex.lock(); + result = 1; // this line should not be reached + }); + std::this_thread::sleep_for(20ms); + test_thread->detach(); + delete test_thread; + EXPECT_EQ(0, result); + + test_mutex.unlock(); +} + +#ifndef _WIN32 + +#include + +TEST(test_mutex, pimutex_priority_inversion) { + // ToDo + EXPECT_FALSE(true); +} + +TEST(test_mutex, rpimutex_priority_inversion) { + // ToDo + EXPECT_FALSE(true); +} + +#endif // _WIN32 From b48991eefdefe88e4f7b3d066957e7545eaa9cee Mon Sep 17 00:00:00 2001 From: Martin Mayer Date: Tue, 3 Jan 2023 20:08:04 +0100 Subject: [PATCH 02/17] simplify testcase Signed-off-by: Martin Mayer --- test/test_mutex.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/test/test_mutex.cpp b/test/test_mutex.cpp index 78f23c4b..090495e8 100644 --- a/test/test_mutex.cpp +++ b/test/test_mutex.cpp @@ -78,14 +78,13 @@ TEST(test_mutex, pimutex_lockthread) { test_mutex.lock(); std::atomic result {-1}; - std::thread* test_thread = new std::thread([&result, &test_mutex]() { + std::thread test_thread([&result, &test_mutex]() { result = 0; test_mutex.lock(); result = 1; // this line should not be reached }); std::this_thread::sleep_for(20ms); - test_thread->detach(); - delete test_thread; + test_thread.detach(); EXPECT_EQ(0, result); test_mutex.unlock(); From f8bcfbeaf0a6361b862a9b03fa1e60b966dc5d8d Mon Sep 17 00:00:00 2001 From: Martin Mayer Date: Wed, 4 Jan 2023 16:58:31 +0100 Subject: [PATCH 03/17] added priority inversion test Signed-off-by: Martin Mayer --- include/rcpputils/thread_config.hpp | 159 ++++++++++++++++++++++++++++ test/test_mutex.cpp | 102 ++++++++++++++---- 2 files changed, 242 insertions(+), 19 deletions(-) create mode 100644 include/rcpputils/thread_config.hpp diff --git a/include/rcpputils/thread_config.hpp b/include/rcpputils/thread_config.hpp new file mode 100644 index 00000000..c3fa2866 --- /dev/null +++ b/include/rcpputils/thread_config.hpp @@ -0,0 +1,159 @@ +// Copyright (c) 2020 Robert Bosch GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCPPUTILS__THREAD_CONFIG_HPP_ +#define RCPPUTILS__THREAD_CONFIG_HPP_ + +#include + +#ifdef _WIN32 // i.e., Windows platform. +// #include +#elif __APPLE__ // i.e., macOS platform. +// #include +// #include +// #include +// #include +// #include +// #include +// #include +#else // POSIX platforms + #include + #ifdef __QNXNTO__ + #include + #include + #endif // __QNXNTO__ +#endif + +namespace rcpputils +{ + +/// Enum for simple configuration of threads in two priority classes. +enum class ThreadPriority +{ + LOW, + MEDIUM, + HIGH +}; + +/// Calculates an OS specific thread priority from a ThreadPriority value. +/** + * \param[in] thread_priority thread priority to be converted + * \param[out] os_priority OS specific thread priority + * \return true on systems that support POSIX + */ +inline bool calculate_os_thread_priority( + const ThreadPriority thread_priority, + int & os_priority) +{ +#ifdef _WIN32 + return false; +#elif __APPLE__ + return false; +#else + if (thread_priority == ThreadPriority::HIGH) { + os_priority = sched_get_priority_max(SCHED_FIFO); + } else if (thread_priority == ThreadPriority::LOW) { + os_priority = sched_get_priority_min(SCHED_FIFO); + } else if (thread_priority == ThreadPriority::MEDIUM) { + // Should be a value of 49 on standard Linux platforms, which is just below + // the default priority of 50 for threaded interrupt handling. + os_priority = + (sched_get_priority_min(SCHED_FIFO) + sched_get_priority_max(SCHED_FIFO)) / 2 - 1; + } else { // unhandled priority + return false; + } + return true; +#endif +} + +/// Sets the priority and cpu affinity of the given native thread. +/** + * This function intentionally only works on operating systems which support a FIFO thread scheduler. + * Note for Linux: using this function requires elevated privileges and a kernel with realtime patch. + * + * Implementation note: For setting thread priorities which are intended for a non-realtime/fair thread + * scheduler a new utility function should be implemented in order to not mix up different use cases. + * + * \param[in] native_handle native thread handle + * \param[in] priority priority to set for the given thread + * \param[in] cpu_bitmask cpu core bitmask for the given thread + * \return true on success + */ +template +bool configure_native_realtime_thread( + T native_handle, const ThreadPriority priority, + const unsigned int cpu_bitmask = (unsigned) -1) +{ + bool success = true; +#ifdef _WIN32 + return false; +#elif __APPLE__ + return false; +#else // POSIX systems + sched_param params; + int policy; + success &= (pthread_getschedparam(native_handle, &policy, ¶ms) == 0); + success &= calculate_os_thread_priority(priority, params.sched_priority); + success &= (pthread_setschedparam(native_handle, SCHED_FIFO, ¶ms) == 0); + +#ifdef __QNXNTO__ + // run_mask is a bit mask to set which cpu a thread runs on + // where each bit corresponds to a cpu core + int64_t run_mask = cpu_bitmask; + + // Function used to change thread affinity of thread associated with native_handle + if (ThreadCtlExt( + 0, native_handle, _NTO_TCTL_RUNMASK, + reinterpret_cast(run_mask)) == -1) + { + success &= 0; + } else { + success &= 1; + } +#else + cpu_set_t cpuset; + CPU_ZERO(&cpuset); + for (unsigned int i = 0; i < sizeof(cpu_bitmask) * 8; i++) { + if ( (cpu_bitmask & (1 << i)) != 0) { + CPU_SET(i, &cpuset); + } + } + success &= (pthread_setaffinity_np(native_handle, sizeof(cpu_set_t), &cpuset) == 0); +#endif // __QNXNTO__ +#endif + + return success; +} + +/// Sets the priority and cpu affinity of the given std thread. +/** + * This function intentionally only works on operating systems which support a FIFO thread scheduler. + * Note for Linux: using this function requires elevated privileges. + * Implementation note: For setting thread priorities which are intended for a non-realtime/fair thread scheduler a new utility function should be implemented. + * + * \param[in] thread std thread instance + * \param[in] priority priority to set for the given thread + * \param[in] cpu_bitmask cpu core bitmask for the given thread + * \return true on success + */ +inline bool configure_realtime_thread( + std::thread & thread, const ThreadPriority priority, + const unsigned int cpu_bitmask = (unsigned) -1) +{ + return configure_native_realtime_thread(thread.native_handle(), priority, cpu_bitmask); +} + +} // namespace rcpputils + +#endif // RCPPUTILS__THREAD_CONFIG_HPP_ diff --git a/test/test_mutex.cpp b/test/test_mutex.cpp index 090495e8..2c18b791 100644 --- a/test/test_mutex.cpp +++ b/test/test_mutex.cpp @@ -18,6 +18,7 @@ #include #include "rcpputils/mutex.hpp" +#include "rcpputils/thread_config.hpp" using namespace std::chrono_literals; @@ -49,14 +50,18 @@ TEST(test_mutex, pimutex_trylocktwice) { mutex.unlock(); } -TEST(test_mutex, rpimutex_trylocktwice) { +TEST(test_mutex, rpimutex_trylockmultiple) { rcpputils::RecursivePIMutex rmutex; - EXPECT_TRUE(rmutex.try_lock()); - EXPECT_TRUE(rmutex.try_lock()); + const unsigned count = 20; - rmutex.unlock(); - rmutex.unlock(); + for (unsigned i = 0; i < count; i++) { + EXPECT_TRUE(rmutex.try_lock()); + } + + for (unsigned i = 0; i < count; i++) { + rmutex.unlock(); + } } TEST(test_mutex, rpimutex_trylockthread) { @@ -81,27 +86,86 @@ TEST(test_mutex, pimutex_lockthread) { std::thread test_thread([&result, &test_mutex]() { result = 0; test_mutex.lock(); - result = 1; // this line should not be reached + result = 1; // this line should not be reached as long as the mutex is locked }); std::this_thread::sleep_for(20ms); - test_thread.detach(); EXPECT_EQ(0, result); test_mutex.unlock(); + test_thread.join(); } -#ifndef _WIN32 - -#include - TEST(test_mutex, pimutex_priority_inversion) { - // ToDo - EXPECT_FALSE(true); -} + rcpputils::PIMutex test_mutex; + std::atomic end_low_prio_thread {false}; + std::atomic end_medium_prio_thread {false}; + const unsigned int cpu_bitmask = 1; // allow only one cpu core to be used! -TEST(test_mutex, rpimutex_priority_inversion) { - // ToDo - EXPECT_FALSE(true); -} + // create low prio thread & take mutex + std::thread low_prio_thread([&test_mutex, &end_low_prio_thread]() { + test_mutex.lock(); + while (!end_low_prio_thread) + { + std::this_thread::sleep_for(1ms); + } + test_mutex.unlock(); + }); + if (rcpputils::configure_realtime_thread( + low_prio_thread, rcpputils::ThreadPriority::LOW, + cpu_bitmask) == false) + { + end_low_prio_thread = true; + low_prio_thread.join(); + std::cerr << "ThreadPriority::LOW could not be set. Skipping testcase.\n"; + GTEST_SKIP(); + return; + } + + // wait until mutex is taken by low prio thread + while (test_mutex.try_lock()) { + test_mutex.unlock(); + std::this_thread::sleep_for(1ms); + } + + // create high prio thread & take mutex + std::thread high_prio_thread([&test_mutex]() { + test_mutex.lock(); // this call will block initially + test_mutex.unlock(); + }); + EXPECT_TRUE( + rcpputils::configure_realtime_thread( + high_prio_thread, + rcpputils::ThreadPriority::HIGH, cpu_bitmask)) << "ThreadPriority::HIGH could not be set."; + + // create medium priority thread that would block the low prio thread + // if there is no priority inheritance + std::thread medium_prio_thread([&end_medium_prio_thread]() { + // create 100% workload on assigned cpu core + while (!end_medium_prio_thread) {} + }); + EXPECT_TRUE( + rcpputils::configure_realtime_thread( + medium_prio_thread, + rcpputils::ThreadPriority::MEDIUM, + cpu_bitmask)) << "ThreadPriority::MEDIUM could not be set."; + + // do the actual test; see if the low prio thread continues unblocked (with high prio) + end_low_prio_thread = true; + std::this_thread::sleep_for(20ms); -#endif // _WIN32 + // if priority inheritance worked the mutex should not be locked anymore + if(test_mutex.try_lock()) + { + test_mutex.unlock(); + } + else + { + FAIL() << "Mutex should not be locked anymore."; + } + + // cleanup + low_prio_thread.detach(); + high_prio_thread.detach(); + end_medium_prio_thread = true; + medium_prio_thread.join(); +} \ No newline at end of file From 814c4a5bf62bec10777988c0c178947c55254e6f Mon Sep 17 00:00:00 2001 From: Martin Mayer Date: Wed, 4 Jan 2023 17:01:10 +0100 Subject: [PATCH 04/17] code style fix Signed-off-by: Martin Mayer --- test/test_mutex.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/test/test_mutex.cpp b/test/test_mutex.cpp index 2c18b791..ea4bb790 100644 --- a/test/test_mutex.cpp +++ b/test/test_mutex.cpp @@ -104,8 +104,7 @@ TEST(test_mutex, pimutex_priority_inversion) { // create low prio thread & take mutex std::thread low_prio_thread([&test_mutex, &end_low_prio_thread]() { test_mutex.lock(); - while (!end_low_prio_thread) - { + while (!end_low_prio_thread) { std::this_thread::sleep_for(1ms); } test_mutex.unlock(); @@ -154,12 +153,9 @@ TEST(test_mutex, pimutex_priority_inversion) { std::this_thread::sleep_for(20ms); // if priority inheritance worked the mutex should not be locked anymore - if(test_mutex.try_lock()) - { + if (test_mutex.try_lock()) { test_mutex.unlock(); - } - else - { + } else { FAIL() << "Mutex should not be locked anymore."; } @@ -168,4 +164,4 @@ TEST(test_mutex, pimutex_priority_inversion) { high_prio_thread.detach(); end_medium_prio_thread = true; medium_prio_thread.join(); -} \ No newline at end of file +} From a51ac25911c13b298f131a2d4597e99e847ac032 Mon Sep 17 00:00:00 2001 From: Martin Mayer Date: Wed, 4 Jan 2023 17:43:25 +0100 Subject: [PATCH 05/17] fix for priority inversion testcase Signed-off-by: Martin Mayer --- test/test_mutex.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/test/test_mutex.cpp b/test/test_mutex.cpp index ea4bb790..8de140c6 100644 --- a/test/test_mutex.cpp +++ b/test/test_mutex.cpp @@ -148,15 +148,15 @@ TEST(test_mutex, pimutex_priority_inversion) { rcpputils::ThreadPriority::MEDIUM, cpu_bitmask)) << "ThreadPriority::MEDIUM could not be set."; - // do the actual test; see if the low prio thread continues unblocked (with high prio) + // do the actual test: see if the low prio thread gets unblocked (through high priority) end_low_prio_thread = true; std::this_thread::sleep_for(20ms); // if priority inheritance worked the mutex should not be locked anymore - if (test_mutex.try_lock()) { + bool try_lock_result = test_mutex.try_lock(); + EXPECT_TRUE(try_lock_result) << "Mutex should not be locked anymore."; + if (try_lock_result) { test_mutex.unlock(); - } else { - FAIL() << "Mutex should not be locked anymore."; } // cleanup From 027f5e87d5b74794508599c70010965529f4e12c Mon Sep 17 00:00:00 2001 From: Martin Mayer Date: Wed, 4 Jan 2023 18:38:03 +0100 Subject: [PATCH 06/17] moved thread functionality to rcutils Signed-off-by: Martin Mayer --- CMakeLists.txt | 2 +- include/rcpputils/thread.hpp | 47 ++++++++ include/rcpputils/thread_config.hpp | 159 ---------------------------- test/test_mutex.cpp | 12 +-- 4 files changed, 54 insertions(+), 166 deletions(-) create mode 100644 include/rcpputils/thread.hpp delete mode 100644 include/rcpputils/thread_config.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6613becd..a46776b4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,8 +27,8 @@ add_library(${PROJECT_NAME} src/asserts.cpp src/filesystem_helper.cpp src/find_library.cpp - src/mutex.cpp src/env.cpp + src/mutex.cpp src/shared_library.cpp) target_include_directories(${PROJECT_NAME} PUBLIC "$" diff --git a/include/rcpputils/thread.hpp b/include/rcpputils/thread.hpp new file mode 100644 index 00000000..3221f23c --- /dev/null +++ b/include/rcpputils/thread.hpp @@ -0,0 +1,47 @@ +// Copyright (c) 2020 Robert Bosch GmbH +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCPPUTILS__THREAD_CONFIG_HPP_ +#define RCPPUTILS__THREAD_CONFIG_HPP_ + +#include + +#include "rcutils/thread.h" +#include "rcpputils/visibility_control.hpp" + +namespace rcpputils +{ + +/// Sets the priority and cpu affinity of the given std thread. +/** + * This function intentionally only works on operating systems which support a FIFO thread scheduler. + * Note for Linux: using this function requires elevated privileges. + * Implementation note: For setting thread priorities which are intended for a non-realtime/fair thread scheduler a new utility function should be implemented. + * + * \param[in] thread std thread instance + * \param[in] priority priority to set for the given thread + * \param[in] cpu_bitmask cpu core bitmask for the given thread + * \return true on success + */ +RCPPUTILS_PUBLIC +bool configure_realtime_thread( + std::thread & thread, const ThreadPriority priority, + const unsigned int cpu_bitmask = (unsigned) -1) +{ + return configure_native_realtime_thread(thread.native_handle(), priority, cpu_bitmask); +} + +} // namespace rcpputils + +#endif // RCPPUTILS__THREAD_CONFIG_HPP_ diff --git a/include/rcpputils/thread_config.hpp b/include/rcpputils/thread_config.hpp deleted file mode 100644 index c3fa2866..00000000 --- a/include/rcpputils/thread_config.hpp +++ /dev/null @@ -1,159 +0,0 @@ -// Copyright (c) 2020 Robert Bosch GmbH -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RCPPUTILS__THREAD_CONFIG_HPP_ -#define RCPPUTILS__THREAD_CONFIG_HPP_ - -#include - -#ifdef _WIN32 // i.e., Windows platform. -// #include -#elif __APPLE__ // i.e., macOS platform. -// #include -// #include -// #include -// #include -// #include -// #include -// #include -#else // POSIX platforms - #include - #ifdef __QNXNTO__ - #include - #include - #endif // __QNXNTO__ -#endif - -namespace rcpputils -{ - -/// Enum for simple configuration of threads in two priority classes. -enum class ThreadPriority -{ - LOW, - MEDIUM, - HIGH -}; - -/// Calculates an OS specific thread priority from a ThreadPriority value. -/** - * \param[in] thread_priority thread priority to be converted - * \param[out] os_priority OS specific thread priority - * \return true on systems that support POSIX - */ -inline bool calculate_os_thread_priority( - const ThreadPriority thread_priority, - int & os_priority) -{ -#ifdef _WIN32 - return false; -#elif __APPLE__ - return false; -#else - if (thread_priority == ThreadPriority::HIGH) { - os_priority = sched_get_priority_max(SCHED_FIFO); - } else if (thread_priority == ThreadPriority::LOW) { - os_priority = sched_get_priority_min(SCHED_FIFO); - } else if (thread_priority == ThreadPriority::MEDIUM) { - // Should be a value of 49 on standard Linux platforms, which is just below - // the default priority of 50 for threaded interrupt handling. - os_priority = - (sched_get_priority_min(SCHED_FIFO) + sched_get_priority_max(SCHED_FIFO)) / 2 - 1; - } else { // unhandled priority - return false; - } - return true; -#endif -} - -/// Sets the priority and cpu affinity of the given native thread. -/** - * This function intentionally only works on operating systems which support a FIFO thread scheduler. - * Note for Linux: using this function requires elevated privileges and a kernel with realtime patch. - * - * Implementation note: For setting thread priorities which are intended for a non-realtime/fair thread - * scheduler a new utility function should be implemented in order to not mix up different use cases. - * - * \param[in] native_handle native thread handle - * \param[in] priority priority to set for the given thread - * \param[in] cpu_bitmask cpu core bitmask for the given thread - * \return true on success - */ -template -bool configure_native_realtime_thread( - T native_handle, const ThreadPriority priority, - const unsigned int cpu_bitmask = (unsigned) -1) -{ - bool success = true; -#ifdef _WIN32 - return false; -#elif __APPLE__ - return false; -#else // POSIX systems - sched_param params; - int policy; - success &= (pthread_getschedparam(native_handle, &policy, ¶ms) == 0); - success &= calculate_os_thread_priority(priority, params.sched_priority); - success &= (pthread_setschedparam(native_handle, SCHED_FIFO, ¶ms) == 0); - -#ifdef __QNXNTO__ - // run_mask is a bit mask to set which cpu a thread runs on - // where each bit corresponds to a cpu core - int64_t run_mask = cpu_bitmask; - - // Function used to change thread affinity of thread associated with native_handle - if (ThreadCtlExt( - 0, native_handle, _NTO_TCTL_RUNMASK, - reinterpret_cast(run_mask)) == -1) - { - success &= 0; - } else { - success &= 1; - } -#else - cpu_set_t cpuset; - CPU_ZERO(&cpuset); - for (unsigned int i = 0; i < sizeof(cpu_bitmask) * 8; i++) { - if ( (cpu_bitmask & (1 << i)) != 0) { - CPU_SET(i, &cpuset); - } - } - success &= (pthread_setaffinity_np(native_handle, sizeof(cpu_set_t), &cpuset) == 0); -#endif // __QNXNTO__ -#endif - - return success; -} - -/// Sets the priority and cpu affinity of the given std thread. -/** - * This function intentionally only works on operating systems which support a FIFO thread scheduler. - * Note for Linux: using this function requires elevated privileges. - * Implementation note: For setting thread priorities which are intended for a non-realtime/fair thread scheduler a new utility function should be implemented. - * - * \param[in] thread std thread instance - * \param[in] priority priority to set for the given thread - * \param[in] cpu_bitmask cpu core bitmask for the given thread - * \return true on success - */ -inline bool configure_realtime_thread( - std::thread & thread, const ThreadPriority priority, - const unsigned int cpu_bitmask = (unsigned) -1) -{ - return configure_native_realtime_thread(thread.native_handle(), priority, cpu_bitmask); -} - -} // namespace rcpputils - -#endif // RCPPUTILS__THREAD_CONFIG_HPP_ diff --git a/test/test_mutex.cpp b/test/test_mutex.cpp index 8de140c6..a32aaf3f 100644 --- a/test/test_mutex.cpp +++ b/test/test_mutex.cpp @@ -18,7 +18,7 @@ #include #include "rcpputils/mutex.hpp" -#include "rcpputils/thread_config.hpp" +#include "rcpputils/thread.hpp" using namespace std::chrono_literals; @@ -110,12 +110,12 @@ TEST(test_mutex, pimutex_priority_inversion) { test_mutex.unlock(); }); if (rcpputils::configure_realtime_thread( - low_prio_thread, rcpputils::ThreadPriority::LOW, + low_prio_thread, THREAD_PRIORITY_LOW, cpu_bitmask) == false) { end_low_prio_thread = true; low_prio_thread.join(); - std::cerr << "ThreadPriority::LOW could not be set. Skipping testcase.\n"; + std::cerr << "THREAD_PRIORITY_LOW could not be set. Skipping testcase.\n"; GTEST_SKIP(); return; } @@ -134,7 +134,7 @@ TEST(test_mutex, pimutex_priority_inversion) { EXPECT_TRUE( rcpputils::configure_realtime_thread( high_prio_thread, - rcpputils::ThreadPriority::HIGH, cpu_bitmask)) << "ThreadPriority::HIGH could not be set."; + THREAD_PRIORITY_HIGH, cpu_bitmask)) << "THREAD_PRIORITY_HIGH could not be set."; // create medium priority thread that would block the low prio thread // if there is no priority inheritance @@ -145,8 +145,8 @@ TEST(test_mutex, pimutex_priority_inversion) { EXPECT_TRUE( rcpputils::configure_realtime_thread( medium_prio_thread, - rcpputils::ThreadPriority::MEDIUM, - cpu_bitmask)) << "ThreadPriority::MEDIUM could not be set."; + THREAD_PRIORITY_MEDIUM, + cpu_bitmask)) << "THREAD_PRIORITY_MEDIUM could not be set."; // do the actual test: see if the low prio thread gets unblocked (through high priority) end_low_prio_thread = true; From 01bd5a8284d46ccc8d8cbf1d410f74ca7d685b79 Mon Sep 17 00:00:00 2001 From: Martin Mayer Date: Wed, 4 Jan 2023 19:18:26 +0100 Subject: [PATCH 07/17] modified return type Signed-off-by: Martin Mayer --- include/rcpputils/thread.hpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/include/rcpputils/thread.hpp b/include/rcpputils/thread.hpp index 3221f23c..0ffc9252 100644 --- a/include/rcpputils/thread.hpp +++ b/include/rcpputils/thread.hpp @@ -27,7 +27,8 @@ namespace rcpputils /** * This function intentionally only works on operating systems which support a FIFO thread scheduler. * Note for Linux: using this function requires elevated privileges. - * Implementation note: For setting thread priorities which are intended for a non-realtime/fair thread scheduler a new utility function should be implemented. + * Implementation note: For setting thread priorities which are intended for a non-realtime/fair + * thread scheduler a new utility function should be implemented. * * \param[in] thread std thread instance * \param[in] priority priority to set for the given thread @@ -39,7 +40,9 @@ bool configure_realtime_thread( std::thread & thread, const ThreadPriority priority, const unsigned int cpu_bitmask = (unsigned) -1) { - return configure_native_realtime_thread(thread.native_handle(), priority, cpu_bitmask); + rcutils_ret_t return_value = configure_native_realtime_thread(thread.native_handle(), + priority, cpu_bitmask); + return return_value == RCUTILS_RET_OK ? true : false; } } // namespace rcpputils From b1114003eb7b492b9a2560e0bf6f7cc4e3948d36 Mon Sep 17 00:00:00 2001 From: Martin Mayer Date: Wed, 4 Jan 2023 19:21:19 +0100 Subject: [PATCH 08/17] comment change Signed-off-by: Martin Mayer --- test/test_mutex.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/test_mutex.cpp b/test/test_mutex.cpp index a32aaf3f..18643f50 100644 --- a/test/test_mutex.cpp +++ b/test/test_mutex.cpp @@ -148,7 +148,7 @@ TEST(test_mutex, pimutex_priority_inversion) { THREAD_PRIORITY_MEDIUM, cpu_bitmask)) << "THREAD_PRIORITY_MEDIUM could not be set."; - // do the actual test: see if the low prio thread gets unblocked (through high priority) + // do the actual test: see if the low prio thread gets unblocked (through priority inheritance) end_low_prio_thread = true; std::this_thread::sleep_for(20ms); From aee9d8d4f49d3fa5cf7dcec30280664e390c7e36 Mon Sep 17 00:00:00 2001 From: Martin Mayer Date: Wed, 4 Jan 2023 19:27:56 +0100 Subject: [PATCH 09/17] style fix Signed-off-by: Martin Mayer --- include/rcpputils/thread.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/include/rcpputils/thread.hpp b/include/rcpputils/thread.hpp index 0ffc9252..958bc4ea 100644 --- a/include/rcpputils/thread.hpp +++ b/include/rcpputils/thread.hpp @@ -40,8 +40,9 @@ bool configure_realtime_thread( std::thread & thread, const ThreadPriority priority, const unsigned int cpu_bitmask = (unsigned) -1) { - rcutils_ret_t return_value = configure_native_realtime_thread(thread.native_handle(), - priority, cpu_bitmask); + rcutils_ret_t return_value = configure_native_realtime_thread( + thread.native_handle(), + priority, cpu_bitmask); return return_value == RCUTILS_RET_OK ? true : false; } From c56cf12a328f8f4965f845a85b93d697e98848f7 Mon Sep 17 00:00:00 2001 From: Martin Mayer Date: Wed, 4 Jan 2023 19:38:48 +0100 Subject: [PATCH 10/17] comment Signed-off-by: Martin Mayer --- include/rcpputils/thread.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/rcpputils/thread.hpp b/include/rcpputils/thread.hpp index 958bc4ea..1256b85a 100644 --- a/include/rcpputils/thread.hpp +++ b/include/rcpputils/thread.hpp @@ -23,7 +23,7 @@ namespace rcpputils { -/// Sets the priority and cpu affinity of the given std thread. +/// Sets a realtime thread priority and a cpu affinity for the given std thread. /** * This function intentionally only works on operating systems which support a FIFO thread scheduler. * Note for Linux: using this function requires elevated privileges. From 0cbf3a3be862a3dd87e09dab3a5d8a9de556445a Mon Sep 17 00:00:00 2001 From: Martin Mayer Date: Thu, 5 Jan 2023 11:36:03 +0100 Subject: [PATCH 11/17] reworked rcpputils unittest Signed-off-by: Martin Mayer --- test/test_mutex.cpp | 51 +++++++++++++++++++++++++++++++++++++-------- 1 file changed, 42 insertions(+), 9 deletions(-) diff --git a/test/test_mutex.cpp b/test/test_mutex.cpp index 18643f50..fb821f89 100644 --- a/test/test_mutex.cpp +++ b/test/test_mutex.cpp @@ -88,26 +88,30 @@ TEST(test_mutex, pimutex_lockthread) { test_mutex.lock(); result = 1; // this line should not be reached as long as the mutex is locked }); - std::this_thread::sleep_for(20ms); + std::this_thread::sleep_for(10ms); EXPECT_EQ(0, result); test_mutex.unlock(); test_thread.join(); } -TEST(test_mutex, pimutex_priority_inversion) { - rcpputils::PIMutex test_mutex; +template +void priority_inheritance_test() { + MutexClass test_mutex; std::atomic end_low_prio_thread {false}; std::atomic end_medium_prio_thread {false}; const unsigned int cpu_bitmask = 1; // allow only one cpu core to be used! // create low prio thread & take mutex std::thread low_prio_thread([&test_mutex, &end_low_prio_thread]() { + std::cout << "Low prio thread starts.\n" << std::flush; test_mutex.lock(); + std::cout << "Low prio thread locked.\n" << std::flush; while (!end_low_prio_thread) { std::this_thread::sleep_for(1ms); } test_mutex.unlock(); + std::cout << "Low prio thread unlocked and ends.\n" << std::flush; }); if (rcpputils::configure_realtime_thread( low_prio_thread, THREAD_PRIORITY_LOW, @@ -125,11 +129,15 @@ TEST(test_mutex, pimutex_priority_inversion) { test_mutex.unlock(); std::this_thread::sleep_for(1ms); } + std::cout << "Test thread try_lock failed as expected.\n" << std::flush; // create high prio thread & take mutex std::thread high_prio_thread([&test_mutex]() { + std::cout << "High prio thread starts.\n" << std::flush; test_mutex.lock(); // this call will block initially + std::cout << "High prio thread locked.\n" << std::flush; test_mutex.unlock(); + std::cout << "High prio thread unlocked and ends.\n" << std::flush; }); EXPECT_TRUE( rcpputils::configure_realtime_thread( @@ -138,30 +146,55 @@ TEST(test_mutex, pimutex_priority_inversion) { // create medium priority thread that would block the low prio thread // if there is no priority inheritance - std::thread medium_prio_thread([&end_medium_prio_thread]() { + std::atomic medium_thread_started {false}; + std::thread medium_prio_thread([&end_medium_prio_thread, &medium_thread_started]() { + std::cout << "Medium prio thread starts.\n" << std::flush; + medium_thread_started = true; // create 100% workload on assigned cpu core while (!end_medium_prio_thread) {} + std::cout << "Medium prio thread ends.\n" << std::flush; }); EXPECT_TRUE( rcpputils::configure_realtime_thread( medium_prio_thread, THREAD_PRIORITY_MEDIUM, cpu_bitmask)) << "THREAD_PRIORITY_MEDIUM could not be set."; + while (medium_thread_started == false) { + std::this_thread::sleep_for(1ms); + } // do the actual test: see if the low prio thread gets unblocked (through priority inheritance) + std::cout << "Signalling end low prio thread.\n" << std::flush; end_low_prio_thread = true; - std::this_thread::sleep_for(20ms); // if priority inheritance worked the mutex should not be locked anymore - bool try_lock_result = test_mutex.try_lock(); - EXPECT_TRUE(try_lock_result) << "Mutex should not be locked anymore."; + bool try_lock_result; + int count = 0; + while((try_lock_result = test_mutex.try_lock()) == false) + { + std::this_thread::sleep_for(1ms); + if(count++ >= 20) + { + EXPECT_TRUE(try_lock_result) << "Mutex should not be locked anymore."; + break; + } + } if (try_lock_result) { test_mutex.unlock(); } // cleanup - low_prio_thread.detach(); - high_prio_thread.detach(); + std::cout << "Signalling end medium prio thread.\n" << std::flush; end_medium_prio_thread = true; medium_prio_thread.join(); + high_prio_thread.join(); + low_prio_thread.join(); +} + +TEST(test_mutex, pimutex_priority_inversion) { + priority_inheritance_test(); +} + +TEST(test_mutex, rpimutex_priority_inversion) { + priority_inheritance_test(); } From d455916c60c0d018c3a4d664c51fa1252d0300fa Mon Sep 17 00:00:00 2001 From: Martin Mayer Date: Thu, 5 Jan 2023 11:37:59 +0100 Subject: [PATCH 12/17] uncrustify Signed-off-by: Martin Mayer --- test/test_mutex.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/test/test_mutex.cpp b/test/test_mutex.cpp index fb821f89..b42dce34 100644 --- a/test/test_mutex.cpp +++ b/test/test_mutex.cpp @@ -95,8 +95,9 @@ TEST(test_mutex, pimutex_lockthread) { test_thread.join(); } -template -void priority_inheritance_test() { +template +void priority_inheritance_test() +{ MutexClass test_mutex; std::atomic end_low_prio_thread {false}; std::atomic end_medium_prio_thread {false}; @@ -170,11 +171,9 @@ void priority_inheritance_test() { // if priority inheritance worked the mutex should not be locked anymore bool try_lock_result; int count = 0; - while((try_lock_result = test_mutex.try_lock()) == false) - { + while ((try_lock_result = test_mutex.try_lock()) == false) { std::this_thread::sleep_for(1ms); - if(count++ >= 20) - { + if (count++ >= 20) { EXPECT_TRUE(try_lock_result) << "Mutex should not be locked anymore."; break; } From 4e81d5879aade7cc198cee7deb7fa6a934e1b47b Mon Sep 17 00:00:00 2001 From: Martin Mayer Date: Fri, 27 Jan 2023 13:28:26 +0100 Subject: [PATCH 13/17] Thread configuration now part of the test implementation Signed-off-by: Martin Mayer --- include/rcpputils/thread.hpp | 51 ------------------- test/test_mutex.cpp | 97 ++++++++++++++++++++++++++++++++++-- 2 files changed, 93 insertions(+), 55 deletions(-) delete mode 100644 include/rcpputils/thread.hpp diff --git a/include/rcpputils/thread.hpp b/include/rcpputils/thread.hpp deleted file mode 100644 index 1256b85a..00000000 --- a/include/rcpputils/thread.hpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright (c) 2020 Robert Bosch GmbH -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RCPPUTILS__THREAD_CONFIG_HPP_ -#define RCPPUTILS__THREAD_CONFIG_HPP_ - -#include - -#include "rcutils/thread.h" -#include "rcpputils/visibility_control.hpp" - -namespace rcpputils -{ - -/// Sets a realtime thread priority and a cpu affinity for the given std thread. -/** - * This function intentionally only works on operating systems which support a FIFO thread scheduler. - * Note for Linux: using this function requires elevated privileges. - * Implementation note: For setting thread priorities which are intended for a non-realtime/fair - * thread scheduler a new utility function should be implemented. - * - * \param[in] thread std thread instance - * \param[in] priority priority to set for the given thread - * \param[in] cpu_bitmask cpu core bitmask for the given thread - * \return true on success - */ -RCPPUTILS_PUBLIC -bool configure_realtime_thread( - std::thread & thread, const ThreadPriority priority, - const unsigned int cpu_bitmask = (unsigned) -1) -{ - rcutils_ret_t return_value = configure_native_realtime_thread( - thread.native_handle(), - priority, cpu_bitmask); - return return_value == RCUTILS_RET_OK ? true : false; -} - -} // namespace rcpputils - -#endif // RCPPUTILS__THREAD_CONFIG_HPP_ diff --git a/test/test_mutex.cpp b/test/test_mutex.cpp index b42dce34..399a203d 100644 --- a/test/test_mutex.cpp +++ b/test/test_mutex.cpp @@ -18,7 +18,11 @@ #include #include "rcpputils/mutex.hpp" -#include "rcpputils/thread.hpp" + +#ifdef __linux__ +#include +#include "rcutils/types/rcutils_ret.h" +#endif // __linux__ using namespace std::chrono_literals; @@ -95,6 +99,89 @@ TEST(test_mutex, pimutex_lockthread) { test_thread.join(); } +#ifdef __linux__ +// +// The test cases pimutex_priority_inversion & rpimutex_priority_inversion provoke +// a thread priority inversion. To do so they need to configure the cpu priority, +// scheduler type and cpu core affinity for a thread. Since this functionality is +// not part of ROS2 yet the necessary functionality is implemented here for the +// purpose of implementing these tests. Moreover, the required functionality is +// OS specific and the current implementation is for RT Linux only. Feel free +// to extend the implementation for other realtime capable operating systems, +// like VxWorks and QNX. +// +// Note: for RT Linux elevated user rights are needed for the process. +// + +/// Enum for OS independent thread priorities +enum ThreadPriority +{ + THREAD_PRIORITY_LOWEST, + THREAD_PRIORITY_LOW, + THREAD_PRIORITY_MEDIUM, + THREAD_PRIORITY_HIGH, + THREAD_PRIORITY_HIGHEST +}; + +rcutils_ret_t calculate_os_fifo_thread_priority( + const int thread_priority, + int * os_priority) +{ + if (thread_priority > THREAD_PRIORITY_HIGHEST || thread_priority < THREAD_PRIORITY_LOWEST) { + return RCUTILS_RET_ERROR; + } + const int max_prio = sched_get_priority_max(SCHED_FIFO); + const int min_prio = sched_get_priority_min(SCHED_FIFO); + const int range_prio = max_prio - min_prio; + + int priority = min_prio + (thread_priority - THREAD_PRIORITY_LOWEST) * + range_prio / (THREAD_PRIORITY_HIGHEST - THREAD_PRIORITY_LOWEST); + if (priority > min_prio && priority < max_prio) { + // on Linux systems THREAD_PRIORITY_MEDIUM should be prio 49 instead of 50 + // in order to not block any interrupt handlers + priority--; + } + + *os_priority = priority; + + return RCUTILS_RET_OK; +} + +rcutils_ret_t configure_native_realtime_thread( + unsigned long int native_handle, const int priority, // NOLINT + const unsigned int cpu_bitmask) +{ + int success = 1; + + struct sched_param params; + int policy; + success &= (pthread_getschedparam(native_handle, &policy, ¶ms) == 0); + success &= (calculate_os_fifo_thread_priority(priority, ¶ms.sched_priority) == + RCUTILS_RET_OK ? 1 : 0); + success &= (pthread_setschedparam(native_handle, SCHED_FIFO, ¶ms) == 0); + + cpu_set_t cpuset; + CPU_ZERO(&cpuset); + for (unsigned int i = 0; i < sizeof(cpu_bitmask) * 8; i++) { + if ( (cpu_bitmask & (1 << i)) != 0) { + CPU_SET(i, &cpuset); + } + } + success &= (pthread_setaffinity_np(native_handle, sizeof(cpu_set_t), &cpuset) == 0); + + return success ? RCUTILS_RET_OK : RCUTILS_RET_ERROR; +} + +bool configure_realtime_thread( + std::thread & thread, const ThreadPriority priority, + const unsigned int cpu_bitmask = (unsigned) -1) +{ + rcutils_ret_t return_value = configure_native_realtime_thread( + thread.native_handle(), + priority, cpu_bitmask); + return return_value == RCUTILS_RET_OK ? true : false; +} + template void priority_inheritance_test() { @@ -114,7 +201,7 @@ void priority_inheritance_test() test_mutex.unlock(); std::cout << "Low prio thread unlocked and ends.\n" << std::flush; }); - if (rcpputils::configure_realtime_thread( + if (configure_realtime_thread( low_prio_thread, THREAD_PRIORITY_LOW, cpu_bitmask) == false) { @@ -141,7 +228,7 @@ void priority_inheritance_test() std::cout << "High prio thread unlocked and ends.\n" << std::flush; }); EXPECT_TRUE( - rcpputils::configure_realtime_thread( + configure_realtime_thread( high_prio_thread, THREAD_PRIORITY_HIGH, cpu_bitmask)) << "THREAD_PRIORITY_HIGH could not be set."; @@ -156,7 +243,7 @@ void priority_inheritance_test() std::cout << "Medium prio thread ends.\n" << std::flush; }); EXPECT_TRUE( - rcpputils::configure_realtime_thread( + configure_realtime_thread( medium_prio_thread, THREAD_PRIORITY_MEDIUM, cpu_bitmask)) << "THREAD_PRIORITY_MEDIUM could not be set."; @@ -197,3 +284,5 @@ TEST(test_mutex, pimutex_priority_inversion) { TEST(test_mutex, rpimutex_priority_inversion) { priority_inheritance_test(); } + +#endif // __linux__ From 54db1d31c3bff2fea60de14d0ebc0826c2ca010e Mon Sep 17 00:00:00 2001 From: Martin Mayer Date: Fri, 17 Feb 2023 11:31:09 +0100 Subject: [PATCH 14/17] added CMake option USE_PI_MUTEX Signed-off-by: Martin Mayer --- CMakeLists.txt | 10 ++++++++++ Doxyfile | 1 + README.md | 1 + include/rcpputils/mutex.hpp | 15 +++++++++++---- src/mutex.cpp | 18 +++++------------- test/test_mutex.cpp | 27 ++++++++++++++++++++++----- 6 files changed, 50 insertions(+), 22 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a46776b4..852c8801 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,6 +37,16 @@ if(WIN32) target_compile_definitions(${PROJECT_NAME} PRIVATE "RCPPUTILS_BUILDING_LIBRARY") endif() + +option(USE_PI_MUTEX "Enables priority inheritance mutexes." ON) +if(USE_PI_MUTEX) + if(WIN32 OR APPLE) + message("Mutexes with priority inheritance are not supported on your system. Using std mutexes.") + else() + target_compile_definitions(${PROJECT_NAME} PUBLIC "RCPPUTILS_USE_PIMUTEX") + endif() +endif() + ament_target_dependencies(${PROJECT_NAME} rcutils) # Export old-style CMake variables diff --git a/Doxyfile b/Doxyfile index 06309b15..bdea43dc 100644 --- a/Doxyfile +++ b/Doxyfile @@ -23,6 +23,7 @@ EXPAND_ONLY_PREDEF = YES PREDEFINED += __declspec(x)= PREDEFINED += RCPPUTILS_PUBLIC= PREDEFINED += RCPPUTILS_PUBLIC_TYPE= +PREDEFINED += RCPPUTILS_USE_PIMUTEX # Tag files that do not exist will produce a warning and cross-project linking will not work. TAGFILES += "../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" diff --git a/README.md b/README.md index 8340ed35..5d61c64c 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,7 @@ This package currently contains: * String helpers * File system helpers * Type traits helpers +* Mutex classes that support thread priority inheritance * Class that dynamically loads, unloads and get symbols from shared libraries at run-time. Features are described in more detail at [docs/FEATURES.md](docs/FEATURES.md) diff --git a/include/rcpputils/mutex.hpp b/include/rcpputils/mutex.hpp index dab8da14..a024afcf 100644 --- a/include/rcpputils/mutex.hpp +++ b/include/rcpputils/mutex.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Open Source Robotics Foundation, Inc. +// Copyright 2023 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,11 +21,18 @@ namespace rcpputils { +#ifndef RCPPUTILS_USE_PIMUTEX + +// Fallback code path +using PIMutex = std::mutex; +using RecursivePIMutex = std::recursive_mutex; + +#else /** * Mutex with priority inheritance on systems that support it. * This class derives from std::mutex to be fully compatible with standard C++. - * This implementation is a workaround that is needed until the C++ standard library offers the same mutex functionality. + * This implementation is needed because the C++ standard library doesn't support priority inheritance. **/ class PIMutex : public std::mutex { @@ -42,7 +49,7 @@ class PIMutex : public std::mutex /** * Recursive mutex with priority inheritance on systems that support it. * This class derives from std::recursive_mutex to be fully compatible with standard C++. - * This implementation is a workaround that is needed until the C++ standard library offers the same mutex functionality. + * This implementation is needed because the C++ standard library doesn't support priority inheritance. **/ class RecursivePIMutex : public std::recursive_mutex { @@ -56,6 +63,6 @@ class RecursivePIMutex : public std::recursive_mutex ~RecursivePIMutex(); }; +#endif // RCPPUTILS_USE_PIMUTEX } // namespace rcpputils - #endif // RCPPUTILS__MUTEX_HPP_ diff --git a/src/mutex.cpp b/src/mutex.cpp index 44cbcfe1..7eafec12 100644 --- a/src/mutex.cpp +++ b/src/mutex.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Open Source Robotics Foundation, Inc. +// Copyright 2023 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,14 +14,13 @@ #include "rcpputils/mutex.hpp" -#ifndef _WIN32 +#ifdef RCPPUTILS_USE_PIMUTEX + #include -#endif namespace rcpputils { -#ifndef _WIN32 PIMutex::PIMutex() { // Destroy the underlying mutex @@ -60,15 +59,6 @@ RecursivePIMutex::RecursivePIMutex() // The attribute object isn't needed any more pthread_mutexattr_destroy(&attr); } -#else -PIMutex::PIMutex() -{ -} - -RecursivePIMutex::RecursivePIMutex() -{ -} -#endif // _WIN32 PIMutex::~PIMutex() { @@ -77,4 +67,6 @@ PIMutex::~PIMutex() RecursivePIMutex::~RecursivePIMutex() { } + } // namespace rcpputils +#endif // RCPPUTILS_USE_PIMUTEX diff --git a/test/test_mutex.cpp b/test/test_mutex.cpp index 399a203d..602a8b70 100644 --- a/test/test_mutex.cpp +++ b/test/test_mutex.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2023 Open Source Robotics Foundation, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -19,10 +19,14 @@ #include "rcpputils/mutex.hpp" -#ifdef __linux__ +#ifdef RCPPUTILS_USE_PIMUTEX #include #include "rcutils/types/rcutils_ret.h" -#endif // __linux__ +#ifdef __QNXNTO__ + #include + #include +#endif // __QNXNTO__ +#endif // RCPPUTILS_USE_PIMUTEX using namespace std::chrono_literals; @@ -99,7 +103,7 @@ TEST(test_mutex, pimutex_lockthread) { test_thread.join(); } -#ifdef __linux__ +#ifdef RCPPUTILS_USE_PIMUTEX // // The test cases pimutex_priority_inversion & rpimutex_priority_inversion provoke // a thread priority inversion. To do so they need to configure the cpu priority, @@ -160,6 +164,16 @@ rcutils_ret_t configure_native_realtime_thread( RCUTILS_RET_OK ? 1 : 0); success &= (pthread_setschedparam(native_handle, SCHED_FIFO, ¶ms) == 0); +#ifdef __QNXNTO__ + // run_mask is a bit mask to set which cpu a thread runs on + // where each bit corresponds to a cpu core + int64_t run_mask = cpu_bitmask; + + // change thread affinity of thread associated with native_handle + success &= (ThreadCtlExt( + 0, native_handle, _NTO_TCTL_RUNMASK, + reinterpret_cast(run_mask)) != -1); +#else // __QNXNTO__ cpu_set_t cpuset; CPU_ZERO(&cpuset); for (unsigned int i = 0; i < sizeof(cpu_bitmask) * 8; i++) { @@ -167,7 +181,10 @@ rcutils_ret_t configure_native_realtime_thread( CPU_SET(i, &cpuset); } } + + // change thread affinity of thread associated with native_handle success &= (pthread_setaffinity_np(native_handle, sizeof(cpu_set_t), &cpuset) == 0); +#endif // __QNXNTO__ return success ? RCUTILS_RET_OK : RCUTILS_RET_ERROR; } @@ -285,4 +302,4 @@ TEST(test_mutex, rpimutex_priority_inversion) { priority_inheritance_test(); } -#endif // __linux__ +#endif // RCPPUTILS_USE_PIMUTEX From 92cf9dc603dfe3efa6a804415366e2a14bdbcdbf Mon Sep 17 00:00:00 2001 From: Martin Mayer Date: Sat, 18 Feb 2023 11:02:18 +0100 Subject: [PATCH 15/17] changed OS detection in CMakeLists.txt Signed-off-by: Martin Mayer --- CMakeLists.txt | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 852c8801..757a416d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,10 +40,12 @@ endif() option(USE_PI_MUTEX "Enables priority inheritance mutexes." ON) if(USE_PI_MUTEX) - if(WIN32 OR APPLE) - message("Mutexes with priority inheritance are not supported on your system. Using std mutexes.") - else() + if(CMAKE_SYSTEM_NAME STREQUAL Linux OR + CMAKE_SYSTEM_NAME STREQUAL VxWorks OR + CMAKE_SYSTEM_NAME STREQUAL QNX) target_compile_definitions(${PROJECT_NAME} PUBLIC "RCPPUTILS_USE_PIMUTEX") + else() + message("Mutexes with priority inheritance are not supported on your system. Using std mutexes.") endif() endif() From 35855fc7eb8120e291db45e0c249d5d6492d4dbe Mon Sep 17 00:00:00 2001 From: WideAwakeTN <30844488+WideAwakeTN@users.noreply.github.com> Date: Sat, 18 Feb 2023 11:08:45 +0100 Subject: [PATCH 16/17] Fix linter complaints Signed-off-by: Martin Mayer --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 757a416d..24a8e6ce 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,8 +41,8 @@ endif() option(USE_PI_MUTEX "Enables priority inheritance mutexes." ON) if(USE_PI_MUTEX) if(CMAKE_SYSTEM_NAME STREQUAL Linux OR - CMAKE_SYSTEM_NAME STREQUAL VxWorks OR - CMAKE_SYSTEM_NAME STREQUAL QNX) + CMAKE_SYSTEM_NAME STREQUAL VxWorks OR + CMAKE_SYSTEM_NAME STREQUAL QNX) target_compile_definitions(${PROJECT_NAME} PUBLIC "RCPPUTILS_USE_PIMUTEX") else() message("Mutexes with priority inheritance are not supported on your system. Using std mutexes.") From e75a51280bb4e0f1c8b72ff7b241c14c1cedb342 Mon Sep 17 00:00:00 2001 From: Martin Mayer Date: Thu, 23 Feb 2023 12:08:04 +0100 Subject: [PATCH 17/17] changed doxygen comments Signed-off-by: Martin Mayer --- include/rcpputils/mutex.hpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/include/rcpputils/mutex.hpp b/include/rcpputils/mutex.hpp index a024afcf..8f57b050 100644 --- a/include/rcpputils/mutex.hpp +++ b/include/rcpputils/mutex.hpp @@ -30,9 +30,10 @@ using RecursivePIMutex = std::recursive_mutex; #else /** - * Mutex with priority inheritance on systems that support it. - * This class derives from std::mutex to be fully compatible with standard C++. - * This implementation is needed because the C++ standard library doesn't support priority inheritance. + * Mutex with enabled thread priority inheritance. + * In case your OS does not support thread priority inheritance for mutexes, or + * the CMake option USE_PI_MUTEX is turned off, then this class is just an alias for + * std::mutex. **/ class PIMutex : public std::mutex { @@ -47,9 +48,10 @@ class PIMutex : public std::mutex }; /** - * Recursive mutex with priority inheritance on systems that support it. - * This class derives from std::recursive_mutex to be fully compatible with standard C++. - * This implementation is needed because the C++ standard library doesn't support priority inheritance. + * Recursive mutex with enabled thread priority inheritance. + * In case your OS does not support thread priority inheritance for mutexes, or + * the CMake option USE_PI_MUTEX is turned off, then this class is just an alias for + * std::recursive_mutex. **/ class RecursivePIMutex : public std::recursive_mutex {