Skip to content

Commit 2734427

Browse files
smorita-esolsmorita-esol
committed
Modified to receive multiple core affinity parameters according to the update of REP-2017 below.
ros-infrastructure/rep#385 Signed-off-by: Shoji Morita <s-morita@esol.co.jp>
1 parent cc3fc4c commit 2734427

5 files changed

Lines changed: 186 additions & 99 deletions

File tree

‎CMakeLists.txt‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,8 @@ target_include_directories(${PROJECT_NAME} PUBLIC
3434
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
3535
if(${CMAKE_SYSTEM_NAME} MATCHES "Linux")
3636
target_sources(${PROJECT_NAME} PRIVATE
37-
src/threads/posix_thread.cpp)
37+
src/threads/posix/thread.cpp
38+
src/threads/posix/linux/cpu_set.cpp)
3839
endif()
3940
if(WIN32)
4041
target_compile_definitions(${PROJECT_NAME}

‎include/rcpputils/threads/posix/linux/cpu_set.hpp‎

Lines changed: 20 additions & 96 deletions
Original file line numberDiff line numberDiff line change
@@ -16,11 +16,10 @@
1616
#define RCPPUTILS__THREADS__POSIX__LINUX__CPU_SET_HPP_
1717

1818
#include <pthread.h>
19-
#include <string.h>
20-
#include <vector>
21-
#include <utility>
2219
#include <memory>
20+
#include <utility>
2321

22+
#include "rcutils/thread_attr.h"
2423
#include "rcpputils/visibility_control.hpp"
2524
#include "rcpputils/threads/posix/utilities.hpp"
2625

@@ -33,75 +32,21 @@ namespace detail
3332
struct CpuSet
3433
{
3534
using NativeCpuSetType = cpu_set_t;
35+
3636
CpuSet() = default;
37-
explicit CpuSet(std::size_t cpu)
38-
{
39-
init_cpu_set();
40-
CPU_ZERO_S(alloc_size(), cpu_set_.get());
41-
CPU_SET_S(cpu, alloc_size(), cpu_set_.get());
42-
}
43-
CpuSet(const CpuSet & other)
44-
{
45-
if (other.cpu_set_) {
46-
init_cpu_set();
47-
memcpy(cpu_set_.get(), other.cpu_set_.get(), alloc_size());
48-
}
49-
}
50-
CpuSet & operator=(const CpuSet & other)
51-
{
52-
if (other.cpu_set_) {
53-
init_cpu_set();
54-
memcpy(cpu_set_.get(), other.cpu_set_.get(), alloc_size());
55-
} else {
56-
clear();
57-
}
58-
return *this;
59-
}
60-
CpuSet(CpuSet && other)
61-
: CpuSet()
62-
{
63-
swap(other);
64-
}
65-
CpuSet & operator=(CpuSet && other)
66-
{
67-
CpuSet tmp;
68-
other.swap(tmp);
69-
tmp.swap(*this);
70-
return *this;
71-
}
72-
void swap(CpuSet & other)
73-
{
74-
using std::swap;
75-
swap(cpu_set_, other.cpu_set_);
76-
swap(num_proc_, other.num_proc_);
77-
}
78-
void set(std::size_t cpu)
79-
{
80-
init_cpu_set();
81-
valid_cpu(cpu);
82-
CPU_SET_S(cpu, alloc_size(), cpu_set_.get());
83-
}
84-
void unset(std::size_t cpu)
85-
{
86-
init_cpu_set();
87-
valid_cpu(cpu);
88-
CPU_CLR_S(cpu, alloc_size(), cpu_set_.get());
89-
}
90-
void clear()
91-
{
92-
if (cpu_set_) {
93-
CPU_ZERO_S(alloc_size(), cpu_set_.get());
94-
}
95-
}
96-
bool is_set(std::size_t cpu) const
97-
{
98-
if (cpu_set_) {
99-
valid_cpu(cpu);
100-
return CPU_ISSET_S(cpu, alloc_size(), cpu_set_.get());
101-
} else {
102-
return false;
103-
}
104-
}
37+
explicit CpuSet(rcutils_thread_core_affinity_t const & affinity);
38+
CpuSet(const CpuSet & other);
39+
CpuSet(CpuSet && other);
40+
41+
CpuSet & operator=(const CpuSet & other);
42+
CpuSet & operator=(CpuSet && other);
43+
void swap(CpuSet & other);
44+
void set(std::size_t cpu);
45+
void unset(std::size_t cpu);
46+
void clear();
47+
bool is_set(std::size_t cpu) const;
48+
49+
void set_rcutils_thread_core_affinity(rcutils_thread_core_affinity_t const & affinity);
10550

10651
std::size_t max_processors() const
10752
{
@@ -111,38 +56,17 @@ struct CpuSet
11156
{
11257
return CPU_ALLOC_SIZE(num_proc_);
11358
}
114-
NativeCpuSetType * native_cpu_set() const
59+
CpuSet::NativeCpuSetType * native_cpu_set() const
11560
{
11661
return cpu_set_.get();
11762
}
11863

11964
private:
120-
void init_cpu_set()
121-
{
122-
if (cpu_set_) {
123-
return;
124-
}
125-
auto num_proc = sysconf(_SC_NPROCESSORS_ONLN);
126-
if (num_proc <= 0) {
127-
throw_if_error(num_proc, "unrecognized sysconf(_SC_NPROCESSORS_ONLN) is not valid");
128-
}
129-
auto p = CPU_ALLOC(CPU_ALLOC_SIZE(num_proc));
130-
cpu_set_ = std::unique_ptr<NativeCpuSetType, CpuSetDeleter>(p);
131-
num_proc_ = num_proc;
132-
}
133-
void valid_cpu(std::size_t cpu) const
134-
{
135-
if (num_proc_ <= cpu) {
136-
auto ec = std::make_error_code(std::errc::invalid_argument);
137-
throw std::system_error{ec, "cpu number is invaild"};
138-
}
139-
}
65+
void init_cpu_set();
66+
void valid_cpu(std::size_t cpu) const;
14067
struct CpuSetDeleter
14168
{
142-
void operator()(NativeCpuSetType * cpu_set) const
143-
{
144-
CPU_FREE(cpu_set);
145-
}
69+
void operator()(NativeCpuSetType * cpu_set) const;
14670
};
14771
std::unique_ptr<NativeCpuSetType, CpuSetDeleter> cpu_set_;
14872
std::size_t num_proc_;

‎include/rcpputils/threads/posix/thread_attribute.hpp‎

Lines changed: 10 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,14 @@ struct ThreadAttribute
3939

4040
ThreadAttribute(const ThreadAttribute &) = default;
4141
ThreadAttribute(ThreadAttribute &&) = default;
42+
43+
explicit ThreadAttribute(rcutils_thread_attr_t const & attr)
44+
: cpu_set_(CpuSet(attr.core_affinity)),
45+
sched_policy_(convert_sched_policy(attr.scheduling_policy)),
46+
priority_(attr.priority),
47+
name_(attr.name)
48+
{}
49+
4250
ThreadAttribute & operator=(const ThreadAttribute &) = default;
4351
ThreadAttribute & operator=(ThreadAttribute &&) = default;
4452

@@ -105,7 +113,7 @@ struct ThreadAttribute
105113
}
106114

107115
void
108-
set_thread_attribute(
116+
set_rcutils_thread_attribute(
109117
const rcutils_thread_attr_t & attr)
110118
{
111119
CpuSet cpu_set(attr.core_affinity);
@@ -136,7 +144,7 @@ struct ThreadAttribute
136144
bool detached_flag_;
137145
std::string name_;
138146

139-
int convert_sched_policy(
147+
static int convert_sched_policy(
140148
rcutils_thread_scheduling_policy_t sched_policy)
141149
{
142150
switch (sched_policy) {

‎src/threads/posix/linux/cpu_set.cpp‎

Lines changed: 154 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,154 @@
1+
// Copyright 2023 eSOL Co.,Ltd.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "rcpputils/threads/posix/linux/cpu_set.hpp"
16+
17+
#include <string.h>
18+
#include <unistd.h>
19+
#include <utility>
20+
21+
#include "rcutils/thread_attr.h"
22+
23+
namespace rcpputils
24+
{
25+
26+
namespace detail
27+
{
28+
29+
CpuSet::CpuSet(rcutils_thread_core_affinity_t const & affinity)
30+
{
31+
init_cpu_set();
32+
CPU_ZERO_S(alloc_size(), cpu_set_.get());
33+
std::size_t size = std::min(CPU_ALLOC_SIZE(affinity.core_count), alloc_size());
34+
// this memcpy dependent to structure of cpu_set_t that only have integer array used as bitset.
35+
memcpy(cpu_set_.get(), affinity.set, size);
36+
if (affinity.core_count > num_proc_) {
37+
for (std::size_t i = num_proc_; i < affinity.core_count; ++i) {
38+
if (rcutils_thread_core_affinity_is_set(&affinity, i)) {
39+
auto ec = std::make_error_code(std::errc::invalid_argument);
40+
throw std::system_error{ec, "invalid cpu number"};
41+
}
42+
}
43+
}
44+
}
45+
46+
CpuSet::CpuSet(const CpuSet & other)
47+
{
48+
if (!other.cpu_set_) {
49+
return;
50+
}
51+
init_cpu_set();
52+
memcpy(cpu_set_.get(), other.cpu_set_.get(), alloc_size());
53+
}
54+
55+
CpuSet::CpuSet(CpuSet && other)
56+
: CpuSet()
57+
{
58+
swap(other);
59+
}
60+
61+
CpuSet & CpuSet::operator=(const CpuSet & other)
62+
{
63+
if (other.cpu_set_) {
64+
init_cpu_set();
65+
memcpy(cpu_set_.get(), other.cpu_set_.get(), alloc_size());
66+
} else {
67+
clear();
68+
}
69+
return *this;
70+
}
71+
72+
CpuSet & CpuSet::operator=(CpuSet && other)
73+
{
74+
CpuSet tmp;
75+
other.swap(tmp);
76+
tmp.swap(*this);
77+
return *this;
78+
}
79+
80+
void CpuSet::swap(CpuSet & other)
81+
{
82+
using std::swap;
83+
swap(cpu_set_, other.cpu_set_);
84+
swap(num_proc_, other.num_proc_);
85+
}
86+
87+
void CpuSet::set(std::size_t cpu)
88+
{
89+
init_cpu_set();
90+
valid_cpu(cpu);
91+
CPU_SET_S(cpu, alloc_size(), cpu_set_.get());
92+
}
93+
94+
void CpuSet::unset(std::size_t cpu)
95+
{
96+
init_cpu_set();
97+
valid_cpu(cpu);
98+
CPU_CLR_S(cpu, alloc_size(), cpu_set_.get());
99+
}
100+
101+
void CpuSet::clear()
102+
{
103+
if (cpu_set_) {
104+
CPU_ZERO_S(alloc_size(), cpu_set_.get());
105+
}
106+
}
107+
108+
bool CpuSet::is_set(std::size_t cpu) const
109+
{
110+
if (cpu_set_) {
111+
valid_cpu(cpu);
112+
return CPU_ISSET_S(cpu, alloc_size(), cpu_set_.get());
113+
} else {
114+
return false;
115+
}
116+
}
117+
118+
void CpuSet::set_rcutils_thread_core_affinity(rcutils_thread_core_affinity_t const & affinity)
119+
{
120+
CpuSet(affinity).swap(*this);
121+
}
122+
123+
void CpuSet::init_cpu_set()
124+
{
125+
if (cpu_set_) {
126+
return;
127+
}
128+
auto num_proc = sysconf(_SC_NPROCESSORS_ONLN);
129+
if (num_proc <= 0) {
130+
throw_if_error(
131+
num_proc,
132+
"invalid return value of sysconf(_SC_NPROCESSORS_ONLN)");
133+
}
134+
auto p = CPU_ALLOC(CPU_ALLOC_SIZE(num_proc));
135+
cpu_set_ = std::unique_ptr<NativeCpuSetType, CpuSetDeleter>(p);
136+
num_proc_ = num_proc;
137+
}
138+
139+
void CpuSet::valid_cpu(std::size_t cpu) const
140+
{
141+
if (num_proc_ <= cpu) {
142+
auto ec = std::make_error_code(std::errc::invalid_argument);
143+
throw std::system_error{ec, "invalid cpu number"};
144+
}
145+
}
146+
147+
void CpuSet::CpuSetDeleter::operator()(NativeCpuSetType * cpu_set) const
148+
{
149+
CPU_FREE(cpu_set);
150+
}
151+
152+
} // namespace detail
153+
154+
} // namespace rcpputils

src/threads/posix_thread.cpp renamed to src/threads/posix/thread.cpp

File renamed without changes.

0 commit comments

Comments
 (0)