From de9fc8775fd385aaf9220e7d9b4529ac82c7f751 Mon Sep 17 00:00:00 2001 From: Shoji Morita Date: Tue, 20 Jun 2023 20:17:16 +0900 Subject: [PATCH 1/6] Decoupled the additional feature from rclcpp to rcpputils, reflecting on the pointing out below. https://github.com/ros2/rclcpp/pull/2205#issuecomment-1593832758 Signed-off-by: Shoji Morita --- CMakeLists.txt | 7 + include/rcpputils/threads.hpp | 26 +++ .../rcpputils/threads/posix/linux/cpu_set.hpp | 160 +++++++++++++ include/rcpputils/threads/posix/thread.hpp | 210 ++++++++++++++++++ .../threads/posix/thread_attribute.hpp | 190 ++++++++++++++++ .../rcpputils/threads/posix/thread_func.hpp | 53 +++++ include/rcpputils/threads/posix/thread_id.hpp | 146 ++++++++++++ include/rcpputils/threads/posix/utilities.hpp | 42 ++++ include/rcpputils/threads/std/thread.hpp | 145 ++++++++++++ .../threads/std/thread_attribute.hpp | 165 ++++++++++++++ include/rcpputils/threads/windows/thread.hpp | 22 ++ src/threads/posix_thread.cpp | 183 +++++++++++++++ src/threads/windows_thread.cpp | 19 ++ 13 files changed, 1368 insertions(+) create mode 100644 include/rcpputils/threads.hpp create mode 100644 include/rcpputils/threads/posix/linux/cpu_set.hpp create mode 100644 include/rcpputils/threads/posix/thread.hpp create mode 100644 include/rcpputils/threads/posix/thread_attribute.hpp create mode 100644 include/rcpputils/threads/posix/thread_func.hpp create mode 100644 include/rcpputils/threads/posix/thread_id.hpp create mode 100644 include/rcpputils/threads/posix/utilities.hpp create mode 100644 include/rcpputils/threads/std/thread.hpp create mode 100644 include/rcpputils/threads/std/thread_attribute.hpp create mode 100644 include/rcpputils/threads/windows/thread.hpp create mode 100644 src/threads/posix_thread.cpp create mode 100644 src/threads/windows_thread.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b2a6772..f8841e3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,6 +32,13 @@ add_library(${PROJECT_NAME} target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") +if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") + target_sources(${PROJECT_NAME} PRIVATE + src/threads/posix_thread.cpp) +elseif(WIN32) + target_sources(${PROJECT_NAME} PRIVATE + src/threads/windows_thread.cpp) +endif() if(WIN32) target_compile_definitions(${PROJECT_NAME} PRIVATE "RCPPUTILS_BUILDING_LIBRARY") diff --git a/include/rcpputils/threads.hpp b/include/rcpputils/threads.hpp new file mode 100644 index 0000000..942fd6a --- /dev/null +++ b/include/rcpputils/threads.hpp @@ -0,0 +1,26 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__THREADS_HPP_ +#define RCPPUTILS__THREADS_HPP_ + +#if defined(__linux__) +#include "rcpputils/threads/posix/thread.hpp" +#elif defined(_WIN32) +#include "rcpputils/threads/windows/thread.hpp" +#else +#include "rcpputils/threads/std/thread.hpp" +#endif + +#endif // RCPPUTILS__THREADS_HPP_ diff --git a/include/rcpputils/threads/posix/linux/cpu_set.hpp b/include/rcpputils/threads/posix/linux/cpu_set.hpp new file mode 100644 index 0000000..b227520 --- /dev/null +++ b/include/rcpputils/threads/posix/linux/cpu_set.hpp @@ -0,0 +1,160 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__THREADS__POSIX__LINUX__CPU_SET_HPP_ +#define RCPPUTILS__THREADS__POSIX__LINUX__CPU_SET_HPP_ + +#include +#include +#include +#include +#include + +#include "rcpputils/visibility_control.hpp" +#include "rcpputils/threads/posix/utilities.hpp" + +namespace rcpputils +{ + +namespace detail +{ + +struct CpuSet +{ + using NativeCpuSetType = cpu_set_t; + CpuSet() = default; + explicit CpuSet(std::size_t cpu) + { + init_cpu_set(); + CPU_ZERO_S(alloc_size(), cpu_set_.get()); + CPU_SET_S(cpu, alloc_size(), cpu_set_.get()); + } + CpuSet(const CpuSet & other) + { + if (other.cpu_set_) { + init_cpu_set(); + memcpy(cpu_set_.get(), other.cpu_set_.get(), alloc_size()); + } + } + CpuSet & operator=(const CpuSet & other) + { + if (other.cpu_set_) { + init_cpu_set(); + memcpy(cpu_set_.get(), other.cpu_set_.get(), alloc_size()); + } else { + clear(); + } + return *this; + } + CpuSet(CpuSet && other) + : CpuSet() + { + swap(other); + } + CpuSet & operator=(CpuSet && other) + { + CpuSet tmp; + other.swap(tmp); + tmp.swap(*this); + return *this; + } + void swap(CpuSet & other) + { + using std::swap; + swap(cpu_set_, other.cpu_set_); + swap(num_proc_, other.num_proc_); + } + void set(std::size_t cpu) + { + init_cpu_set(); + valid_cpu(cpu); + CPU_SET_S(cpu, alloc_size(), cpu_set_.get()); + } + void unset(std::size_t cpu) + { + init_cpu_set(); + valid_cpu(cpu); + CPU_CLR_S(cpu, alloc_size(), cpu_set_.get()); + } + void clear() + { + if (cpu_set_) { + CPU_ZERO_S(alloc_size(), cpu_set_.get()); + } + } + bool is_set(std::size_t cpu) const + { + if (cpu_set_) { + valid_cpu(cpu); + return CPU_ISSET_S(cpu, alloc_size(), cpu_set_.get()); + } else { + return false; + } + } + + std::size_t max_processors() const + { + return num_proc_; + } + std::size_t alloc_size() const + { + return CPU_ALLOC_SIZE(num_proc_); + } + NativeCpuSetType * native_cpu_set() const + { + return cpu_set_.get(); + } + +private: + void init_cpu_set() + { + if (cpu_set_) { + return; + } + auto num_proc = sysconf(_SC_NPROCESSORS_ONLN); + if (num_proc <= 0) { + throw_if_error(num_proc, "unrecognized sysconf(_SC_NPROCESSORS_ONLN) is not valid"); + } + auto p = CPU_ALLOC(CPU_ALLOC_SIZE(num_proc)); + cpu_set_ = std::unique_ptr(p); + num_proc_ = num_proc; + } + void valid_cpu(std::size_t cpu) const + { + if (num_proc_ <= cpu) { + auto ec = std::make_error_code(std::errc::invalid_argument); + throw std::system_error{ec, "cpu number is invaild"}; + } + } + struct CpuSetDeleter + { + void operator()(NativeCpuSetType * cpu_set) const + { + CPU_FREE(cpu_set); + } + }; + std::unique_ptr cpu_set_; + std::size_t num_proc_; +}; + +inline void swap(CpuSet & a, CpuSet & b) +{ + a.swap(b); +} + +} // namespace detail + +} // namespace rcpputils + +#endif // RCPPUTILS__THREADS__POSIX__LINUX__CPU_SET_HPP_ diff --git a/include/rcpputils/threads/posix/thread.hpp b/include/rcpputils/threads/posix/thread.hpp new file mode 100644 index 0000000..37c15cf --- /dev/null +++ b/include/rcpputils/threads/posix/thread.hpp @@ -0,0 +1,210 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__THREADS__POSIX__THREAD_HPP_ +#define RCPPUTILS__THREADS__POSIX__THREAD_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rcpputils/threads/posix/thread_attribute.hpp" +#include "rcpputils/threads/posix/thread_func.hpp" +#include "rcpputils/threads/posix/thread_id.hpp" +#include "rcpputils/threads/posix/utilities.hpp" +#include "rcpputils/visibility_control.hpp" + +namespace rcpputils +{ + +RCPPUTILS_PUBLIC_TYPE +struct Thread +{ + using NativeHandleType = pthread_t; + using Attribute = detail::ThreadAttribute; + using Id = detail::ThreadId; + + // Assume pthread_t is an invalid handle if it's 0 + Thread() noexcept + : handle_{} {} + Thread(Thread && other) + : handle_{} + { + swap(other); + } + template, Attribute>::value>> + explicit Thread(F && f, Args && ... args) + : Thread( + static_cast(nullptr), + make_thread_func(std::forward(f), std::forward(args)...)) + {} + template + Thread(Attribute const & attr, F && f, Args && ... args) + : Thread( + &attr, + make_thread_func_with_attr(attr, std::forward(f), std::forward(args)...)) + {} + Thread(Thread const &) = delete; + ~Thread() + { + if (handle_) { + std::terminate(); + } + } + + Thread & operator=(Thread && other) noexcept + { + if (handle_) { + std::terminate(); + } + swap(other); + return *this; + } + + Thread & operator=(Thread const &) = delete; + + void swap(Thread & other) + { + using std::swap; + swap(handle_, other.handle_); + swap(name_, other.name_); + } + + void join() + { + void * p; + int r = pthread_join(handle_, &p); + detail::throw_if_error(r, "Error in pthread_join "); + handle_ = NativeHandleType{}; + } + + bool joinable() const noexcept + { + return 0 == pthread_equal(handle_, NativeHandleType{}); + } + + void detach() + { + int r = pthread_detach(handle_); + detail::throw_if_error(r, "Error in pthread_detach "); + handle_ = NativeHandleType{}; + } + + NativeHandleType native_handle() const + { + return handle_; + } + + Id get_id() const noexcept + { + return Id{handle_}; + } + + static unsigned int hardware_concurrency() noexcept + { + auto r = sysconf(_SC_NPROCESSORS_ONLN); + if (r == -1) { + return 0u; + } else { + return static_cast(r); + } + } + +private: + using ThreadFuncBase = detail::ThreadFuncBase; + template + static std::unique_ptr make_thread_func(F && f, Args && ... args) + { + static_assert( + !std::is_member_object_pointer_v>, + "F is a pointer to member, that has no effect on a thread"); + + detail::ThreadFuncBase * func = new detail::ThreadFunc( + [f = std::forward(f), args = std::tuple(std::forward(args)...)]() mutable + { + std::apply(f, args); + }); + return std::unique_ptr(func); + } + template + static std::unique_ptr make_thread_func_with_attr( + Attribute const & attr, + F && f, + Args && ... args) + { + static_assert( + !std::is_member_object_pointer_v>, + "F is a pointer to member, that has no effect on a thread"); + + detail::ThreadFuncBase * func = new detail::ThreadFunc( + [attr, f = std::forward(f), args = std::tuple(std::forward(args)...)]() mutable + { + std::apply(f, args); + }); + return std::unique_ptr(func); + } + + Thread(Attribute const * attr, std::unique_ptr func); + + static void apply_attr(Attribute const & attr); + + NativeHandleType handle_; + std::string name_; +}; + +inline void swap(Thread & t1, Thread & t2) +{ + t1.swap(t2); +} + +namespace detail +{ +void apply_attr_to_current_thread(ThreadAttribute const & attr); +} + +namespace this_thread +{ + +template +void run_with_thread_attribute( + detail::ThreadAttribute const & attr, F && f, Args && ... args) +{ + static_assert( + !std::is_member_object_pointer_v>, + "F is a pointer to member, that has no effect on a thread"); + + detail::apply_attr_to_current_thread(attr); + std::invoke(std::forward(f), std::forward(args)...); +} + +inline void yield() noexcept +{ + sched_yield(); +} + +} // namespace this_thread + +} // namespace rcpputils + +#endif // RCPPUTILS__THREADS__POSIX__THREAD_HPP_ diff --git a/include/rcpputils/threads/posix/thread_attribute.hpp b/include/rcpputils/threads/posix/thread_attribute.hpp new file mode 100644 index 0000000..e9f2925 --- /dev/null +++ b/include/rcpputils/threads/posix/thread_attribute.hpp @@ -0,0 +1,190 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__THREADS__POSIX__THREAD_ATTRIBUTE_HPP_ +#define RCPPUTILS__THREADS__POSIX__THREAD_ATTRIBUTE_HPP_ + +#include +#include +#include + +#include "rcutils/thread_attr.h" + +#include "rcpputils/visibility_control.hpp" + +#ifdef __linux__ +#include "rcpputils/threads/posix/linux/cpu_set.hpp" +#endif + +namespace rcpputils +{ + +namespace detail +{ + +struct ThreadAttribute +{ + ThreadAttribute(); + + ThreadAttribute(const ThreadAttribute &) = default; + ThreadAttribute(ThreadAttribute &&) = default; + ThreadAttribute & operator=(const ThreadAttribute &) = default; + ThreadAttribute & operator=(ThreadAttribute &&) = default; + + using NativeAttributeType = pthread_attr_t; + + ThreadAttribute & set_affinity(CpuSet cs) + { + cpu_set_ = std::move(cs); + return *this; + } + const CpuSet & get_affinity() const + { + return cpu_set_; + } + + ThreadAttribute & set_sched_policy(rcutils_thread_scheduling_policy_t sp) + { + sched_policy_ = convert_sched_policy(sp); + return *this; + } + int get_sched_policy() const + { + return sched_policy_; + } + + ThreadAttribute & set_stack_size(std::size_t sz) + { + stack_size_ = sz; + return *this; + } + std::size_t get_stack_size() const + { + return stack_size_; + } + + ThreadAttribute & set_priority(int prio) + { + priority_ = prio; + return *this; + } + int get_priority() const + { + return priority_; + } + + ThreadAttribute & set_run_as_detached(bool detach) + { + detached_flag_ = detach; + return *this; + } + bool get_run_as_detached() const + { + return detached_flag_; + } + + ThreadAttribute & set_name(std::string name) + { + name_ = std::move(name); + return *this; + } + const std::string & get_name() const + { + return name_; + } + + void + set_thread_attribute( + const rcutils_thread_attr_t & attr) + { + CpuSet cpu_set(attr.core_affinity); + set_affinity(std::move(cpu_set)); + set_sched_policy(attr.scheduling_policy); + set_priority(attr.priority); + set_name(attr.name); + } + + void + swap( + ThreadAttribute & other) + { + using std::swap; + swap(cpu_set_, other.cpu_set_); + swap(sched_policy_, other.sched_policy_); + swap(stack_size_, other.stack_size_); + swap(priority_, other.priority_); + swap(detached_flag_, other.detached_flag_); + swap(name_, other.name_); + } + +private: + CpuSet cpu_set_; + int sched_policy_; + std::size_t stack_size_; + int priority_; + bool detached_flag_; + std::string name_; + + int convert_sched_policy( + rcutils_thread_scheduling_policy_t sched_policy) + { + switch (sched_policy) { +#ifdef SCHED_FIFO + case RCUTILS_THREAD_SCHEDULING_POLICY_FIFO: + return SCHED_FIFO; +#endif +#ifdef SCHED_RR + case RCUTILS_THREAD_SCHEDULING_POLICY_RR: + return SCHED_RR; +#endif +#ifdef SCHED_OTHER + case RCUTILS_THREAD_SCHEDULING_POLICY_OTHER: + return SCHED_OTHER; +#endif +#ifdef SCHED_IDLE + case RCUTILS_THREAD_SCHEDULING_POLICY_IDLE: + return SCHED_IDLE; +#endif +#ifdef SCHED_BATCH + case RCUTILS_THREAD_SCHEDULING_POLICY_BATCH: + return SCHED_BATCH; +#endif +#ifdef SCHED_SPORADIC + case RCUTILS_THREAD_SCHEDULING_POLICY_SPORADIC: + return SCHED_SPORADIC; +#endif + /* Todo: Necessity and setting method need to be considered + #ifdef SCHED_DEADLINE + case RCUTILS_THREAD_SCHEDULING_POLICY_DEADLINE: + return SCHED_DEADLINE; + break; + #endif + */ + default: + throw std::runtime_error("Invalid scheduling policy"); + } + return -1; + } +}; + +inline void swap(ThreadAttribute & a, ThreadAttribute & b) +{ + a.swap(b); +} + +} // namespace detail + +} // namespace rcpputils + +#endif // RCPPUTILS__THREADS__POSIX__THREAD_ATTRIBUTE_HPP_ diff --git a/include/rcpputils/threads/posix/thread_func.hpp b/include/rcpputils/threads/posix/thread_func.hpp new file mode 100644 index 0000000..ec4ebd6 --- /dev/null +++ b/include/rcpputils/threads/posix/thread_func.hpp @@ -0,0 +1,53 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__THREADS__POSIX__THREAD_FUNC_HPP_ +#define RCPPUTILS__THREADS__POSIX__THREAD_FUNC_HPP_ + +#include +#include +#include + +namespace rcpputils::detail +{ + +struct ThreadFuncBase +{ + virtual ~ThreadFuncBase() = default; + virtual void run() = 0; +}; + +template +struct ThreadFunc : ThreadFuncBase +{ + template + explicit ThreadFunc(G && g) + : func_(std::forward(g)) + {} + +private: + void run() override + { + func_(); + } + + F func_; +}; + +template +ThreadFunc(F &&)->ThreadFunc>; + +} // namespace rcpputils::detail + +#endif // RCPPUTILS__THREADS__POSIX__THREAD_FUNC_HPP_ diff --git a/include/rcpputils/threads/posix/thread_id.hpp b/include/rcpputils/threads/posix/thread_id.hpp new file mode 100644 index 0000000..5f46ce7 --- /dev/null +++ b/include/rcpputils/threads/posix/thread_id.hpp @@ -0,0 +1,146 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__THREADS__POSIX__THREAD_ID_HPP_ +#define RCPPUTILS__THREADS__POSIX__THREAD_ID_HPP_ + +#include + +#include "rcpputils/visibility_control.hpp" + +namespace rcpputils +{ + +#ifdef __clang__ +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wmismatched-tags" +#endif + +struct Thread; + +namespace detail +{ + +namespace thread_id_ns +{ + +struct ThreadId; + +inline ThreadId get_id() noexcept; +inline bool operator==(ThreadId id1, ThreadId id2); +inline bool operator!=(ThreadId id1, ThreadId id2); +inline bool operator<(ThreadId id1, ThreadId id2); +inline bool operator>(ThreadId id1, ThreadId id2); +inline bool operator<=(ThreadId id1, ThreadId id2); +inline bool operator>=(ThreadId id1, ThreadId id2); +template +inline std::basic_ostream & operator<<( + std::basic_ostream &, + ThreadId); + +struct ThreadId +{ + ThreadId() = default; + ThreadId(ThreadId const &) = default; + ThreadId(ThreadId &&) = default; + ThreadId & operator=(ThreadId const &) = default; + ThreadId & operator=(ThreadId &&) = default; + + friend bool operator==(ThreadId id1, ThreadId id2) + { + return pthread_equal(id1.h, id2.h); + } + friend bool operator<(ThreadId id1, ThreadId id2) + { + return id1.h < id2.h; + } + template + friend std::basic_ostream & operator<<( + std::basic_ostream & ost, + ThreadId id) + { + return ost << id.h; + } + +private: + friend class rcpputils::Thread; + friend ThreadId get_id() noexcept; + friend struct std::hash; + explicit ThreadId(pthread_t h) + : h(h) {} + pthread_t h; +}; + +ThreadId get_id() noexcept +{ + return ThreadId{pthread_self()}; +} + +bool operator!=(ThreadId id1, ThreadId id2) +{ + return !(id1 == id2); +} + +bool operator>(ThreadId id1, ThreadId id2) +{ + return id2 < id1; +} + +bool operator<=(ThreadId id1, ThreadId id2) +{ + return !(id1 > id2); +} + +bool operator>=(ThreadId id1, ThreadId id2) +{ + return !(id1 < id2); +} + +} // namespace thread_id_ns + +using thread_id_ns::ThreadId; +using thread_id_ns::operator==; +using thread_id_ns::operator!=; +using thread_id_ns::operator<; // NOLINT +using thread_id_ns::operator>; // NOLINT +using thread_id_ns::operator<=; +using thread_id_ns::operator>=; +using thread_id_ns::operator<<; + +} // namespace detail + +namespace this_thread +{ + +using detail::thread_id_ns::get_id; + +} // namespace this_thread + +} // namespace rcpputils + +namespace std +{ + +template<> +struct hash +{ + std::size_t operator()(rcpputils::detail::thread_id_ns::ThreadId id) + { + return id.h; + } +}; + +} // namespace std + +#endif // RCPPUTILS__THREADS__POSIX__THREAD_ID_HPP_ diff --git a/include/rcpputils/threads/posix/utilities.hpp b/include/rcpputils/threads/posix/utilities.hpp new file mode 100644 index 0000000..4d9b054 --- /dev/null +++ b/include/rcpputils/threads/posix/utilities.hpp @@ -0,0 +1,42 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__THREADS__POSIX__UTILITIES_HPP_ +#define RCPPUTILS__THREADS__POSIX__UTILITIES_HPP_ + +#include + +namespace rcpputils +{ + +namespace detail +{ + +namespace +{ + +inline void throw_if_error(int r, char const * msg) +{ + if (r != 0) { + throw std::system_error(r, std::system_category(), msg); + } +} + +} // namespace + +} // namespace detail + +} // namespace rcpputils + +#endif // RCPPUTILS__THREADS__POSIX__UTILITIES_HPP_ diff --git a/include/rcpputils/threads/std/thread.hpp b/include/rcpputils/threads/std/thread.hpp new file mode 100644 index 0000000..c0ac507 --- /dev/null +++ b/include/rcpputils/threads/std/thread.hpp @@ -0,0 +1,145 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__THREADS__STD__THREAD_HPP_ +#define RCPPUTILS__THREADS__STD__THREAD_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rcpputils/threads/std/thread_attribute.hpp" +#include "rclcpp/visibility_control.hpp" + +namespace rcpputils +{ + +struct Thread +{ + using NativeHandleType = std::thread::native_handle_type; + using Attribute = detail::ThreadAttribute; + using Id = std::thread::id; + + Thread() noexcept + : thread_{} + {} + Thread(Thread && other) + : thread_{} + { + swap(other); + } + template, Attribute>::value>> + explicit Thread(F && f, Args && ... args) + : thread_(std::forward(f), std::forward(args)...) + {} + template + Thread(Attribute & attr, F && f, Args && ... args) + : thread_(std::forward(f), std::forward(args)...) + { + if (attr.set_unavailable_items_) { + throw std::runtime_error("std::thread can't set thread attribute"); + } + if (attr.get_run_as_detached()) { + thread_.detach(); + } + } + Thread(Thread const &) = delete; + ~Thread() {} + + Thread & operator=(Thread && other) noexcept + { + swap(other); + return *this; + } + + Thread & operator=(Thread const &) = delete; + + void swap(Thread & other) + { + using std::swap; + swap(thread_, other.thread_); + } + + void join() + { + thread_.join(); + thread_ = std::thread{}; + } + + bool joinable() const noexcept + { + return thread_.joinable(); + } + + void detach() + { + thread_.detach(); + thread_ = std::thread{}; + } + + NativeHandleType native_handle() + { + return thread_.native_handle(); + } + + Id get_id() const noexcept + { + return thread_.get_id(); + } + + static int hardware_concurrency() noexcept + { + return std::thread::hardware_concurrency(); + } + +private: + std::thread thread_; +}; + +inline void swap(Thread & t1, Thread & t2) +{ + t1.swap(t2); +} + +namespace this_thread +{ + +template +void run_with_thread_attribute(Thread::Attribute & attr, F && f, Args && ... args) +{ + static_assert( + !std::is_member_object_pointer_v>, + "F is a pointer to member, that is ineffective on thread"); + + if (attr.set_unavailable_items_) { + throw std::runtime_error("std::thread can't set thread attribute"); + } + + std::invoke(f, std::forward(args)...); +} + +} // namespace this_thread + +} // namespace rcpputils + +#endif // RCPPUTILS__THREADS__STD__THREAD_HPP_ diff --git a/include/rcpputils/threads/std/thread_attribute.hpp b/include/rcpputils/threads/std/thread_attribute.hpp new file mode 100644 index 0000000..b799cd6 --- /dev/null +++ b/include/rcpputils/threads/std/thread_attribute.hpp @@ -0,0 +1,165 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__THREADS__STD__THREAD_ATTRIBUTE_HPP_ +#define RCPPUTILS__THREADS__STD__THREAD_ATTRIBUTE_HPP_ + +#include +#include +#include + +#include "rcl_yaml_param_parser/types.h" +#include "rclcpp/visibility_control.hpp" + +namespace rcpputils +{ + +struct Thread; + +namespace detail +{ +struct ThreadAttribute; +} // namespace detail + +namespace this_thread +{ +template +void run_with_thread_attribute( + detail::ThreadAttribute & attr, F && f, Args && ... args); +} // namespace this_thread + +namespace detail +{ + +struct CpuSet +{ + using NativeCpuSetType = std::size_t; + CpuSet() {} + explicit CpuSet(std::size_t) {} + CpuSet(const CpuSet &) {} + CpuSet & operator=(const CpuSet &) + { + return *this; + } + CpuSet(CpuSet &&) = delete; + CpuSet & operator=(CpuSet &&) = delete; + ~CpuSet() {} + void set(std::size_t) {} + void unset(std::size_t) {} + void clear() {} + bool is_set(std::size_t) + { + return false; + } + std::size_t get_max_processors() const + { + return 0; + } + NativeCpuSetType native_cpu_set() const + { + return 0; + } +}; + +struct ThreadAttribute +{ + using PriorityType = int; + + ThreadAttribute() + : set_unavailable_items_(false), run_as_detached_(false) {} + + ThreadAttribute(const ThreadAttribute &) = default; + ThreadAttribute(ThreadAttribute &&) = default; + ThreadAttribute & operator=(const ThreadAttribute &) = default; + ThreadAttribute & operator=(ThreadAttribute &&) = default; + + ThreadAttribute & set_affinity(CpuSet &) + { + set_unavailable_items_ = true; + return *this; + } + CpuSet get_affinity() + { + return CpuSet{}; + } + + ThreadAttribute & set_stack_size(std::size_t) + { + set_unavailable_items_ = true; + return *this; + } + std::size_t get_stack_size() const + { + return 0; + } + + ThreadAttribute & set_priority(int prio) + { + (void)prio; + set_unavailable_items_ = true; + return *this; + } + int get_priority() const + { + return 0; + } + + ThreadAttribute & set_run_as_detached(bool detach) + { + run_as_detached_ = detach; + return *this; + } + bool get_run_as_detached() const + { + return run_as_detached_; + } + + ThreadAttribute & set_name(std::string const &) + { + set_unavailable_items_ = true; + return *this; + } + const char * get_name() const + { + return ""; + } + + void + set_thread_attribute( + const rcutils_thread_attr_t &) + { + set_unavailable_items_ = true; + } + + void swap( + ThreadAttribute & other) + { + std::swap(*this, other); + } + +private: + friend struct rcpputils::Thread; + template + friend void this_thread::run_with_thread_attribute( + ThreadAttribute & attr, F && f, Args && ... args); + + bool set_unavailable_items_; + bool run_as_detached_; +}; + +} // namespace detail + +} // namespace rcpputils + +#endif // RCPPUTILS__THREADS__STD__THREAD_ATTRIBUTE_HPP_ diff --git a/include/rcpputils/threads/windows/thread.hpp b/include/rcpputils/threads/windows/thread.hpp new file mode 100644 index 0000000..ec6613d --- /dev/null +++ b/include/rcpputils/threads/windows/thread.hpp @@ -0,0 +1,22 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__THREADS__WINDOWS__THREAD_HPP_ +#define RCPPUTILS__THREADS__WINDOWS__THREAD_HPP_ + +// Not implemented so far. +// The windows specific code will be implemented +// while discussing the scheduling parameter passing feature at Real-time WG. + +#endif // RCPPUTILS__THREADS__WINDOWS__THREAD_HPP_ diff --git a/src/threads/posix_thread.cpp b/src/threads/posix_thread.cpp new file mode 100644 index 0000000..e60b848 --- /dev/null +++ b/src/threads/posix_thread.cpp @@ -0,0 +1,183 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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 "rcpputils/threads/posix/thread.hpp" +#include "rcpputils/threads/posix/utilities.hpp" + + +namespace rcpputils +{ + +namespace detail +{ + +namespace +{ +void set_pthread_attr(pthread_attr_t & native_attr, rcpputils::Thread::Attribute const & attr); +void * thread_main(void * p); +} // namespace + +} // namespace detail + +Thread::Thread(Attribute const * attr, std::unique_ptr func) +: handle_(NativeHandleType{}), name_(attr ? attr->get_name() : std::string{}) +{ + Attribute::NativeAttributeType native_attr; + int r = pthread_attr_init(&native_attr); + detail::throw_if_error(r, "Error in pthread_attr_init "); + + if (attr != nullptr) { + detail::set_pthread_attr(native_attr, *attr); + } + + NativeHandleType h; + r = pthread_create(&h, &native_attr, detail::thread_main, func.get()); + detail::throw_if_error(r, "Error in pthread_create "); + + if (attr == nullptr || !attr->get_run_as_detached()) { + this->handle_ = h; + } + + pthread_attr_destroy(&native_attr); + + func.release(); +} + +void Thread::apply_attr(Attribute const & attr) +{ + int r; + int policy = attr.get_sched_policy(); +#if __linux__ + if (policy != SCHED_FIFO && policy != SCHED_RR && policy != SCHED_OTHER) { + sched_param param; + param.sched_priority = attr.get_priority(); + r = pthread_setschedparam(pthread_self(), policy, ¶m); + detail::throw_if_error(r, "Error in pthread_setschedparam "); + } +#endif // #if __linux__ +} + +namespace detail +{ + +ThreadAttribute::ThreadAttribute() +{ + NativeAttributeType attr; + int r; + + r = pthread_attr_init(&attr); + throw_if_error(r, "Error in pthread_attr_init "); + + r = pthread_attr_getschedpolicy(&attr, &sched_policy_); + throw_if_error(r, "Error in pthread_attr_getschedpolicy "); + + r = pthread_attr_getstacksize(&attr, &stack_size_); + throw_if_error(r, "Error in pthread_attr_getstacksize "); + + sched_param param; + r = pthread_attr_getschedparam(&attr, ¶m); + throw_if_error(r, "Error in pthread_attr_getschedparam "); + priority_ = param.sched_priority; + + int flag; + r = pthread_attr_getdetachstate(&attr, &flag); + throw_if_error(r, "Error in pthread_attr_getdetachstate "); + detached_flag_ = (flag == PTHREAD_CREATE_DETACHED); + + pthread_attr_destroy(&attr); +} + + +void apply_attr_to_current_thread(ThreadAttribute const & attr) +{ + int r; + +#if __linux__ + CpuSet cpu_set = attr.get_affinity(); + CpuSet::NativeCpuSetType * native_cpu_set = cpu_set.native_cpu_set(); + if (native_cpu_set) { + std::size_t alloc_size = cpu_set.alloc_size(); + r = pthread_setaffinity_np(pthread_self(), alloc_size, native_cpu_set); + throw_if_error(r, "Error in sched_setaffinity "); + } +#endif // #if __linux__ + + sched_param param; + param.sched_priority = attr.get_priority(); + int policy = attr.get_sched_policy(); + r = pthread_setschedparam(pthread_self(), policy, ¶m); + throw_if_error(r, "Error in sched_setscheduler"); +} + +namespace +{ + +void * thread_main(void * p) +{ + std::unique_ptr func(reinterpret_cast(p)); + + try { + func->run(); + } catch (...) { + std::cerr << "failed to run thread" << std::endl; + std::terminate(); + } + + return nullptr; +} + +void set_pthread_attr(pthread_attr_t & native_attr, Thread::Attribute const & attr) +{ + int r; + +#if defined(__linux__) + CpuSet affinity = attr.get_affinity(); + size_t cpu_size = CPU_ALLOC_SIZE(static_cast(sysconf(_SC_NPROCESSORS_ONLN))); + r = pthread_attr_setaffinity_np(&native_attr, cpu_size, affinity.native_cpu_set()); + throw_if_error(r, "Error in pthread_attr_setaffinity_np "); +#endif + + std::size_t stack_size = attr.get_stack_size(); + r = pthread_attr_setstacksize(&native_attr, stack_size); + throw_if_error(r, "Error in pthread_attr_setstacksize "); + + int flag = attr.get_run_as_detached() ? PTHREAD_CREATE_DETACHED : PTHREAD_CREATE_JOINABLE; + r = pthread_attr_setdetachstate(&native_attr, flag); + throw_if_error(r, "Error in pthread_attr_setdetachstate "); + + int sched_policy = attr.get_sched_policy(); + if (sched_policy == SCHED_FIFO || sched_policy == SCHED_RR) { + r = pthread_attr_setinheritsched(&native_attr, PTHREAD_EXPLICIT_SCHED); + throw_if_error(r, "Error in pthread_attr_setinheritsched "); + + r = pthread_attr_setschedpolicy(&native_attr, sched_policy); + throw_if_error(r, "Error in pthread_attr_setschedpolicy "); + + sched_param param; + param.sched_priority = attr.get_priority(); + r = pthread_attr_setschedparam(&native_attr, ¶m); + throw_if_error(r, "Error in pthread_attr_setschedparam "); + } +} + +} // namespace + +} // namespace detail + +} // namespace rcpputils diff --git a/src/threads/windows_thread.cpp b/src/threads/windows_thread.cpp new file mode 100644 index 0000000..a37a201 --- /dev/null +++ b/src/threads/windows_thread.cpp @@ -0,0 +1,19 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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/threads/windows/thread.hpp" + +// Not implemented so far. +// The windows specific code will be implemented +// while discussing the scheduling parameter passing feature at Real-time WG. From cc3fc4c539ebdf87d0710eddb0f1f26c920cd52e Mon Sep 17 00:00:00 2001 From: Shoji Morita Date: Wed, 12 Jul 2023 16:02:23 +0900 Subject: [PATCH 2/6] Removed some incomplete code for the Windows platform. Please, refer to the comment below. https://github.com/ros2/rclcpp/pull/2205#issuecomment-1598619448 Signed-off-by: Shoji Morita --- CMakeLists.txt | 3 --- include/rcpputils/threads.hpp | 2 -- include/rcpputils/threads/windows/thread.hpp | 22 -------------------- src/threads/windows_thread.cpp | 19 ----------------- 4 files changed, 46 deletions(-) delete mode 100644 include/rcpputils/threads/windows/thread.hpp delete mode 100644 src/threads/windows_thread.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index f8841e3..843a710 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,9 +35,6 @@ target_include_directories(${PROJECT_NAME} PUBLIC if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") target_sources(${PROJECT_NAME} PRIVATE src/threads/posix_thread.cpp) -elseif(WIN32) - target_sources(${PROJECT_NAME} PRIVATE - src/threads/windows_thread.cpp) endif() if(WIN32) target_compile_definitions(${PROJECT_NAME} diff --git a/include/rcpputils/threads.hpp b/include/rcpputils/threads.hpp index 942fd6a..a298a4c 100644 --- a/include/rcpputils/threads.hpp +++ b/include/rcpputils/threads.hpp @@ -17,8 +17,6 @@ #if defined(__linux__) #include "rcpputils/threads/posix/thread.hpp" -#elif defined(_WIN32) -#include "rcpputils/threads/windows/thread.hpp" #else #include "rcpputils/threads/std/thread.hpp" #endif diff --git a/include/rcpputils/threads/windows/thread.hpp b/include/rcpputils/threads/windows/thread.hpp deleted file mode 100644 index ec6613d..0000000 --- a/include/rcpputils/threads/windows/thread.hpp +++ /dev/null @@ -1,22 +0,0 @@ -// Copyright 2023 eSOL Co.,Ltd. -// -// 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__THREADS__WINDOWS__THREAD_HPP_ -#define RCPPUTILS__THREADS__WINDOWS__THREAD_HPP_ - -// Not implemented so far. -// The windows specific code will be implemented -// while discussing the scheduling parameter passing feature at Real-time WG. - -#endif // RCPPUTILS__THREADS__WINDOWS__THREAD_HPP_ diff --git a/src/threads/windows_thread.cpp b/src/threads/windows_thread.cpp deleted file mode 100644 index a37a201..0000000 --- a/src/threads/windows_thread.cpp +++ /dev/null @@ -1,19 +0,0 @@ -// Copyright 2023 eSOL Co.,Ltd. -// -// 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/threads/windows/thread.hpp" - -// Not implemented so far. -// The windows specific code will be implemented -// while discussing the scheduling parameter passing feature at Real-time WG. From 2734427f240212be9923117337349c937cd8f4b5 Mon Sep 17 00:00:00 2001 From: Shoji Morita Date: Thu, 26 Oct 2023 13:40:51 +0900 Subject: [PATCH 3/6] Modified to receive multiple core affinity parameters according to the update of REP-2017 below. https://github.com/ros-infrastructure/rep/pull/385 Signed-off-by: Shoji Morita --- CMakeLists.txt | 3 +- .../rcpputils/threads/posix/linux/cpu_set.hpp | 116 +++---------- .../threads/posix/thread_attribute.hpp | 12 +- src/threads/posix/linux/cpu_set.cpp | 154 ++++++++++++++++++ .../{posix_thread.cpp => posix/thread.cpp} | 0 5 files changed, 186 insertions(+), 99 deletions(-) create mode 100644 src/threads/posix/linux/cpu_set.cpp rename src/threads/{posix_thread.cpp => posix/thread.cpp} (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 843a710..8ac3769 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,7 +34,8 @@ target_include_directories(${PROJECT_NAME} PUBLIC "$") if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") target_sources(${PROJECT_NAME} PRIVATE - src/threads/posix_thread.cpp) + src/threads/posix/thread.cpp + src/threads/posix/linux/cpu_set.cpp) endif() if(WIN32) target_compile_definitions(${PROJECT_NAME} diff --git a/include/rcpputils/threads/posix/linux/cpu_set.hpp b/include/rcpputils/threads/posix/linux/cpu_set.hpp index b227520..1fb1a64 100644 --- a/include/rcpputils/threads/posix/linux/cpu_set.hpp +++ b/include/rcpputils/threads/posix/linux/cpu_set.hpp @@ -16,11 +16,10 @@ #define RCPPUTILS__THREADS__POSIX__LINUX__CPU_SET_HPP_ #include -#include -#include -#include #include +#include +#include "rcutils/thread_attr.h" #include "rcpputils/visibility_control.hpp" #include "rcpputils/threads/posix/utilities.hpp" @@ -33,75 +32,21 @@ namespace detail struct CpuSet { using NativeCpuSetType = cpu_set_t; + CpuSet() = default; - explicit CpuSet(std::size_t cpu) - { - init_cpu_set(); - CPU_ZERO_S(alloc_size(), cpu_set_.get()); - CPU_SET_S(cpu, alloc_size(), cpu_set_.get()); - } - CpuSet(const CpuSet & other) - { - if (other.cpu_set_) { - init_cpu_set(); - memcpy(cpu_set_.get(), other.cpu_set_.get(), alloc_size()); - } - } - CpuSet & operator=(const CpuSet & other) - { - if (other.cpu_set_) { - init_cpu_set(); - memcpy(cpu_set_.get(), other.cpu_set_.get(), alloc_size()); - } else { - clear(); - } - return *this; - } - CpuSet(CpuSet && other) - : CpuSet() - { - swap(other); - } - CpuSet & operator=(CpuSet && other) - { - CpuSet tmp; - other.swap(tmp); - tmp.swap(*this); - return *this; - } - void swap(CpuSet & other) - { - using std::swap; - swap(cpu_set_, other.cpu_set_); - swap(num_proc_, other.num_proc_); - } - void set(std::size_t cpu) - { - init_cpu_set(); - valid_cpu(cpu); - CPU_SET_S(cpu, alloc_size(), cpu_set_.get()); - } - void unset(std::size_t cpu) - { - init_cpu_set(); - valid_cpu(cpu); - CPU_CLR_S(cpu, alloc_size(), cpu_set_.get()); - } - void clear() - { - if (cpu_set_) { - CPU_ZERO_S(alloc_size(), cpu_set_.get()); - } - } - bool is_set(std::size_t cpu) const - { - if (cpu_set_) { - valid_cpu(cpu); - return CPU_ISSET_S(cpu, alloc_size(), cpu_set_.get()); - } else { - return false; - } - } + explicit CpuSet(rcutils_thread_core_affinity_t const & affinity); + CpuSet(const CpuSet & other); + CpuSet(CpuSet && other); + + CpuSet & operator=(const CpuSet & other); + CpuSet & operator=(CpuSet && other); + void swap(CpuSet & other); + void set(std::size_t cpu); + void unset(std::size_t cpu); + void clear(); + bool is_set(std::size_t cpu) const; + + void set_rcutils_thread_core_affinity(rcutils_thread_core_affinity_t const & affinity); std::size_t max_processors() const { @@ -111,38 +56,17 @@ struct CpuSet { return CPU_ALLOC_SIZE(num_proc_); } - NativeCpuSetType * native_cpu_set() const + CpuSet::NativeCpuSetType * native_cpu_set() const { return cpu_set_.get(); } private: - void init_cpu_set() - { - if (cpu_set_) { - return; - } - auto num_proc = sysconf(_SC_NPROCESSORS_ONLN); - if (num_proc <= 0) { - throw_if_error(num_proc, "unrecognized sysconf(_SC_NPROCESSORS_ONLN) is not valid"); - } - auto p = CPU_ALLOC(CPU_ALLOC_SIZE(num_proc)); - cpu_set_ = std::unique_ptr(p); - num_proc_ = num_proc; - } - void valid_cpu(std::size_t cpu) const - { - if (num_proc_ <= cpu) { - auto ec = std::make_error_code(std::errc::invalid_argument); - throw std::system_error{ec, "cpu number is invaild"}; - } - } + void init_cpu_set(); + void valid_cpu(std::size_t cpu) const; struct CpuSetDeleter { - void operator()(NativeCpuSetType * cpu_set) const - { - CPU_FREE(cpu_set); - } + void operator()(NativeCpuSetType * cpu_set) const; }; std::unique_ptr cpu_set_; std::size_t num_proc_; diff --git a/include/rcpputils/threads/posix/thread_attribute.hpp b/include/rcpputils/threads/posix/thread_attribute.hpp index e9f2925..0447dfc 100644 --- a/include/rcpputils/threads/posix/thread_attribute.hpp +++ b/include/rcpputils/threads/posix/thread_attribute.hpp @@ -39,6 +39,14 @@ struct ThreadAttribute ThreadAttribute(const ThreadAttribute &) = default; ThreadAttribute(ThreadAttribute &&) = default; + + explicit ThreadAttribute(rcutils_thread_attr_t const & attr) + : cpu_set_(CpuSet(attr.core_affinity)), + sched_policy_(convert_sched_policy(attr.scheduling_policy)), + priority_(attr.priority), + name_(attr.name) + {} + ThreadAttribute & operator=(const ThreadAttribute &) = default; ThreadAttribute & operator=(ThreadAttribute &&) = default; @@ -105,7 +113,7 @@ struct ThreadAttribute } void - set_thread_attribute( + set_rcutils_thread_attribute( const rcutils_thread_attr_t & attr) { CpuSet cpu_set(attr.core_affinity); @@ -136,7 +144,7 @@ struct ThreadAttribute bool detached_flag_; std::string name_; - int convert_sched_policy( + static int convert_sched_policy( rcutils_thread_scheduling_policy_t sched_policy) { switch (sched_policy) { diff --git a/src/threads/posix/linux/cpu_set.cpp b/src/threads/posix/linux/cpu_set.cpp new file mode 100644 index 0000000..1212db2 --- /dev/null +++ b/src/threads/posix/linux/cpu_set.cpp @@ -0,0 +1,154 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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/threads/posix/linux/cpu_set.hpp" + +#include +#include +#include + +#include "rcutils/thread_attr.h" + +namespace rcpputils +{ + +namespace detail +{ + +CpuSet::CpuSet(rcutils_thread_core_affinity_t const & affinity) +{ + init_cpu_set(); + CPU_ZERO_S(alloc_size(), cpu_set_.get()); + std::size_t size = std::min(CPU_ALLOC_SIZE(affinity.core_count), alloc_size()); + // this memcpy dependent to structure of cpu_set_t that only have integer array used as bitset. + memcpy(cpu_set_.get(), affinity.set, size); + if (affinity.core_count > num_proc_) { + for (std::size_t i = num_proc_; i < affinity.core_count; ++i) { + if (rcutils_thread_core_affinity_is_set(&affinity, i)) { + auto ec = std::make_error_code(std::errc::invalid_argument); + throw std::system_error{ec, "invalid cpu number"}; + } + } + } +} + +CpuSet::CpuSet(const CpuSet & other) +{ + if (!other.cpu_set_) { + return; + } + init_cpu_set(); + memcpy(cpu_set_.get(), other.cpu_set_.get(), alloc_size()); +} + +CpuSet::CpuSet(CpuSet && other) +: CpuSet() +{ + swap(other); +} + +CpuSet & CpuSet::operator=(const CpuSet & other) +{ + if (other.cpu_set_) { + init_cpu_set(); + memcpy(cpu_set_.get(), other.cpu_set_.get(), alloc_size()); + } else { + clear(); + } + return *this; +} + +CpuSet & CpuSet::operator=(CpuSet && other) +{ + CpuSet tmp; + other.swap(tmp); + tmp.swap(*this); + return *this; +} + +void CpuSet::swap(CpuSet & other) +{ + using std::swap; + swap(cpu_set_, other.cpu_set_); + swap(num_proc_, other.num_proc_); +} + +void CpuSet::set(std::size_t cpu) +{ + init_cpu_set(); + valid_cpu(cpu); + CPU_SET_S(cpu, alloc_size(), cpu_set_.get()); +} + +void CpuSet::unset(std::size_t cpu) +{ + init_cpu_set(); + valid_cpu(cpu); + CPU_CLR_S(cpu, alloc_size(), cpu_set_.get()); +} + +void CpuSet::clear() +{ + if (cpu_set_) { + CPU_ZERO_S(alloc_size(), cpu_set_.get()); + } +} + +bool CpuSet::is_set(std::size_t cpu) const +{ + if (cpu_set_) { + valid_cpu(cpu); + return CPU_ISSET_S(cpu, alloc_size(), cpu_set_.get()); + } else { + return false; + } +} + +void CpuSet::set_rcutils_thread_core_affinity(rcutils_thread_core_affinity_t const & affinity) +{ + CpuSet(affinity).swap(*this); +} + +void CpuSet::init_cpu_set() +{ + if (cpu_set_) { + return; + } + auto num_proc = sysconf(_SC_NPROCESSORS_ONLN); + if (num_proc <= 0) { + throw_if_error( + num_proc, + "invalid return value of sysconf(_SC_NPROCESSORS_ONLN)"); + } + auto p = CPU_ALLOC(CPU_ALLOC_SIZE(num_proc)); + cpu_set_ = std::unique_ptr(p); + num_proc_ = num_proc; +} + +void CpuSet::valid_cpu(std::size_t cpu) const +{ + if (num_proc_ <= cpu) { + auto ec = std::make_error_code(std::errc::invalid_argument); + throw std::system_error{ec, "invalid cpu number"}; + } +} + +void CpuSet::CpuSetDeleter::operator()(NativeCpuSetType * cpu_set) const +{ + CPU_FREE(cpu_set); +} + +} // namespace detail + +} // namespace rcpputils diff --git a/src/threads/posix_thread.cpp b/src/threads/posix/thread.cpp similarity index 100% rename from src/threads/posix_thread.cpp rename to src/threads/posix/thread.cpp From a3661ff9968904b9ac71594d629866c53cc25043 Mon Sep 17 00:00:00 2001 From: Shoji Morita Date: Thu, 14 Dec 2023 14:48:27 +0900 Subject: [PATCH 4/6] Revised interfaces/implementations and added tests. Signed-off-by: Shoji Morita --- CMakeLists.txt | 8 +- include/rcpputils/{threads.hpp => thread.hpp} | 13 +- .../rcpputils/thread/detail/posix/cpu_set.hpp | 22 +++ .../detail}/posix/linux/cpu_set.hpp | 37 ++-- .../detail}/posix/thread.hpp | 66 +++---- .../detail}/posix/thread_attribute.hpp | 125 +++++------- .../thread/detail/posix/thread_func.hpp | 68 +++++++ .../thread/detail/posix/thread_id.hpp | 47 +++++ .../detail}/posix/utilities.hpp | 16 +- .../rcpputils/thread/detail/std/cpu_set.hpp | 80 ++++++++ .../thread/detail/std/this_thread.hpp | 35 ++++ .../{threads => thread/detail}/std/thread.hpp | 40 ++-- .../detail}/std/thread_attribute.hpp | 86 ++------ .../rcpputils/thread/detail/std/thread_id.hpp | 47 +++++ .../posix => thread/detail}/thread_id.hpp | 76 +++----- include/rcpputils/thread/this_thread.hpp | 24 +++ include/rcpputils/thread/thread.hpp | 24 +++ include/rcpputils/thread/thread_attribute.hpp | 24 +++ .../rcpputils/threads/posix/thread_func.hpp | 53 ----- .../detail}/posix/linux/cpu_set.cpp | 87 +++++++-- src/thread/detail/posix/thread.cpp | 166 ++++++++++++++++ src/thread/detail/posix/thread_attribute.cpp | 129 ++++++++++++ src/threads/posix/thread.cpp | 183 ------------------ test/test_thread.cpp | 158 +++++++++++++++ 24 files changed, 1049 insertions(+), 565 deletions(-) rename include/rcpputils/{threads.hpp => thread.hpp} (72%) create mode 100644 include/rcpputils/thread/detail/posix/cpu_set.hpp rename include/rcpputils/{threads => thread/detail}/posix/linux/cpu_set.hpp (68%) rename include/rcpputils/{threads => thread/detail}/posix/thread.hpp (70%) rename include/rcpputils/{threads => thread/detail}/posix/thread_attribute.hpp (56%) create mode 100644 include/rcpputils/thread/detail/posix/thread_func.hpp create mode 100644 include/rcpputils/thread/detail/posix/thread_id.hpp rename include/rcpputils/{threads => thread/detail}/posix/utilities.hpp (79%) create mode 100644 include/rcpputils/thread/detail/std/cpu_set.hpp create mode 100644 include/rcpputils/thread/detail/std/this_thread.hpp rename include/rcpputils/{threads => thread/detail}/std/thread.hpp (71%) rename include/rcpputils/{threads => thread/detail}/std/thread_attribute.hpp (56%) create mode 100644 include/rcpputils/thread/detail/std/thread_id.hpp rename include/rcpputils/{threads/posix => thread/detail}/thread_id.hpp (66%) create mode 100644 include/rcpputils/thread/this_thread.hpp create mode 100644 include/rcpputils/thread/thread.hpp create mode 100644 include/rcpputils/thread/thread_attribute.hpp delete mode 100644 include/rcpputils/threads/posix/thread_func.hpp rename src/{threads => thread/detail}/posix/linux/cpu_set.cpp (60%) create mode 100644 src/thread/detail/posix/thread.cpp create mode 100644 src/thread/detail/posix/thread_attribute.cpp delete mode 100644 src/threads/posix/thread.cpp create mode 100644 test/test_thread.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 8ac3769..fdf0b19 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,8 +34,9 @@ target_include_directories(${PROJECT_NAME} PUBLIC "$") if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") target_sources(${PROJECT_NAME} PRIVATE - src/threads/posix/thread.cpp - src/threads/posix/linux/cpu_set.cpp) + src/thread/detail/posix/thread.cpp + src/thread/detail/posix/thread_attribute.cpp + src/thread/detail/posix/linux/cpu_set.cpp) endif() if(WIN32) target_compile_definitions(${PROJECT_NAME} @@ -145,6 +146,9 @@ if(BUILD_TESTING) ament_add_gtest(test_accumulator test/test_accumulator.cpp) target_link_libraries(test_accumulator ${PROJECT_NAME}) + + ament_add_gtest(test_thread test/test_thread.cpp) + target_link_libraries(test_thread ${PROJECT_NAME}) endif() ament_package() diff --git a/include/rcpputils/threads.hpp b/include/rcpputils/thread.hpp similarity index 72% rename from include/rcpputils/threads.hpp rename to include/rcpputils/thread.hpp index a298a4c..2fdcd57 100644 --- a/include/rcpputils/threads.hpp +++ b/include/rcpputils/thread.hpp @@ -12,13 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCPPUTILS__THREADS_HPP_ -#define RCPPUTILS__THREADS_HPP_ +#ifndef RCPPUTILS__THREAD_HPP_ +#define RCPPUTILS__THREAD_HPP_ -#if defined(__linux__) -#include "rcpputils/threads/posix/thread.hpp" -#else -#include "rcpputils/threads/std/thread.hpp" -#endif +#include "rcpputils/thread/thread.hpp" +#include "rcpputils/thread/thread_attribute.hpp" -#endif // RCPPUTILS__THREADS_HPP_ +#endif // RCPPUTILS__THREAD_HPP_ diff --git a/include/rcpputils/thread/detail/posix/cpu_set.hpp b/include/rcpputils/thread/detail/posix/cpu_set.hpp new file mode 100644 index 0000000..650cdbf --- /dev/null +++ b/include/rcpputils/thread/detail/posix/cpu_set.hpp @@ -0,0 +1,22 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__DETAIL__POSIX__CPU_SET_HPP_ +#define RCPPUTILS__THREAD__DETAIL__POSIX__CPU_SET_HPP_ + +#if __linux__ +#include "rcpputils/thread/detail/posix/linux/cpu_set.hpp" +#endif + +#endif // RCPPUTILS__THREAD__DETAIL__POSIX__CPU_SET_HPP_ diff --git a/include/rcpputils/threads/posix/linux/cpu_set.hpp b/include/rcpputils/thread/detail/posix/linux/cpu_set.hpp similarity index 68% rename from include/rcpputils/threads/posix/linux/cpu_set.hpp rename to include/rcpputils/thread/detail/posix/linux/cpu_set.hpp index 1fb1a64..30ee4a9 100644 --- a/include/rcpputils/threads/posix/linux/cpu_set.hpp +++ b/include/rcpputils/thread/detail/posix/linux/cpu_set.hpp @@ -12,26 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCPPUTILS__THREADS__POSIX__LINUX__CPU_SET_HPP_ -#define RCPPUTILS__THREADS__POSIX__LINUX__CPU_SET_HPP_ +#ifndef RCPPUTILS__THREAD__DETAIL__POSIX__LINUX__CPU_SET_HPP_ +#define RCPPUTILS__THREAD__DETAIL__POSIX__LINUX__CPU_SET_HPP_ #include #include #include #include "rcutils/thread_attr.h" +#include "rcpputils/thread/detail/posix/utilities.hpp" + #include "rcpputils/visibility_control.hpp" -#include "rcpputils/threads/posix/utilities.hpp" namespace rcpputils { -namespace detail -{ - struct CpuSet { - using NativeCpuSetType = cpu_set_t; + using NativeCpuSetType = cpu_set_t *; CpuSet() = default; explicit CpuSet(rcutils_thread_core_affinity_t const & affinity); @@ -45,31 +43,22 @@ struct CpuSet void unset(std::size_t cpu); void clear(); bool is_set(std::size_t cpu) const; + std::size_t count() const; void set_rcutils_thread_core_affinity(rcutils_thread_core_affinity_t const & affinity); - std::size_t max_processors() const - { - return num_proc_; - } - std::size_t alloc_size() const - { - return CPU_ALLOC_SIZE(num_proc_); - } - CpuSet::NativeCpuSetType * native_cpu_set() const - { - return cpu_set_.get(); - } + static std::size_t num_processors(); + CpuSet::NativeCpuSetType native_cpu_set() const; private: void init_cpu_set(); void valid_cpu(std::size_t cpu) const; + static std::size_t alloc_size(); struct CpuSetDeleter { - void operator()(NativeCpuSetType * cpu_set) const; + void operator()(NativeCpuSetType cpu_set) const; }; - std::unique_ptr cpu_set_; - std::size_t num_proc_; + std::unique_ptr cpu_set_; }; inline void swap(CpuSet & a, CpuSet & b) @@ -77,8 +66,6 @@ inline void swap(CpuSet & a, CpuSet & b) a.swap(b); } -} // namespace detail - } // namespace rcpputils -#endif // RCPPUTILS__THREADS__POSIX__LINUX__CPU_SET_HPP_ +#endif // RCPPUTILS__THREAD__DETAIL__POSIX__LINUX__CPU_SET_HPP_ diff --git a/include/rcpputils/threads/posix/thread.hpp b/include/rcpputils/thread/detail/posix/thread.hpp similarity index 70% rename from include/rcpputils/threads/posix/thread.hpp rename to include/rcpputils/thread/detail/posix/thread.hpp index 37c15cf..eb85eea 100644 --- a/include/rcpputils/threads/posix/thread.hpp +++ b/include/rcpputils/thread/detail/posix/thread.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCPPUTILS__THREADS__POSIX__THREAD_HPP_ -#define RCPPUTILS__THREADS__POSIX__THREAD_HPP_ +#ifndef RCPPUTILS__THREAD__DETAIL__POSIX__THREAD_HPP_ +#define RCPPUTILS__THREAD__DETAIL__POSIX__THREAD_HPP_ #include #include @@ -24,14 +24,14 @@ #include #include #include -#include #include #include -#include "rcpputils/threads/posix/thread_attribute.hpp" -#include "rcpputils/threads/posix/thread_func.hpp" -#include "rcpputils/threads/posix/thread_id.hpp" -#include "rcpputils/threads/posix/utilities.hpp" +#include "rcpputils/thread/detail/thread_id.hpp" +#include "rcpputils/thread/detail/posix/thread_attribute.hpp" +#include "rcpputils/thread/detail/posix/thread_func.hpp" +#include "rcpputils/thread/detail/posix/utilities.hpp" + #include "rcpputils/visibility_control.hpp" namespace rcpputils @@ -41,10 +41,9 @@ RCPPUTILS_PUBLIC_TYPE struct Thread { using NativeHandleType = pthread_t; - using Attribute = detail::ThreadAttribute; - using Id = detail::ThreadId; + using Attribute = ThreadAttribute; + using Id = ThreadId; - // Assume pthread_t is an invalid handle if it's 0 Thread() noexcept : handle_{} {} Thread(Thread && other) @@ -68,6 +67,7 @@ struct Thread Thread(Thread const &) = delete; ~Thread() { + // Assume pthread_t is an invalid handle if it's 0 if (handle_) { std::terminate(); } @@ -88,14 +88,13 @@ struct Thread { using std::swap; swap(handle_, other.handle_); - swap(name_, other.name_); } void join() { void * p; int r = pthread_join(handle_, &p); - detail::throw_if_error(r, "Error in pthread_join "); + thread::detail::throw_if_error(r, "error in pthread_join"); handle_ = NativeHandleType{}; } @@ -107,7 +106,7 @@ struct Thread void detach() { int r = pthread_detach(handle_); - detail::throw_if_error(r, "Error in pthread_detach "); + thread::detail::throw_if_error(r, "error in pthread_detach"); handle_ = NativeHandleType{}; } @@ -132,19 +131,17 @@ struct Thread } private: - using ThreadFuncBase = detail::ThreadFuncBase; + using ThreadFuncBase = thread::detail::ThreadFuncBase; template static std::unique_ptr make_thread_func(F && f, Args && ... args) { + using thread::detail::ThreadFunc; + static_assert( !std::is_member_object_pointer_v>, "F is a pointer to member, that has no effect on a thread"); - detail::ThreadFuncBase * func = new detail::ThreadFunc( - [f = std::forward(f), args = std::tuple(std::forward(args)...)]() mutable - { - std::apply(f, args); - }); + ThreadFuncBase * func = new ThreadFunc(std::forward(f), std::forward(args)...); return std::unique_ptr(func); } template @@ -153,15 +150,18 @@ struct Thread F && f, Args && ... args) { + using thread::detail::ThreadFunc; + static_assert( !std::is_member_object_pointer_v>, "F is a pointer to member, that has no effect on a thread"); - detail::ThreadFuncBase * func = new detail::ThreadFunc( - [attr, f = std::forward(f), args = std::tuple(std::forward(args)...)]() mutable + ThreadFuncBase * func = new ThreadFunc( + [](F & f, Attribute & attr, Args & ... args) { - std::apply(f, args); - }); + apply_attr(attr); + std::invoke(f, args ...); + }, std::forward(f), attr, std::forward(args)...); return std::unique_ptr(func); } @@ -170,7 +170,6 @@ struct Thread static void apply_attr(Attribute const & attr); NativeHandleType handle_; - std::string name_; }; inline void swap(Thread & t1, Thread & t2) @@ -178,26 +177,9 @@ inline void swap(Thread & t1, Thread & t2) t1.swap(t2); } -namespace detail -{ -void apply_attr_to_current_thread(ThreadAttribute const & attr); -} - namespace this_thread { -template -void run_with_thread_attribute( - detail::ThreadAttribute const & attr, F && f, Args && ... args) -{ - static_assert( - !std::is_member_object_pointer_v>, - "F is a pointer to member, that has no effect on a thread"); - - detail::apply_attr_to_current_thread(attr); - std::invoke(std::forward(f), std::forward(args)...); -} - inline void yield() noexcept { sched_yield(); @@ -207,4 +189,4 @@ inline void yield() noexcept } // namespace rcpputils -#endif // RCPPUTILS__THREADS__POSIX__THREAD_HPP_ +#endif // RCPPUTILS__THREAD__DETAIL__POSIX__THREAD_HPP_ diff --git a/include/rcpputils/threads/posix/thread_attribute.hpp b/include/rcpputils/thread/detail/posix/thread_attribute.hpp similarity index 56% rename from include/rcpputils/threads/posix/thread_attribute.hpp rename to include/rcpputils/thread/detail/posix/thread_attribute.hpp index 0447dfc..e133f1b 100644 --- a/include/rcpputils/threads/posix/thread_attribute.hpp +++ b/include/rcpputils/thread/detail/posix/thread_attribute.hpp @@ -12,27 +12,61 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCPPUTILS__THREADS__POSIX__THREAD_ATTRIBUTE_HPP_ -#define RCPPUTILS__THREADS__POSIX__THREAD_ATTRIBUTE_HPP_ +#ifndef RCPPUTILS__THREAD__DETAIL__POSIX__THREAD_ATTRIBUTE_HPP_ +#define RCPPUTILS__THREAD__DETAIL__POSIX__THREAD_ATTRIBUTE_HPP_ #include -#include + #include #include "rcutils/thread_attr.h" +#include "rcpputils/thread/detail/posix/cpu_set.hpp" #include "rcpputils/visibility_control.hpp" -#ifdef __linux__ -#include "rcpputils/threads/posix/linux/cpu_set.hpp" -#endif - namespace rcpputils { +namespace thread +{ namespace detail { +constexpr unsigned int sched_policy_explicit_bit = 0x8000'0000; + +} +} + +enum struct SchedPolicy : unsigned int +{ + inherit, + other = thread::detail::sched_policy_explicit_bit | SCHED_OTHER, +#ifdef SCHED_FIFO + fifo = thread::detail::sched_policy_explicit_bit | SCHED_FIFO, +#endif +#ifdef SCHED_RR + rr = thread::detail::sched_policy_explicit_bit | SCHED_RR, +#endif +#ifdef SCHED_IDLE + idle = thread::detail::sched_policy_explicit_bit | SCHED_IDLE, +#endif +#ifdef SCHED_BATCH + batch = thread::detail::sched_policy_explicit_bit | SCHED_BATCH, +#endif +#ifdef SCHED_SPORADIC + sporadic = thread::detail::sched_policy_explicit_bit | SCHED_SPORADIC, +#endif +// #if __linux__ +// linux deadline scheduler requires more parameter, not supported now +// #ifdef SCHED_DEADLINE +// deadline = SCHED_DEADLINE, +// #endif +// #endif +}; + +SchedPolicy from_rcutils_thread_scheduling_policy( + rcutils_thread_scheduling_policy_t rcutils_sched_policy); + struct ThreadAttribute { ThreadAttribute(); @@ -40,18 +74,11 @@ struct ThreadAttribute ThreadAttribute(const ThreadAttribute &) = default; ThreadAttribute(ThreadAttribute &&) = default; - explicit ThreadAttribute(rcutils_thread_attr_t const & attr) - : cpu_set_(CpuSet(attr.core_affinity)), - sched_policy_(convert_sched_policy(attr.scheduling_policy)), - priority_(attr.priority), - name_(attr.name) - {} + explicit ThreadAttribute(const rcutils_thread_attr_t & attr); ThreadAttribute & operator=(const ThreadAttribute &) = default; ThreadAttribute & operator=(ThreadAttribute &&) = default; - using NativeAttributeType = pthread_attr_t; - ThreadAttribute & set_affinity(CpuSet cs) { cpu_set_ = std::move(cs); @@ -62,12 +89,12 @@ struct ThreadAttribute return cpu_set_; } - ThreadAttribute & set_sched_policy(rcutils_thread_scheduling_policy_t sp) + ThreadAttribute & set_sched_policy(SchedPolicy policy) { - sched_policy_ = convert_sched_policy(sp); + sched_policy_ = policy; return *this; } - int get_sched_policy() const + SchedPolicy get_sched_policy() const { return sched_policy_; } @@ -102,25 +129,14 @@ struct ThreadAttribute return detached_flag_; } - ThreadAttribute & set_name(std::string name) - { - name_ = std::move(name); - return *this; - } - const std::string & get_name() const - { - return name_; - } - void set_rcutils_thread_attribute( const rcutils_thread_attr_t & attr) { CpuSet cpu_set(attr.core_affinity); set_affinity(std::move(cpu_set)); - set_sched_policy(attr.scheduling_policy); + set_sched_policy(from_rcutils_thread_scheduling_policy(attr.scheduling_policy)); set_priority(attr.priority); - set_name(attr.name); } void @@ -133,57 +149,14 @@ struct ThreadAttribute swap(stack_size_, other.stack_size_); swap(priority_, other.priority_); swap(detached_flag_, other.detached_flag_); - swap(name_, other.name_); } private: CpuSet cpu_set_; - int sched_policy_; + SchedPolicy sched_policy_; std::size_t stack_size_; int priority_; bool detached_flag_; - std::string name_; - - static int convert_sched_policy( - rcutils_thread_scheduling_policy_t sched_policy) - { - switch (sched_policy) { -#ifdef SCHED_FIFO - case RCUTILS_THREAD_SCHEDULING_POLICY_FIFO: - return SCHED_FIFO; -#endif -#ifdef SCHED_RR - case RCUTILS_THREAD_SCHEDULING_POLICY_RR: - return SCHED_RR; -#endif -#ifdef SCHED_OTHER - case RCUTILS_THREAD_SCHEDULING_POLICY_OTHER: - return SCHED_OTHER; -#endif -#ifdef SCHED_IDLE - case RCUTILS_THREAD_SCHEDULING_POLICY_IDLE: - return SCHED_IDLE; -#endif -#ifdef SCHED_BATCH - case RCUTILS_THREAD_SCHEDULING_POLICY_BATCH: - return SCHED_BATCH; -#endif -#ifdef SCHED_SPORADIC - case RCUTILS_THREAD_SCHEDULING_POLICY_SPORADIC: - return SCHED_SPORADIC; -#endif - /* Todo: Necessity and setting method need to be considered - #ifdef SCHED_DEADLINE - case RCUTILS_THREAD_SCHEDULING_POLICY_DEADLINE: - return SCHED_DEADLINE; - break; - #endif - */ - default: - throw std::runtime_error("Invalid scheduling policy"); - } - return -1; - } }; inline void swap(ThreadAttribute & a, ThreadAttribute & b) @@ -191,8 +164,6 @@ inline void swap(ThreadAttribute & a, ThreadAttribute & b) a.swap(b); } -} // namespace detail - } // namespace rcpputils -#endif // RCPPUTILS__THREADS__POSIX__THREAD_ATTRIBUTE_HPP_ +#endif // RCPPUTILS__THREAD__DETAIL__POSIX__THREAD_ATTRIBUTE_HPP_ diff --git a/include/rcpputils/thread/detail/posix/thread_func.hpp b/include/rcpputils/thread/detail/posix/thread_func.hpp new file mode 100644 index 0000000..da17539 --- /dev/null +++ b/include/rcpputils/thread/detail/posix/thread_func.hpp @@ -0,0 +1,68 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__DETAIL__POSIX__THREAD_FUNC_HPP_ +#define RCPPUTILS__THREAD__DETAIL__POSIX__THREAD_FUNC_HPP_ + +#include +#include + +namespace rcpputils +{ +namespace thread +{ +namespace detail +{ + +template +struct ThreadArg +{ + Arg arg_; +}; + +struct ThreadFuncBase +{ + virtual ~ThreadFuncBase() = default; + virtual void run() = 0; +}; + +template +struct ThreadFunc; +template +struct ThreadFunc, Args...> + : ThreadFuncBase, private ThreadArg... +{ + template + explicit ThreadFunc(G && g, As && ... args) + : ThreadArg{std::forward(args)}..., func_{std::forward(g)} + {} + +private: + void run() override + { + std::invoke(func_, ThreadArg::arg_ ...); + } + + F func_; +}; + +template +ThreadFunc(F &&, Args && ...)->ThreadFunc< + std::decay_t, std::index_sequence_for, std::decay_t...>; + +} // namespace detail +} // namespace thread +} // namespace rcpputils + +#endif // RCPPUTILS__THREAD__DETAIL__POSIX__THREAD_FUNC_HPP_ diff --git a/include/rcpputils/thread/detail/posix/thread_id.hpp b/include/rcpputils/thread/detail/posix/thread_id.hpp new file mode 100644 index 0000000..5df8be9 --- /dev/null +++ b/include/rcpputils/thread/detail/posix/thread_id.hpp @@ -0,0 +1,47 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__DETAIL__POSIX__THREAD_ID_HPP_ +#define RCPPUTILS__THREAD__DETAIL__POSIX__THREAD_ID_HPP_ + +#include + +namespace rcpputils +{ + +namespace thread +{ + +namespace detail +{ + +using NativeIdType = pthread_t; + +inline bool id_equal(NativeIdType id1, NativeIdType id2) +{ + return pthread_equal(id1, id2) != 0; +} + +inline NativeIdType get_native_thread_id() noexcept +{ + return pthread_self(); +} + +} // namespace detail + +} // namespace thread + +} // namespace rcpputils + +#endif // RCPPUTILS__THREAD__DETAIL__POSIX__THREAD_ID_HPP_ diff --git a/include/rcpputils/threads/posix/utilities.hpp b/include/rcpputils/thread/detail/posix/utilities.hpp similarity index 79% rename from include/rcpputils/threads/posix/utilities.hpp rename to include/rcpputils/thread/detail/posix/utilities.hpp index 4d9b054..28d31e8 100644 --- a/include/rcpputils/threads/posix/utilities.hpp +++ b/include/rcpputils/thread/detail/posix/utilities.hpp @@ -12,18 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCPPUTILS__THREADS__POSIX__UTILITIES_HPP_ -#define RCPPUTILS__THREADS__POSIX__UTILITIES_HPP_ +#ifndef RCPPUTILS__THREAD__DETAIL__POSIX__UTILITIES_HPP_ +#define RCPPUTILS__THREAD__DETAIL__POSIX__UTILITIES_HPP_ #include namespace rcpputils { - -namespace detail +namespace thread { - -namespace +namespace detail { inline void throw_if_error(int r, char const * msg) @@ -33,10 +31,8 @@ inline void throw_if_error(int r, char const * msg) } } -} // namespace - } // namespace detail - +} // namespace thread } // namespace rcpputils -#endif // RCPPUTILS__THREADS__POSIX__UTILITIES_HPP_ +#endif // RCPPUTILS__THREAD__DETAIL__POSIX__UTILITIES_HPP_ diff --git a/include/rcpputils/thread/detail/std/cpu_set.hpp b/include/rcpputils/thread/detail/std/cpu_set.hpp new file mode 100644 index 0000000..a29d48d --- /dev/null +++ b/include/rcpputils/thread/detail/std/cpu_set.hpp @@ -0,0 +1,80 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__DETAIL__STD__CPU_SET_HPP_ +#define RCPPUTILS__THREAD__DETAIL__STD__CPU_SET_HPP_ + +#include +#include + +#include "rcutils/thread_attr.h" +#include "rcpputils/visibility_control.hpp" + +namespace rcpputils +{ + +namespace detail +{ + +struct EmptyType {}; + +struct CpuSet +{ + using NativeCpuSetType = EmptyType; + CpuSet() {} + explicit CpuSet(const rcutils_thread_core_affinity_t &) {} + CpuSet(const CpuSet &) {} + CpuSet & operator=(const CpuSet &) + { + return *this; + } + CpuSet(CpuSet &&) = delete; + CpuSet & operator=(CpuSet &&) = delete; + ~CpuSet() {} + void swap(CpuSet &) {} + void set(std::size_t) {} + void unset(std::size_t) {} + void clear() {} + bool is_set(std::size_t) + { + return false; + } + std::size_t count() + { + return 0; + } + void set_rcutils_thread_core_affinity(rcutils_thread_core_affinity_t const &) {} + static std::size_t num_processors() + { + return std::thread::hardware_concurrency(); + } + NativeCpuSetType native_cpu_set() const + { + return EmptyType{}; + } +}; + +inline void swap(CpuSet & a, CpuSet & b) +{ + a.swap(b); +} + +} // namespace detail + +using detail::CpuSet; +using detail::swap; + +} // namespace rcpputils + +#endif // RCPPUTILS__THREAD__DETAIL__STD__CPU_SET_HPP_ diff --git a/include/rcpputils/thread/detail/std/this_thread.hpp b/include/rcpputils/thread/detail/std/this_thread.hpp new file mode 100644 index 0000000..53f7f07 --- /dev/null +++ b/include/rcpputils/thread/detail/std/this_thread.hpp @@ -0,0 +1,35 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__DETAIL__STD__THIS_THREAD_HPP_ +#define RCPPUTILS__THREAD__DETAIL__STD__THIS_THREAD_HPP_ + +#include + +namespace rcpputils +{ + +namespace this_thread +{ + +inline void yield() noexcept +{ + std::this_thread::yield(); +} + +} // namespace this_thread + +} // namespace rcpputils + +#endif // RCPPUTILS__THREAD__DETAIL__STD__THIS_THREAD_HPP_ diff --git a/include/rcpputils/threads/std/thread.hpp b/include/rcpputils/thread/detail/std/thread.hpp similarity index 71% rename from include/rcpputils/threads/std/thread.hpp rename to include/rcpputils/thread/detail/std/thread.hpp index c0ac507..71e1846 100644 --- a/include/rcpputils/threads/std/thread.hpp +++ b/include/rcpputils/thread/detail/std/thread.hpp @@ -12,32 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCPPUTILS__THREADS__STD__THREAD_HPP_ -#define RCPPUTILS__THREADS__STD__THREAD_HPP_ +#ifndef RCPPUTILS__THREAD__DETAIL__STD__THREAD_HPP_ +#define RCPPUTILS__THREAD__DETAIL__STD__THREAD_HPP_ -#include #include -#include -#include -#include -#include -#include -#include -#include #include #include -#include "rcpputils/threads/std/thread_attribute.hpp" -#include "rclcpp/visibility_control.hpp" +#include "rcpputils/thread/detail/thread_id.hpp" +#include "rcpputils/thread/detail/std/thread_attribute.hpp" + +#include "rcpputils/visibility_control.hpp" namespace rcpputils { +RCPPUTILS_PUBLIC_TYPE struct Thread { using NativeHandleType = std::thread::native_handle_type; - using Attribute = detail::ThreadAttribute; - using Id = std::thread::id; + using Attribute = ThreadAttribute; + using Id = ThreadId; Thread() noexcept : thread_{} @@ -104,7 +99,7 @@ struct Thread Id get_id() const noexcept { - return thread_.get_id(); + return Id{thread_.get_id()}; } static int hardware_concurrency() noexcept @@ -124,22 +119,13 @@ inline void swap(Thread & t1, Thread & t2) namespace this_thread { -template -void run_with_thread_attribute(Thread::Attribute & attr, F && f, Args && ... args) +inline void yield() noexcept { - static_assert( - !std::is_member_object_pointer_v>, - "F is a pointer to member, that is ineffective on thread"); - - if (attr.set_unavailable_items_) { - throw std::runtime_error("std::thread can't set thread attribute"); - } - - std::invoke(f, std::forward(args)...); + std::this_thread::yield(); } } // namespace this_thread } // namespace rcpputils -#endif // RCPPUTILS__THREADS__STD__THREAD_HPP_ +#endif // RCPPUTILS__THREAD__DETAIL__STD__THREAD_HPP_ diff --git a/include/rcpputils/threads/std/thread_attribute.hpp b/include/rcpputils/thread/detail/std/thread_attribute.hpp similarity index 56% rename from include/rcpputils/threads/std/thread_attribute.hpp rename to include/rcpputils/thread/detail/std/thread_attribute.hpp index b799cd6..bfa9e83 100644 --- a/include/rcpputils/threads/std/thread_attribute.hpp +++ b/include/rcpputils/thread/detail/std/thread_attribute.hpp @@ -12,70 +12,33 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCPPUTILS__THREADS__STD__THREAD_ATTRIBUTE_HPP_ -#define RCPPUTILS__THREADS__STD__THREAD_ATTRIBUTE_HPP_ +#ifndef RCPPUTILS__THREAD__DETAIL__STD__THREAD_ATTRIBUTE_HPP_ +#define RCPPUTILS__THREAD__DETAIL__STD__THREAD_ATTRIBUTE_HPP_ -#include -#include #include -#include "rcl_yaml_param_parser/types.h" -#include "rclcpp/visibility_control.hpp" +#include "rcutils/thread_attr.h" + +#include "rcpputils/thread/detail/std/cpu_set.hpp" +#include "rcpputils/visibility_control.hpp" namespace rcpputils { struct Thread; -namespace detail -{ -struct ThreadAttribute; -} // namespace detail - -namespace this_thread -{ -template -void run_with_thread_attribute( - detail::ThreadAttribute & attr, F && f, Args && ... args); -} // namespace this_thread - -namespace detail +enum struct SchedPolicy : unsigned { +}; -struct CpuSet +SchedPolicy from_rcutils_thread_scheduling_policy( + rcutils_thread_scheduling_policy_t) { - using NativeCpuSetType = std::size_t; - CpuSet() {} - explicit CpuSet(std::size_t) {} - CpuSet(const CpuSet &) {} - CpuSet & operator=(const CpuSet &) - { - return *this; - } - CpuSet(CpuSet &&) = delete; - CpuSet & operator=(CpuSet &&) = delete; - ~CpuSet() {} - void set(std::size_t) {} - void unset(std::size_t) {} - void clear() {} - bool is_set(std::size_t) - { - return false; - } - std::size_t get_max_processors() const - { - return 0; - } - NativeCpuSetType native_cpu_set() const - { - return 0; - } -}; + return SchedPolicy{}; +} struct ThreadAttribute { - using PriorityType = int; - ThreadAttribute() : set_unavailable_items_(false), run_as_detached_(false) {} @@ -125,18 +88,8 @@ struct ThreadAttribute return run_as_detached_; } - ThreadAttribute & set_name(std::string const &) - { - set_unavailable_items_ = true; - return *this; - } - const char * get_name() const - { - return ""; - } - void - set_thread_attribute( + set_rcutils_thread_attribute( const rcutils_thread_attr_t &) { set_unavailable_items_ = true; @@ -149,17 +102,16 @@ struct ThreadAttribute } private: - friend struct rcpputils::Thread; - template - friend void this_thread::run_with_thread_attribute( - ThreadAttribute & attr, F && f, Args && ... args); - + friend struct Thread; bool set_unavailable_items_; bool run_as_detached_; }; -} // namespace detail +inline void swap(ThreadAttribute & a, ThreadAttribute & b) +{ + a.swap(b); +} } // namespace rcpputils -#endif // RCPPUTILS__THREADS__STD__THREAD_ATTRIBUTE_HPP_ +#endif // RCPPUTILS__THREAD__DETAIL__STD__THREAD_ATTRIBUTE_HPP_ diff --git a/include/rcpputils/thread/detail/std/thread_id.hpp b/include/rcpputils/thread/detail/std/thread_id.hpp new file mode 100644 index 0000000..64e162b --- /dev/null +++ b/include/rcpputils/thread/detail/std/thread_id.hpp @@ -0,0 +1,47 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__DETAIL__STD__THREAD_ID_HPP_ +#define RCPPUTILS__THREAD__DETAIL__STD__THREAD_ID_HPP_ + +#include + +namespace rcpputils +{ + +namespace thread +{ + +namespace detail +{ + +using NativeIdType = std::thread::id; + +inline bool id_equal(NativeIdType id1, NativeIdType id2) +{ + return id1 == id2; +} + +inline NativeIdType get_native_thread_id() noexcept +{ + return std::this_thread::get_id(); +} + +} // namespace detail + +} // namespace thread + +} // namespace rcpputils + +#endif // RCPPUTILS__THREAD__DETAIL__STD__THREAD_ID_HPP_ diff --git a/include/rcpputils/threads/posix/thread_id.hpp b/include/rcpputils/thread/detail/thread_id.hpp similarity index 66% rename from include/rcpputils/threads/posix/thread_id.hpp rename to include/rcpputils/thread/detail/thread_id.hpp index 5f46ce7..cd9343b 100644 --- a/include/rcpputils/threads/posix/thread_id.hpp +++ b/include/rcpputils/thread/detail/thread_id.hpp @@ -12,32 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCPPUTILS__THREADS__POSIX__THREAD_ID_HPP_ -#define RCPPUTILS__THREADS__POSIX__THREAD_ID_HPP_ +#ifndef RCPPUTILS__THREAD__DETAIL__THREAD_ID_HPP_ +#define RCPPUTILS__THREAD__DETAIL__THREAD_ID_HPP_ -#include +#include -#include "rcpputils/visibility_control.hpp" - -namespace rcpputils -{ - -#ifdef __clang__ -#pragma clang diagnostic push -#pragma clang diagnostic ignored "-Wmismatched-tags" +#if __linux__ +#include "rcpputils/thread/detail/posix/thread_id.hpp" +#else +#include "rcpputils/thread/detail/std/thread_id.hpp" #endif -struct Thread; - -namespace detail -{ - -namespace thread_id_ns +namespace rcpputils { struct ThreadId; -inline ThreadId get_id() noexcept; inline bool operator==(ThreadId id1, ThreadId id2); inline bool operator!=(ThreadId id1, ThreadId id2); inline bool operator<(ThreadId id1, ThreadId id2); @@ -49,6 +39,11 @@ inline std::basic_ostream & operator<<( std::basic_ostream &, ThreadId); +namespace this_thread +{ +inline ThreadId get_id() noexcept; +} + struct ThreadId { ThreadId() = default; @@ -59,7 +54,7 @@ struct ThreadId friend bool operator==(ThreadId id1, ThreadId id2) { - return pthread_equal(id1.h, id2.h); + return thread::detail::id_equal(id1.h, id2.h); } friend bool operator<(ThreadId id1, ThreadId id2) { @@ -74,19 +69,14 @@ struct ThreadId } private: - friend class rcpputils::Thread; - friend ThreadId get_id() noexcept; + friend class Thread; + friend ThreadId this_thread::get_id() noexcept; friend struct std::hash; - explicit ThreadId(pthread_t h) + explicit ThreadId(thread::detail::NativeIdType h) : h(h) {} - pthread_t h; + thread::detail::NativeIdType h; }; -ThreadId get_id() noexcept -{ - return ThreadId{pthread_self()}; -} - bool operator!=(ThreadId id1, ThreadId id2) { return !(id1 == id2); @@ -107,23 +97,13 @@ bool operator>=(ThreadId id1, ThreadId id2) return !(id1 < id2); } -} // namespace thread_id_ns - -using thread_id_ns::ThreadId; -using thread_id_ns::operator==; -using thread_id_ns::operator!=; -using thread_id_ns::operator<; // NOLINT -using thread_id_ns::operator>; // NOLINT -using thread_id_ns::operator<=; -using thread_id_ns::operator>=; -using thread_id_ns::operator<<; - -} // namespace detail - namespace this_thread { -using detail::thread_id_ns::get_id; +inline ThreadId get_id() noexcept +{ + return ThreadId{thread::detail::get_native_thread_id()}; +} } // namespace this_thread @@ -133,14 +113,10 @@ namespace std { template<> -struct hash -{ - std::size_t operator()(rcpputils::detail::thread_id_ns::ThreadId id) - { - return id.h; - } -}; +struct hash + : hash +{}; } // namespace std -#endif // RCPPUTILS__THREADS__POSIX__THREAD_ID_HPP_ +#endif // RCPPUTILS__THREAD__DETAIL__THREAD_ID_HPP_ diff --git a/include/rcpputils/thread/this_thread.hpp b/include/rcpputils/thread/this_thread.hpp new file mode 100644 index 0000000..557e744 --- /dev/null +++ b/include/rcpputils/thread/this_thread.hpp @@ -0,0 +1,24 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__THIS_THREAD_HPP_ +#define RCPPUTILS__THREAD__THIS_THREAD_HPP_ + +#if __linux__ +#include "rcpputils/thread/detail/posix/this_thread.hpp" +#else +#include "rcpputils/thread/detail/std/this_thread.hpp" +#endif + +#endif // RCPPUTILS__THREAD__THIS_THREAD_HPP_ diff --git a/include/rcpputils/thread/thread.hpp b/include/rcpputils/thread/thread.hpp new file mode 100644 index 0000000..bc959d1 --- /dev/null +++ b/include/rcpputils/thread/thread.hpp @@ -0,0 +1,24 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__THREAD_HPP_ +#define RCPPUTILS__THREAD__THREAD_HPP_ + +#if __linux__ +#include "rcpputils/thread/detail/posix/thread.hpp" +#else +#include "rcpputils/thread/detail/std/thread.hpp" +#endif + +#endif // RCPPUTILS__THREAD__THREAD_HPP_ diff --git a/include/rcpputils/thread/thread_attribute.hpp b/include/rcpputils/thread/thread_attribute.hpp new file mode 100644 index 0000000..f8bea35 --- /dev/null +++ b/include/rcpputils/thread/thread_attribute.hpp @@ -0,0 +1,24 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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__THREAD_ATTRIBUTE_HPP_ +#define RCPPUTILS__THREAD__THREAD_ATTRIBUTE_HPP_ + +#if __linux__ +#include "rcpputils/thread/detail/posix/thread_attribute.hpp" +#else +#include "rcpputils/thread/detail/std/thread_attribute.hpp" +#endif + +#endif // RCPPUTILS__THREAD__THREAD_ATTRIBUTE_HPP_ diff --git a/include/rcpputils/threads/posix/thread_func.hpp b/include/rcpputils/threads/posix/thread_func.hpp deleted file mode 100644 index ec4ebd6..0000000 --- a/include/rcpputils/threads/posix/thread_func.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2023 eSOL Co.,Ltd. -// -// 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__THREADS__POSIX__THREAD_FUNC_HPP_ -#define RCPPUTILS__THREADS__POSIX__THREAD_FUNC_HPP_ - -#include -#include -#include - -namespace rcpputils::detail -{ - -struct ThreadFuncBase -{ - virtual ~ThreadFuncBase() = default; - virtual void run() = 0; -}; - -template -struct ThreadFunc : ThreadFuncBase -{ - template - explicit ThreadFunc(G && g) - : func_(std::forward(g)) - {} - -private: - void run() override - { - func_(); - } - - F func_; -}; - -template -ThreadFunc(F &&)->ThreadFunc>; - -} // namespace rcpputils::detail - -#endif // RCPPUTILS__THREADS__POSIX__THREAD_FUNC_HPP_ diff --git a/src/threads/posix/linux/cpu_set.cpp b/src/thread/detail/posix/linux/cpu_set.cpp similarity index 60% rename from src/threads/posix/linux/cpu_set.cpp rename to src/thread/detail/posix/linux/cpu_set.cpp index 1212db2..029ee4e 100644 --- a/src/threads/posix/linux/cpu_set.cpp +++ b/src/thread/detail/posix/linux/cpu_set.cpp @@ -12,34 +12,46 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rcpputils/threads/posix/linux/cpu_set.hpp" +#include "rcpputils/thread/detail/posix/linux/cpu_set.hpp" #include #include + +#include #include #include "rcutils/thread_attr.h" +#include "rcpputils/thread/detail/posix/utilities.hpp" -namespace rcpputils +static inline std::size_t as_mem_size(std::size_t n) { + return (n + CHAR_BIT - 1) / CHAR_BIT; +} -namespace detail +namespace rcpputils { -CpuSet::CpuSet(rcutils_thread_core_affinity_t const & affinity) +using thread::detail::throw_if_error; + +CpuSet::CpuSet(rcutils_thread_core_affinity_t const & src_set) { init_cpu_set(); CPU_ZERO_S(alloc_size(), cpu_set_.get()); - std::size_t size = std::min(CPU_ALLOC_SIZE(affinity.core_count), alloc_size()); + std::size_t dst_core_count = num_processors(); + std::size_t dst_mem_size = alloc_size(); + std::size_t src_mem_size = as_mem_size(src_set.core_count); + std::size_t copy_size = std::min(src_mem_size, dst_mem_size); // this memcpy dependent to structure of cpu_set_t that only have integer array used as bitset. - memcpy(cpu_set_.get(), affinity.set, size); - if (affinity.core_count > num_proc_) { - for (std::size_t i = num_proc_; i < affinity.core_count; ++i) { - if (rcutils_thread_core_affinity_is_set(&affinity, i)) { + memcpy(cpu_set_.get(), src_set.set, copy_size); + if (src_set.core_count > dst_core_count) { + for (std::size_t i = dst_core_count; i < src_set.core_count; ++i) { + if (rcutils_thread_core_affinity_is_set(&src_set, i)) { auto ec = std::make_error_code(std::errc::invalid_argument); throw std::system_error{ec, "invalid cpu number"}; } } + } else { + memset(reinterpret_cast(cpu_set_.get()) + src_mem_size, 0, dst_mem_size - src_mem_size); } } @@ -81,7 +93,6 @@ void CpuSet::swap(CpuSet & other) { using std::swap; swap(cpu_set_, other.cpu_set_); - swap(num_proc_, other.num_proc_); } void CpuSet::set(std::size_t cpu) @@ -115,40 +126,74 @@ bool CpuSet::is_set(std::size_t cpu) const } } +std::size_t CpuSet::count() const +{ + if (cpu_set_) { + return CPU_COUNT_S(num_processors(), cpu_set_.get()); + } else { + return 0; + } +} + void CpuSet::set_rcutils_thread_core_affinity(rcutils_thread_core_affinity_t const & affinity) { CpuSet(affinity).swap(*this); } -void CpuSet::init_cpu_set() +std::size_t CpuSet::num_processors() { - if (cpu_set_) { - return; - } auto num_proc = sysconf(_SC_NPROCESSORS_ONLN); if (num_proc <= 0) { throw_if_error( num_proc, "invalid return value of sysconf(_SC_NPROCESSORS_ONLN)"); } - auto p = CPU_ALLOC(CPU_ALLOC_SIZE(num_proc)); - cpu_set_ = std::unique_ptr(p); - num_proc_ = num_proc; + return num_proc; +} + +CpuSet::NativeCpuSetType CpuSet::native_cpu_set() const +{ + std::size_t num_proc = num_processors(); + cpu_set_t * result = CPU_ALLOC(num_proc); + if (!result) { + throw std::system_error(errno, std::system_category(), "failed to allocate memory"); + } + + std::size_t size = alloc_size(); + if (cpu_set_) { + memcpy(result, cpu_set_.get(), size); + } else { + CPU_ZERO_S(size, result); + } + return result; +} + +void CpuSet::init_cpu_set() +{ + if (cpu_set_) { + return; + } + auto p = CPU_ALLOC(num_processors()); + CPU_ZERO_S(alloc_size(), p); + cpu_set_ = std::unique_ptr(p); } void CpuSet::valid_cpu(std::size_t cpu) const { - if (num_proc_ <= cpu) { + if (num_processors() <= cpu) { auto ec = std::make_error_code(std::errc::invalid_argument); throw std::system_error{ec, "invalid cpu number"}; } } -void CpuSet::CpuSetDeleter::operator()(NativeCpuSetType * cpu_set) const +std::size_t CpuSet::alloc_size() { - CPU_FREE(cpu_set); + return CPU_ALLOC_SIZE(num_processors()); } -} // namespace detail +void CpuSet::CpuSetDeleter::operator()(NativeCpuSetType cpu_set) const +{ + CPU_FREE(cpu_set); +} } // namespace rcpputils diff --git a/src/thread/detail/posix/thread.cpp b/src/thread/detail/posix/thread.cpp new file mode 100644 index 0000000..9c9fc5a --- /dev/null +++ b/src/thread/detail/posix/thread.cpp @@ -0,0 +1,166 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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 + +#if __linux__ +#include +#endif + +#include +#include + +#include "rcpputils/thread/detail/posix/thread.hpp" +#include "rcpputils/thread/detail/posix/utilities.hpp" + +namespace rcpputils +{ + +using thread::detail::ThreadFuncBase, thread::detail::throw_if_error; +using thread::detail::sched_policy_explicit_bit; + +namespace +{ + +void set_pthread_attr(pthread_attr_t & native_attr, ThreadAttribute const & attr); +void * thread_main(void * p); +inline bool is_explicit_sched_policy(int native_policy); +inline int to_native_sched_policy(SchedPolicy policy); + +} // namespace + +Thread::Thread(Attribute const * attr, std::unique_ptr func) +: handle_(NativeHandleType{}) +{ + pthread_attr_t native_attr; + int r = pthread_attr_init(&native_attr); + throw_if_error(r, "error in pthread_attr_init"); + + if (attr != nullptr) { + set_pthread_attr(native_attr, *attr); + } + + NativeHandleType h; + r = pthread_create(&h, &native_attr, thread_main, func.get()); + throw_if_error(r, "error in pthread_create"); + + if (attr == nullptr || !attr->get_run_as_detached()) { + this->handle_ = h; + } + + pthread_attr_destroy(&native_attr); + + func.release(); +} + +void Thread::apply_attr(Attribute const & attr) +{ +#if __linux__ + int r; + SchedPolicy policy = attr.get_sched_policy(); + if (policy == SchedPolicy::inherit) { + return; + } + int native_policy = to_native_sched_policy(policy); + if (native_policy != SCHED_FIFO && native_policy != SCHED_RR && native_policy != SCHED_OTHER) { + sched_param param; + param.sched_priority = attr.get_priority(); + r = pthread_setschedparam(pthread_self(), native_policy, ¶m); + throw_if_error(r, "error in pthread_setschedparam"); + } +#endif // #if __linux__ +} + +namespace +{ + +void * thread_main(void * p) +{ + std::unique_ptr func(reinterpret_cast(p)); + + try { + func->run(); + } catch (...) { + std::cerr << "failed to run thread" << std::endl; + std::terminate(); + } + + return nullptr; +} + +void set_pthread_attr(pthread_attr_t & native_attr, ThreadAttribute const & attr) +{ + int r; + +#if __linux__ + CpuSet cpu_set = attr.get_affinity(); + if (cpu_set.count()) { + std::size_t alloc_size = CPU_ALLOC_SIZE(cpu_set.num_processors()); + CpuSet::NativeCpuSetType native_cpu_set = cpu_set.native_cpu_set(); + if (CPU_COUNT(native_cpu_set) > 0) { + r = pthread_attr_setaffinity_np(&native_attr, alloc_size, native_cpu_set); + } + CPU_FREE(native_cpu_set); + throw_if_error(r, "error in pthread_attr_setaffinity_np"); + } +#endif + + std::size_t stack_size = attr.get_stack_size(); + r = pthread_attr_setstacksize(&native_attr, stack_size); + throw_if_error(r, "error in pthread_attr_setstacksize"); + + int flag = attr.get_run_as_detached() ? PTHREAD_CREATE_DETACHED : PTHREAD_CREATE_JOINABLE; + r = pthread_attr_setdetachstate(&native_attr, flag); + throw_if_error(r, "error in pthread_attr_setdetachstate"); + + SchedPolicy policy = attr.get_sched_policy(); + if (policy == SchedPolicy::inherit) { + r = pthread_attr_setinheritsched(&native_attr, PTHREAD_INHERIT_SCHED); + } else { + bool has_attr_sched_option = policy == SchedPolicy::other; +#if defined(SCHED_FIFO) + has_attr_sched_option |= policy == SchedPolicy::fifo; +#endif +#if defined(SCHED_RR) + has_attr_sched_option |= policy == SchedPolicy::rr; +#endif + + if (has_attr_sched_option) { + r = pthread_attr_setinheritsched(&native_attr, PTHREAD_EXPLICIT_SCHED); + throw_if_error(r, "error in pthread_attr_setinheritsched"); + + r = pthread_attr_setschedpolicy(&native_attr, to_native_sched_policy(policy)); + throw_if_error(r, "error in pthread_attr_setschedpolicy"); + + sched_param param; + param.sched_priority = attr.get_priority(); + r = pthread_attr_setschedparam(&native_attr, ¶m); + throw_if_error(r, "error in pthread_attr_setschedparam"); + } + } +} + +bool is_explicit_sched_policy(int native_policy) +{ + return (static_cast(native_policy) & sched_policy_explicit_bit) != 0; +} + +int to_native_sched_policy(rcpputils::SchedPolicy policy) +{ + return static_cast(policy) & ~sched_policy_explicit_bit; +} + +} // namespace + +} // namespace rcpputils diff --git a/src/thread/detail/posix/thread_attribute.cpp b/src/thread/detail/posix/thread_attribute.cpp new file mode 100644 index 0000000..a475c58 --- /dev/null +++ b/src/thread/detail/posix/thread_attribute.cpp @@ -0,0 +1,129 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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/thread/detail/posix/thread_attribute.hpp" + +#include + +#include "rcpputils/scope_exit.hpp" +#include "rcpputils/thread/detail/posix/utilities.hpp" + +namespace rcpputils +{ + +static inline SchedPolicy from_native_sched_policy(int native_policy); + +using thread::detail::throw_if_error; +using thread::detail::sched_policy_explicit_bit; + +ThreadAttribute::ThreadAttribute() +{ + pthread_attr_t pt_attr; + int r; + int native_policy; + + r = pthread_attr_init(&pt_attr); + throw_if_error(r, "error in pthread_attr_init"); + + RCPPUTILS_SCOPE_EXIT(pthread_attr_destroy(&pt_attr)); + + r = pthread_attr_getschedpolicy(&pt_attr, &native_policy); + throw_if_error(r, "error in pthread_attr_getschedpolicy"); + + int explicit_sched; + r = pthread_attr_getinheritsched(&pt_attr, &explicit_sched); + throw_if_error(r, "error in pthread_attr_getinheritedsched"); + if (explicit_sched == PTHREAD_EXPLICIT_SCHED) { + sched_policy_ = from_native_sched_policy(native_policy); + } else { + sched_policy_ = SchedPolicy::inherit; + } + + r = pthread_attr_getstacksize(&pt_attr, &stack_size_); + throw_if_error(r, "error in pthread_attr_getstacksize"); + + sched_param param; + r = pthread_attr_getschedparam(&pt_attr, ¶m); + throw_if_error(r, "error in pthread_attr_getschedparam"); + priority_ = param.sched_priority; + + int flag; + r = pthread_attr_getdetachstate(&pt_attr, &flag); + throw_if_error(r, "error in pthread_attr_getdetachstate"); + detached_flag_ = (flag == PTHREAD_CREATE_DETACHED); +} + +ThreadAttribute::ThreadAttribute(const rcutils_thread_attr_t & attr) +: cpu_set_(CpuSet(attr.core_affinity)), + sched_policy_(from_rcutils_thread_scheduling_policy(attr.scheduling_policy)), + priority_(attr.priority) +{ + pthread_attr_t pt_attr; + int r; + + r = pthread_attr_init(&pt_attr); + throw_if_error(r, "error in pthread_attr_init"); + + RCPPUTILS_SCOPE_EXIT(pthread_attr_destroy(&pt_attr)); + + r = pthread_attr_getstacksize(&pt_attr, &stack_size_); + throw_if_error(r, "error in pthread_attr_getstacksize"); + + int flag; + r = pthread_attr_getdetachstate(&pt_attr, &flag); + throw_if_error(r, "error in pthread_attr_getdetachstate"); + detached_flag_ = (flag == PTHREAD_CREATE_DETACHED); +} + +SchedPolicy from_rcutils_thread_scheduling_policy( + rcutils_thread_scheduling_policy_t rcutils_sched_policy) +{ + switch (rcutils_sched_policy) { + case RCUTILS_THREAD_SCHEDULING_POLICY_OTHER: + return SchedPolicy::other; +#ifdef SCHED_FIFO + case RCUTILS_THREAD_SCHEDULING_POLICY_FIFO: + return SchedPolicy::fifo; +#endif +#ifdef SCHED_RR + case RCUTILS_THREAD_SCHEDULING_POLICY_RR: + return SchedPolicy::rr; +#endif +#ifdef SCHED_IDLE + case RCUTILS_THREAD_SCHEDULING_POLICY_IDLE: + return SchedPolicy::idle; +#endif +#ifdef SCHED_BATCH + case RCUTILS_THREAD_SCHEDULING_POLICY_BATCH: + return SchedPolicy::batch; +#endif +#ifdef SCHED_SPORADIC + case RCUTILS_THREAD_SCHEDULING_POLICY_SPORADIC: + return SchedPolicy::sporadic; +#endif +// #ifdef SCHED_DEADLINE +// case RCUTILS_THREAD_SCHEDULING_POLICY_DEADLINE: +// return SCHED_DEADLINE; +// #endif + default: + throw std::invalid_argument("Invalid scheduling policy"); + } +} + +SchedPolicy from_native_sched_policy(int native_policy) +{ + return SchedPolicy(native_policy | sched_policy_explicit_bit); +} + +} // namespace rcpputils diff --git a/src/threads/posix/thread.cpp b/src/threads/posix/thread.cpp deleted file mode 100644 index e60b848..0000000 --- a/src/threads/posix/thread.cpp +++ /dev/null @@ -1,183 +0,0 @@ -// Copyright 2023 eSOL Co.,Ltd. -// -// 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 "rcpputils/threads/posix/thread.hpp" -#include "rcpputils/threads/posix/utilities.hpp" - - -namespace rcpputils -{ - -namespace detail -{ - -namespace -{ -void set_pthread_attr(pthread_attr_t & native_attr, rcpputils::Thread::Attribute const & attr); -void * thread_main(void * p); -} // namespace - -} // namespace detail - -Thread::Thread(Attribute const * attr, std::unique_ptr func) -: handle_(NativeHandleType{}), name_(attr ? attr->get_name() : std::string{}) -{ - Attribute::NativeAttributeType native_attr; - int r = pthread_attr_init(&native_attr); - detail::throw_if_error(r, "Error in pthread_attr_init "); - - if (attr != nullptr) { - detail::set_pthread_attr(native_attr, *attr); - } - - NativeHandleType h; - r = pthread_create(&h, &native_attr, detail::thread_main, func.get()); - detail::throw_if_error(r, "Error in pthread_create "); - - if (attr == nullptr || !attr->get_run_as_detached()) { - this->handle_ = h; - } - - pthread_attr_destroy(&native_attr); - - func.release(); -} - -void Thread::apply_attr(Attribute const & attr) -{ - int r; - int policy = attr.get_sched_policy(); -#if __linux__ - if (policy != SCHED_FIFO && policy != SCHED_RR && policy != SCHED_OTHER) { - sched_param param; - param.sched_priority = attr.get_priority(); - r = pthread_setschedparam(pthread_self(), policy, ¶m); - detail::throw_if_error(r, "Error in pthread_setschedparam "); - } -#endif // #if __linux__ -} - -namespace detail -{ - -ThreadAttribute::ThreadAttribute() -{ - NativeAttributeType attr; - int r; - - r = pthread_attr_init(&attr); - throw_if_error(r, "Error in pthread_attr_init "); - - r = pthread_attr_getschedpolicy(&attr, &sched_policy_); - throw_if_error(r, "Error in pthread_attr_getschedpolicy "); - - r = pthread_attr_getstacksize(&attr, &stack_size_); - throw_if_error(r, "Error in pthread_attr_getstacksize "); - - sched_param param; - r = pthread_attr_getschedparam(&attr, ¶m); - throw_if_error(r, "Error in pthread_attr_getschedparam "); - priority_ = param.sched_priority; - - int flag; - r = pthread_attr_getdetachstate(&attr, &flag); - throw_if_error(r, "Error in pthread_attr_getdetachstate "); - detached_flag_ = (flag == PTHREAD_CREATE_DETACHED); - - pthread_attr_destroy(&attr); -} - - -void apply_attr_to_current_thread(ThreadAttribute const & attr) -{ - int r; - -#if __linux__ - CpuSet cpu_set = attr.get_affinity(); - CpuSet::NativeCpuSetType * native_cpu_set = cpu_set.native_cpu_set(); - if (native_cpu_set) { - std::size_t alloc_size = cpu_set.alloc_size(); - r = pthread_setaffinity_np(pthread_self(), alloc_size, native_cpu_set); - throw_if_error(r, "Error in sched_setaffinity "); - } -#endif // #if __linux__ - - sched_param param; - param.sched_priority = attr.get_priority(); - int policy = attr.get_sched_policy(); - r = pthread_setschedparam(pthread_self(), policy, ¶m); - throw_if_error(r, "Error in sched_setscheduler"); -} - -namespace -{ - -void * thread_main(void * p) -{ - std::unique_ptr func(reinterpret_cast(p)); - - try { - func->run(); - } catch (...) { - std::cerr << "failed to run thread" << std::endl; - std::terminate(); - } - - return nullptr; -} - -void set_pthread_attr(pthread_attr_t & native_attr, Thread::Attribute const & attr) -{ - int r; - -#if defined(__linux__) - CpuSet affinity = attr.get_affinity(); - size_t cpu_size = CPU_ALLOC_SIZE(static_cast(sysconf(_SC_NPROCESSORS_ONLN))); - r = pthread_attr_setaffinity_np(&native_attr, cpu_size, affinity.native_cpu_set()); - throw_if_error(r, "Error in pthread_attr_setaffinity_np "); -#endif - - std::size_t stack_size = attr.get_stack_size(); - r = pthread_attr_setstacksize(&native_attr, stack_size); - throw_if_error(r, "Error in pthread_attr_setstacksize "); - - int flag = attr.get_run_as_detached() ? PTHREAD_CREATE_DETACHED : PTHREAD_CREATE_JOINABLE; - r = pthread_attr_setdetachstate(&native_attr, flag); - throw_if_error(r, "Error in pthread_attr_setdetachstate "); - - int sched_policy = attr.get_sched_policy(); - if (sched_policy == SCHED_FIFO || sched_policy == SCHED_RR) { - r = pthread_attr_setinheritsched(&native_attr, PTHREAD_EXPLICIT_SCHED); - throw_if_error(r, "Error in pthread_attr_setinheritsched "); - - r = pthread_attr_setschedpolicy(&native_attr, sched_policy); - throw_if_error(r, "Error in pthread_attr_setschedpolicy "); - - sched_param param; - param.sched_priority = attr.get_priority(); - r = pthread_attr_setschedparam(&native_attr, ¶m); - throw_if_error(r, "Error in pthread_attr_setschedparam "); - } -} - -} // namespace - -} // namespace detail - -} // namespace rcpputils diff --git a/test/test_thread.cpp b/test/test_thread.cpp new file mode 100644 index 0000000..18b33bb --- /dev/null +++ b/test/test_thread.cpp @@ -0,0 +1,158 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// 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 "rcpputils/thread.hpp" + +using rcpputils::Thread; +using rcpputils::ThreadId; +using rcpputils::ThreadAttribute; +using rcpputils::SchedPolicy; +using rcpputils::CpuSet; + +#if __linux__ +using rcpputils::thread::detail::sched_policy_explicit_bit; +#endif + +TEST(test_thread, basic_run) { + std::atomic pass = false; + Thread thread( + [&] { + pass = true; + }); + thread.join(); + EXPECT_TRUE(pass); +} + +TEST(test_thread, run_with_attribtue) { + std::atomic pass = false; + ThreadAttribute attr; +#if __linux__ + pthread_t parent_thread = pthread_self(); + { + sched_param param; + param.sched_priority = 0; + int r = pthread_setschedparam(parent_thread, SCHED_OTHER, ¶m); + ASSERT_EQ(r, 0); + + attr.set_sched_policy(SchedPolicy::batch); + } +#endif + Thread thread( + attr, + [&] { +#if __linux__ + int policy; + sched_param param; + pthread_t sub_thread = pthread_self(); + int r = pthread_getschedparam(sub_thread, &policy, ¶m); + SchedPolicy rclcpp_policy = SchedPolicy(policy | sched_policy_explicit_bit); + EXPECT_EQ(0, r); + EXPECT_EQ(SchedPolicy::batch, rclcpp_policy); + EXPECT_FALSE(pthread_equal(parent_thread, sub_thread)); +#endif + pass = true; + }); + thread.join(); + EXPECT_TRUE(pass); +} + +TEST(thread, attribute) { + ThreadAttribute attr; + + std::size_t stack_size = attr.get_stack_size(); + EXPECT_NE(0, stack_size); + std::size_t increased_stack_size = stack_size + 4 * 1024 * 1024; + attr.set_stack_size(increased_stack_size); + EXPECT_EQ(increased_stack_size, attr.get_stack_size()); + + // copy + { + ThreadAttribute attr2 = attr; + EXPECT_EQ(increased_stack_size, attr2.get_stack_size()); + } + // swap + { + ThreadAttribute attr2; +#if __linux__ + std::size_t stack_size2 = attr2.get_stack_size(); + const std::size_t increased_stack_size2 = stack_size2 + 2 * 1024 * 1024; + attr2.set_stack_size(increased_stack_size2); +#endif // __linux__ + swap(attr, attr2); + EXPECT_EQ(increased_stack_size, attr2.get_stack_size()); + EXPECT_EQ(increased_stack_size2, attr.get_stack_size()); + } + // convert from rcutils_thread_attr_t + { + rcutils_thread_attr_t rcutils_attr; + rcutils_attr.core_affinity = rcutils_get_zero_initialized_thread_core_affinity(); + rcutils_attr.scheduling_policy = RCUTILS_THREAD_SCHEDULING_POLICY_FIFO; + rcutils_attr.priority = 42; + rcutils_attr.name = NULL; + + attr.set_rcutils_thread_attribute(rcutils_attr); + + EXPECT_EQ(0, attr.get_affinity().count()); + EXPECT_EQ(SchedPolicy::fifo, attr.get_sched_policy()); + EXPECT_EQ(42, attr.get_priority()); + } +} + + +TEST(attribute, cpu_set) { +#if __linux__ + std::size_t n = CpuSet::num_processors(); + CpuSet cpu_set; + for (std::size_t i = 0; i < n; ++i) { + if (i % 3 == 0) { + cpu_set.set(i); + } + } + for (std::size_t i = 0; i < n; ++i) { + EXPECT_EQ(i % 3 == 0, cpu_set.is_set(i)); + } + + // copy + { + CpuSet cpu_set2 = cpu_set; + for (std::size_t i = 0; i < n; ++i) { + EXPECT_EQ(cpu_set.is_set(i), cpu_set2.is_set(i)); + } + } + + // swap + { + CpuSet cpu_set2; + for (std::size_t i = 0; i < n; ++i) { + if (i % 2 == 0) { + cpu_set2.set(i); + } + } + swap(cpu_set, cpu_set2); + for (std::size_t i = 0; i < n; ++i) { + EXPECT_EQ(i % 2 == 0, cpu_set.is_set(i)); + EXPECT_EQ(i % 3 == 0, cpu_set2.is_set(i)); + } + } + + // convert from rcutils_thread_core_affinity_t affinity; +#else + GTEST_SKIP(); +#endif +} From e70ec85fbd69f17b1626c4a4bb3346e1928f16ce Mon Sep 17 00:00:00 2001 From: Shoji Morita Date: Fri, 26 Jan 2024 18:56:46 +0900 Subject: [PATCH 5/6] Modified the structure of member names to reflect the point made on the thread below. https://github.com/ros-infrastructure/rep/pull/385#discussion_r1350512216 Signed-off-by: Shoji Morita --- test/test_thread.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/test_thread.cpp b/test/test_thread.cpp index 18b33bb..cf7b87d 100644 --- a/test/test_thread.cpp +++ b/test/test_thread.cpp @@ -104,7 +104,7 @@ TEST(thread, attribute) { rcutils_attr.core_affinity = rcutils_get_zero_initialized_thread_core_affinity(); rcutils_attr.scheduling_policy = RCUTILS_THREAD_SCHEDULING_POLICY_FIFO; rcutils_attr.priority = 42; - rcutils_attr.name = NULL; + rcutils_attr.tag = NULL; attr.set_rcutils_thread_attribute(rcutils_attr); From 7d35f5fdabff4400ba6787bf5f6455b0aa7f0ba1 Mon Sep 17 00:00:00 2001 From: Shoji Morita Date: Fri, 16 Feb 2024 14:10:27 +0900 Subject: [PATCH 6/6] Added self thread attribute control interface to reflect the point below raised at RTWG. >It would be interesting to have some rcpputils functions to configure real-time attributes for an existing thread. Signed-off-by: Shoji Morita --- CMakeLists.txt | 3 +- .../thread/detail/posix/linux/cpu_set.hpp | 20 ++++-- .../thread/detail/posix/sched_options.hpp | 50 ++++++++++++++ .../thread/detail/posix/sched_policy.hpp | 69 +++++++++++++++++++ .../rcpputils/thread/detail/posix/thread.hpp | 3 + .../thread/detail/posix/thread_attribute.hpp | 41 +---------- .../thread/detail/posix/utilities.hpp | 15 ++++ src/thread/detail/posix/linux/cpu_set.cpp | 12 +++- src/thread/detail/posix/linux/thread.cpp | 66 ++++++++++++++++++ src/thread/detail/posix/linux/thread_impl.hpp | 38 ++++++++++ src/thread/detail/posix/thread.cpp | 25 ++----- 11 files changed, 276 insertions(+), 66 deletions(-) create mode 100644 include/rcpputils/thread/detail/posix/sched_options.hpp create mode 100644 include/rcpputils/thread/detail/posix/sched_policy.hpp create mode 100644 src/thread/detail/posix/linux/thread.cpp create mode 100644 src/thread/detail/posix/linux/thread_impl.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index fdf0b19..6ec4933 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,7 +36,8 @@ if(${CMAKE_SYSTEM_NAME} MATCHES "Linux") target_sources(${PROJECT_NAME} PRIVATE src/thread/detail/posix/thread.cpp src/thread/detail/posix/thread_attribute.cpp - src/thread/detail/posix/linux/cpu_set.cpp) + src/thread/detail/posix/linux/cpu_set.cpp + src/thread/detail/posix/linux/thread.cpp) endif() if(WIN32) target_compile_definitions(${PROJECT_NAME} diff --git a/include/rcpputils/thread/detail/posix/linux/cpu_set.hpp b/include/rcpputils/thread/detail/posix/linux/cpu_set.hpp index 30ee4a9..e33ee69 100644 --- a/include/rcpputils/thread/detail/posix/linux/cpu_set.hpp +++ b/include/rcpputils/thread/detail/posix/linux/cpu_set.hpp @@ -27,6 +27,20 @@ namespace rcpputils { +namespace thread +{ +namespace detail +{ + +struct CpuSetDeleter +{ + void operator()(cpu_set_t * p) const; +}; +using UniqueNativeCpuSet = std::unique_ptr; + +} // namespace detail +} // namespace thread + struct CpuSet { using NativeCpuSetType = cpu_set_t *; @@ -54,11 +68,7 @@ struct CpuSet void init_cpu_set(); void valid_cpu(std::size_t cpu) const; static std::size_t alloc_size(); - struct CpuSetDeleter - { - void operator()(NativeCpuSetType cpu_set) const; - }; - std::unique_ptr cpu_set_; + thread::detail::UniqueNativeCpuSet cpu_set_; }; inline void swap(CpuSet & a, CpuSet & b) diff --git a/include/rcpputils/thread/detail/posix/sched_options.hpp b/include/rcpputils/thread/detail/posix/sched_options.hpp new file mode 100644 index 0000000..ebfdb9a --- /dev/null +++ b/include/rcpputils/thread/detail/posix/sched_options.hpp @@ -0,0 +1,50 @@ +// Copyright 2024 eSOL Co.,Ltd. +// +// 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__DETAIL__POSIX__SCHED_OPTIONS_HPP_ +#define RCPPUTILS__THREAD__DETAIL__POSIX__SCHED_OPTIONS_HPP_ + +#include +#include + +#include "rcpputils/thread/detail/posix/cpu_set.hpp" +#include "rcpputils/thread/detail/posix/sched_policy.hpp" +#include "rcpputils/visibility_control.hpp" + +namespace rcpputils +{ + +struct SchedOptions +{ + void swap(SchedOptions & other) + { + using std::swap; + swap(policy, other.policy); + swap(priority, other.priority); + swap(core_affinity, other.core_affinity); + } + + std::optional priority; + std::optional policy; + std::optional core_affinity; +}; + +inline void swap(SchedOptions & a, SchedOptions & b) +{ + a.swap(b); +} + +} // namespace rcpputils + +#endif // RCPPUTILS__THREAD__DETAIL__POSIX__SCHED_OPTIONS_HPP_ diff --git a/include/rcpputils/thread/detail/posix/sched_policy.hpp b/include/rcpputils/thread/detail/posix/sched_policy.hpp new file mode 100644 index 0000000..9ec9688 --- /dev/null +++ b/include/rcpputils/thread/detail/posix/sched_policy.hpp @@ -0,0 +1,69 @@ +// Copyright 2024 eSOL Co.,Ltd. +// +// 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__DETAIL__POSIX__SCHED_POLICY_HPP_ +#define RCPPUTILS__THREAD__DETAIL__POSIX__SCHED_POLICY_HPP_ + +#include + +#include "rcutils/thread_attr.h" + +#include "rcpputils/visibility_control.hpp" + +namespace rcpputils +{ + +namespace thread +{ +namespace detail +{ + +constexpr unsigned int sched_policy_explicit_bit = 0x8000'0000; + +} // namespace detail +} // namespace thread + +enum struct SchedPolicy : unsigned int +{ + inherit, + other = thread::detail::sched_policy_explicit_bit | SCHED_OTHER, +#ifdef SCHED_FIFO + fifo = thread::detail::sched_policy_explicit_bit | SCHED_FIFO, +#endif +#ifdef SCHED_RR + rr = thread::detail::sched_policy_explicit_bit | SCHED_RR, +#endif +#ifdef SCHED_IDLE + idle = thread::detail::sched_policy_explicit_bit | SCHED_IDLE, +#endif +#ifdef SCHED_BATCH + batch = thread::detail::sched_policy_explicit_bit | SCHED_BATCH, +#endif +#ifdef SCHED_SPORADIC + sporadic = thread::detail::sched_policy_explicit_bit | SCHED_SPORADIC, +#endif +// #if __linux__ +// linux deadline scheduler requires more parameter, not supported now +// #ifdef SCHED_DEADLINE +// deadline = SCHED_DEADLINE, +// #endif +// #endif +}; + +SchedPolicy from_rcutils_thread_scheduling_policy( + rcutils_thread_scheduling_policy_t rcutils_sched_policy); + +} // namespace rcpputils + +#endif // RCPPUTILS__THREAD__DETAIL__POSIX__SCHED_POLICY_HPP_ diff --git a/include/rcpputils/thread/detail/posix/thread.hpp b/include/rcpputils/thread/detail/posix/thread.hpp index eb85eea..cd32e02 100644 --- a/include/rcpputils/thread/detail/posix/thread.hpp +++ b/include/rcpputils/thread/detail/posix/thread.hpp @@ -28,6 +28,7 @@ #include #include "rcpputils/thread/detail/thread_id.hpp" +#include "rcpputils/thread/detail/posix/sched_options.hpp" #include "rcpputils/thread/detail/posix/thread_attribute.hpp" #include "rcpputils/thread/detail/posix/thread_func.hpp" #include "rcpputils/thread/detail/posix/utilities.hpp" @@ -185,6 +186,8 @@ inline void yield() noexcept sched_yield(); } +void apply_sched_options(SchedOptions const & options); + } // namespace this_thread } // namespace rcpputils diff --git a/include/rcpputils/thread/detail/posix/thread_attribute.hpp b/include/rcpputils/thread/detail/posix/thread_attribute.hpp index e133f1b..bd41a18 100644 --- a/include/rcpputils/thread/detail/posix/thread_attribute.hpp +++ b/include/rcpputils/thread/detail/posix/thread_attribute.hpp @@ -22,51 +22,12 @@ #include "rcutils/thread_attr.h" #include "rcpputils/thread/detail/posix/cpu_set.hpp" +#include "rcpputils/thread/detail/posix/sched_policy.hpp" #include "rcpputils/visibility_control.hpp" namespace rcpputils { -namespace thread -{ -namespace detail -{ - -constexpr unsigned int sched_policy_explicit_bit = 0x8000'0000; - -} -} - -enum struct SchedPolicy : unsigned int -{ - inherit, - other = thread::detail::sched_policy_explicit_bit | SCHED_OTHER, -#ifdef SCHED_FIFO - fifo = thread::detail::sched_policy_explicit_bit | SCHED_FIFO, -#endif -#ifdef SCHED_RR - rr = thread::detail::sched_policy_explicit_bit | SCHED_RR, -#endif -#ifdef SCHED_IDLE - idle = thread::detail::sched_policy_explicit_bit | SCHED_IDLE, -#endif -#ifdef SCHED_BATCH - batch = thread::detail::sched_policy_explicit_bit | SCHED_BATCH, -#endif -#ifdef SCHED_SPORADIC - sporadic = thread::detail::sched_policy_explicit_bit | SCHED_SPORADIC, -#endif -// #if __linux__ -// linux deadline scheduler requires more parameter, not supported now -// #ifdef SCHED_DEADLINE -// deadline = SCHED_DEADLINE, -// #endif -// #endif -}; - -SchedPolicy from_rcutils_thread_scheduling_policy( - rcutils_thread_scheduling_policy_t rcutils_sched_policy); - struct ThreadAttribute { ThreadAttribute(); diff --git a/include/rcpputils/thread/detail/posix/utilities.hpp b/include/rcpputils/thread/detail/posix/utilities.hpp index 28d31e8..8f923d8 100644 --- a/include/rcpputils/thread/detail/posix/utilities.hpp +++ b/include/rcpputils/thread/detail/posix/utilities.hpp @@ -15,8 +15,11 @@ #ifndef RCPPUTILS__THREAD__DETAIL__POSIX__UTILITIES_HPP_ #define RCPPUTILS__THREAD__DETAIL__POSIX__UTILITIES_HPP_ +#include #include +#include "rcpputils/thread/detail/posix/sched_policy.hpp" + namespace rcpputils { namespace thread @@ -31,6 +34,18 @@ inline void throw_if_error(int r, char const * msg) } } +using thread::detail::sched_policy_explicit_bit; + +inline bool is_explicit_sched_policy(int native_policy) +{ + return (static_cast(native_policy) & sched_policy_explicit_bit) != 0; +} + +inline int to_native_sched_policy(rcpputils::SchedPolicy policy) +{ + return static_cast(policy) & ~sched_policy_explicit_bit; +} + } // namespace detail } // namespace thread } // namespace rcpputils diff --git a/src/thread/detail/posix/linux/cpu_set.cpp b/src/thread/detail/posix/linux/cpu_set.cpp index 029ee4e..5be8b30 100644 --- a/src/thread/detail/posix/linux/cpu_set.cpp +++ b/src/thread/detail/posix/linux/cpu_set.cpp @@ -175,7 +175,7 @@ void CpuSet::init_cpu_set() } auto p = CPU_ALLOC(num_processors()); CPU_ZERO_S(alloc_size(), p); - cpu_set_ = std::unique_ptr(p); + cpu_set_ = thread::detail::UniqueNativeCpuSet(p); } void CpuSet::valid_cpu(std::size_t cpu) const @@ -191,9 +191,17 @@ std::size_t CpuSet::alloc_size() return CPU_ALLOC_SIZE(num_processors()); } -void CpuSet::CpuSetDeleter::operator()(NativeCpuSetType cpu_set) const + +namespace thread +{ +namespace detail +{ + +void CpuSetDeleter::operator()(cpu_set_t * cpu_set) const { CPU_FREE(cpu_set); } +} // namespace detail +} // namespace thread } // namespace rcpputils diff --git a/src/thread/detail/posix/linux/thread.cpp b/src/thread/detail/posix/linux/thread.cpp new file mode 100644 index 0000000..3497a6a --- /dev/null +++ b/src/thread/detail/posix/linux/thread.cpp @@ -0,0 +1,66 @@ +// Copyright 2024 eSOL Co.,Ltd. +// +// 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/thread/detail/posix/thread.hpp" +#include "rcpputils/thread/detail/posix/utilities.hpp" +#include "./thread_impl.hpp" + +namespace rcpputils +{ + +namespace this_thread +{ + +using thread::detail::UniqueNativeCpuSet; +using thread::detail::make_unique_native_cpu_set; +using thread::detail::throw_if_error; +using thread::detail::to_native_sched_policy; + +void apply_sched_options(SchedOptions const & options) +{ + pid_t tid = gettid(); + if (options.policy) { + int native_sched_policy = to_native_sched_policy(*options.policy); + sched_param param; + int r; + if (options.priority) { + param.sched_priority = *options.priority; + } else { + r = sched_getparam(tid, ¶m); + throw_if_error(r, "error in sched_getparam"); + } + r = sched_setscheduler(tid, native_sched_policy, ¶m); + throw_if_error(r, "error in sched_setscheduler"); + } else if (options.priority) { + sched_param param; + param.sched_priority = *options.priority; + int r = sched_setparam(tid, ¶m); + throw_if_error(r, "error in sched_setparam"); + } + if (options.core_affinity) { + UniqueNativeCpuSet native_cpu_set = make_unique_native_cpu_set(*options.core_affinity); + std::size_t sz = CPU_ALLOC_SIZE(options.core_affinity->count()); + int r = sched_setaffinity(tid, sz, native_cpu_set.get()); + throw_if_error(r, "error in sched_setaffinity"); + } +} + +} // namespace this_thread +} // namespace rcpputils diff --git a/src/thread/detail/posix/linux/thread_impl.hpp b/src/thread/detail/posix/linux/thread_impl.hpp new file mode 100644 index 0000000..1f60691 --- /dev/null +++ b/src/thread/detail/posix/linux/thread_impl.hpp @@ -0,0 +1,38 @@ +// Copyright 2024 eSOL Co.,Ltd. +// +// 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 THREAD__DETAIL__POSIX__LINUX__THREAD_IMPL_HPP_ +#define THREAD__DETAIL__POSIX__LINUX__THREAD_IMPL_HPP_ + +#include + +#include "rcpputils/thread/detail/posix/linux/cpu_set.hpp" + +namespace rcpputils +{ +namespace thread +{ +namespace detail +{ + +inline UniqueNativeCpuSet make_unique_native_cpu_set(CpuSet const & cpu_set) +{ + return UniqueNativeCpuSet{cpu_set.native_cpu_set()}; +} + +} // namespace detail +} // namespace thread +} // namespace rcpputils + +#endif // THREAD__DETAIL__POSIX__LINUX__THREAD_IMPL_HPP_ diff --git a/src/thread/detail/posix/thread.cpp b/src/thread/detail/posix/thread.cpp index 9c9fc5a..46787bc 100644 --- a/src/thread/detail/posix/thread.cpp +++ b/src/thread/detail/posix/thread.cpp @@ -23,20 +23,21 @@ #include "rcpputils/thread/detail/posix/thread.hpp" #include "rcpputils/thread/detail/posix/utilities.hpp" +#if __linux__ +#include "./linux/thread_impl.hpp" +#endif namespace rcpputils { using thread::detail::ThreadFuncBase, thread::detail::throw_if_error; -using thread::detail::sched_policy_explicit_bit; +using thread::detail::to_native_sched_policy; namespace { void set_pthread_attr(pthread_attr_t & native_attr, ThreadAttribute const & attr); void * thread_main(void * p); -inline bool is_explicit_sched_policy(int native_policy); -inline int to_native_sched_policy(SchedPolicy policy); } // namespace @@ -106,12 +107,10 @@ void set_pthread_attr(pthread_attr_t & native_attr, ThreadAttribute const & attr #if __linux__ CpuSet cpu_set = attr.get_affinity(); if (cpu_set.count()) { + namespace impl = thread::detail; + impl::UniqueNativeCpuSet native_cpu_set = impl::make_unique_native_cpu_set(cpu_set); std::size_t alloc_size = CPU_ALLOC_SIZE(cpu_set.num_processors()); - CpuSet::NativeCpuSetType native_cpu_set = cpu_set.native_cpu_set(); - if (CPU_COUNT(native_cpu_set) > 0) { - r = pthread_attr_setaffinity_np(&native_attr, alloc_size, native_cpu_set); - } - CPU_FREE(native_cpu_set); + r = pthread_attr_setaffinity_np(&native_attr, alloc_size, native_cpu_set.get()); throw_if_error(r, "error in pthread_attr_setaffinity_np"); } #endif @@ -151,16 +150,6 @@ void set_pthread_attr(pthread_attr_t & native_attr, ThreadAttribute const & attr } } -bool is_explicit_sched_policy(int native_policy) -{ - return (static_cast(native_policy) & sched_policy_explicit_bit) != 0; -} - -int to_native_sched_policy(rcpputils::SchedPolicy policy) -{ - return static_cast(policy) & ~sched_policy_explicit_bit; -} - } // namespace } // namespace rcpputils