Mutexes with priority inheritance

CMakeLists.txt

@@ -98,6 +98,11 @@ if(BUILD_TESTING)
   ament_add_gmock(test_async_function_handler test/test_async_function_handler.cpp)
   target_link_libraries(test_async_function_handler realtime_tools thread_priority)
   ament_target_dependencies(test_async_function_handler lifecycle_msgs rclcpp_lifecycle)
+
+if(NOT WIN32)
+  ament_add_gmock(realtime_mutex_tests test/realtime_mutex_tests.cpp)
+  target_link_libraries(realtime_mutex_tests realtime_tools)
+endif()
 endif()
 
 # Install

include/realtime_tools/mutex.hpp

@@ -0,0 +1,201 @@
+// Copyright 2024 PAL Robotics S.L.
+//
+// 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.
+
+/// \author Sai Kishor Kothakota
+
+#ifndef REALTIME_TOOLS__MUTEX_HPP_
+#define REALTIME_TOOLS__MUTEX_HPP_
+
+#ifdef _WIN32
+#error "The mutex.hpp header is not supported on Windows platforms"
+#endif
+
+#include <pthread.h>
+#include <cerrno>
+#include <cstring>
+#include <iostream>
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+/**
+ * @brief A pthread mutex wrapper that provides a mutex with the priority inheritance
+ * protocol and a priority ceiling of 99.
+ * The mutex is also error checked and robust.
+ * This mutex is intended to be used in real-time contexts.
+ * @note This mutex is not recursive.
+ */
+namespace realtime_tools
+{
+namespace detail
+{
+struct error_mutex_type_t
+{
+  static constexpr int value = PTHREAD_MUTEX_ERRORCHECK;
+};
+
+struct recursive_mutex_type_t
+{
+  static constexpr int value = PTHREAD_MUTEX_RECURSIVE;
+};
+
+struct stalled_robustness_t
+{
+  static constexpr int value = PTHREAD_MUTEX_STALLED;
+};
+
+struct robust_robustness_t
+{
+  static constexpr int value = PTHREAD_MUTEX_ROBUST;
+};
+/**
+ * @brief A class template that provides a pthread mutex with the priority inheritance protocol
+ *
+ * @tparam MutexType The type of the mutex. It can be one of the following: PTHREAD_MUTEX_NORMAL, PTHREAD_MUTEX_RECURSIVE, PTHREAD_MUTEX_ERRORCHECK, PTHREAD_MUTEX_DEFAULT
+ * @tparam MutexRobustness The robustness of the mutex. It can be one of the following: PTHREAD_MUTEX_STALLED, PTHREAD_MUTEX_ROBUST
+ */
+template <typename MutexType, typename MutexRobustness>
+class mutex
+{
+public:
+  using native_handle_type = pthread_mutex_t *;
+  using type = MutexType;
+  using robustness = MutexRobustness;
+
+  mutex()
+  {
+    pthread_mutexattr_t attr;
+
+    const auto attr_destroy = [](pthread_mutexattr_t * mutex_attr) {
+      // Destroy the mutex attributes
+      const auto res_destroy = pthread_mutexattr_destroy(mutex_attr);
+      if (res_destroy != 0) {
+        throw std::system_error(
+          res_destroy, std::generic_category(), "Failed to destroy mutex attribute");
+      }
+    };
+    using attr_cleanup_t = std::unique_ptr<pthread_mutexattr_t, decltype(attr_destroy)>;
+    auto attr_cleanup = attr_cleanup_t(&attr, attr_destroy);
+
+    // Initialize the mutex attributes
+    const auto res_attr = pthread_mutexattr_init(&attr);
+    if (res_attr != 0) {
+      throw std::system_error(
+        res_attr, std::system_category(), "Failed to initialize mutex attribute");
+    }
+
+    // Set the mutex type to MutexType
+    const auto res_type = pthread_mutexattr_settype(&attr, MutexType::value);
+
+    if (res_type != 0) {
+      throw std::system_error(res_type, std::system_category(), "Failed to set mutex type");
+    }
+
+    // Set the mutex attribute to use the protocol PTHREAD_PRIO_INHERIT
+    const auto res_protocol = pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT);
+    if (res_protocol != 0) {
+      throw std::system_error(res_protocol, std::system_category(), "Failed to set mutex protocol");
+    }
+
+    // Set the mutex attribute robustness to MutexRobustness
+    const auto res_robust = pthread_mutexattr_setrobust(&attr, MutexRobustness::value);
+    if (res_robust != 0) {
+      throw std::system_error(res_robust, std::system_category(), "Failed to set mutex robustness");
+    }
+
+    // Initialize the mutex with the attributes
+    const auto res_init = pthread_mutex_init(&mutex_, &attr);
+    if (res_init != 0) {
+      throw std::system_error(res_init, std::system_category(), "Failed to initialize mutex");
+    }
+  }
+
+  ~mutex()
+  {
+    const auto res = pthread_mutex_destroy(&mutex_);
+    if (res != 0) {
+      std::cerr << "Failed to destroy mutex : " << std::strerror(res) << std::endl;
+    }
+  }
+
+  mutex(const mutex &) = delete;
+
+  mutex & operator=(const mutex &) = delete;
+
+  native_handle_type native_handle() noexcept { return &mutex_; }
+
+  void lock()
+  {
+    const auto res = pthread_mutex_lock(&mutex_);
+    if (res == 0) {
+      return;
+    }
+    if (res == EOWNERDEAD) {
+      const auto res_consistent = pthread_mutex_consistent(&mutex_);
+      if (res_consistent != 0) {
+        throw std::runtime_error(
+          std::string("Failed to make mutex consistent : ") + std::strerror(res_consistent));
+      }
+      std::cerr << "Mutex owner died, but the mutex is consistent now. This shouldn't happen!"
+                << std::endl;
+    } else if (res == EDEADLK) {
+      throw std::system_error(res, std::system_category(), "Deadlock detected");
+    } else {
+      throw std::runtime_error(std::string("Failed to lock mutex : ") + std::strerror(res));
+    }
+  }
+
+  void unlock() noexcept
+  {
+    // As per the requirements of BasicLockable concept, unlock should not throw
+    const auto res = pthread_mutex_unlock(&mutex_);
+    if (res != 0) {
+      std::cerr << "Failed to unlock mutex : " << std::strerror(res) << std::endl;
+    }
+  }
+
+  bool try_lock()
+  {
+    const auto res = pthread_mutex_trylock(&mutex_);
+    if (res == 0) {
+      return true;
+    }
+    if (res == EBUSY) {
+      return false;
+    } else if (res == EOWNERDEAD) {
+      const auto res_consistent = pthread_mutex_consistent(&mutex_);
+      if (res_consistent != 0) {
+        throw std::runtime_error(
+          std::string("Failed to make mutex consistent : ") + std::strerror(res_consistent));
+      }
+      std::cerr << "Mutex owner died, but the mutex is consistent now. This shouldn't happen!"
+                << std::endl;
+    } else if (res == EDEADLK) {
+      throw std::system_error(res, std::system_category(), "Deadlock detected");
+    } else {
+      throw std::runtime_error(std::string("Failed to try lock mutex : ") + std::strerror(res));
+    }
+    return true;
+  }
+
+private:
+  pthread_mutex_t mutex_;
+};
+}  // namespace detail
+using prio_inherit_mutex = detail::mutex<detail::error_mutex_type_t, detail::robust_robustness_t>;
+using prio_inherit_recursive_mutex =
+  detail::mutex<detail::recursive_mutex_type_t, detail::robust_robustness_t>;
+}  // namespace realtime_tools
+
+#endif  // REALTIME_TOOLS__MUTEX_HPP_

include/realtime_tools/realtime_helpers.hpp

@@ -32,6 +32,7 @@
 #include <string>
 #include <thread>
 #include <utility>
+#include <vector>
 
 #ifdef _WIN32
 #include <windows.h>
@@ -62,10 +63,37 @@ bool configure_sched_fifo(int priority);
  * will not swap out the pages to disk i.e., the pages are guaranteed to stay in
  * RAM until later unlocked - which is important for realtime applications.
  * \param[out] message a message describing the result of the operation
- * \returns true if memory locking succeeded, false otherwise
+ * \returns true if memory locking succeeded, false otherwise.
 */
+
+[[deprecated("Use std::pair<bool, std::string> lock_memory() instead.")]]
 bool lock_memory(std::string & message);
 
+/**
+ * Locks the memory pages of the calling thread to prevent page faults.
+ * By calling this method, the programs locks all pages mapped into the address
+ * space of the calling process and future mappings. This means that the kernel
+ * will not swap out the pages to disk i.e., the pages are guaranteed to stay in
+ * RAM until later unlocked - which is important for realtime applications.
+ * \param[out] message a message describing the result of the operation
+ * \returns  a pair of a boolean indicating whether the operation succeeded or not
+ * and a message describing the result of the operation
+*/
+std::pair<bool, std::string> lock_memory();
+
+/**
+ * Configure the caller thread affinity - Tell the scheduler to prefer a certain
+ * set of cores for the given thread handle.
+ * \note The threads created by the calling thread will inherit the affinity.
+ * \param[in] thread the thread handle of the thread
+ * \param[in] core the cpu numbers of the core. If an empty vector is passed,
+ * the affinity is reset to the default.
+ * \returns a pair of a boolean indicating whether the operation succeeded or not
+ * and a message describing the result of the operation
+*/
+std::pair<bool, std::string> set_thread_affinity(
+  NATIVE_THREAD_HANDLE thread, const std::vector<int> & cores);
+
 /**
  * Configure the caller thread affinity - Tell the scheduler to prefer a certain
  * core for the given thread handle.
@@ -101,6 +129,17 @@ std::pair<bool, std::string> set_thread_affinity(std::thread & thread, int core)
 */
 std::pair<bool, std::string> set_current_thread_affinity(int core);
 
+/**
+ * Configure the current thread affinity - Tell the scheduler to prefer a certain
+ * set of cores for the current thread.
+ * \note The threads created by the calling thread will inherit the affinity.
+ * \param[in] core the cpu numbers of the core. If an empty vector is passed,
+ * the affinity is reset to the default.
+ * \returns a pair of a boolean indicating whether the operation succeeded or not
+ * and a message describing the result of the operation
+*/
+std::pair<bool, std::string> set_current_thread_affinity(const std::vector<int> & cores);
+
 /**
  * Method to get the amount of available cpu cores
  * \ref https://man7.org/linux/man-pages/man3/sysconf.3.html

src/realtime_helpers.cpp

@@ -65,10 +65,16 @@ bool configure_sched_fifo(int priority)
 }
 
 bool lock_memory(std::string & message)
+{
+  const auto lock_result = lock_memory();
+  message = lock_result.second;
+  return lock_result.first;
+}
+
+std::pair<bool, std::string> lock_memory()
 {
 #ifdef _WIN32
-  message = "Memory locking is not supported on Windows.";
-  return false;
+  return {false, "Memory locking is not supported on Windows."};
 #else
   auto is_capable = [](cap_value_t v) -> bool {
     bool rc = false;
@@ -86,6 +92,7 @@ bool lock_memory(std::string & message)
     return rc;
   };
 
+  std::string message;
   if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
     if (!is_capable(CAP_IPC_LOCK)) {
       message = "No proper privileges to lock the memory!";
@@ -105,15 +112,16 @@ bool lock_memory(std::string & message)
     } else {
       message = "Unknown error occurred!";
     }
-    return false;
+    return {false, message};
   } else {
     message = "Memory locked successfully!";
-    return true;
+    return {true, message};
   }
 #endif
 }
 
-std::pair<bool, std::string> set_thread_affinity(NATIVE_THREAD_HANDLE thread, int core)
+std::pair<bool, std::string> set_thread_affinity(
+  NATIVE_THREAD_HANDLE thread, const std::vector<int> & cores)
 {
   std::string message;
 #ifdef _WIN32
@@ -149,34 +157,48 @@ std::pair<bool, std::string> set_thread_affinity(NATIVE_THREAD_HANDLE thread, in
   // Obtain available processors
   const auto number_of_cores = get_number_of_available_processors();
 
+  bool valid_cpu_set = true;
   // Reset affinity by setting it to all cores
-  if (core < 0) {
+  if (cores.empty()) {
     for (auto i = 0; i < number_of_cores; i++) {
       CPU_SET(i, &cpuset);
     }
-    // And actually tell the schedular to set the affinity of the thread of respective pid
-    const auto result = set_affinity_result_message(
-      pthread_setaffinity_np(thread, sizeof(cpu_set_t), &cpuset), message);
-    return std::make_pair(result, message);
+  } else {
+    for (const auto core : cores) {
+      if (core < 0 || core >= number_of_cores) {
+        valid_cpu_set = false;
+        break;
+      }
+      CPU_SET(core, &cpuset);
+    }
   }
 
-  if (core < number_of_cores) {
-    // Set the passed core to the cpu set
-    CPU_SET(core, &cpuset);
+  if (valid_cpu_set) {
     // And actually tell the schedular to set the affinity of the thread of respective pid
     const auto result = set_affinity_result_message(
       pthread_setaffinity_np(thread, sizeof(cpu_set_t), &cpuset), message);
     return std::make_pair(result, message);
   }
+  // create a string from the core numbers
+  std::string core_numbers;
+  for (const auto core : cores) {
+    core_numbers += std::to_string(core) + " ";
+  }
   // Invalid core number passed
-  message = "Invalid core number : '" + std::to_string(core) + "' passed! The system has " +
+  message = "Invalid core numbers : ['" + core_numbers + "'] passed! The system has " +
             std::to_string(number_of_cores) +
             " cores. Parsed core number should be between 0 and " +
             std::to_string(number_of_cores - 1);
   return std::make_pair(false, message);
 #endif
 }
 
+std::pair<bool, std::string> set_thread_affinity(NATIVE_THREAD_HANDLE thread, int core)
+{
+  const std::vector<int> affinity_cores = core < 0 ? std::vector<int>() : std::vector<int>{core};
+  return set_thread_affinity(thread, affinity_cores);
+}
+
 std::pair<bool, std::string> set_thread_affinity(std::thread & thread, int core)
 {
   if (!thread.joinable()) {
@@ -195,6 +217,15 @@ std::pair<bool, std::string> set_current_thread_affinity(int core)
 #endif
 }
 
+std::pair<bool, std::string> set_current_thread_affinity(const std::vector<int> & cores)
+{
+#ifdef _WIN32
+  return set_thread_affinity(GetCurrentThread(), cores);
+#else
+  return set_thread_affinity(pthread_self(), cores);
+#endif
+}
+
 int64_t get_number_of_available_processors()
 {
 #ifdef _WIN32