diff --git a/.gitmodules b/.gitmodules index 8e0d234..20ae4e2 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,7 @@ [submodule "submodules/libcanard"] path = submodules/libcanard url = https://github.com/UAVCAN/libcanard + branch = v4 [submodule "submodules/public_regulated_data_types"] path = submodules/public_regulated_data_types url = https://github.com/UAVCAN/public_regulated_data_types @@ -16,4 +17,4 @@ [submodule "submodules/libcyphal"] path = submodules/libcyphal url = https://github.com/OpenCyphal-Garage/libcyphal.git - branch = sshirokov/async_destroy + branch = issue/352_zero_copy diff --git a/differential_pressure_sensor/src/main.c b/differential_pressure_sensor/src/main.c index d9263d2..238b56b 100644 --- a/differential_pressure_sensor/src/main.c +++ b/differential_pressure_sensor/src/main.c @@ -153,16 +153,12 @@ static void send(State* const state, const CanardMicrosecond tx_deadline_usec, const CanardTransferMetadata* const metadata, const size_t payload_size, - const void* const payload) + const void* const payload_data) { for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) { - (void) canardTxPush(&state->canard_tx_queues[ifidx], - &state->canard, - tx_deadline_usec, - metadata, - payload_size, - payload); + const struct CanardPayload payload = {.size = payload_size, .data = payload_data}; + (void) canardTxPush(&state->canard_tx_queues[ifidx], &state->canard, tx_deadline_usec, metadata, payload); } } @@ -170,11 +166,11 @@ static void sendResponse(State* const state, const CanardMicrosecond tx_deadline_usec, const CanardTransferMetadata* const request_metadata, const size_t payload_size, - const void* const payload) + const void* const payload_data) { CanardTransferMetadata meta = *request_metadata; meta.transfer_kind = CanardTransferKindResponse; - send(state, tx_deadline_usec, &meta, payload_size, payload); + send(state, tx_deadline_usec, &meta, payload_size, payload_data); } /// Invoked at the rate of the fastest loop. @@ -524,11 +520,11 @@ static void processReceivedTransfer(State* const state, const CanardRxTransfer* { if (transfer->metadata.transfer_kind == CanardTransferKindMessage) { - size_t size = transfer->payload_size; + size_t size = transfer->payload.size; if (transfer->metadata.port_id == uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_) { uavcan_pnp_NodeIDAllocationData_2_0 msg = {0}; - if (uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&msg, transfer->payload, &size) >= 0) + if (uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&msg, transfer->payload.data, &size) >= 0) { processMessagePlugAndPlayNodeIDAllocation(state, &msg); } @@ -563,8 +559,8 @@ static void processReceivedTransfer(State* const state, const CanardRxTransfer* else if (transfer->metadata.port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) { uavcan_register_Access_Request_1_0 req = {0}; - size_t size = transfer->payload_size; - if (uavcan_register_Access_Request_1_0_deserialize_(&req, transfer->payload, &size) >= 0) + size_t size = transfer->payload.size; + if (uavcan_register_Access_Request_1_0_deserialize_(&req, transfer->payload.data, &size) >= 0) { const uavcan_register_Access_Response_1_0 resp = processRequestRegisterAccess(&req); uint8_t serialized[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; @@ -582,8 +578,8 @@ static void processReceivedTransfer(State* const state, const CanardRxTransfer* else if (transfer->metadata.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) { uavcan_register_List_Request_1_0 req = {0}; - size_t size = transfer->payload_size; - if (uavcan_register_List_Request_1_0_deserialize_(&req, transfer->payload, &size) >= 0) + size_t size = transfer->payload.size; + if (uavcan_register_List_Request_1_0_deserialize_(&req, transfer->payload.data, &size) >= 0) { const uavcan_register_List_Response_1_0 resp = {.name = registerGetNameByIndex(req.index)}; uint8_t serialized[uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; @@ -601,8 +597,8 @@ static void processReceivedTransfer(State* const state, const CanardRxTransfer* else if (transfer->metadata.port_id == uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_) { uavcan_node_ExecuteCommand_Request_1_1 req = {0}; - size_t size = transfer->payload_size; - if (uavcan_node_ExecuteCommand_Request_1_1_deserialize_(&req, transfer->payload, &size) >= 0) + size_t size = transfer->payload.size; + if (uavcan_node_ExecuteCommand_Request_1_1_deserialize_(&req, transfer->payload.data, &size) >= 0) { const uavcan_node_ExecuteCommand_Response_1_1 resp = processRequestExecuteCommand(&req); uint8_t serialized[uavcan_node_ExecuteCommand_Response_1_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; @@ -628,16 +624,17 @@ static void processReceivedTransfer(State* const state, const CanardRxTransfer* } } -static void* canardAllocate(CanardInstance* const ins, const size_t amount) +static void* canardAllocate(void* const user_reference, const size_t amount) { - O1HeapInstance* const heap = ((State*) ins->user_reference)->heap; + O1HeapInstance* const heap = ((State*) user_reference)->heap; assert(o1heapDoInvariantsHold(heap)); return o1heapAllocate(heap, amount); } -static void canardFree(CanardInstance* const ins, void* const pointer) +static void canardDeallocate(void* const user_reference, const size_t amount, void* const pointer) { - O1HeapInstance* const heap = ((State*) ins->user_reference)->heap; + (void) amount; + O1HeapInstance* const heap = ((State*) user_reference)->heap; o1heapFree(heap, pointer); } @@ -659,9 +656,14 @@ int main(const int argc, char* const argv[]) _Alignas(O1HEAP_ALIGNMENT) static uint8_t heap_arena[1024 * 16] = {0}; state.heap = o1heapInit(heap_arena, sizeof(heap_arena)); assert(NULL != state.heap); + struct CanardMemoryResource canard_memory = { + .user_reference = &state, + .deallocate = canardDeallocate, + .allocate = canardAllocate, + }; // The libcanard instance requires the allocator for managing protocol states. - state.canard = canardInit(&canardAllocate, &canardFree); + state.canard = canardInit(canard_memory); state.canard.user_reference = &state; // Make the state reachable from the canard instance. // Restore the node-ID from the corresponding standard register. Default to anonymous. @@ -712,7 +714,8 @@ int main(const int argc, char* const argv[]) { return -sock[ifidx]; } - state.canard_tx_queues[ifidx] = canardTxInit(CAN_TX_QUEUE_CAPACITY, val.natural16.value.elements[0]); + state.canard_tx_queues[ifidx] = + canardTxInit(CAN_TX_QUEUE_CAPACITY, val.natural16.value.elements[0], canard_memory); } // Load the port-IDs from the registers. You can implement hot-reloading at runtime if desired. @@ -848,7 +851,8 @@ int main(const int argc, char* const argv[]) return -result; // SocketCAN interface failure (link down?) } } - state.canard.memory_free(&state.canard, canardTxPop(que, tqi)); + CanardTxQueueItem* const mut_tqi = canardTxPop(que, tqi); + que->memory.deallocate(que->memory.user_reference, mut_tqi->allocated_size, mut_tqi); tqi = canardTxPeek(que); } } @@ -878,7 +882,9 @@ int main(const int argc, char* const argv[]) if (canard_result > 0) { processReceivedTransfer(&state, &transfer); - state.canard.memory_free(&state.canard, (void*) transfer.payload); + state.canard.memory.deallocate(state.canard.memory.user_reference, + transfer.payload.allocated_size, + transfer.payload.data); } else if ((canard_result == 0) || (canard_result == -CANARD_ERROR_OUT_OF_MEMORY)) { diff --git a/libcyphal_demo/CMakeLists.txt b/libcyphal_demo/CMakeLists.txt index 64f4aba..cd35298 100644 --- a/libcyphal_demo/CMakeLists.txt +++ b/libcyphal_demo/CMakeLists.txt @@ -13,6 +13,8 @@ project(libcyphal_demo set(CMAKE_CXX_STANDARD "14" CACHE STRING "C++ standard to use when compiling.") set(DISABLE_CPP_EXCEPTIONS ON CACHE STRING "Disable C++ exceptions.") +option(CETL_ENABLE_DEBUG_ASSERT "Enable or disable runtime CETL asserts." ON) + if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang" OR CMAKE_CXX_COMPILER_ID STREQUAL "GNU") if (DISABLE_CPP_EXCEPTIONS) message(STATUS "DISABLE_CPP_EXCEPTIONS is true. Adding -fno-exceptions to compiler flags.") @@ -22,6 +24,10 @@ endif() add_compile_options("$<$:${CXX_FLAG_SET}>") +if (CETL_ENABLE_DEBUG_ASSERT) + add_compile_definitions("CETL_ENABLE_DEBUG_ASSERT=1") +endif() + set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake/modules") set(submodules "${CMAKE_SOURCE_DIR}/../submodules") diff --git a/libcyphal_demo/src/application.cpp b/libcyphal_demo/src/application.cpp index 0332126..0158110 100644 --- a/libcyphal_demo/src/application.cpp +++ b/libcyphal_demo/src/application.cpp @@ -32,7 +32,7 @@ Application::Application() : o1_heap_mr_{s_heap_arena} , storage_{"/tmp/" NODE_NAME} , registry_{o1_heap_mr_} - , regs_{o1_heap_mr_, registry_} + , regs_{o1_heap_mr_, registry_, media_block_mr_} { cetl::pmr::set_default_resource(&o1_heap_mr_); @@ -60,13 +60,21 @@ Application::~Application() { save(storage_, registry_); - const auto mr_diag = o1_heap_mr_.queryDiagnostics(); + const auto o1_diag = o1_heap_mr_.queryDiagnostics(); std::cout << "O(1) Heap diagnostics:" << "\n" - << " capacity=" << mr_diag.capacity << "\n" - << " allocated=" << mr_diag.allocated << "\n" - << " peak_allocated=" << mr_diag.peak_allocated << "\n" - << " peak_request_size=" << mr_diag.peak_request_size << "\n" - << " oom_count=" << mr_diag.oom_count << "\n"; + << " capacity=" << o1_diag.capacity << "\n" + << " allocated=" << o1_diag.allocated << "\n" + << " peak_allocated=" << o1_diag.peak_allocated << "\n" + << " peak_request_size=" << o1_diag.peak_request_size << "\n" + << " oom_count=" << o1_diag.oom_count << "\n"; + + const auto blk_diag = media_block_mr_.queryDiagnostics(); + std::cout << "Media block memory diagnostics:" << "\n" + << " capacity=" << blk_diag.capacity << "\n" + << " allocated=" << blk_diag.allocated << "\n" + << " peak_allocated=" << blk_diag.peak_allocated << "\n" + << " block_size=" << blk_diag.block_size << "\n" + << " oom_count=" << blk_diag.oom_count << "\n"; cetl::pmr::set_default_resource(cetl::pmr::new_delete_resource()); } @@ -96,13 +104,29 @@ Application::UniqueId Application::getUniqueId() return out_unique_id; } -Application::Regs::Value Application::Regs::getSysInfoMem() const +Application::Regs::Value Application::Regs::getSysInfoMemBlock() const +{ + Value value{{&o1_heap_mr_}}; + auto& uint64s = value.set_natural64(); + + const auto diagnostics = media_block_mr_.queryDiagnostics(); + uint64s.value.reserve(5); // NOLINT five fields gonna push + uint64s.value.push_back(diagnostics.capacity); + uint64s.value.push_back(diagnostics.allocated); + uint64s.value.push_back(diagnostics.peak_allocated); + uint64s.value.push_back(diagnostics.block_size); + uint64s.value.push_back(diagnostics.oom_count); + + return value; +} + +Application::Regs::Value Application::Regs::getSysInfoMemGeneral() const { Value value{{&o1_heap_mr_}}; auto& uint64s = value.set_natural64(); const auto diagnostics = o1_heap_mr_.queryDiagnostics(); - uint64s.value.reserve(5); // five fields gonna push + uint64s.value.reserve(5); // NOLINT five fields gonna push uint64s.value.push_back(diagnostics.capacity); uint64s.value.push_back(diagnostics.allocated); uint64s.value.push_back(diagnostics.peak_allocated); diff --git a/libcyphal_demo/src/application.hpp b/libcyphal_demo/src/application.hpp index 5c17820..7a05b3c 100644 --- a/libcyphal_demo/src/application.hpp +++ b/libcyphal_demo/src/application.hpp @@ -7,6 +7,7 @@ #ifndef APPLICATION_HPP #define APPLICATION_HPP +#include "platform/block_memory_resource.hpp" #include "platform/linux/epoll_single_threaded_executor.hpp" #include "platform/o1_heap_memory_resource.hpp" #include "platform/storage.hpp" @@ -34,6 +35,11 @@ class Application final static constexpr std::size_t MaxIfaceLen = 64; static constexpr std::size_t MaxNodeDesc = 50; + // Defines maximum alignment requirement for media block memory resource. + // Currently, it's `std::max_align_t` but could be as small as 1-byte for raw bytes fragments. + static constexpr std::size_t MediaBlockMaxAlign = alignof(std::max_align_t); + using MediaMemoryResource = platform::BlockMemoryResource; + struct Regs { using Value = libcyphal::application::registry::IRegister::Value; @@ -153,27 +159,34 @@ class Application final }; // Natural16Param - Regs(platform::O1HeapMemoryResource& o1_heap_mr, libcyphal::application::registry::Registry& registry) + Regs(platform::O1HeapMemoryResource& o1_heap_mr, + libcyphal::application::registry::Registry& registry, + MediaMemoryResource& media_block_mr) : o1_heap_mr_{o1_heap_mr} , registry_{registry} - , sys_info_mem_{registry.route("sys.info.mem", [this] { return getSysInfoMem(); })} + , media_block_mr_{media_block_mr} + , sys_info_mem_block_{registry.route("sys.info.mem.blk", [this] { return getSysInfoMemBlock(); })} + , sys_info_mem_general_{registry.route("sys.info.mem.gen", [this] { return getSysInfoMemGeneral(); })} { } private: friend class Application; - Value getSysInfoMem() const; + Value getSysInfoMemBlock() const; + Value getSysInfoMemGeneral() const; platform::O1HeapMemoryResource& o1_heap_mr_; libcyphal::application::registry::Registry& registry_; + MediaMemoryResource& media_block_mr_; // clang-format off StringParam can_iface_ { "uavcan.can.iface", registry_, {"vcan0"}, {true}}; StringParam node_desc_ { "uavcan.node.description", registry_, {NODE_NAME}, {true}}; Natural16Param<1> node_id_ { "uavcan.node.id", registry_, {65535U}, {true}}; StringParam udp_iface_ { "uavcan.udp.iface", registry_, {"127.0.0.1"}, {true}}; - Register sys_info_mem_; + Register sys_info_mem_block_; + Register sys_info_mem_general_; // clang-format on }; // Regs @@ -203,11 +216,16 @@ class Application final return executor_; } - CETL_NODISCARD cetl::pmr::memory_resource& memory() noexcept + CETL_NODISCARD platform::O1HeapMemoryResource& general_memory() noexcept { return o1_heap_mr_; } + CETL_NODISCARD MediaMemoryResource& media_block_memory() noexcept + { + return media_block_mr_; + } + CETL_NODISCARD libcyphal::application::registry::Registry& registry() noexcept { return registry_; @@ -225,7 +243,7 @@ class Application final /// Returns the 128-bit unique-ID of the local node. This value is used in `uavcan.node.GetInfo.Response`. /// - using UniqueId = std::array; + using UniqueId = std::array; // NOLINT UniqueId getUniqueId(); private: @@ -233,6 +251,7 @@ class Application final platform::Linux::EpollSingleThreadedExecutor executor_; platform::O1HeapMemoryResource o1_heap_mr_; + MediaMemoryResource media_block_mr_; platform::storage::KeyValue storage_; libcyphal::application::registry::Registry registry_; Regs regs_; diff --git a/libcyphal_demo/src/main.cpp b/libcyphal_demo/src/main.cpp index e88230f..f0f9852 100644 --- a/libcyphal_demo/src/main.cpp +++ b/libcyphal_demo/src/main.cpp @@ -105,16 +105,17 @@ libcyphal::Expected run_application() std::cout << "\nšŸŸ¢ ***************** LibCyphal demo *******************\n"; Application application; - auto& memory = application.memory(); - auto& executor = application.executor(); + auto& executor = application.executor(); + auto& general_mr = application.general_memory(); + auto& media_block_mr = application.media_block_memory(); auto node_params = application.getNodeParams(); auto iface_params = application.getIfaceParams(); // 1. Create the transport layer object. First try CAN, then UDP. // - TransportBagCan transport_bag_can{memory, executor}; - TransportBagUdp transport_bag_udp{memory, executor}; + TransportBagCan transport_bag_can{general_mr, executor, media_block_mr}; + TransportBagUdp transport_bag_udp{general_mr, executor, media_block_mr}; // libcyphal::transport::ITransport* transport_iface = transport_bag_can.create(iface_params); if (transport_iface == nullptr) @@ -126,15 +127,20 @@ libcyphal::Expected run_application() std::cerr << "āŒ Failed to create any transport.\n"; return ExitCode::TransportCreationFailure; } + + // 2. Allocate block memory for media of the transport layer. + // + std::array block_memory_blob; // NOLINT + media_block_mr.setup(block_memory_blob.size(), block_memory_blob.data(), 256U); + + // 3. Create the presentation layer object. + // (void) transport_iface->setLocalNodeId(node_params.id.value()[0]); std::cout << "Node ID : " << transport_iface->getLocalNodeId().value_or(65535) << "\n"; std::cout << "Node Name : '" << node_params.description.value().c_str() << "'\n"; + libcyphal::presentation::Presentation presentation{general_mr, executor, *transport_iface}; - // 2. Create the presentation layer object. - // - libcyphal::presentation::Presentation presentation{memory, executor, *transport_iface}; - - // 3. Create the node object with name. + // 4. Create the node object with name. // auto maybe_node = libcyphal::application::Node::make(presentation); if (const auto* failure = cetl::get_if(&maybe_node)) @@ -146,7 +152,7 @@ libcyphal::Expected run_application() } auto node = cetl::get(std::move(maybe_node)); - // 4. Populate the node info. + // 5. Populate the node info. // // The hardware version is not populated in this demo because it runs on no specific hardware. // An embedded node would usually determine the version by querying the hardware. @@ -156,8 +162,19 @@ libcyphal::Expected run_application() .setSoftwareVersion(VERSION_MAJOR, VERSION_MINOR) .setSoftwareVcsRevisionId(VCS_REVISION_ID) .setUniqueId(application.getUniqueId()); + // + // Update node's health according to states of memory resources. + node.heartbeatProducer().setUpdateCallback([&](const auto& arg) { + // + const auto gen_diag = general_mr.queryDiagnostics(); + const auto blk_diag = media_block_mr.queryDiagnostics(); + if ((gen_diag.oom_count > 0) || (blk_diag.oom_count > 0)) + { + arg.message.health.value = uavcan::node::Health_1_0::CAUTION; + } + }); - // 5. Bring up registry provider. + // 6. Bring up registry provider. // if (const auto failure = node.makeRegistryProvider(application.registry())) { @@ -165,7 +182,7 @@ libcyphal::Expected run_application() return ExitCode::RegistryCreationFailure; } - // 6. Bring up the command execution provider. + // 7. Bring up the command execution provider. // auto maybe_exec_cmd_provider = AppExecCmdProvider::make(presentation); if (const auto* failure = cetl::get_if(&maybe_node)) diff --git a/libcyphal_demo/src/platform/block_memory_resource.hpp b/libcyphal_demo/src/platform/block_memory_resource.hpp new file mode 100644 index 0000000..e1e7aa9 --- /dev/null +++ b/libcyphal_demo/src/platform/block_memory_resource.hpp @@ -0,0 +1,165 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (C) OpenCyphal Development Team +// Copyright Amazon.com Inc. or its affiliates. +// SPDX-License-Identifier: MIT +// Author: Sergei Shirokov + +#ifndef PLATFORM_BLOCK_MEMORY_RESOURCE_HPP +#define PLATFORM_BLOCK_MEMORY_RESOURCE_HPP + +#include +#include + +#include +#include +#include + +namespace platform +{ + +/// Implements a C++17 PMR memory resource that uses a pool of pre-allocated blocks. +/// +template +class BlockMemoryResource : public cetl::pmr::memory_resource +{ +public: + struct Diagnostics final + { + std::size_t capacity; + std::size_t allocated; + std::size_t peak_allocated; + std::size_t block_size; + std::uint64_t oom_count; + + }; // Diagnostics + + BlockMemoryResource() = default; + ~BlockMemoryResource() override = default; + + BlockMemoryResource(BlockMemoryResource&&) = delete; + BlockMemoryResource(const BlockMemoryResource&) = delete; + BlockMemoryResource& operator=(BlockMemoryResource&&) = delete; + BlockMemoryResource& operator=(const BlockMemoryResource&) = delete; + + void setup(const std::size_t pool_size_bytes, void* const pool, const std::size_t block_size_bytes) + { + CETL_DEBUG_ASSERT(nullptr != pool, ""); + CETL_DEBUG_ASSERT(block_size_bytes > 0U, ""); + CETL_DEBUG_ASSERT(pool_size_bytes >= MaxAlign, ""); + + // Enforce alignment and padding of the input arguments. We may waste some space as a result. + const std::size_t bs = (block_size_bytes + MaxAlign - 1U) & ~(MaxAlign - 1U); + std::size_t sz_bytes = pool_size_bytes; + auto* ptr = static_cast(pool); + while ((reinterpret_cast(ptr) % MaxAlign) != 0U) // NOLINT + { + ptr++; // NOLINT + if (sz_bytes > 0U) + { + sz_bytes--; + } + } + + block_size_bytes_ = bs; + block_count_ = sz_bytes / bs; + head_ = reinterpret_cast(ptr); // NOLINT + + for (std::size_t i = 0U; i < block_count_; i++) + { + *reinterpret_cast(ptr + (i * bs)) = // NOLINT + ((i + 1U) < block_count_) ? static_cast(ptr + ((i + 1U) * bs)) : nullptr; // NOLINT + } + } + + Diagnostics queryDiagnostics() const noexcept + { + return {block_count_, used_blocks_, used_blocks_peak_, block_size_bytes_, oom_count_}; + } + +protected: + // MARK: cetl::pmr::memory_resource + + void* do_allocate(std::size_t size_bytes, std::size_t alignment) override // NOLINT + { + if (alignment > MaxAlign) + { +#if defined(__cpp_exceptions) + throw std::bad_alloc(); +#else + return nullptr; +#endif + } + + // C++ standard (basic.stc.dynamic.allocation) requires that a memory allocation never returns + // nullptr (even for the zero). + // So, we have to handle this case explicitly by returning a non-null pointer to an empty storage. + if (size_bytes == 0U) + { + return empty_storage_.data(); + } + + void* out = nullptr; + request_count_++; + if (size_bytes <= block_size_bytes_) + { + out = static_cast(head_); // NOLINT + if (head_ != nullptr) + { + head_ = static_cast(*head_); // NOLINT + used_blocks_++; + used_blocks_peak_ = std::max(used_blocks_, used_blocks_peak_); + } + } + if (out == nullptr) + { + oom_count_++; + } + return out; + } + + void do_deallocate(void* ptr, std::size_t size_bytes, std::size_t alignment) override // NOLINT + { + CETL_DEBUG_ASSERT((nullptr != ptr) || (0U == size_bytes), ""); + CETL_DEBUG_ASSERT(size_bytes <= block_size_bytes_, ""); + (void) size_bytes; + (void) alignment; + + // See `do_allocate` special case for zero bytes. + if (ptr == empty_storage_.data()) + { + CETL_DEBUG_ASSERT(0U == size_bytes, ""); + return; + } + + if (ptr != nullptr) + { + *static_cast(ptr) = static_cast(head_); + head_ = static_cast(ptr); + CETL_DEBUG_ASSERT(used_blocks_ > 0U, ""); + used_blocks_--; + } + } + + bool do_is_equal(const cetl::pmr::memory_resource& other) const noexcept override + { + return this == &other; + } + +private: + void** head_{nullptr}; + std::size_t block_count_{0U}; + std::size_t block_size_bytes_{0U}; + std::size_t used_blocks_{0U}; + std::size_t used_blocks_peak_{0U}; + std::size_t request_count_{0U}; + std::size_t oom_count_{0U}; + + // See `do_allocate` special case for zero bytes. + // Note that we still need at least one byte - b/c `std::array<..., 0>::data()` returns `nullptr`. + std::array empty_storage_{}; + +}; // BlockMemoryResource + +} // namespace platform + +#endif // PLATFORM_BLOCK_MEMORY_RESOURCE_HPP diff --git a/libcyphal_demo/src/platform/linux/can/can_media.hpp b/libcyphal_demo/src/platform/linux/can/can_media.hpp index f79acbe..6e47229 100644 --- a/libcyphal_demo/src/platform/linux/can/can_media.hpp +++ b/libcyphal_demo/src/platform/linux/can/can_media.hpp @@ -39,9 +39,10 @@ class CanMedia final : public libcyphal::transport::can::IMedia { public: CETL_NODISCARD static cetl::variant make( - cetl::pmr::memory_resource& memory, + cetl::pmr::memory_resource& general_mr, libcyphal::IExecutor& executor, - const cetl::string_view iface_address_sv) + const cetl::string_view iface_address_sv, + cetl::pmr::memory_resource& tx_mr) { const IfaceAddrString iface_address{iface_address_sv}; @@ -62,7 +63,7 @@ class CanMedia final : public libcyphal::transport::can::IMedia return libcyphal::transport::PlatformError{posix::PosixPlatformError{error_code}}; } - return CanMedia{memory, executor, socket_can_rx_fd, socket_can_tx_fd, iface_address}; + return CanMedia{general_mr, executor, socket_can_rx_fd, socket_can_tx_fd, iface_address, tx_mr}; } ~CanMedia() @@ -81,11 +82,12 @@ class CanMedia final : public libcyphal::transport::can::IMedia CanMedia& operator=(const CanMedia&) = delete; CanMedia(CanMedia&& other) noexcept - : memory_{other.memory_} + : general_mr_{other.general_mr_} , executor_{other.executor_} , socket_can_rx_fd_{std::exchange(other.socket_can_rx_fd_, -1)} , socket_can_tx_fd_{std::exchange(other.socket_can_tx_fd_, -1)} , iface_address_{other.iface_address_} + , tx_mr_{other.tx_mr_} { } CanMedia* operator=(CanMedia&&) noexcept = delete; @@ -126,16 +128,18 @@ class CanMedia final : public libcyphal::transport::can::IMedia template using VarArray = cetl::VariableLengthArray>; - CanMedia(cetl::pmr::memory_resource& memory, + CanMedia(cetl::pmr::memory_resource& general_mr, libcyphal::IExecutor& executor, const SocketCANFD socket_can_rx_fd, const SocketCANFD socket_can_tx_fd, - const IfaceAddrString& iface_address) - : memory_{memory} + const IfaceAddrString& iface_address, + cetl::pmr::memory_resource& tx_mr) + : general_mr_{general_mr} , executor_{executor} , socket_can_rx_fd_{socket_can_rx_fd} , socket_can_tx_fd_{socket_can_tx_fd} , iface_address_{iface_address} + , tx_mr_{tx_mr} { } @@ -161,7 +165,7 @@ class CanMedia final : public libcyphal::transport::can::IMedia cetl::optional setFilters(const Filters filters) noexcept override { - const cetl::pmr::polymorphic_allocator alloc{&memory_}; + const cetl::pmr::polymorphic_allocator alloc{&general_mr_}; VarArray can_filters{alloc}; can_filters.reserve(filters.size()); std::transform(filters.begin(), filters.end(), std::back_inserter(can_filters), [](const Filter filter) { @@ -213,7 +217,7 @@ class CanMedia final : public libcyphal::transport::can::IMedia return cetl::nullopt; } - return PopResult::Metadata{executor_.now(), canard_frame.extended_can_id, canard_frame.payload_size}; + return PopResult::Metadata{executor_.now(), canard_frame.extended_can_id, canard_frame.payload.size}; } CETL_NODISCARD libcyphal::IExecutor::Callback::Any registerPushCallback( @@ -230,13 +234,19 @@ class CanMedia final : public libcyphal::transport::can::IMedia return registerAwaitableCallback(std::move(function), ReadableTrigger{socket_can_rx_fd_}); } + cetl::pmr::memory_resource& getTxMemoryResource() override + { + return tx_mr_; + } + // MARK: Data members: - cetl::pmr::memory_resource& memory_; + cetl::pmr::memory_resource& general_mr_; libcyphal::IExecutor& executor_; SocketCANFD socket_can_rx_fd_; SocketCANFD socket_can_tx_fd_; IfaceAddrString iface_address_; + cetl::pmr::memory_resource& tx_mr_; }; // CanMedia @@ -244,10 +254,13 @@ class CanMedia final : public libcyphal::transport::can::IMedia struct CanMediaCollection { - CanMediaCollection(cetl::pmr::memory_resource& memory, libcyphal::IExecutor& executor) - : memory_{memory} + CanMediaCollection(cetl::pmr::memory_resource& general_mr, + libcyphal::IExecutor& executor, + cetl::pmr::memory_resource& tx_mr) + : general_mr_{general_mr} , executor_{executor} , media_array_{{cetl::nullopt, cetl::nullopt, cetl::nullopt}} + , tx_mr_{tx_mr} { } @@ -270,7 +283,7 @@ struct CanMediaCollection const auto iface_address = iface_addresses.substr(curr, next - curr); if (!iface_address.empty()) { - auto maybe_media = CanMedia::make(memory_, executor_, iface_address); + auto maybe_media = CanMedia::make(general_mr_, executor_, iface_address, tx_mr_); if (auto* const media_ptr = cetl::get_if(&maybe_media)) { media_array_[index].emplace(std::move(*media_ptr)); // NOLINT @@ -291,10 +304,11 @@ struct CanMediaCollection private: static constexpr std::size_t MaxCanMedia = 3; - cetl::pmr::memory_resource& memory_; + cetl::pmr::memory_resource& general_mr_; libcyphal::IExecutor& executor_; std::array, MaxCanMedia> media_array_; std::array media_ifaces_{}; + cetl::pmr::memory_resource& tx_mr_; }; // CanMediaCollection diff --git a/libcyphal_demo/src/platform/posix/udp/udp_media.hpp b/libcyphal_demo/src/platform/posix/udp/udp_media.hpp index cf7a353..c848f91 100644 --- a/libcyphal_demo/src/platform/posix/udp/udp_media.hpp +++ b/libcyphal_demo/src/platform/posix/udp/udp_media.hpp @@ -26,10 +26,14 @@ namespace posix class UdpMedia final : public libcyphal::transport::udp::IMedia { public: - UdpMedia(cetl::pmr::memory_resource& memory, libcyphal::IExecutor& executor, const cetl::string_view iface_address) - : memory_{memory} + UdpMedia(cetl::pmr::memory_resource& general_mr, + libcyphal::IExecutor& executor, + const cetl::string_view iface_address, + cetl::pmr::memory_resource& tx_mr) + : general_mr_{general_mr} , executor_{executor} , iface_address_{iface_address} + , tx_mr_{tx_mr} { } @@ -40,9 +44,10 @@ class UdpMedia final : public libcyphal::transport::udp::IMedia UdpMedia* operator=(UdpMedia&&) noexcept = delete; UdpMedia(UdpMedia&& other) noexcept - : memory_{other.memory_} + : general_mr_{other.general_mr_} , executor_{other.executor_} , iface_address_{other.iface_address_} + , tx_mr_{other.tx_mr_} { } @@ -56,19 +61,25 @@ class UdpMedia final : public libcyphal::transport::udp::IMedia MakeTxSocketResult::Type makeTxSocket() override { - return UdpTxSocket::make(memory_, executor_, iface_address_.data()); + return UdpTxSocket::make(general_mr_, executor_, iface_address_.data()); } MakeRxSocketResult::Type makeRxSocket(const libcyphal::transport::udp::IpEndpoint& multicast_endpoint) override { - return UdpRxSocket::make(memory_, executor_, iface_address_.data(), multicast_endpoint); + return UdpRxSocket::make(general_mr_, executor_, iface_address_.data(), multicast_endpoint); + } + + cetl::pmr::memory_resource& getTxMemoryResource() override + { + return tx_mr_; } // MARK: Data members: - cetl::pmr::memory_resource& memory_; + cetl::pmr::memory_resource& general_mr_; libcyphal::IExecutor& executor_; String<64> iface_address_; + cetl::pmr::memory_resource& tx_mr_; }; // UdpMedia @@ -76,8 +87,13 @@ class UdpMedia final : public libcyphal::transport::udp::IMedia struct UdpMediaCollection { - UdpMediaCollection(cetl::pmr::memory_resource& memory, libcyphal::IExecutor& executor) - : media_array_{{{memory, executor, ""}, {memory, executor, ""}, {memory, executor, ""}}} + UdpMediaCollection(cetl::pmr::memory_resource& general_mr, + libcyphal::IExecutor& executor, + cetl::pmr::memory_resource& tx_mr) + : media_array_{{// + {general_mr, executor, "", tx_mr}, + {general_mr, executor, "", tx_mr}, + {general_mr, executor, "", tx_mr}}} { } diff --git a/libcyphal_demo/src/transport_bag_can.hpp b/libcyphal_demo/src/transport_bag_can.hpp index 0214fad..bdc225d 100644 --- a/libcyphal_demo/src/transport_bag_can.hpp +++ b/libcyphal_demo/src/transport_bag_can.hpp @@ -22,10 +22,12 @@ /// struct TransportBagCan final { - TransportBagCan(cetl::pmr::memory_resource& memory, libcyphal::IExecutor& executor) - : memory_{memory} + TransportBagCan(cetl::pmr::memory_resource& general_mr, + libcyphal::IExecutor& executor, + cetl::pmr::memory_resource& block_mr) + : general_mr_{general_mr} , executor_{executor} - , media_collection_{memory, executor} + , media_collection_{general_mr, executor, block_mr} { } @@ -37,7 +39,7 @@ struct TransportBagCan final } media_collection_.parse(params.can_iface.value()); - auto maybe_can_transport = makeTransport({memory_}, executor_, media_collection_.span(), TxQueueCapacity); + auto maybe_can_transport = makeTransport({general_mr_}, executor_, media_collection_.span(), TxQueueCapacity); if (const auto* failure = cetl::get_if(&maybe_can_transport)) { std::cerr << "āŒ Failed to create CAN transport (iface='" @@ -57,7 +59,7 @@ struct TransportBagCan final private: static constexpr std::size_t TxQueueCapacity = 16; - cetl::pmr::memory_resource& memory_; + cetl::pmr::memory_resource& general_mr_; libcyphal::IExecutor& executor_; platform::Linux::CanMediaCollection media_collection_; libcyphal::UniquePtr transport_; diff --git a/libcyphal_demo/src/transport_bag_udp.hpp b/libcyphal_demo/src/transport_bag_udp.hpp index c76bd27..2f29f66 100644 --- a/libcyphal_demo/src/transport_bag_udp.hpp +++ b/libcyphal_demo/src/transport_bag_udp.hpp @@ -22,10 +22,12 @@ /// struct TransportBagUdp final { - TransportBagUdp(cetl::pmr::memory_resource& memory, libcyphal::IExecutor& executor) - : memory_{memory} + TransportBagUdp(cetl::pmr::memory_resource& general_memory, + libcyphal::IExecutor& executor, + cetl::pmr::memory_resource& block_mr) + : general_mr_{general_memory} , executor_{executor} - , media_collection_{memory, executor} + , media_collection_{general_memory, executor, block_mr} { } @@ -37,7 +39,7 @@ struct TransportBagUdp final } media_collection_.parse(params.udp_iface.value()); - auto maybe_udp_transport = makeTransport({memory_}, executor_, media_collection_.span(), TxQueueCapacity); + auto maybe_udp_transport = makeTransport({general_mr_}, executor_, media_collection_.span(), TxQueueCapacity); if (const auto* failure = cetl::get_if(&maybe_udp_transport)) { std::cerr << "āŒ Failed to create UDP transport (iface='" @@ -57,7 +59,7 @@ struct TransportBagUdp final private: static constexpr std::size_t TxQueueCapacity = 16; - cetl::pmr::memory_resource& memory_; + cetl::pmr::memory_resource& general_mr_; libcyphal::IExecutor& executor_; platform::posix::UdpMediaCollection media_collection_; libcyphal::UniquePtr transport_; diff --git a/shared/socketcan/socketcan.c b/shared/socketcan/socketcan.c index 0729792..e11762a 100644 --- a/shared/socketcan/socketcan.c +++ b/shared/socketcan/socketcan.c @@ -136,7 +136,7 @@ SocketCANFD socketcanOpen(const char* const iface_name, const bool can_fd) int16_t socketcanPush(const SocketCANFD fd, const CanardFrame* const frame, const CanardMicrosecond timeout_usec) { - if ((frame == NULL) || (frame->payload == NULL) || (frame->payload_size > UINT8_MAX)) + if ((frame == NULL) || (frame->payload.data == NULL) || (frame->payload.size > UINT8_MAX)) { return -EINVAL; } @@ -149,15 +149,15 @@ int16_t socketcanPush(const SocketCANFD fd, const CanardFrame* const frame, cons struct canfd_frame cfd; (void) memset(&cfd, 0, sizeof(cfd)); cfd.can_id = frame->extended_can_id | CAN_EFF_FLAG; - cfd.len = (uint8_t) frame->payload_size; + cfd.len = (uint8_t) frame->payload.size; // We set the bit rate switch on the assumption that it will be ignored by non-CAN-FD-capable hardware. cfd.flags = CANFD_BRS; - (void) memcpy(cfd.data, frame->payload, frame->payload_size); + (void) memcpy(cfd.data, frame->payload.data, frame->payload.size); // If the payload is small, use the smaller MTU for compatibility with non-FD sockets. // This way, if the user attempts to transmit a CAN FD frame without having the CAN FD socket option enabled, // an error will be triggered here. This is convenient -- we can handle both FD and Classic CAN uniformly. - const size_t mtu = (frame->payload_size > CAN_MAX_DLEN) ? CANFD_MTU : CAN_MTU; + const size_t mtu = (frame->payload.size > CAN_MAX_DLEN) ? CANFD_MTU : CAN_MTU; if (write(fd, &cfd, mtu) < 0) { return getNegatedErrno(); @@ -267,8 +267,8 @@ int16_t socketcanPop(const SocketCANFD fd, *out_timestamp_usec = (CanardMicrosecond) (((uint64_t) tv.tv_sec * MEGA) + (uint64_t) tv.tv_usec); } out_frame->extended_can_id = sockcan_frame.can_id & CAN_EFF_MASK; - out_frame->payload_size = sockcan_frame.len; - out_frame->payload = payload_buffer; + out_frame->payload.size = sockcan_frame.len; + out_frame->payload.data = payload_buffer; (void) memcpy(payload_buffer, &sockcan_frame.data[0], sockcan_frame.len); } return poll_result; diff --git a/submodules/libcanard b/submodules/libcanard index 03fc5fe..dc19e93 160000 --- a/submodules/libcanard +++ b/submodules/libcanard @@ -1 +1 @@ -Subproject commit 03fc5fecc2c81bd6bfc6e474af8059fca2db4647 +Subproject commit dc19e935bbe0d95d8c1c6d2b357750dacb7b3b95 diff --git a/submodules/libcyphal b/submodules/libcyphal index 88d7c4f..edded3f 160000 --- a/submodules/libcyphal +++ b/submodules/libcyphal @@ -1 +1 @@ -Subproject commit 88d7c4f9e7a9bc71d42279aab8bff327b066b177 +Subproject commit edded3fd02e63963416d3b5ca35ac28fac80309b diff --git a/udral_servo/src/main.c b/udral_servo/src/main.c index d56d5c9..ad23680 100644 --- a/udral_servo/src/main.c +++ b/udral_servo/src/main.c @@ -191,27 +191,23 @@ static void send(State* const state, const CanardMicrosecond tx_deadline_usec, const CanardTransferMetadata* const metadata, const size_t payload_size, - const void* const payload) + const void* const payload_data) { for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++) { - (void) canardTxPush(&state->canard_tx_queues[ifidx], - &state->canard, - tx_deadline_usec, - metadata, - payload_size, - payload); + const struct CanardPayload payload = {.size = payload_size, .data = payload_data}; + (void) canardTxPush(&state->canard_tx_queues[ifidx], &state->canard, tx_deadline_usec, metadata, payload); } } static void sendResponse(State* const state, const CanardRxTransfer* const original_request_transfer, const size_t payload_size, - const void* const payload) + const void* const payload_data) { CanardTransferMetadata meta = original_request_transfer->metadata; meta.transfer_kind = CanardTransferKindResponse; - send(state, original_request_transfer->timestamp_usec + MEGA, &meta, payload_size, payload); + send(state, original_request_transfer->timestamp_usec + MEGA, &meta, payload_size, payload_data); } /// Invoked at the rate of the fastest loop. @@ -685,11 +681,12 @@ static void processReceivedTransfer(State* const state, const CanardRxTransfer* { if (transfer->metadata.transfer_kind == CanardTransferKindMessage) { - size_t size = transfer->payload_size; + size_t size = transfer->payload.size; if (transfer->metadata.port_id == state->port_id.sub.servo_setpoint) { reg_udral_physics_dynamics_translation_Linear_0_1 msg = {0}; - if (reg_udral_physics_dynamics_translation_Linear_0_1_deserialize_(&msg, transfer->payload, &size) >= 0) + if (reg_udral_physics_dynamics_translation_Linear_0_1_deserialize_(&msg, transfer->payload.data, &size) >= + 0) { processMessageServoSetpoint(state, &msg); } @@ -697,7 +694,7 @@ static void processReceivedTransfer(State* const state, const CanardRxTransfer* else if (transfer->metadata.port_id == state->port_id.sub.servo_readiness) { reg_udral_service_common_Readiness_0_1 msg = {0}; - if (reg_udral_service_common_Readiness_0_1_deserialize_(&msg, transfer->payload, &size) >= 0) + if (reg_udral_service_common_Readiness_0_1_deserialize_(&msg, transfer->payload.data, &size) >= 0) { processMessageServiceReadiness(state, &msg, transfer->timestamp_usec); } @@ -705,7 +702,7 @@ static void processReceivedTransfer(State* const state, const CanardRxTransfer* else if (transfer->metadata.port_id == uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_) { uavcan_pnp_NodeIDAllocationData_2_0 msg = {0}; - if (uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&msg, transfer->payload, &size) >= 0) + if (uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&msg, transfer->payload.data, &size) >= 0) { processMessagePlugAndPlayNodeIDAllocation(state, &msg); } @@ -736,8 +733,8 @@ static void processReceivedTransfer(State* const state, const CanardRxTransfer* else if (transfer->metadata.port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) { uavcan_register_Access_Request_1_0 req = {0}; - size_t size = transfer->payload_size; - if (uavcan_register_Access_Request_1_0_deserialize_(&req, transfer->payload, &size) >= 0) + size_t size = transfer->payload.size; + if (uavcan_register_Access_Request_1_0_deserialize_(&req, transfer->payload.data, &size) >= 0) { const uavcan_register_Access_Response_1_0 resp = processRequestRegisterAccess(&req); uint8_t serialized[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; @@ -751,8 +748,8 @@ static void processReceivedTransfer(State* const state, const CanardRxTransfer* else if (transfer->metadata.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) { uavcan_register_List_Request_1_0 req = {0}; - size_t size = transfer->payload_size; - if (uavcan_register_List_Request_1_0_deserialize_(&req, transfer->payload, &size) >= 0) + size_t size = transfer->payload.size; + if (uavcan_register_List_Request_1_0_deserialize_(&req, transfer->payload.data, &size) >= 0) { const uavcan_register_List_Response_1_0 resp = {.name = registerGetNameByIndex(req.index)}; uint8_t serialized[uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; @@ -766,8 +763,8 @@ static void processReceivedTransfer(State* const state, const CanardRxTransfer* else if (transfer->metadata.port_id == uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_) { uavcan_node_ExecuteCommand_Request_1_1 req = {0}; - size_t size = transfer->payload_size; - if (uavcan_node_ExecuteCommand_Request_1_1_deserialize_(&req, transfer->payload, &size) >= 0) + size_t size = transfer->payload.size; + if (uavcan_node_ExecuteCommand_Request_1_1_deserialize_(&req, transfer->payload.data, &size) >= 0) { const uavcan_node_ExecuteCommand_Response_1_1 resp = processRequestExecuteCommand(&req); uint8_t serialized[uavcan_node_ExecuteCommand_Response_1_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; @@ -789,16 +786,17 @@ static void processReceivedTransfer(State* const state, const CanardRxTransfer* } } -static void* canardAllocate(CanardInstance* const ins, const size_t amount) +static void* canardAllocate(void* const user_reference, const size_t amount) { - O1HeapInstance* const heap = ((State*) ins->user_reference)->heap; + O1HeapInstance* const heap = ((State*) user_reference)->heap; assert(o1heapDoInvariantsHold(heap)); return o1heapAllocate(heap, amount); } -static void canardFree(CanardInstance* const ins, void* const pointer) +static void canardDeallocate(void* const user_reference, const size_t amount, void* const pointer) { - O1HeapInstance* const heap = ((State*) ins->user_reference)->heap; + (void) amount; + O1HeapInstance* const heap = ((State*) user_reference)->heap; o1heapFree(heap, pointer); } @@ -820,9 +818,14 @@ int main(const int argc, char* const argv[]) _Alignas(O1HEAP_ALIGNMENT) static uint8_t heap_arena[1024 * 20] = {0}; state.heap = o1heapInit(heap_arena, sizeof(heap_arena)); assert(NULL != state.heap); + struct CanardMemoryResource canard_memory = { + .user_reference = &state, + .deallocate = canardDeallocate, + .allocate = canardAllocate, + }; // The libcanard instance requires the allocator for managing protocol states. - state.canard = canardInit(&canardAllocate, &canardFree); + state.canard = canardInit(canard_memory); state.canard.user_reference = &state; // Make the state reachable from the canard instance. // Restore the node-ID from the corresponding standard register. Default to anonymous. @@ -873,7 +876,8 @@ int main(const int argc, char* const argv[]) { return -sock[ifidx]; } - state.canard_tx_queues[ifidx] = canardTxInit(CAN_TX_QUEUE_CAPACITY, val.natural16.value.elements[0]); + state.canard_tx_queues[ifidx] = + canardTxInit(CAN_TX_QUEUE_CAPACITY, val.natural16.value.elements[0], canard_memory); } // Load the port-IDs from the registers. You can implement hot-reloading at runtime if desired. Specification here: @@ -1061,7 +1065,8 @@ int main(const int argc, char* const argv[]) return -result; // SocketCAN interface failure (link down?) } } - state.canard.memory_free(&state.canard, canardTxPop(que, tqi)); + CanardTxQueueItem* const mut_tqi = canardTxPop(que, tqi); + que->memory.deallocate(que->memory.user_reference, mut_tqi->allocated_size, mut_tqi); tqi = canardTxPeek(que); } @@ -1088,7 +1093,9 @@ int main(const int argc, char* const argv[]) if (canard_result > 0) { processReceivedTransfer(&state, &transfer); - state.canard.memory_free(&state.canard, (void*) transfer.payload); + state.canard.memory.deallocate(state.canard.memory.user_reference, + transfer.payload.allocated_size, + transfer.payload.data); } else if ((canard_result == 0) || (canard_result == -CANARD_ERROR_OUT_OF_MEMORY)) {