diff --git a/CMakeLists.txt b/CMakeLists.txt index 35d4df0c..8e9b0324 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,6 +72,7 @@ set(rcutils_sources src/strerror.c src/string_array.c src/string_map.c + src/thread_attr.c src/testing/fault_injection.c src/time.c ${time_impl_c} @@ -584,6 +585,13 @@ if(BUILD_TESTING) if(TARGET test_macros) target_link_libraries(test_macros ${PROJECT_NAME}) endif() + + ament_add_gtest(test_thread_attr + test/test_thread_attr.cpp + ) + if(TARGET test_thread_attr) + target_link_libraries(test_thread_attr ${PROJECT_NAME}) + endif() endif() # Export old-style CMake variables diff --git a/include/rcutils/thread_attr.h b/include/rcutils/thread_attr.h new file mode 100644 index 00000000..278e0a27 --- /dev/null +++ b/include/rcutils/thread_attr.h @@ -0,0 +1,312 @@ +// 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 RCUTILS__THREAD_ATTR_H_ +#define RCUTILS__THREAD_ATTR_H_ + +#include + +#include "rcutils/visibility_control.h" + +#include "rcutils/allocator.h" +#include "rcutils/macros.h" +#include "rcutils/types/rcutils_ret.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +typedef enum rcutils_thread_scheduling_policy_e +{ + RCUTILS_THREAD_SCHEDULING_POLICY_UNKNOWN = 0, + RCUTILS_THREAD_SCHEDULING_POLICY_FIFO = 1, + RCUTILS_THREAD_SCHEDULING_POLICY_RR = 2, + RCUTILS_THREAD_SCHEDULING_POLICY_SPORADIC = 3, + RCUTILS_THREAD_SCHEDULING_POLICY_OTHER = 4, + RCUTILS_THREAD_SCHEDULING_POLICY_IDLE = 5, + RCUTILS_THREAD_SCHEDULING_POLICY_BATCH = 6, + 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 + rcutils_thread_core_affinity_t core_affinity; + /// Thread scheduling policy. + rcutils_thread_scheduling_policy_t scheduling_policy; + /// Thread priority. + int priority; + /// Thread attribute tag + char const * tag; +} rcutils_thread_attr_t; + +/// Hold thread attribute rules. +typedef struct rcutils_thread_attrs_s +{ + /// Private implementation array. + rcutils_thread_attr_t * attributes; + /// Number of threads attribute + size_t num_attributes; + /// Number of threads attribute capacity + size_t capacity_attributes; + /// Allocator used to allocate objects in this struct + rcutils_allocator_t allocator; +} rcutils_thread_attrs_t; + +/** + * \brief Return a rcutils_thread_attrs_t struct with members initialized to zero value. + * \return a rcutils_thread_attrs_t struct with members initialized to zero value. + */ +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_thread_attrs_t +rcutils_get_zero_initialized_thread_attrs(void); + +/** + * \brief Initialize list of thread attributes. + * \param[out] thread_attrs list of thread attributes to be initialized + * \param[in] allocator memory allocator to be used + * \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 + * \return #RCUTILS_RET_ERROR an unspecified error occur. + */ +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t +rcutils_thread_attrs_init( + rcutils_thread_attrs_t * thread_attrs, + rcutils_allocator_t allocator); + +/** + * \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 + * \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 + * \return #RCUTILS_RET_ERROR an unspecified error occur. + */ +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t +rcutils_thread_attrs_init_with_capacity( + rcutils_thread_attrs_t * thread_attrs, + rcutils_allocator_t allocator, + size_t capacity); + +/** + * \brief Free list of thread attributes + * \param[in] thread_attrs structure to be deallocated. + * \return #RCUTILS_RET_OK if the memory was successfully freed, or + * \return #RCUTILS_RET_INVALID_ARGUMENT if any function arguments are invalid + */ +RCUTILS_PUBLIC +RCUTILS_WARN_UNUSED +rcutils_ret_t +rcutils_thread_attrs_fini( + rcutils_thread_attrs_t * thread_attrs); + +/** + * \brief Copy list of thread attributes + * \param[in] thread_attrs Source list of thread attributes + * \param[out] out_thread_attrs Destination location + * \return #RCUTILS_RET_OK if the source list was succesfully copied to the destination, or + * \return #RCUTILS_RET_INVALID_ARGUMENT if may function arguments are invalid, or + * \return #RCUTILS_RET_BAD_ALLOC if allocating memory failed + */ +rcutils_ret_t +rcutils_thread_attrs_copy( + rcutils_thread_attrs_t const * thread_attrs, + rcutils_thread_attrs_t * out_thread_attrs); + +/** + * \brief Add thread attribute to the list of thread attributes. + * \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 + * \param[in] tag thread attribute tag of adding attribute + * \return #RCUTILS_RET_OK if the thread attribute was successfully added, 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_attrs_add_attr( + rcutils_thread_attrs_t * thread_attrs, + rcutils_thread_scheduling_policy_t sched_policy, + rcutils_thread_core_affinity_t const * core_affinity, + int priority, + char const * tag); + +/** + * \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 + +#endif // RCUTILS__THREAD_ATTR_H_ diff --git a/src/thread_attr.c b/src/thread_attr.c new file mode 100644 index 00000000..b01794fc --- /dev/null +++ b/src/thread_attr.c @@ -0,0 +1,470 @@ +// 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 +#include + +#include + +#include "rcutils/allocator.h" +#include "rcutils/error_handling.h" +#include "rcutils/strdup.h" +#include "rcutils/thread_attr.h" +#include "rcutils/types/rcutils_ret.h" + +#define INIT_NUM_THREAD_ATTRIBUTE 0U + +rcutils_thread_attrs_t +rcutils_get_zero_initialized_thread_attrs(void) +{ + rcutils_thread_attrs_t ret = { + NULL, + }; + return ret; +} + +rcutils_ret_t +rcutils_thread_attrs_init( + rcutils_thread_attrs_t * thread_attrs, + rcutils_allocator_t allocator) +{ + return rcutils_thread_attrs_init_with_capacity( + thread_attrs, allocator, INIT_NUM_THREAD_ATTRIBUTE); +} + +rcutils_ret_t +rcutils_thread_attrs_init_with_capacity( + rcutils_thread_attrs_t * thread_attrs, + rcutils_allocator_t allocator, + size_t capacity) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(thread_attrs, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + &allocator, "invalid allocator", return RCUTILS_RET_INVALID_ARGUMENT); + + thread_attrs->allocator = allocator; + thread_attrs->num_attributes = 0U; + thread_attrs->capacity_attributes = capacity; + if (capacity > 0) { + thread_attrs->attributes = + allocator.zero_allocate(capacity, sizeof(rcutils_thread_attr_t), allocator.state); + if (NULL == thread_attrs->attributes) { + *thread_attrs = rcutils_get_zero_initialized_thread_attrs(); + RCUTILS_SET_ERROR_MSG("Failed to allocate memory for thread attributes"); + return RCUTILS_RET_BAD_ALLOC; + } + } + return RCUTILS_RET_OK; +} + +rcutils_ret_t +rcutils_thread_attrs_fini(rcutils_thread_attrs_t * thread_attrs) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(thread_attrs, RCUTILS_RET_INVALID_ARGUMENT); + rcutils_allocator_t * allocator = &thread_attrs->allocator; + if (NULL == thread_attrs->attributes) { + return RCUTILS_RET_OK; + } + // check the allocator only if attributes is available to avoid checking after zero-initialized + RCUTILS_CHECK_ALLOCATOR(allocator, return RCUTILS_RET_INVALID_ARGUMENT); + for (size_t i = 0; i < thread_attrs->num_attributes; ++i) { + rcutils_thread_attr_t * attr = thread_attrs->attributes + i; + if (NULL != attr->tag) { + allocator->deallocate((char *)attr->tag, allocator->state); + } + } + allocator->deallocate(thread_attrs->attributes, allocator->state); + *thread_attrs = rcutils_get_zero_initialized_thread_attrs(); + + return RCUTILS_RET_OK; +} + +rcutils_ret_t +rcutils_thread_attrs_copy( + rcutils_thread_attrs_t const * thread_attrs, + rcutils_thread_attrs_t * out_thread_attrs) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(thread_attrs, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(out_thread_attrs, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR(&thread_attrs->allocator, return RCUTILS_RET_INVALID_ARGUMENT); + if (NULL != out_thread_attrs->attributes) { + RCUTILS_SET_ERROR_MSG("The destination must be zero initialized"); + return RCUTILS_RET_INVALID_ARGUMENT; + } + if (0 == thread_attrs->num_attributes) { + return rcutils_thread_attrs_init(out_thread_attrs, thread_attrs->allocator); + } + + rcutils_ret_t ret; + size_t i; + rcutils_allocator_t allocator = thread_attrs->allocator; + size_t new_size = thread_attrs->num_attributes * sizeof(rcutils_thread_attr_t); + rcutils_thread_attr_t * new_attrs = allocator.allocate(new_size, allocator.state); + + if (NULL == new_attrs) { + ret = RCUTILS_RET_BAD_ALLOC; + goto error; + } + + for (i = 0; i < thread_attrs->num_attributes; ++i) { + char * dup_tag = rcutils_strdup(thread_attrs->attributes[i].tag, allocator); + if (NULL == dup_tag) { + ret = RCUTILS_RET_BAD_ALLOC; + goto error; + } + new_attrs[i] = thread_attrs->attributes[i]; + new_attrs[i].tag = dup_tag; + } + *out_thread_attrs = *thread_attrs; + out_thread_attrs->attributes = new_attrs; + + return RCUTILS_RET_OK; + +error: + if (NULL != new_attrs) { + for (size_t j = 0; j < i; ++j) { + allocator.deallocate((char *)new_attrs[i].tag, allocator.state); + } + allocator.deallocate(new_attrs, allocator.state); + } + return ret; +} + +static inline rcutils_ret_t extend_thread_attrs_capacity( + rcutils_thread_attrs_t * attrs, + size_t new_cap) +{ + size_t cap = attrs->capacity_attributes; + size_t size = cap * sizeof(rcutils_thread_attr_t); + size_t new_size = new_cap * sizeof(rcutils_thread_attr_t); + rcutils_thread_attr_t * new_attrs = attrs->allocator.reallocate( + attrs->attributes, new_size, attrs->allocator.state); + + if (NULL == new_attrs) { + RCUTILS_SET_ERROR_MSG("Failed to allocate memory for thread attributes"); + return RCUTILS_RET_BAD_ALLOC; + } + + memset(new_attrs + cap, 0, new_size - size); + + attrs->capacity_attributes = new_cap; + attrs->attributes = new_attrs; + + return RCUTILS_RET_OK; +} + +rcutils_ret_t +rcutils_thread_attrs_add_attr( + rcutils_thread_attrs_t * thread_attrs, + rcutils_thread_scheduling_policy_t sched_policy, + rcutils_thread_core_affinity_t const * core_affinity, + int priority, + char const * tag) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(thread_attrs, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(tag, RCUTILS_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(core_affinity, RCUTILS_RET_INVALID_ARGUMENT); + + rcutils_ret_t ret; + char const * dup_tag = 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; + if (0 == thread_attrs->capacity_attributes) { + new_cap = 1; + } else { + new_cap = thread_attrs->capacity_attributes * 2; + } + // Extend the capacity + ret = extend_thread_attrs_capacity(thread_attrs, new_cap); + if (RCUTILS_RET_OK != ret) { + goto error; + } + } + + dup_tag = rcutils_strdup(tag, thread_attrs->allocator); + if (NULL == dup_tag) { + 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 = new_affinity; + attr->priority = priority; + attr->tag = dup_tag; + + ++thread_attrs->num_attributes; + + return RCUTILS_RET_OK; + +error: + if (NULL != dup_tag) { + thread_attrs->allocator.deallocate((char *)dup_tag, 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 new file mode 100644 index 00000000..67013058 --- /dev/null +++ b/test/test_thread_attr.cpp @@ -0,0 +1,310 @@ +// 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 "rcutils/thread_attr.h" +#include "rcutils/error_handling.h" + +struct TestThreadAffinity : ::testing::Test +{ + void SetUp() override + { + rcutils_reset_error(); + 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; +}; + +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_attrs[sizeof(rcutils_thread_attrs_t)] = {0}; + + EXPECT_EQ(0, memcmp(&attrs, zeros_attrs, sizeof(rcutils_thread_attrs_t))); +} + +TEST_F(TestThreadAttrs, initialization_without_cap) { + rcutils_ret_t ret = rcutils_thread_attrs_init(&attrs, alloc); + + EXPECT_EQ(RCUTILS_RET_OK, ret); + EXPECT_EQ(0ul, attrs.capacity_attributes); + EXPECT_EQ(0ul, attrs.num_attributes); + EXPECT_EQ(nullptr, attrs.attributes); + EXPECT_EQ(alloc.allocate, attrs.allocator.allocate); + EXPECT_EQ(alloc.reallocate, attrs.allocator.reallocate); + EXPECT_EQ(alloc.zero_allocate, attrs.allocator.zero_allocate); + EXPECT_EQ(alloc.deallocate, attrs.allocator.deallocate); + EXPECT_EQ(alloc.state, attrs.allocator.state); +} + +TEST_F(TestThreadAttrs, initialization_with_cap) { + rcutils_ret_t ret = rcutils_thread_attrs_init_with_capacity(&attrs, alloc, 100); + + EXPECT_EQ(RCUTILS_RET_OK, ret); + EXPECT_EQ(100ul, attrs.capacity_attributes); + EXPECT_EQ(0ul, attrs.num_attributes); + EXPECT_NE(nullptr, attrs.attributes); + EXPECT_EQ(alloc.allocate, attrs.allocator.allocate); + EXPECT_EQ(alloc.reallocate, attrs.allocator.reallocate); + EXPECT_EQ(alloc.zero_allocate, attrs.allocator.zero_allocate); + EXPECT_EQ(alloc.deallocate, attrs.allocator.deallocate); + EXPECT_EQ(alloc.state, attrs.allocator.state); +} + +TEST_F(TestThreadAttrs, finalization) { + rcutils_ret_t ret = rcutils_thread_attrs_init_with_capacity(&attrs, alloc, 100); + + ret = rcutils_thread_attrs_fini(&attrs); + EXPECT_EQ(RCUTILS_RET_OK, ret); + EXPECT_EQ(nullptr, attrs.attributes); +} + +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 attr_tag[32]; + for (size_t i = 0; i < 100; ++i) { + snprintf(attr_tag, sizeof(attr_tag), "attr tag %lu", i); + ret = rcutils_thread_attrs_add_attr( + &attrs, RCUTILS_THREAD_SCHEDULING_POLICY_FIFO, &affinity, 0xbb, attr_tag); + EXPECT_EQ(RCUTILS_RET_OK, ret); + ASSERT_NE(nullptr, attrs.attributes); + ASSERT_LE(i + 1, attrs.capacity_attributes); + EXPECT_EQ(i + 1, attrs.num_attributes); + } + + for (size_t i = 0; i < 100; ++i) { + rcutils_thread_attr_t attr = attrs.attributes[i]; + + snprintf(attr_tag, sizeof(attr_tag), "attr tag %lu", i); + EXPECT_EQ(RCUTILS_THREAD_SCHEDULING_POLICY_FIFO, attr.scheduling_policy); + 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(attr_tag, attr.tag); + EXPECT_STREQ(attr_tag, attr.tag); + } + + ret = rcutils_thread_attrs_fini(&attrs); + EXPECT_EQ(RCUTILS_RET_OK, ret); + EXPECT_EQ(nullptr, attrs.attributes); +} + +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 attr_tag[32]; + for (size_t i = 0; i < 100; ++i) { + snprintf(attr_tag, sizeof(attr_tag), "attr tag %lu", i); + ret = rcutils_thread_attrs_add_attr( + &attrs, RCUTILS_THREAD_SCHEDULING_POLICY_FIFO, &affinity, 0xbb, attr_tag); + ASSERT_EQ(RCUTILS_RET_OK, ret); + } + + rcutils_thread_attrs_t attrs_copy = rcutils_get_zero_initialized_thread_attrs(); + ret = rcutils_thread_attrs_copy(&attrs, &attrs_copy); + EXPECT_EQ(RCUTILS_RET_OK, ret); + EXPECT_EQ(100, attrs_copy.num_attributes); + EXPECT_LE(100, attrs_copy.capacity_attributes); + + 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(0, memcmp(affinity.set, attr.core_affinity.set, affinity.core_count / CHAR_BIT)); + EXPECT_EQ(0xbb, attr.priority); + EXPECT_NE(attrs.attributes[i].tag, attr.tag); + EXPECT_STREQ(attrs.attributes[i].tag, attr.tag); + } + + ret = rcutils_thread_attrs_fini(&attrs); + EXPECT_EQ(RCUTILS_RET_OK, ret); + EXPECT_EQ(nullptr, attrs.attributes); + + ret = rcutils_thread_attrs_fini(&attrs_copy); + EXPECT_EQ(RCUTILS_RET_OK, ret); + EXPECT_EQ(nullptr, attrs_copy.attributes); +}