diff --git a/CMakeLists.txt b/CMakeLists.txt index b2a6772..24a8e6c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,7 @@ add_library(${PROJECT_NAME} src/filesystem_helper.cpp src/find_library.cpp src/env.cpp + src/mutex.cpp src/shared_library.cpp) target_include_directories(${PROJECT_NAME} PUBLIC "$" @@ -36,6 +37,18 @@ 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(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() + ament_target_dependencies(${PROJECT_NAME} rcutils) # Export old-style CMake variables @@ -73,6 +86,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/Doxyfile b/Doxyfile index 06309b1..bdea43d 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 8340ed3..5d61c64 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 new file mode 100644 index 0000000..8f57b05 --- /dev/null +++ b/include/rcpputils/mutex.hpp @@ -0,0 +1,70 @@ +// 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. +// 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 +{ +#ifndef RCPPUTILS_USE_PIMUTEX + +// Fallback code path +using PIMutex = std::mutex; +using RecursivePIMutex = std::recursive_mutex; + +#else + +/** + * 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 +{ +public: + /// Creates a mutex with priority inheritance enabled + RCPPUTILS_PUBLIC + PIMutex(); + + /// Releases the mutex + RCPPUTILS_PUBLIC + ~PIMutex(); +}; + +/** + * 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 +{ +public: + /// Creates a recursive mutex with priority inheritance enabled + RCPPUTILS_PUBLIC + RecursivePIMutex(); + + /// Releases the mutex + RCPPUTILS_PUBLIC + ~RecursivePIMutex(); +}; + +#endif // RCPPUTILS_USE_PIMUTEX +} // namespace rcpputils +#endif // RCPPUTILS__MUTEX_HPP_ diff --git a/src/mutex.cpp b/src/mutex.cpp new file mode 100644 index 0000000..7eafec1 --- /dev/null +++ b/src/mutex.cpp @@ -0,0 +1,72 @@ +// 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. +// 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" + +#ifdef RCPPUTILS_USE_PIMUTEX + +#include + +namespace rcpputils +{ + +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); +} + +PIMutex::~PIMutex() +{ +} + +RecursivePIMutex::~RecursivePIMutex() +{ +} + +} // namespace rcpputils +#endif // RCPPUTILS_USE_PIMUTEX diff --git a/test/test_mutex.cpp b/test/test_mutex.cpp new file mode 100644 index 0000000..602a8b7 --- /dev/null +++ b/test/test_mutex.cpp @@ -0,0 +1,305 @@ +// 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. +// 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" + +#ifdef RCPPUTILS_USE_PIMUTEX +#include +#include "rcutils/types/rcutils_ret.h" +#ifdef __QNXNTO__ + #include + #include +#endif // __QNXNTO__ +#endif // RCPPUTILS_USE_PIMUTEX + +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_trylockmultiple) { + rcpputils::RecursivePIMutex rmutex; + + const unsigned count = 20; + + 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) { + 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([&result, &test_mutex]() { + result = 0; + test_mutex.lock(); + result = 1; // this line should not be reached as long as the mutex is locked + }); + std::this_thread::sleep_for(10ms); + EXPECT_EQ(0, result); + + test_mutex.unlock(); + test_thread.join(); +} + +#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, +// 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); + +#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++) { + if ( (cpu_bitmask & (1 << i)) != 0) { + 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; +} + +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() +{ + 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 (configure_realtime_thread( + low_prio_thread, THREAD_PRIORITY_LOW, + cpu_bitmask) == false) + { + end_low_prio_thread = true; + low_prio_thread.join(); + std::cerr << "THREAD_PRIORITY_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); + } + 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( + configure_realtime_thread( + high_prio_thread, + 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 + 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( + 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; + + // 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) { + 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 + 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(); +} + +#endif // RCPPUTILS_USE_PIMUTEX