diff --git a/include/rcutils/thread_attr.h b/include/rcutils/thread_attr.h index 132f2ef6..7100e82e 100644 --- a/include/rcutils/thread_attr.h +++ b/include/rcutils/thread_attr.h @@ -15,6 +15,8 @@ #ifndef RCUTILS__THREAD_ATTR_H_ #define RCUTILS__THREAD_ATTR_H_ +#include + #include "rcutils/visibility_control.h" #include "rcutils/allocator.h" @@ -38,10 +40,20 @@ typedef enum rcutils_thread_scheduling_policy_e RCUTILS_THREAD_SCHEDULING_POLICY_DEADLINE = 7 } rcutils_thread_scheduling_policy_t; +typedef struct rcutils_thread_core_affinity_s +{ + // Array for bit pattern of core affinity + uint8_t * set; + // Bit count in the set + size_t core_count; + // Allocator used to allocate the set + rcutils_allocator_t allocator; +} rcutils_thread_core_affinity_t; + typedef struct rcutils_thread_attr_s { /// Thread core affinity - int core_affinity; + rcutils_thread_core_affinity_t core_affinity; /// Thread scheduling policy. rcutils_thread_scheduling_policy_t scheduling_policy; /// Thread priority. @@ -92,7 +104,6 @@ rcutils_thread_attrs_init( * \brief Initialize list of thread attributes with a capacity. * \param[out] thread_attrs list of thread attributes to be initialized * \param[in] allocator memory allocator to be used - * \param[in] capacity initial capacity of the list of thread attributes * \return #RCUTILS_RET_OK if the structure was initialized succeessfully, or * \return #RCUTILS_RET_INVALID_ARGUMENT if any function arguments are invalid, or * \return #RCUTILS_RET_BAD_ALLOC if allocating memory failed, or @@ -133,7 +144,7 @@ rcutils_thread_attrs_copy( /** * \brief Add thread attribute to the list of thread attributes. - * \param[inout] thread_attrs list of thread attributes to add a thread attribute to + * \param[in,out] thread_attrs list of thread attributes to add a thread attribute to * \param[in] sched_policy thread scheduling policy of adding attribute * \param[in] core_affinity thread core affinity of adding attribute * \param[in] priority thread priority of adding attribute @@ -149,10 +160,151 @@ rcutils_ret_t rcutils_thread_attrs_add_attr( rcutils_thread_attrs_t * thread_attrs, rcutils_thread_scheduling_policy_t sched_policy, - int core_affinity, + rcutils_thread_core_affinity_t const * core_affinity, int priority, char const * name); +/** + * \brief Return a zero-initialized rcutils_thread_core_affinity_t object. + * \return zero-initialized rcutils_thread_core_affinity_t. + */ +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_thread_core_affinity_t +rcutils_get_zero_initialized_thread_core_affinity(void); + +/** + * \brief Initialize thread core affinity. + * \param[out] affinity thread core affinity to be initialized. + * \param[in] allocator allocator for internal memory operations for the thread core affinity. + * \return #RCUTILS_RET_OK if affinity was successfully initialized, or + * \return #RCUTILS_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCUTILS_RET_ERROR an unspecified error occur. + */ +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t +rcutils_thread_core_affinity_init( + rcutils_thread_core_affinity_t * affinity, + rcutils_allocator_t allocator); + +/** + * \brief Initialize thread core affinity with number of cores. + * \param[out] affinity thread core affinity to be initialized. + * \param[in] num_cores initial number of core in the thread core affinity + * \param[in] allocator allocator for internal memory operations for the thread core affinity. + * \return #RCUTILS_RET_OK if affinity was successfully initialized, or + * \return #RCUTILS_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCUTILS_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCUTILS_RET_ERROR an unspecified error occur. + */ +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t +rcutils_thread_core_affinity_init_with_capacity( + rcutils_thread_core_affinity_t * affinity, + size_t num_cores, + rcutils_allocator_t allocator); + +/** + * \brief Copy thread core affinity. + * \param[in] src core affinity to be copied. + * \param[out] dest the destination. + * \return #RCUTILS_RET_OK if affinity was successfully copied, or + * \return #RCUTILS_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCUTILS_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCUTILS_RET_ERROR an unspecified error occur. + */ +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t +rcutils_thread_core_affinity_copy( + rcutils_thread_core_affinity_t const * src, rcutils_thread_core_affinity_t * dest); + +/** + * \brief Free thread core affinity. + * \param[in,out] affinity thread core affinity to be freed + * \return #RCUTILS_RET_OK if affitiny was successfully freed, or + * \return #RCUTILS_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCUTILS_RET_ERROR an unspecified error occur. + */ +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t +rcutils_thread_core_affinity_fini(rcutils_thread_core_affinity_t * affinity); + +/** + * \brief Add processor number to thread core affinity. + * \param[in,out] affinity thread core affinity + * \param[in] no processor number to be added to the affinity set + * \return #RCUTILS_RET_OK if the processor number is successfully added to the affinity. + * \return #RCUTILS_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCUTILS_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCUTILS_RET_ERROR an unspecified error occur. + */ +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t +rcutils_thread_core_affinity_set(rcutils_thread_core_affinity_t * affinity, size_t no); + +/** + * \brief Remove processor number from thread core affinity. + * \param[in,out] affinity thread core affinity + * \param[in] no processor number to be removed from the affinity set + * \return #RCUTILS_RET_OK if the processor number is successfully removed from the affinity. + * \return #RCUTILS_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCUTILS_RET_ERROR an unspecified error occur. + */ +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t +rcutils_thread_core_affinity_unset(rcutils_thread_core_affinity_t * affinity, size_t no); + +/** + * \brief Add processor numbers which are included in a range to thread core affinity. + * \param[in,out] affinity thread core affinity + * \param[in] min_no lower bound of the processor number range to be added (inclusive) + * \param[in] max_no upper bound of the processor number range to be added (inclusive) + * \return #RCUTILS_RET_OK if the processor numbers are successfully added to the affinity. + * \return #RCUTILS_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCUTILS_RET_INVALID_ARGUMENT if max_no less than min_no, or + * \return #RCUTILS_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCUTILS_RET_ERROR an unspecified error occur. + */ +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t +rcutils_thread_core_affinity_fill( + rcutils_thread_core_affinity_t * affinity, size_t min_no, size_t max_no); + +/** + * \brief Remove processor numbers which are included in a range from thread core affinity. + * \param[in,out] affinity thread core affinity + * \param[in] min_no lower bound of the processor number range to be removed (inclusive) + * \param[in] max_no upper bound of the processor number range to be removed (inclusive) + * \return #RCUTILS_RET_OK if the processor numbers are successfully removed from the affinity. + * \return #RCUTILS_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCUTILS_RET_INVALID_ARGUMENT if max_no less than min_no, or + * \return #RCUTILS_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCUTILS_RET_ERROR an unspecified error occur. + */ +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t +rcutils_thread_core_affinity_clear( + rcutils_thread_core_affinity_t * affinity, size_t min_no, size_t max_no); + +/** + * \brief check if thread core affinity contains processor number. + * \param[in] affinity thread core affinity + * \param[in] no processor number to check + * \return true if the thread core affinity includes the number, false otherwise. + */ +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +bool +rcutils_thread_core_affinity_is_set(rcutils_thread_core_affinity_t const * affinity, size_t no); + #ifdef __cplusplus } #endif diff --git a/src/thread_attr.c b/src/thread_attr.c index e2e9e08f..95a58ca9 100644 --- a/src/thread_attr.c +++ b/src/thread_attr.c @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -170,12 +171,17 @@ rcutils_ret_t rcutils_thread_attrs_add_attr( rcutils_thread_attrs_t * thread_attrs, rcutils_thread_scheduling_policy_t sched_policy, - int core_affinity, + rcutils_thread_core_affinity_t const * core_affinity, int priority, char const * name) { RCUTILS_CHECK_ARGUMENT_FOR_NULL(thread_attrs, RCUTILS_RET_INVALID_ARGUMENT); RCUTILS_CHECK_ARGUMENT_FOR_NULL(name, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(core_affinity, RCUTILS_RET_INVALID_ARGUMENT); + + rcutils_ret_t ret; + char const * dup_name = NULL; + rcutils_thread_core_affinity_t new_affinity = rcutils_get_zero_initialized_thread_core_affinity(); if (thread_attrs->num_attributes == thread_attrs->capacity_attributes) { size_t new_cap = 0; @@ -185,24 +191,280 @@ rcutils_thread_attrs_add_attr( new_cap = thread_attrs->capacity_attributes * 2; } // Extend the capacity - rcutils_ret_t ret = extend_thread_attrs_capacity(thread_attrs, new_cap); + ret = extend_thread_attrs_capacity(thread_attrs, new_cap); if (RCUTILS_RET_OK != ret) { - return ret; + goto error; } } - char const * dup_name = rcutils_strdup(name, thread_attrs->allocator); + dup_name = rcutils_strdup(name, thread_attrs->allocator); if (NULL == dup_name) { - return RCUTILS_RET_BAD_ALLOC; + goto error; + } + + rcutils_thread_core_affinity_t src_affinity = *core_affinity; + src_affinity.allocator = thread_attrs->allocator; + ret = rcutils_thread_core_affinity_copy(&src_affinity, &new_affinity); + if (ret != RCUTILS_RET_OK) { + goto error; } rcutils_thread_attr_t * attr = thread_attrs->attributes + thread_attrs->num_attributes; attr->scheduling_policy = sched_policy; - attr->core_affinity = core_affinity; + attr->core_affinity = new_affinity; attr->priority = priority; attr->name = dup_name; ++thread_attrs->num_attributes; return RCUTILS_RET_OK; + +error: + if (NULL != dup_name) { + thread_attrs->allocator.deallocate((char *)dup_name, thread_attrs->allocator.state); + } + if (0 < new_affinity.core_count) { + rcutils_ret_t tmp_ret = rcutils_thread_core_affinity_fini(&new_affinity); + (void)tmp_ret; + } + + return RCUTILS_RET_ERROR; +} + +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_thread_core_affinity_t +rcutils_get_zero_initialized_thread_core_affinity(void) +{ + rcutils_thread_core_affinity_t ret = { + 0, + }; + return ret; +} + +#define BITSET_UNIT_BITS ((size_t)(CHAR_BIT - 1)) +#define BITSET_UNIT_SHIFT 3 // popcount(BITSET_UNIT_BITS) +static size_t as_bitset_count(size_t n) +{ + return (n + BITSET_UNIT_BITS) >> BITSET_UNIT_SHIFT; +} +static size_t as_bitset_index(size_t n) +{ + return n >> BITSET_UNIT_SHIFT; +} +static size_t round_up_to_bitset_unit(size_t n) +{ + return (n + BITSET_UNIT_BITS) & ~BITSET_UNIT_BITS; +} +static uint8_t bitset_unit_mask(size_t n) +{ + return 1 << (n & BITSET_UNIT_BITS); +} + +static rcutils_ret_t extend_thread_core_affinity( + rcutils_thread_core_affinity_t * aff, size_t new_core_count) +{ + size_t new_bitset_size = as_bitset_count(new_core_count); + size_t cur_bitset_size = as_bitset_count(aff->core_count); + rcutils_allocator_t * alloc = &aff->allocator; + uint8_t * buf = alloc->reallocate(aff->set, new_bitset_size, alloc->state); + + if (NULL == buf) { + return RCUTILS_RET_BAD_ALLOC; + } + + memset(buf + cur_bitset_size, 0, new_bitset_size - cur_bitset_size); + aff->set = buf; + aff->core_count = round_up_to_bitset_unit(new_core_count); + + return RCUTILS_RET_OK; +} + +rcutils_ret_t +rcutils_thread_core_affinity_init( + rcutils_thread_core_affinity_t * aff, + rcutils_allocator_t alloc) +{ + return rcutils_thread_core_affinity_init_with_capacity(aff, 0, alloc); +} + +rcutils_ret_t +rcutils_thread_core_affinity_init_with_capacity( + rcutils_thread_core_affinity_t * aff, + size_t num_cores, + rcutils_allocator_t alloc) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(aff, RCUTILS_RET_INVALID_ARGUMENT); + + rcutils_ret_t ret; + rcutils_thread_core_affinity_t tmp = { + .allocator = alloc, + }; + + if (0 < num_cores) { + ret = extend_thread_core_affinity(&tmp, num_cores); + if (RCUTILS_RET_OK != ret) { + goto error; + } + } + + *aff = tmp; + return RCUTILS_RET_OK; + +error: + return ret; +} + +rcutils_ret_t +rcutils_thread_core_affinity_fini(rcutils_thread_core_affinity_t * aff) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(aff, RCUTILS_RET_INVALID_ARGUMENT); + + if (NULL == aff->set) { + return RCUTILS_RET_OK; + } + + rcutils_allocator_t * alloc = &aff->allocator; + alloc->deallocate(aff->set, alloc->state); + *aff = rcutils_get_zero_initialized_thread_core_affinity(); + + return RCUTILS_RET_OK; +} + +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t +rcutils_thread_core_affinity_copy( + rcutils_thread_core_affinity_t const * src, rcutils_thread_core_affinity_t * dest) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(dest, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(src, RCUTILS_RET_INVALID_ARGUMENT); + + rcutils_thread_core_affinity_t tmp = *src; + rcutils_allocator_t const * alloc = &src->allocator; + + tmp.set = alloc->allocate(as_bitset_count(src->core_count), alloc->state); + if (NULL == tmp.set) { + return RCUTILS_RET_BAD_ALLOC; + } + memcpy(tmp.set, src->set, as_bitset_count(src->core_count)); + + *dest = tmp; + + return RCUTILS_RET_OK; +} + +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t +rcutils_thread_core_affinity_set(rcutils_thread_core_affinity_t * aff, size_t no) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(aff, RCUTILS_RET_INVALID_ARGUMENT); + + rcutils_ret_t ret; + if (as_bitset_index(no) >= as_bitset_count(aff->core_count)) { + ret = extend_thread_core_affinity(aff, (no + 1) * 2); + if (RCUTILS_RET_OK != ret) { + return ret; + } + } + aff->set[as_bitset_index(no)] |= bitset_unit_mask(no); + + return RCUTILS_RET_OK; +} + +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t +rcutils_thread_core_affinity_unset(rcutils_thread_core_affinity_t * aff, size_t no) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(aff, RCUTILS_RET_INVALID_ARGUMENT); + + if (no >= aff->core_count) { + return RCUTILS_RET_OK; + } + aff->set[as_bitset_index(no)] &= ~bitset_unit_mask(no); + + return RCUTILS_RET_OK; +} + +static +rcutils_ret_t +fill_bits(rcutils_thread_core_affinity_t * aff, size_t min_no, size_t max_no, bool set) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(aff, RCUTILS_RET_INVALID_ARGUMENT); + if (min_no > max_no) { + return RCUTILS_RET_INVALID_ARGUMENT; + } + + if (!set && min_no >= aff->core_count) { + return RCUTILS_RET_OK; + } + + rcutils_ret_t ret; + + if (as_bitset_index(max_no) >= as_bitset_count(aff->core_count)) { + ret = extend_thread_core_affinity(aff, (max_no + 1) * 2); + if (RCUTILS_RET_OK != ret) { + return ret; + } + } + + max_no += 1; + + size_t min_index = as_bitset_index(min_no); + size_t max_index = as_bitset_index(max_no); + uint8_t lower_bound_bit = bitset_unit_mask(min_no); + uint8_t lower_bound_byte_mask = ~(lower_bound_bit - 1); + uint8_t upper_bound_bit = bitset_unit_mask(max_no); + uint8_t upper_bound_byte_mask = upper_bound_bit - 1; + if (min_index == max_index) { + if (set) { + aff->set[min_index] |= lower_bound_byte_mask & upper_bound_byte_mask; + } else { + aff->set[min_index] &= ~(lower_bound_byte_mask & upper_bound_byte_mask); + } + } else { + if (set) { + aff->set[min_index] |= lower_bound_byte_mask; + memset(aff->set + min_index + 1, 0xff, max_index - (min_index + 1)); + aff->set[max_index] |= upper_bound_byte_mask; + } else { + aff->set[min_index] &= ~lower_bound_byte_mask; + memset(aff->set + min_index + 1, 0x00, max_index - (min_index + 1)); + aff->set[max_index] &= ~upper_bound_byte_mask; + } + } + return RCUTILS_RET_OK; +} + +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t +rcutils_thread_core_affinity_fill( + rcutils_thread_core_affinity_t * aff, size_t min_no, size_t max_no) +{ + return fill_bits(aff, min_no, max_no, true); +} + +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t +rcutils_thread_core_affinity_clear( + rcutils_thread_core_affinity_t * aff, size_t min_no, size_t max_no) +{ + return fill_bits(aff, min_no, max_no, false); +} + +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +bool +rcutils_thread_core_affinity_is_set(rcutils_thread_core_affinity_t const * aff, size_t no) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(aff, RCUTILS_RET_INVALID_ARGUMENT); + + if (no >= aff->core_count) { + return false; + } + return (aff->set[as_bitset_index(no)] & bitset_unit_mask(no)) != 0; } diff --git a/test/test_thread_attr.cpp b/test/test_thread_attr.cpp index d882b35a..ba9ab688 100644 --- a/test/test_thread_attr.cpp +++ b/test/test_thread_attr.cpp @@ -13,31 +13,193 @@ // limitations under the License. #include +#include +#include #include "rcutils/thread_attr.h" #include "rcutils/error_handling.h" -struct TestThreadAttrs : ::testing::Test +struct TestThreadAffinity : ::testing::Test { void SetUp() override { rcutils_reset_error(); - attrs = rcutils_get_zero_initialized_thread_attrs(); + affinity = rcutils_get_zero_initialized_thread_core_affinity(); alloc = rcutils_get_default_allocator(); } void TearDown() override + { + rcutils_ret_t ret = rcutils_thread_core_affinity_fini(&affinity); + EXPECT_EQ(RCUTILS_RET_OK, ret); + } + rcutils_thread_core_affinity_t affinity; + rcutils_allocator_t alloc; +}; + +struct TestThreadAttrs : TestThreadAffinity +{ + void SetUp() override + { + TestThreadAffinity::SetUp(); + attrs = rcutils_get_zero_initialized_thread_attrs(); + } + void TearDown() override { rcutils_ret_t ret = rcutils_thread_attrs_fini(&attrs); EXPECT_EQ(RCUTILS_RET_OK, ret); + TestThreadAffinity::TearDown(); } rcutils_thread_attrs_t attrs; - rcutils_allocator_t alloc; }; +TEST_F(TestThreadAffinity, zero_initialized_object) { + char zeros_aff[sizeof(rcutils_thread_core_affinity_t)] = {0}; + + EXPECT_EQ(0, memcmp(&affinity, zeros_aff, sizeof(rcutils_thread_core_affinity_t))); +} + +TEST_F(TestThreadAffinity, initialization_without_cap) { + rcutils_ret_t ret = rcutils_thread_core_affinity_init(&affinity, alloc); + + EXPECT_EQ(RCUTILS_RET_OK, ret); + EXPECT_EQ(0, affinity.core_count); + EXPECT_EQ(nullptr, affinity.set); + EXPECT_EQ(alloc.allocate, affinity.allocator.allocate); + EXPECT_EQ(alloc.reallocate, affinity.allocator.reallocate); + EXPECT_EQ(alloc.zero_allocate, affinity.allocator.zero_allocate); + EXPECT_EQ(alloc.deallocate, affinity.allocator.deallocate); + EXPECT_EQ(alloc.state, affinity.allocator.state); +} + +TEST_F(TestThreadAffinity, initialization_with_cap) { + rcutils_ret_t ret = rcutils_thread_core_affinity_init_with_capacity(&affinity, 60, alloc); + + EXPECT_EQ(RCUTILS_RET_OK, ret); + EXPECT_EQ(64, affinity.core_count); + ASSERT_NE(nullptr, affinity.set); + EXPECT_EQ(alloc.allocate, affinity.allocator.allocate); + EXPECT_EQ(alloc.reallocate, affinity.allocator.reallocate); + EXPECT_EQ(alloc.zero_allocate, affinity.allocator.zero_allocate); + EXPECT_EQ(alloc.deallocate, affinity.allocator.deallocate); + EXPECT_EQ(alloc.state, affinity.allocator.state); + + for (size_t i = 0; i < 64 / CHAR_BIT; ++i) { + EXPECT_EQ(0, affinity.set[i]); + } +} + +TEST_F(TestThreadAffinity, set_bits) { + rcutils_ret_t ret = rcutils_thread_core_affinity_init(&affinity, alloc); + + ASSERT_EQ(RCUTILS_RET_OK, ret); + + ret = rcutils_thread_core_affinity_set(&affinity, 0); + + EXPECT_EQ(RCUTILS_RET_OK, ret); + EXPECT_LT(0, affinity.core_count); + ASSERT_NE(nullptr, affinity.set); + for (unsigned i = 0; i < 8; ++i) { + EXPECT_EQ(i == 0, rcutils_thread_core_affinity_is_set(&affinity, i)); + } + + ret = rcutils_thread_core_affinity_set(&affinity, 8); + + EXPECT_EQ(RCUTILS_RET_OK, ret); + EXPECT_LT(0, affinity.core_count); + ASSERT_NE(nullptr, affinity.set); + for (unsigned i = 0; i < 16; ++i) { + EXPECT_EQ(i == 0 || i == 8, rcutils_thread_core_affinity_is_set(&affinity, i)); + } + + ret = rcutils_thread_core_affinity_set(&affinity, 60); + + EXPECT_EQ(RCUTILS_RET_OK, ret); + EXPECT_LT(60, affinity.core_count); + ASSERT_NE(nullptr, affinity.set); + for (unsigned i = 0; i < 64; ++i) { + EXPECT_EQ(i == 0 || i == 8 || i == 60, rcutils_thread_core_affinity_is_set(&affinity, i)); + } + + ret = rcutils_thread_core_affinity_set(&affinity, 30); + + EXPECT_EQ(RCUTILS_RET_OK, ret); + EXPECT_LT(60, affinity.core_count); + ASSERT_NE(nullptr, affinity.set); + for (unsigned i = 0; i < 64; ++i) { + EXPECT_EQ( + i == 0 || i == 8 || i == 30 || i == 60, + rcutils_thread_core_affinity_is_set(&affinity, i)); + } + + ret = rcutils_thread_core_affinity_set(&affinity, 90); + + EXPECT_EQ(RCUTILS_RET_OK, ret); + EXPECT_LT(90, affinity.core_count); + ASSERT_NE(nullptr, affinity.set); + for (unsigned i = 0; i < 96; ++i) { + EXPECT_EQ( + i == 0 || i == 8 || i == 30 || i == 60 || i == 90, + rcutils_thread_core_affinity_is_set(&affinity, i)); + } +} + +TEST_F(TestThreadAffinity, copy) { + rcutils_ret_t ret = rcutils_thread_core_affinity_init(&affinity, alloc); + + EXPECT_EQ(RCUTILS_RET_OK, ret); + + ret = rcutils_thread_core_affinity_set(&affinity, 0); + ret = rcutils_thread_core_affinity_set(&affinity, 10); + ret = rcutils_thread_core_affinity_set(&affinity, 20); + ret = rcutils_thread_core_affinity_set(&affinity, 30); + + rcutils_thread_core_affinity_t dest = rcutils_get_zero_initialized_thread_core_affinity(); + ret = rcutils_thread_core_affinity_copy(&affinity, &dest); + + EXPECT_EQ(RCUTILS_RET_OK, ret); + ASSERT_NE(nullptr, dest.set); + EXPECT_LT(30, dest.core_count); + EXPECT_EQ(affinity.allocator.allocate, dest.allocator.allocate); + EXPECT_EQ(affinity.allocator.deallocate, dest.allocator.deallocate); + EXPECT_EQ(affinity.allocator.reallocate, dest.allocator.reallocate); + EXPECT_EQ(affinity.allocator.zero_allocate, dest.allocator.zero_allocate); + EXPECT_EQ(affinity.allocator.state, dest.allocator.state); + for (unsigned i = 0; i < dest.core_count; ++i) { + EXPECT_EQ( + rcutils_thread_core_affinity_is_set(&affinity, i), + rcutils_thread_core_affinity_is_set(&dest, i)); + } +} + +TEST_F(TestThreadAffinity, bit_range_ops) { + rcutils_ret_t ret = rcutils_thread_core_affinity_init_with_capacity(&affinity, 30, alloc); + + EXPECT_EQ(RCUTILS_RET_OK, ret); + EXPECT_LE(32, affinity.core_count); + + ret = rcutils_thread_core_affinity_fill(&affinity, 0, affinity.core_count - 1); + + for (unsigned i = 0; i < 32; ++i) { + EXPECT_EQ(true, rcutils_thread_core_affinity_is_set(&affinity, i)); + } + + ret = rcutils_thread_core_affinity_clear(&affinity, 8, 24); + + for (unsigned i = 0; i < 8; ++i) { + EXPECT_EQ(true, rcutils_thread_core_affinity_is_set(&affinity, i)); + } + for (unsigned i = 8; i < 25; ++i) { + EXPECT_EQ(false, rcutils_thread_core_affinity_is_set(&affinity, i)); + } + for (unsigned i = 25; i < 32; ++i) { + EXPECT_EQ(true, rcutils_thread_core_affinity_is_set(&affinity, i)); + } +} + TEST_F(TestThreadAttrs, zero_initialized_object) { - char zeros[sizeof(rcutils_thread_attrs_t)] = {0}; + char zeros_attrs[sizeof(rcutils_thread_attrs_t)] = {0}; - EXPECT_EQ(memcmp(&attrs, zeros, sizeof(rcutils_thread_attrs_t)), 0); + EXPECT_EQ(0, memcmp(&attrs, zeros_attrs, sizeof(rcutils_thread_attrs_t))); } TEST_F(TestThreadAttrs, initialization_without_cap) { @@ -78,12 +240,14 @@ TEST_F(TestThreadAttrs, finalization) { TEST_F(TestThreadAttrs, add_attribute) { rcutils_ret_t ret = rcutils_thread_attrs_init(&attrs, alloc); + ret = rcutils_thread_core_affinity_init(&affinity, alloc); + ret = rcutils_thread_core_affinity_set(&affinity, 0xaa); char thread_name[32]; for (size_t i = 0; i < 100; ++i) { snprintf(thread_name, sizeof(thread_name), "thread name %lu", i); ret = rcutils_thread_attrs_add_attr( - &attrs, RCUTILS_THREAD_SCHEDULING_POLICY_FIFO, 0xaa, 0xbb, thread_name); + &attrs, RCUTILS_THREAD_SCHEDULING_POLICY_FIFO, &affinity, 0xbb, thread_name); EXPECT_EQ(RCUTILS_RET_OK, ret); ASSERT_NE(nullptr, attrs.attributes); ASSERT_LE(i + 1, attrs.capacity_attributes); @@ -95,7 +259,9 @@ TEST_F(TestThreadAttrs, add_attribute) { snprintf(thread_name, sizeof(thread_name), "thread name %lu", i); EXPECT_EQ(RCUTILS_THREAD_SCHEDULING_POLICY_FIFO, attr.scheduling_policy); - EXPECT_EQ(0xaa, attr.core_affinity); + EXPECT_NE(affinity.set, attr.core_affinity.set); + EXPECT_EQ(affinity.core_count, attr.core_affinity.core_count); + EXPECT_EQ(0, memcmp(affinity.set, attr.core_affinity.set, affinity.core_count / CHAR_BIT)); EXPECT_EQ(0xbb, attr.priority); EXPECT_NE(thread_name, attr.name); EXPECT_STREQ(thread_name, attr.name); @@ -108,12 +274,14 @@ TEST_F(TestThreadAttrs, add_attribute) { TEST_F(TestThreadAttrs, copy) { rcutils_ret_t ret = rcutils_thread_attrs_init(&attrs, alloc); + ret = rcutils_thread_core_affinity_init(&affinity, alloc); + ret = rcutils_thread_core_affinity_set(&affinity, 0xaa); char thread_name[32]; for (size_t i = 0; i < 100; ++i) { snprintf(thread_name, sizeof(thread_name), "thread name %lu", i); ret = rcutils_thread_attrs_add_attr( - &attrs, RCUTILS_THREAD_SCHEDULING_POLICY_FIFO, 0xaa, 0xbb, thread_name); + &attrs, RCUTILS_THREAD_SCHEDULING_POLICY_FIFO, &affinity, 0xbb, thread_name); ASSERT_EQ(RCUTILS_RET_OK, ret); } @@ -126,7 +294,7 @@ TEST_F(TestThreadAttrs, copy) { for (size_t i = 0; i < 100; ++i) { rcutils_thread_attr_t attr = attrs_copy.attributes[i]; EXPECT_EQ(RCUTILS_THREAD_SCHEDULING_POLICY_FIFO, attr.scheduling_policy); - EXPECT_EQ(0xaa, attr.core_affinity); + EXPECT_EQ(0, memcmp(affinity.set, attr.core_affinity.set, affinity.core_count / CHAR_BIT)); EXPECT_EQ(0xbb, attr.priority); EXPECT_NE(attrs.attributes[i].name, attr.name); EXPECT_STREQ(attrs.attributes[i].name, attr.name);