From 147b2bb8f51697209942bcec70d7bb1743c66b96 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 28 Nov 2024 10:51:54 +0100 Subject: [PATCH] Add support to parse multiple cores for setting CPU affinity (#208) (cherry picked from commit efcb7862e39b048ad0effcafbb4e5b6a960e68b3) --- include/realtime_tools/realtime_helpers.hpp | 25 ++++++++++++ src/realtime_helpers.cpp | 44 ++++++++++++++++----- test/thread_priority_tests.cpp | 13 ++++++ 3 files changed, 72 insertions(+), 10 deletions(-) diff --git a/include/realtime_tools/realtime_helpers.hpp b/include/realtime_tools/realtime_helpers.hpp index 371f9643..3f1cd10f 100644 --- a/include/realtime_tools/realtime_helpers.hpp +++ b/include/realtime_tools/realtime_helpers.hpp @@ -32,6 +32,7 @@ #include #include #include +#include #ifdef _WIN32 #include @@ -66,6 +67,19 @@ bool configure_sched_fifo(int priority); */ bool lock_memory(std::string & message); +/** + * 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 set_thread_affinity( + NATIVE_THREAD_HANDLE thread, const std::vector & cores); + /** * Configure the caller thread affinity - Tell the scheduler to prefer a certain * core for the given thread handle. @@ -101,6 +115,17 @@ std::pair set_thread_affinity(std::thread & thread, int core) */ std::pair 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 set_current_thread_affinity(const std::vector & cores); + /** * Method to get the amount of available cpu cores * \ref https://man7.org/linux/man-pages/man3/sysconf.3.html diff --git a/src/realtime_helpers.cpp b/src/realtime_helpers.cpp index a9ce1a73..17720c23 100644 --- a/src/realtime_helpers.cpp +++ b/src/realtime_helpers.cpp @@ -113,7 +113,8 @@ bool lock_memory(std::string & message) #endif } -std::pair set_thread_affinity(NATIVE_THREAD_HANDLE thread, int core) +std::pair set_thread_affinity( + NATIVE_THREAD_HANDLE thread, const std::vector & cores) { std::string message; #ifdef _WIN32 @@ -149,27 +150,35 @@ std::pair 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); @@ -177,6 +186,12 @@ std::pair set_thread_affinity(NATIVE_THREAD_HANDLE thread, in #endif } +std::pair set_thread_affinity(NATIVE_THREAD_HANDLE thread, int core) +{ + const std::vector affinity_cores = core < 0 ? std::vector() : std::vector{core}; + return set_thread_affinity(thread, affinity_cores); +} + std::pair set_thread_affinity(std::thread & thread, int core) { if (!thread.joinable()) { @@ -195,6 +210,15 @@ std::pair set_current_thread_affinity(int core) #endif } +std::pair set_current_thread_affinity(const std::vector & 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 diff --git a/test/thread_priority_tests.cpp b/test/thread_priority_tests.cpp index fc552cdd..e5318b8e 100644 --- a/test/thread_priority_tests.cpp +++ b/test/thread_priority_tests.cpp @@ -56,6 +56,11 @@ TEST(thread_priority, set_thread_affinity_invalid_too_many_cores) const int count = static_cast(realtime_tools::get_number_of_available_processors()); // We should always have at least one core EXPECT_FALSE(realtime_tools::set_thread_affinity(t.native_handle(), count + 10).first); + EXPECT_FALSE(realtime_tools::set_thread_affinity(t.native_handle(), {0, 1, count + 2}).first); + EXPECT_FALSE(realtime_tools::set_thread_affinity(t.native_handle(), {0, -1, 2}).first); + EXPECT_FALSE(realtime_tools::set_thread_affinity(t.native_handle(), {0, 3, -1}).first); + EXPECT_FALSE( + realtime_tools::set_thread_affinity(t.native_handle(), std::vector({-1})).first); t.join(); } @@ -64,6 +69,10 @@ TEST(thread_priority, set_current_thread_affinity_invalid_too_many_cores) const int count = static_cast(realtime_tools::get_number_of_available_processors()); // We should always have at least one core EXPECT_FALSE(realtime_tools::set_current_thread_affinity(count + 10).first); + EXPECT_FALSE(realtime_tools::set_current_thread_affinity({count + 10}).first); + EXPECT_FALSE(realtime_tools::set_current_thread_affinity({0, count + 10}).first); + EXPECT_FALSE(realtime_tools::set_current_thread_affinity({0, -1}).first); + EXPECT_FALSE(realtime_tools::set_current_thread_affinity(std::vector({-1})).first); } TEST(thread_priority, set_thread_affinity_valid_reset) @@ -72,6 +81,7 @@ TEST(thread_priority, set_thread_affinity_valid_reset) std::thread t([]() { std::this_thread::sleep_for(std::chrono::milliseconds(100)); }); // Reset core affinity EXPECT_TRUE(realtime_tools::set_thread_affinity(t.native_handle(), -1).first); + EXPECT_TRUE(realtime_tools::set_thread_affinity(t.native_handle(), {}).first); t.join(); } @@ -79,10 +89,13 @@ TEST(thread_priority, set_current_thread_affinity_valid_reset) { // Reset core affinity EXPECT_TRUE(realtime_tools::set_current_thread_affinity(-1).first); + EXPECT_TRUE(realtime_tools::set_current_thread_affinity({}).first); } TEST(thread_priority, set_current_thread_affinity_valid) { // We should always have at least one core EXPECT_TRUE(realtime_tools::set_current_thread_affinity(0).first); + const int count = static_cast(realtime_tools::get_number_of_available_processors()); + EXPECT_TRUE(realtime_tools::set_current_thread_affinity({0, count - 1}).first); }