diff --git a/CMakeLists.txt b/CMakeLists.txt index b2a6772..6ec4933 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/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/thread.cpp) +endif() if(WIN32) target_compile_definitions(${PROJECT_NAME} PRIVATE "RCPPUTILS_BUILDING_LIBRARY") @@ -140,6 +147,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/thread.hpp b/include/rcpputils/thread.hpp new file mode 100644 index 0000000..2fdcd57 --- /dev/null +++ b/include/rcpputils/thread.hpp @@ -0,0 +1,21 @@ +// 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_HPP_ +#define RCPPUTILS__THREAD_HPP_ + +#include "rcpputils/thread/thread.hpp" +#include "rcpputils/thread/thread_attribute.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/thread/detail/posix/linux/cpu_set.hpp b/include/rcpputils/thread/detail/posix/linux/cpu_set.hpp new file mode 100644 index 0000000..e33ee69 --- /dev/null +++ b/include/rcpputils/thread/detail/posix/linux/cpu_set.hpp @@ -0,0 +1,81 @@ +// 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__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" + +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 *; + + CpuSet() = default; + 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; + std::size_t count() const; + + void set_rcutils_thread_core_affinity(rcutils_thread_core_affinity_t const & affinity); + + 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(); + thread::detail::UniqueNativeCpuSet cpu_set_; +}; + +inline void swap(CpuSet & a, CpuSet & b) +{ + a.swap(b); +} + +} // namespace rcpputils + +#endif // RCPPUTILS__THREAD__DETAIL__POSIX__LINUX__CPU_SET_HPP_ 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 new file mode 100644 index 0000000..cd32e02 --- /dev/null +++ b/include/rcpputils/thread/detail/posix/thread.hpp @@ -0,0 +1,195 @@ +// 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_HPP_ +#define RCPPUTILS__THREAD__DETAIL__POSIX__THREAD_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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" + +#include "rcpputils/visibility_control.hpp" + +namespace rcpputils +{ + +RCPPUTILS_PUBLIC_TYPE +struct Thread +{ + using NativeHandleType = pthread_t; + using Attribute = ThreadAttribute; + using Id = ThreadId; + + 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() + { + // Assume pthread_t is an invalid handle if it's 0 + 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_); + } + + void join() + { + void * p; + int r = pthread_join(handle_, &p); + thread::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_); + thread::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 = 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"); + + ThreadFuncBase * func = new ThreadFunc(std::forward(f), std::forward(args)...); + return std::unique_ptr(func); + } + template + static std::unique_ptr make_thread_func_with_attr( + Attribute const & attr, + 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"); + + ThreadFuncBase * func = new ThreadFunc( + [](F & f, Attribute & attr, Args & ... args) + { + apply_attr(attr); + std::invoke(f, args ...); + }, std::forward(f), attr, std::forward(args)...); + return std::unique_ptr(func); + } + + Thread(Attribute const * attr, std::unique_ptr func); + + static void apply_attr(Attribute const & attr); + + NativeHandleType handle_; +}; + +inline void swap(Thread & t1, Thread & t2) +{ + t1.swap(t2); +} + +namespace this_thread +{ + +inline void yield() noexcept +{ + sched_yield(); +} + +void apply_sched_options(SchedOptions const & options); + +} // namespace this_thread + +} // namespace rcpputils + +#endif // RCPPUTILS__THREAD__DETAIL__POSIX__THREAD_HPP_ diff --git a/include/rcpputils/thread/detail/posix/thread_attribute.hpp b/include/rcpputils/thread/detail/posix/thread_attribute.hpp new file mode 100644 index 0000000..bd41a18 --- /dev/null +++ b/include/rcpputils/thread/detail/posix/thread_attribute.hpp @@ -0,0 +1,130 @@ +// 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_ATTRIBUTE_HPP_ +#define RCPPUTILS__THREAD__DETAIL__POSIX__THREAD_ATTRIBUTE_HPP_ + +#include + +#include + +#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 +{ + +struct ThreadAttribute +{ + ThreadAttribute(); + + ThreadAttribute(const ThreadAttribute &) = default; + ThreadAttribute(ThreadAttribute &&) = default; + + explicit ThreadAttribute(const rcutils_thread_attr_t & attr); + + ThreadAttribute & operator=(const ThreadAttribute &) = default; + ThreadAttribute & operator=(ThreadAttribute &&) = default; + + ThreadAttribute & set_affinity(CpuSet cs) + { + cpu_set_ = std::move(cs); + return *this; + } + const CpuSet & get_affinity() const + { + return cpu_set_; + } + + ThreadAttribute & set_sched_policy(SchedPolicy policy) + { + sched_policy_ = policy; + return *this; + } + SchedPolicy 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_; + } + + 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(from_rcutils_thread_scheduling_policy(attr.scheduling_policy)); + set_priority(attr.priority); + } + + 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_); + } + +private: + CpuSet cpu_set_; + SchedPolicy sched_policy_; + std::size_t stack_size_; + int priority_; + bool detached_flag_; +}; + +inline void swap(ThreadAttribute & a, ThreadAttribute & b) +{ + a.swap(b); +} + +} // namespace rcpputils + +#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/thread/detail/posix/utilities.hpp b/include/rcpputils/thread/detail/posix/utilities.hpp new file mode 100644 index 0000000..8f923d8 --- /dev/null +++ b/include/rcpputils/thread/detail/posix/utilities.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__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 +{ +namespace detail +{ + +inline void throw_if_error(int r, char const * msg) +{ + if (r != 0) { + throw std::system_error(r, std::system_category(), 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 + +#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/thread/detail/std/thread.hpp b/include/rcpputils/thread/detail/std/thread.hpp new file mode 100644 index 0000000..71e1846 --- /dev/null +++ b/include/rcpputils/thread/detail/std/thread.hpp @@ -0,0 +1,131 @@ +// 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_HPP_ +#define RCPPUTILS__THREAD__DETAIL__STD__THREAD_HPP_ + +#include +#include +#include + +#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 = ThreadAttribute; + using Id = ThreadId; + + 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 Id{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 +{ + +inline void yield() noexcept +{ + std::this_thread::yield(); +} + +} // namespace this_thread + +} // namespace rcpputils + +#endif // RCPPUTILS__THREAD__DETAIL__STD__THREAD_HPP_ diff --git a/include/rcpputils/thread/detail/std/thread_attribute.hpp b/include/rcpputils/thread/detail/std/thread_attribute.hpp new file mode 100644 index 0000000..bfa9e83 --- /dev/null +++ b/include/rcpputils/thread/detail/std/thread_attribute.hpp @@ -0,0 +1,117 @@ +// 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_ATTRIBUTE_HPP_ +#define RCPPUTILS__THREAD__DETAIL__STD__THREAD_ATTRIBUTE_HPP_ + +#include + +#include "rcutils/thread_attr.h" + +#include "rcpputils/thread/detail/std/cpu_set.hpp" +#include "rcpputils/visibility_control.hpp" + +namespace rcpputils +{ + +struct Thread; + +enum struct SchedPolicy : unsigned +{ +}; + +SchedPolicy from_rcutils_thread_scheduling_policy( + rcutils_thread_scheduling_policy_t) +{ + return SchedPolicy{}; +} + +struct ThreadAttribute +{ + 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_; + } + + void + set_rcutils_thread_attribute( + const rcutils_thread_attr_t &) + { + set_unavailable_items_ = true; + } + + void swap( + ThreadAttribute & other) + { + std::swap(*this, other); + } + +private: + friend struct Thread; + bool set_unavailable_items_; + bool run_as_detached_; +}; + +inline void swap(ThreadAttribute & a, ThreadAttribute & b) +{ + a.swap(b); +} + +} // namespace rcpputils + +#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/thread/detail/thread_id.hpp b/include/rcpputils/thread/detail/thread_id.hpp new file mode 100644 index 0000000..cd9343b --- /dev/null +++ b/include/rcpputils/thread/detail/thread_id.hpp @@ -0,0 +1,122 @@ +// 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__THREAD_ID_HPP_ +#define RCPPUTILS__THREAD__DETAIL__THREAD_ID_HPP_ + +#include + +#if __linux__ +#include "rcpputils/thread/detail/posix/thread_id.hpp" +#else +#include "rcpputils/thread/detail/std/thread_id.hpp" +#endif + +namespace rcpputils +{ + +struct ThreadId; + +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); + +namespace this_thread +{ +inline ThreadId get_id() noexcept; +} + +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 thread::detail::id_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 Thread; + friend ThreadId this_thread::get_id() noexcept; + friend struct std::hash; + explicit ThreadId(thread::detail::NativeIdType h) + : h(h) {} + thread::detail::NativeIdType h; +}; + +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 this_thread +{ + +inline ThreadId get_id() noexcept +{ + return ThreadId{thread::detail::get_native_thread_id()}; +} + +} // namespace this_thread + +} // namespace rcpputils + +namespace std +{ + +template<> +struct hash + : hash +{}; + +} // namespace std + +#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/src/thread/detail/posix/linux/cpu_set.cpp b/src/thread/detail/posix/linux/cpu_set.cpp new file mode 100644 index 0000000..5be8b30 --- /dev/null +++ b/src/thread/detail/posix/linux/cpu_set.cpp @@ -0,0 +1,207 @@ +// 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/linux/cpu_set.hpp" + +#include +#include + +#include +#include + +#include "rcutils/thread_attr.h" +#include "rcpputils/thread/detail/posix/utilities.hpp" + +static inline std::size_t as_mem_size(std::size_t n) +{ + return (n + CHAR_BIT - 1) / CHAR_BIT; +} + +namespace rcpputils +{ + +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 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(), 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); + } +} + +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_); +} + +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; + } +} + +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); +} + +std::size_t CpuSet::num_processors() +{ + auto num_proc = sysconf(_SC_NPROCESSORS_ONLN); + if (num_proc <= 0) { + throw_if_error( + num_proc, + "invalid return value of sysconf(_SC_NPROCESSORS_ONLN)"); + } + 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_ = thread::detail::UniqueNativeCpuSet(p); +} + +void CpuSet::valid_cpu(std::size_t cpu) const +{ + if (num_processors() <= cpu) { + auto ec = std::make_error_code(std::errc::invalid_argument); + throw std::system_error{ec, "invalid cpu number"}; + } +} + +std::size_t CpuSet::alloc_size() +{ + return CPU_ALLOC_SIZE(num_processors()); +} + + +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 new file mode 100644 index 0000000..46787bc --- /dev/null +++ b/src/thread/detail/posix/thread.cpp @@ -0,0 +1,155 @@ +// 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" +#if __linux__ +#include "./linux/thread_impl.hpp" +#endif + +namespace rcpputils +{ + +using thread::detail::ThreadFuncBase, thread::detail::throw_if_error; +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); + +} // 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()) { + 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()); + r = pthread_attr_setaffinity_np(&native_attr, alloc_size, native_cpu_set.get()); + 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"); + } + } +} + +} // 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/test/test_thread.cpp b/test/test_thread.cpp new file mode 100644 index 0000000..cf7b87d --- /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.tag = 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 +}