Added realtime thread configuration support

CMakeLists.txt

@@ -72,6 +72,7 @@ set(rcutils_sources
   src/string_array.c
   src/string_map.c
   src/testing/fault_injection.c
+  src/thread.c
   src/time.c
   ${time_impl_c}
   src/uint8_array.c
@@ -561,6 +562,14 @@ if(BUILD_TESTING)
     test/test_macros.cpp
   )
 
+  ament_add_gtest(test_thread
+    test/test_thread.cpp
+    src/thread.c # to test RCUTILS_LOCAL functions
+  )
+  if(TARGET test_thread)
+    target_link_libraries(test_thread ${PROJECT_NAME})
+  endif()
+
   add_performance_test(benchmark_logging test/benchmark/benchmark_logging.cpp)
   if(TARGET benchmark_logging)
     target_link_libraries(benchmark_logging ${PROJECT_NAME})

include/rcutils/thread.h

@@ -0,0 +1,93 @@
+// 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 RCUTILS__THREAD_H_
+#define RCUTILS__THREAD_H_
+
+#include "rcutils/visibility_control.h"
+#include "rcutils/types/rcutils_ret.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/// Enum for OS independent thread priorities
+enum ThreadPriority
+{
+  THREAD_PRIORITY_LOWEST,
+  THREAD_PRIORITY_LOWEST_PLUS1,
+  THREAD_PRIORITY_LOWEST_PLUS2,
+  THREAD_PRIORITY_LOWEST_PLUS3,
+  THREAD_PRIORITY_LOW_MINUS3,
+  THREAD_PRIORITY_LOW_MINUS2,
+  THREAD_PRIORITY_LOW_MINUS1,
+  THREAD_PRIORITY_LOW,
+  THREAD_PRIORITY_LOW_PLUS1,
+  THREAD_PRIORITY_LOW_PLUS2,
+  THREAD_PRIORITY_LOW_PLUS3,
+  THREAD_PRIORITY_MEDIUM_MINUS3,
+  THREAD_PRIORITY_MEDIUM_MINUS2,
+  THREAD_PRIORITY_MEDIUM_MINUS1,
+  THREAD_PRIORITY_MEDIUM,
+  THREAD_PRIORITY_MEDIUM_PLUS1,
+  THREAD_PRIORITY_MEDIUM_PLUS2,
+  THREAD_PRIORITY_MEDIUM_PLUS3,
+  THREAD_PRIORITY_HIGH_MINUS3,
+  THREAD_PRIORITY_HIGH_MINUS2,
+  THREAD_PRIORITY_HIGH_MINUS1,
+  THREAD_PRIORITY_HIGH,
+  THREAD_PRIORITY_HIGH_PLUS1,
+  THREAD_PRIORITY_HIGH_PLUS2,
+  THREAD_PRIORITY_HIGH_PLUS3,
+  THREAD_PRIORITY_HIGHEST_MINUS3,
+  THREAD_PRIORITY_HIGHEST_MINUS2,
+  THREAD_PRIORITY_HIGHEST_MINUS1,
+  THREAD_PRIORITY_HIGHEST
+};
+
+/// Calculates an OS specific thread priority from a ThreadPriority value.
+/**
+ * \param[in] thread_priority thread priority of type ThreadPriority
+ * \param[out] os_priority OS specific thread priority
+ * \return RCUTILS_RET_OK on systems that support POSIX
+ */
+RCUTILS_LOCAL
+rcutils_ret_t calculate_os_fifo_thread_priority(
+  const int thread_priority,
+  int * os_priority);
+
+/// Sets a realtime priority and a cpu affinity for 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 of type ThreadPriority to be set for the given thread
+ * \param[in] cpu_bitmask cpu core bitmask for the given thread; use (unsigned) -1 for all cores
+ * \return RCUTILS_RET_OK on success
+ */
+RCUTILS_PUBLIC
+rcutils_ret_t configure_native_realtime_thread(
+  unsigned long int native_handle, const int priority, // NOLINT
+  const unsigned int cpu_bitmask);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // RCUTILS__THREAD_H_

src/thread.c

@@ -0,0 +1,118 @@
+// 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.
+
+#include "rcutils/thread.h"
+
+#ifdef _WIN32  // i.e., Windows platform.
+// #include <windows.h>
+#elif __APPLE__  // i.e., macOS platform.
+// #include <pthread.h>
+// #include <mach/mach_init.h>
+// #include <mach/mach_port.h>
+// #include <mach/mach_time.h>
+// #include <mach/thread_act.h>
+// #include <mach/thread_policy.h>
+// #include <sys/sysctl.h>
+#else  // POSIX platforms
+  #include <pthread.h>
+  #ifdef __QNXNTO__
+    #include <sys/neutrino.h>
+    #include <sys/syspage.h>
+  #endif  // __QNXNTO__
+#endif
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+rcutils_ret_t calculate_os_fifo_thread_priority(
+  const int thread_priority,
+  int * os_priority)
+{
+#ifdef _WIN32
+  return RCUTILS_RET_ERROR;
+#elif __APPLE__
+  return RCUTILS_RET_ERROR;
+#else
+  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;
+#endif
+}
+
+rcutils_ret_t configure_native_realtime_thread(
+  unsigned long int native_handle, const int priority, // NOLINT
+  const unsigned int cpu_bitmask)
+{
+  int success = 1;
+#ifdef _WIN32
+  return RCUTILS_RET_ERROR;
+#elif __APPLE__
+  return RCUTILS_RET_ERROR;
+#else  // POSIX systems
+  struct sched_param params;
+  int policy;
+  success &= (pthread_getschedparam(native_handle, &policy, &params) == 0);
+  success &= (calculate_os_fifo_thread_priority(priority, &params.sched_priority) ==
+    RCUTILS_RET_OK ? 1 : 0);
+  success &= (pthread_setschedparam(native_handle, SCHED_FIFO, &params) == 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,
+      (void *)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 ? RCUTILS_RET_OK : RCUTILS_RET_ERROR;
+}
+
+#ifdef __cplusplus
+}
+#endif

test/test_thread.cpp

@@ -0,0 +1,67 @@
+// 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 <gtest/gtest.h>
+
+#include "rcutils/thread.h"
+#include "rcutils/error_handling.h"
+
+#ifdef __linux__
+#include <pthread.h>
+#endif  // __linux__
+
+TEST(test_thread, config_rt_thread) {
+#ifdef __linux__
+  const unsigned cpu_id = 0;
+  const unsigned long cpu_bitmask = 1 << cpu_id;  // NOLINT
+  const int priority = THREAD_PRIORITY_MEDIUM;
+  if (configure_native_realtime_thread(pthread_self(), priority, cpu_bitmask) != RCUTILS_RET_OK) {
+    GTEST_SKIP() << "Unable to set realtime thread priority.";
+    return;
+  }
+
+  struct sched_param params_self;
+  int policy_self;
+  EXPECT_EQ(0, pthread_getschedparam(pthread_self(), &policy_self, &params_self));
+  int os_prio_calculated;
+  EXPECT_EQ(RCUTILS_RET_OK, calculate_os_fifo_thread_priority(priority, &os_prio_calculated));
+  EXPECT_EQ(os_prio_calculated, params_self.sched_priority);
+  EXPECT_EQ(SCHED_FIFO, policy_self);
+
+  cpu_set_t cpuset_self;
+  EXPECT_EQ(0, pthread_getaffinity_np(pthread_self(), sizeof(cpuset_self), &cpuset_self));
+  cpu_set_t cpuset;
+  CPU_ZERO(&cpuset);
+  CPU_SET(cpu_id, &cpuset);
+  EXPECT_EQ(0, memcmp(&cpuset, &cpuset_self, sizeof(cpu_set_t)));
+#else
+  GTEST_SKIP() << "Testcase not implemented for this platform."
+#endif  // __linux__
+}
+
+TEST(test_thread, calculate_rt_priorities) {
+#ifdef __linux__
+  int prio = -1;
+  const int max_prio = sched_get_priority_max(SCHED_FIFO);
+  const int min_prio = sched_get_priority_min(SCHED_FIFO);
+  EXPECT_EQ(RCUTILS_RET_OK, calculate_os_fifo_thread_priority(THREAD_PRIORITY_LOWEST, &prio));
+  EXPECT_EQ(min_prio, prio);
+  EXPECT_EQ(RCUTILS_RET_OK, calculate_os_fifo_thread_priority(THREAD_PRIORITY_MEDIUM, &prio));
+  EXPECT_EQ((max_prio + min_prio) / 2 - 1, prio);
+  EXPECT_EQ(RCUTILS_RET_OK, calculate_os_fifo_thread_priority(THREAD_PRIORITY_HIGHEST, &prio));
+  EXPECT_EQ(max_prio, prio);
+#elif
+  GTEST_SKIP() << "Testcase not implemented for this platform."
+#endif  // __linux__
+}