From aeb3495af704cd43d5097849d1a84179d7ede821 Mon Sep 17 00:00:00 2001 From: Shoji Morita Date: Tue, 20 Jun 2023 20:22:33 +0900 Subject: [PATCH] Decoupled the additional feature from rcl to rcutils, reflecting on the pointing out below. https://github.com/ros2/rclcpp/pull/2205#issuecomment-1593832758 Signed-off-by: Shoji Morita --- CMakeLists.txt | 1 + include/rcutils/thread_attr.h | 146 ++++++++++++++++++++++++++++++++ src/thread_attr.c | 155 ++++++++++++++++++++++++++++++++++ 3 files changed, 302 insertions(+) create mode 100644 include/rcutils/thread_attr.h create mode 100644 src/thread_attr.c diff --git a/CMakeLists.txt b/CMakeLists.txt index 35d4df0c..e98189a5 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} diff --git a/include/rcutils/thread_attr.h b/include/rcutils/thread_attr.h new file mode 100644 index 00000000..5a3b8d3a --- /dev/null +++ b/include/rcutils/thread_attr.h @@ -0,0 +1,146 @@ +// Copyright 2023 eSOL Co.,Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCUTILS__THREAD_ATTR_H_ +#define RCUTILS__THREAD_ATTR_H_ + +#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_attr_s +{ + /// Thread core affinity + int core_affinity; + /// Thread scheduling policy. + rcutils_thread_scheduling_policy_t scheduling_policy; + /// Thread priority. + int priority; + /// Thread name + char const * name; +} 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 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] 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] name thread name 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, + int core_affinity, + int priority, + char const * name); + +#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..f2bd122c --- /dev/null +++ b/src/thread_attr.c @@ -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 +#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->name) { + allocator->deallocate((char *)attr->name, allocator->state); + } + } + allocator->deallocate(thread_attrs->attributes, allocator->state); + *thread_attrs = rcutils_get_zero_initialized_thread_attrs(); + + return RCUTILS_RET_OK; +} + +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, + int 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); + + 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 + rcutils_ret_t ret = extend_thread_attrs_capacity(thread_attrs, new_cap); + if (RCUTILS_RET_OK != ret) { + return ret; + } + } + + char const * dup_name = rcutils_strdup(name, thread_attrs->allocator); + if (NULL == dup_name) { + return RCUTILS_RET_BAD_ALLOC; + } + + rcutils_thread_attr_t * attr = thread_attrs->attributes + thread_attrs->num_attributes; + attr->scheduling_policy = sched_policy; + attr->core_affinity = core_affinity; + attr->priority = priority; + attr->name = dup_name; + + return RCUTILS_RET_OK; +}