From 502e51240e647cde8d9e0856d4d37720c36b3cb7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 19 Jan 2020 03:44:33 +0200 Subject: [PATCH 001/100] Restructuring WIP --- .travis.yml | 68 +- libcanard/canard.c | 1415 +--------------------------------- libcanard/canard.h | 854 ++++++++++---------- libcanard/canard_internals.h | 124 --- 4 files changed, 468 insertions(+), 1993 deletions(-) delete mode 100644 libcanard/canard_internals.h diff --git a/.travis.yml b/.travis.yml index 09e851a3..b6bb7b08 100644 --- a/.travis.yml +++ b/.travis.yml @@ -3,42 +3,42 @@ dist: xenial matrix: include: # -------------------- GCC + SonarCloud -------------------- - - language: cpp - addons: - sonarcloud: - organization: uavcan - token: - secure: "LbaAOcguJspiraxAb9RNkwtGvWDh1aMTEPdz2MCbeICxIIiJ6LRoabD58rLvPsrNSWsR3AAep9Q551dDzT8c04UX+C67N3CZ/oof4DwBT+XQCSfB8FMj2QpbglJqwFPclYUorROjH31VwGcgIq3kfQq3Cw8G+nsL8vRaaJXsJb/KPp6MU698NGzHJIgRyn29VW76dW0NSxOv4ub3e6aKOnwfI+h1Ctx4p3hCdzd402PaZspv1VgEmirf5sVUJvE67PVIzlwov+CF+2PlrIpGUWI98Gl6HqYHv3hkvSP+4iLvCMD99Zmee4yLnCFY3xcJuZ8zKCRBBoquuUxdzK0f/4l9TZXePDXDMhaj3cXLlaAPWDw+emqTcm+hzP1mt/DaIqopAf54bQojVWELbL6QcjBNkphSvWBeIoyKWuUWU2LWJcJNPXFNUug//D99uXNurkzAIWR+lcsx6zO+cr4EN00N92W6hPt7mhKCF0prs7SvMleEi9mAbxvd4lOHFT56RvcB5ny6IapX9/q1+xm5iSoAzLhbvU1aUCnX74S/yFFejvClxxhW+P0bXYNtZ9RRfl8BdSgENTgA9RSnqdtIJGA4cU3OxIHDyJIC2cgmsE38u7QaMO49r1liJFH+xmDPa6bkGGHiPoHaPu9+g+wYFttK9FNt5ozyHY+VpjwTrY4=" - apt: - sources: - - ubuntu-toolchain-r-test - packages: - - g++-9 - - g++-9-multilib - - gcc-9-multilib - - linux-libc-dev:i386 - script: - - CC=gcc-9 && CXX=g++-9 && cmake tests - - build-wrapper-linux-x86-64 --out-dir sonar-dump make all # The build wrapper comes from Sonar Cloud. - - ./run_tests --rng-seed time - - sonar-scanner +# - language: cpp +# addons: +# sonarcloud: +# organization: uavcan +# token: +# secure: "LbaAOcguJspiraxAb9RNkwtGvWDh1aMTEPdz2MCbeICxIIiJ6LRoabD58rLvPsrNSWsR3AAep9Q551dDzT8c04UX+C67N3CZ/oof4DwBT+XQCSfB8FMj2QpbglJqwFPclYUorROjH31VwGcgIq3kfQq3Cw8G+nsL8vRaaJXsJb/KPp6MU698NGzHJIgRyn29VW76dW0NSxOv4ub3e6aKOnwfI+h1Ctx4p3hCdzd402PaZspv1VgEmirf5sVUJvE67PVIzlwov+CF+2PlrIpGUWI98Gl6HqYHv3hkvSP+4iLvCMD99Zmee4yLnCFY3xcJuZ8zKCRBBoquuUxdzK0f/4l9TZXePDXDMhaj3cXLlaAPWDw+emqTcm+hzP1mt/DaIqopAf54bQojVWELbL6QcjBNkphSvWBeIoyKWuUWU2LWJcJNPXFNUug//D99uXNurkzAIWR+lcsx6zO+cr4EN00N92W6hPt7mhKCF0prs7SvMleEi9mAbxvd4lOHFT56RvcB5ny6IapX9/q1+xm5iSoAzLhbvU1aUCnX74S/yFFejvClxxhW+P0bXYNtZ9RRfl8BdSgENTgA9RSnqdtIJGA4cU3OxIHDyJIC2cgmsE38u7QaMO49r1liJFH+xmDPa6bkGGHiPoHaPu9+g+wYFttK9FNt5ozyHY+VpjwTrY4=" +# apt: +# sources: +# - ubuntu-toolchain-r-test +# packages: +# - g++-9 +# - g++-9-multilib +# - gcc-9-multilib +# - linux-libc-dev:i386 +# script: +# - CC=gcc-9 && CXX=g++-9 && cmake tests +# - build-wrapper-linux-x86-64 --out-dir sonar-dump make all # The build wrapper comes from Sonar Cloud. +# - ./run_tests --rng-seed time +# - sonar-scanner # -------------------- Clang 9 -------------------- - - language: cpp - addons: - apt: - sources: - - ubuntu-toolchain-r-test - packages: - - libstdc++-7-dev:i386 - - linux-libc-dev:i386 - - libc6-dev-i386 - script: - - wget https://apt.llvm.org/llvm.sh && chmod +x llvm.sh && sudo ./llvm.sh 9 - - clang++-9 -E -x c++ - -v < /dev/null # Print the Clang configuration for troubleshooting purposes - - cmake -DCMAKE_C_COMPILER=clang-9 -DCMAKE_CXX_COMPILER=clang++-9 tests - - make - - ./run_tests --rng-seed time +# - language: cpp +# addons: +# apt: +# sources: +# - ubuntu-toolchain-r-test +# packages: +# - libstdc++-7-dev:i386 +# - linux-libc-dev:i386 +# - libc6-dev-i386 +# script: +# - wget https://apt.llvm.org/llvm.sh && chmod +x llvm.sh && sudo ./llvm.sh 9 +# - clang++-9 -E -x c++ - -v < /dev/null # Print the Clang configuration for troubleshooting purposes +# - cmake -DCMAKE_C_COMPILER=clang-9 -DCMAKE_CXX_COMPILER=clang++-9 tests +# - make +# - ./run_tests --rng-seed time # -------------------- Static analysis -------------------- - language: cpp diff --git a/libcanard/canard.c b/libcanard/canard.c index 29643514..b51d8699 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -22,1400 +22,69 @@ * Contributors: https://github.com/UAVCAN/libcanard/contributors */ -#include "canard_internals.h" +#include "canard.h" #include +#include -#undef MIN -#undef MAX -#define MIN(a, b) (((a) < (b)) ? (a) : (b)) -#define MAX(a, b) (((a) > (b)) ? (a) : (b)) +/// This macro is needed only for testing and for library development. Do not redefine this in production. +#ifndef CANARD_INTERNAL +# define CANARD_INTERNAL static +#endif -#define TRANSFER_CRC_INITIAL 0xFFFFU +// ---------------------------------------- CONSTANTS ---------------------------------------- -#define TRANSFER_TIMEOUT_USEC 2000000 +#define TRANSFER_CRC_INITIAL 0xFFFFU #define TRANSFER_ID_BIT_LEN 5U -#define SOURCE_ID_FROM_ID(x) ((uint8_t)(((x) >> 1U) & 0x7FU)) -#define SERVICE_NOT_MSG_FROM_ID(x) ((bool) (((x) >> 25U) & 0x1U)) -#define REQUEST_NOT_RESPONSE_FROM_ID(x) ((bool) (((x) >> 24U) & 0x1U)) -#define DEST_ID_FROM_ID(x) ((uint8_t)(((x) >> 8U) & 0x7FU)) -#define PRIORITY_FROM_ID(x) ((uint8_t)(((x) >> 26U) & 0x7U)) -#define SUBJECT_TYPE_FROM_ID(x) ((uint16_t)(((x) >> 8U) & 0x7FFFU)) -#define SRV_TYPE_FROM_ID(x) ((uint16_t)(((x) >> 15U) & 0x1FFU)) - -#define MAKE_SESSION_SPECIFIER(port_id, transfer_kind, src_node_id, dst_node_id) \ - (((uint32_t)(port_id)) | (((uint32_t)(transfer_kind)) << 16U) | (((uint32_t)(src_node_id)) << 18U) | \ - (((uint32_t)(dst_node_id)) << 25U)) - -#define TRANSFER_ID_FROM_TAIL_BYTE(x) ((uint8_t)((x) &0x1FU)) +#define CAN_EXT_ID_MASK ((1UL << 29U) - 1U) -// The extra cast to unsigned is needed to squelch warnings from clang-tidy -#define IS_START_OF_TRANSFER(x) ((bool) (((uint32_t)(x) >> 7U) & 0x1U)) -#define IS_END_OF_TRANSFER(x) ((bool) (((uint32_t)(x) >> 6U) & 0x1U)) -#define TOGGLE_BIT(x) ((bool) (((uint32_t)(x) >> 5U) & 0x1U)) +// ---------------------------------------- INTERNAL TYPES ---------------------------------------- -struct CanardTxQueueItem +struct CanardInternalTxQueueItem { - CanardTxQueueItem* next; - CanardCANFrame frame; + CanardInternalTxQueueItem* next; + uint64_t deadline_usec; + CanardCANFrame frame; }; -void canardInit(CanardInstance* out_ins, - void* mem_arena, - size_t mem_arena_size, - CanardOnTransferReception on_reception, - CanardShouldAcceptTransfer should_accept, - void* user_reference) -{ - CANARD_ASSERT(out_ins != NULL); - - memset(out_ins, 0, sizeof(*out_ins)); - - out_ins->node_id = CANARD_BROADCAST_NODE_ID; - out_ins->on_reception = on_reception; - out_ins->should_accept = should_accept; - out_ins->rx_states = NULL; - out_ins->tx_queue = NULL; - out_ins->user_reference = user_reference; - - const uint16_t pool_capacity = (uint16_t) MIN(0xFFFFU, mem_arena_size / CANARD_MEM_BLOCK_SIZE); - initPoolAllocator(&out_ins->allocator, mem_arena, pool_capacity); -} - -void* canardGetUserReference(CanardInstance* ins) -{ - CANARD_ASSERT(ins != NULL); - return ins->user_reference; -} - -int16_t canardSetLocalNodeID(CanardInstance* ins, uint8_t self_node_id) -{ - CANARD_ASSERT(ins != NULL); - - if ((ins->node_id == CANARD_BROADCAST_NODE_ID) && (self_node_id <= CANARD_MAX_NODE_ID)) - { - ins->node_id = self_node_id; - } - else - { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - - return CANARD_OK; -} - -uint8_t canardGetLocalNodeID(const CanardInstance* ins) -{ - return ins->node_id; -} - -int16_t canardPublishMessage(CanardInstance* ins, - uint16_t subject_id, - uint8_t* inout_transfer_id, - uint8_t priority, - const void* payload, - uint16_t payload_len) -{ - if (payload == NULL && payload_len > 0) - { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - if (priority > CANARD_TRANSFER_PRIORITY_OPTIONAL) - { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - - uint32_t can_id = 0; - uint16_t crc = TRANSFER_CRC_INITIAL; - - can_id = ((uint32_t) priority << 26U) | ((uint32_t) subject_id << 8U) | 1U; - - if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) // Anonymous message transfer - { - if (payload_len > 7) - { - return -CANARD_ERROR_NODE_ID_NOT_SET; - } - - uint8_t pseudo_id = crcAdd(TRANSFER_CRC_INITIAL, payload, payload_len) & 0x7FU; - can_id |= (1U << 24U) | ((uint32_t) pseudo_id << 1U); - } - else - { - if (payload_len > 7) - { - crc = crcAdd(crc, payload, payload_len); - } - - can_id |= ((uint32_t)(canardGetLocalNodeID(ins) & 0x7FU) << 1U); - } - - const int16_t result = enqueueTxFrames(ins, can_id, inout_transfer_id, crc, payload, payload_len); - - incrementTransferID(inout_transfer_id); - - return result; -} - -int16_t canardRequestOrRespond(CanardInstance* ins, - uint8_t destination_node_id, - uint16_t service_id, - uint8_t* inout_transfer_id, - uint8_t priority, - CanardRequestResponse kind, - const void* payload, - uint16_t payload_len) +/// The fields are ordered to minimize padding on all platforms. +struct CanardInternalInputSession { - if (payload == NULL && payload_len > 0) - { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - if (priority > CANARD_TRANSFER_PRIORITY_OPTIONAL) - { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) - { - return -CANARD_ERROR_NODE_ID_NOT_SET; // Service transfers not supported for anonymous nodes. - } - - const uint32_t can_id = ((uint32_t) priority << 26U) | ((uint32_t) service_id << 15U) | ((uint32_t) kind << 24U) | - ((uint32_t) destination_node_id << 8U) | (1U << 25U) | - ((uint32_t) canardGetLocalNodeID(ins) << 1U) | 1U; - uint16_t crc = TRANSFER_CRC_INITIAL; - - if (payload_len > 7) - { - crc = crcAdd(crc, payload, payload_len); - } - - const int16_t result = enqueueTxFrames(ins, can_id, inout_transfer_id, crc, payload, payload_len); - - if (kind == CanardRequest) // Response Transfer ID must not be altered - { - incrementTransferID(inout_transfer_id); - } - - return result; -} - -const CanardCANFrame* canardPeekTxQueue(const CanardInstance* ins) -{ - if (ins->tx_queue == NULL) - { - return NULL; - } - return &ins->tx_queue->frame; -} - -void canardPopTxQueue(CanardInstance* ins) -{ - CanardTxQueueItem* item = ins->tx_queue; - ins->tx_queue = item->next; - freeBlock(&ins->allocator, item); -} - -int16_t canardHandleRxFrame(CanardInstance* ins, const CanardCANFrame* frame, uint64_t timestamp_usec) -{ - const CanardTransferKind transfer_kind = extractTransferKind(frame->id); - const uint8_t destination_node_id = (transfer_kind == CanardTransferKindMessagePublication) - ? (uint8_t) CANARD_BROADCAST_NODE_ID - : DEST_ID_FROM_ID(frame->id); - - if ((frame->id & CANARD_CAN_FRAME_EFF) == 0 || (frame->id & CANARD_CAN_FRAME_RTR) != 0 || - (frame->id & CANARD_CAN_FRAME_ERR) != 0 || (frame->data_len < 1)) - { - return -CANARD_ERROR_RX_INCOMPATIBLE_PACKET; - } - - if (transfer_kind != CanardTransferKindMessagePublication && destination_node_id != canardGetLocalNodeID(ins)) - { - return -CANARD_ERROR_RX_WRONG_ADDRESS; - } - - const uint8_t priority = PRIORITY_FROM_ID(frame->id); - const uint8_t source_node_id = SOURCE_ID_FROM_ID(frame->id); - const uint16_t port_id = extractDataType(frame->id); - const uint8_t tail_byte = frame->data[frame->data_len - 1]; - const uint32_t session_specifier = - MAKE_SESSION_SPECIFIER(port_id, transfer_kind, source_node_id, destination_node_id); - - CanardRxState* rx_state = NULL; - - if (IS_START_OF_TRANSFER(tail_byte)) - { - if (ins->should_accept(ins, port_id, transfer_kind, source_node_id)) - { - rx_state = traverseRxStates(ins, session_specifier); - - if (rx_state == NULL) - { - return -CANARD_ERROR_OUT_OF_MEMORY; - } - - rx_state->calculated_crc = TRANSFER_CRC_INITIAL; - rx_state->timestamp_usec = timestamp_usec; - rx_state->next_toggle = 1; - rx_state->transfer_id = TRANSFER_ID_FROM_TAIL_BYTE(tail_byte); - rx_state->payload_len = 0; - } - else - { - return -CANARD_ERROR_RX_NOT_WANTED; - } - } - else - { - rx_state = findRxState(ins->rx_states, session_specifier); - - if (rx_state == NULL) - { - return -CANARD_ERROR_RX_MISSED_START; - } - } - - CANARD_ASSERT(rx_state != NULL); // All paths that lead to NULL should be terminated with return above - - // Resolving the state flags: - const bool not_initialized = rx_state->timestamp_usec == 0; - const bool tid_timed_out = (timestamp_usec - rx_state->timestamp_usec) > TRANSFER_TIMEOUT_USEC; - const bool first_frame = IS_START_OF_TRANSFER(tail_byte); - const bool not_previous_tid = - computeTransferIDForwardDistance((uint8_t) rx_state->transfer_id, TRANSFER_ID_FROM_TAIL_BYTE(tail_byte)) > 1; - - const bool need_restart = (not_initialized) || (tid_timed_out) || (first_frame && not_previous_tid); - if (need_restart) - { - rx_state->transfer_id = TRANSFER_ID_FROM_TAIL_BYTE(tail_byte); - rx_state->next_toggle = 1; - releaseStatePayload(ins, rx_state); - if (!IS_START_OF_TRANSFER(tail_byte)) - { - rx_state->transfer_id++; - return -CANARD_ERROR_RX_MISSED_START; - } - } - - if (IS_START_OF_TRANSFER(tail_byte) && IS_END_OF_TRANSFER(tail_byte)) // single frame transfer - { - CanardRxTransfer rx_transfer = {.timestamp_usec = timestamp_usec, - .payload_head = frame->data, - .payload_len = (uint8_t)(frame->data_len - 1U), - .port_id = port_id, - .transfer_kind = (uint8_t) transfer_kind, - .transfer_id = TRANSFER_ID_FROM_TAIL_BYTE(tail_byte), - .priority = priority, - .source_node_id = source_node_id}; - ins->on_reception(ins, &rx_transfer); - prepareForNextTransfer(rx_state); - } - else // multi frame transfer - { - if (IS_START_OF_TRANSFER(tail_byte) && frame->data_len <= 3) - { - return -CANARD_ERROR_RX_SHORT_FRAME; - } - if (TOGGLE_BIT(tail_byte) != (bool) rx_state->next_toggle) - { - return -CANARD_ERROR_RX_WRONG_TOGGLE; - } - if (TRANSFER_ID_FROM_TAIL_BYTE(tail_byte) != (uint8_t) rx_state->transfer_id) - { - return -CANARD_ERROR_RX_UNEXPECTED_TID; - } - if (!IS_END_OF_TRANSFER(tail_byte)) // Any multi frame that do not complete the transfer - { - const int16_t ret = - bufferBlockPushBytes(&ins->allocator, rx_state, frame->data, (uint8_t)(frame->data_len - 1)); - if (ret < 0) - { - releaseStatePayload(ins, rx_state); - prepareForNextTransfer(rx_state); - return -CANARD_ERROR_OUT_OF_MEMORY; - } - rx_state->calculated_crc = - crcAdd((uint16_t) rx_state->calculated_crc, frame->data, (uint8_t)(frame->data_len - 1)); - } - else // End of a multi-frame transfer - { - const uint8_t frame_payload_size = (uint8_t)(frame->data_len - 1); - - uint8_t tail_offset = 0; - - if (rx_state->payload_len < CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE) - { - // Copy the beginning of the frame into the head, point the tail pointer to the remainder - for (size_t i = rx_state->payload_len; - (i < CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE) && (tail_offset < frame_payload_size); - i++, tail_offset++) - { - rx_state->buffer_head[i] = frame->data[tail_offset]; - } - } - else - { - // Like above, except that the beginning goes into the last block of the storage - CanardBufferBlock* block = rx_state->buffer_blocks; - if (block != NULL) // If there's no middle, that's fine, we'll use only head and tail - { - size_t offset = CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE; // Payload offset of the first block - while (block->next != NULL) - { - block = block->next; - offset += CANARD_BUFFER_BLOCK_DATA_SIZE; - } - CANARD_ASSERT(block != NULL); - - const size_t offset_within_block = rx_state->payload_len - offset; - CANARD_ASSERT(offset_within_block < CANARD_BUFFER_BLOCK_DATA_SIZE); - - for (size_t i = offset_within_block; - (i < CANARD_BUFFER_BLOCK_DATA_SIZE) && (tail_offset < frame_payload_size); - i++, tail_offset++) - { - block->data[i] = frame->data[tail_offset]; - } - } - } - - CanardRxTransfer rx_transfer = {.timestamp_usec = timestamp_usec, - .payload_head = rx_state->buffer_head, - .payload_middle = rx_state->buffer_blocks, - .payload_tail = (tail_offset >= frame_payload_size) - ? NULL - : (&frame->data[tail_offset]), - .payload_len = - (uint16_t)((size_t) rx_state->payload_len + frame_payload_size - - CANARD_CAN_MULTI_FRAME_CRC_LENGTH), - .port_id = port_id, - .transfer_kind = (uint8_t) transfer_kind, - .transfer_id = TRANSFER_ID_FROM_TAIL_BYTE(tail_byte), - .priority = priority, - .source_node_id = source_node_id}; - - rx_state->buffer_blocks = NULL; // Block list ownership has been transferred to rx_transfer! - - // CRC validation - rx_state->calculated_crc = crcAdd((uint16_t) rx_state->calculated_crc, frame->data, frame->data_len - 1U); - if (rx_state->calculated_crc == 0U) - { - ins->on_reception(ins, &rx_transfer); - } - - // Making sure the payload is released even if the application didn't bother with it - canardReleaseRxTransferPayload(ins, &rx_transfer); - prepareForNextTransfer(rx_state); - - if (rx_state->calculated_crc != 0U) - { - return -CANARD_ERROR_RX_BAD_CRC; - } - } - - rx_state->next_toggle = (bool) rx_state->next_toggle ? 0 : 1; - } - - return CANARD_OK; -} - -int16_t canardDecodePrimitive(const CanardRxTransfer* transfer, - uint32_t bit_offset, - uint8_t bit_length, - bool value_is_signed, - void* out_value) -{ - if (transfer == NULL || out_value == NULL) - { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - if (bit_length < 1 || bit_length > 64) - { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - if (bit_length == 1 && value_is_signed) - { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - - /* - * Reading raw bytes into the temporary storage. - * Luckily, C guarantees that every element is aligned at the beginning (lower address) of the union. - */ - union - { - bool boolean; ///< sizeof(bool) is implementation-defined, so it has to be handled separately - uint8_t u8; ///< Also char - int8_t s8; - uint16_t u16; - int16_t s16; - uint32_t u32; - int32_t s32; ///< Also float, possibly double, possibly long double (depends on implementation) - uint64_t u64; - int64_t s64; ///< Also double, possibly float, possibly long double (depends on implementation) - uint8_t bytes[8]; - } storage; - - memset(&storage, 0, sizeof(storage)); // This is important - - const int16_t result = descatterTransferPayload(transfer, bit_offset, bit_length, &storage.bytes[0]); - if (result <= 0) - { - return result; - } - - CANARD_ASSERT((result > 0) && (result <= 64) && (result <= bit_length)); - - /* - * The bit copy algorithm assumes that more significant bits have lower index, so we need to shift some. - * Extra most significant bits will be filled with zeroes, which is fine. - * Coverity Scan mistakenly believes that the array may be overrun if bit_length == 64; however, this branch will - * not be taken if bit_length == 64, because 64 % 8 == 0. - */ - if ((bit_length % 8) != 0) - { - // coverity[overrun-local] - storage.bytes[bit_length / 8U] = (uint8_t)(storage.bytes[bit_length / 8U] >> ((8U - (bit_length % 8U)) & 7U)); - } - - /* - * Determining the closest standard byte length - this will be needed for byte reordering and sign bit extension. - */ - uint8_t std_byte_length = 0; // clang-format off - if (bit_length == 1) { std_byte_length = sizeof(bool); } - else if (bit_length <= 8) { std_byte_length = 1; } - else if (bit_length <= 16) { std_byte_length = 2; } - else if (bit_length <= 32) { std_byte_length = 4; } - else if (bit_length <= 64) { std_byte_length = 8; } - else // clang-format on - { - CANARD_ASSERT(false); - return -CANARD_ERROR_INTERNAL; - } - - CANARD_ASSERT((std_byte_length > 0) && (std_byte_length <= 8)); - - /* - * Flipping the byte order if needed. - */ - if (isBigEndian()) - { - swapByteOrder(&storage.bytes[0], std_byte_length); - } - - /* - * Extending the sign bit if needed. I miss templates. - * Note that we operate on unsigned values in order to avoid undefined behaviors. - */ - if (value_is_signed && (std_byte_length * 8 != bit_length)) - { - if (bit_length <= 8) - { - if ((storage.u8 & (1U << (bit_length - 1U))) != 0) // If the sign bit is set... - { - storage.u8 |= (uint8_t)(0xFFU & (uint8_t) ~((1U << bit_length) - 1U)); // ...set all bits above it. - } - } - else if (bit_length <= 16) - { - if ((storage.u16 & (1U << (bit_length - 1U))) != 0) - { - storage.u16 |= (uint16_t)(0xFFFFU & (uint16_t) ~((1U << bit_length) - 1U)); - } - } - else if (bit_length <= 32) - { - if ((storage.u32 & (((uint32_t) 1) << (bit_length - 1U))) != 0) - { - storage.u32 |= (uint32_t)(0xFFFFFFFFUL & (uint32_t) ~((((uint32_t) 1) << bit_length) - 1U)); - } - } - else if (bit_length < 64) // Strictly less, this is not a typo - { - if ((storage.u64 & (((uint64_t) 1) << (bit_length - 1U))) != 0) - { - storage.u64 |= (uint64_t)(0xFFFFFFFFFFFFFFFFULL & (uint64_t) ~((((uint64_t) 1) << bit_length) - 1U)); - } - } - else - { - CANARD_ASSERT(false); - return -CANARD_ERROR_INTERNAL; - } - } - - /* - * Copying the result out. - */ - if (value_is_signed) - { // clang-format off - if (bit_length <= 8) { *( (int8_t*) out_value) = storage.s8; } - else if (bit_length <= 16) { *((int16_t*) out_value) = storage.s16; } - else if (bit_length <= 32) { *((int32_t*) out_value) = storage.s32; } - else if (bit_length <= 64) { *((int64_t*) out_value) = storage.s64; } - else // clang-format on - { - CANARD_ASSERT(false); - return -CANARD_ERROR_INTERNAL; - } - } - else - { // clang-format off - if (bit_length == 1) { *( (bool*) out_value) = storage.boolean; } - else if (bit_length <= 8) { *( (uint8_t*) out_value) = storage.u8; } - else if (bit_length <= 16) { *((uint16_t*) out_value) = storage.u16; } - else if (bit_length <= 32) { *((uint32_t*) out_value) = storage.u32; } - else if (bit_length <= 64) { *((uint64_t*) out_value) = storage.u64; } - else // clang-format on - { - CANARD_ASSERT(false); - return -CANARD_ERROR_INTERNAL; - } - } - - CANARD_ASSERT(result <= bit_length); - CANARD_ASSERT(result > 0); - return result; -} - -void canardEncodePrimitive(void* destination, uint32_t bit_offset, uint8_t bit_length, const void* value) -{ - /* - * This function can only fail due to invalid arguments, so it was decided to make it return void, - * and in the case of bad arguments try the best effort or just trigger an assertion failure. - * Maybe not the best solution, but it simplifies the API. - */ - CANARD_ASSERT(destination != NULL); - CANARD_ASSERT(value != NULL); - - if (bit_length > 64) - { - CANARD_ASSERT(false); - bit_length = 64; - } - - if (bit_length < 1) - { - CANARD_ASSERT(false); - bit_length = 1; - } - - /* - * Preparing the data in the temporary storage. - */ - union - { - bool boolean; - uint8_t u8; - uint16_t u16; - uint32_t u32; - uint64_t u64; - uint8_t bytes[8]; - } storage; - - memset(&storage, 0, sizeof(storage)); - - uint8_t std_byte_length = 0; - - // Extra most significant bits can be safely ignored here. - if (bit_length == 1) - { - std_byte_length = sizeof(bool); - storage.boolean = (*((bool*) value) != 0); - } - else if (bit_length <= 8) - { - std_byte_length = 1; - storage.u8 = *((uint8_t*) value); - } - else if (bit_length <= 16) - { - std_byte_length = 2; - storage.u16 = *((uint16_t*) value); - } - else if (bit_length <= 32) - { - std_byte_length = 4; - storage.u32 = *((uint32_t*) value); - } - else if (bit_length <= 64) - { - std_byte_length = 8; - storage.u64 = *((uint64_t*) value); - } - else - { - CANARD_ASSERT(false); - } - - CANARD_ASSERT(std_byte_length > 0); - - if (isBigEndian()) - { - swapByteOrder(&storage.bytes[0], std_byte_length); - } - - /* - * The bit copy algorithm assumes that more significant bits have lower index, so we need to shift some. - * Extra least significant bits will be filled with zeroes, which is fine. - * Extra most significant bits will be discarded here. - * Coverity Scan mistakenly believes that the array may be overrun if bit_length == 64; however, this branch will - * not be taken if bit_length == 64, because 64 % 8 == 0. - */ - if ((bit_length % 8) != 0) - { - // coverity[overrun-local] - storage.bytes[bit_length / 8U] = (uint8_t)(storage.bytes[bit_length / 8U] << ((8U - (bit_length % 8U)) & 7U)); - } - - /* - * Now, the storage contains properly serialized scalar. Copying it out. - */ - copyBitArray(&storage.bytes[0], 0, bit_length, (uint8_t*) destination, bit_offset); -} - -void canardReleaseRxTransferPayload(CanardInstance* ins, CanardRxTransfer* transfer) -{ - while (transfer->payload_middle != NULL) - { - CanardBufferBlock* const temp = transfer->payload_middle->next; - freeBlock(&ins->allocator, transfer->payload_middle); - transfer->payload_middle = temp; - } - - transfer->payload_middle = NULL; - transfer->payload_head = NULL; - transfer->payload_tail = NULL; - transfer->payload_len = 0; -} - -CanardPoolAllocatorStatistics canardGetPoolAllocatorStatistics(CanardInstance* ins) -{ - return ins->allocator.statistics; -} - -uint16_t canardConvertNativeFloatToFloat16(float value) -{ - union FP32 - { - uint32_t u; - float f; - }; - - const union FP32 f32inf = {255UL << 23U}; - const union FP32 f16inf = {31UL << 23U}; - const union FP32 magic = {15UL << 23U}; - const uint32_t sign_mask = 0x80000000UL; - const uint32_t round_mask = ~0xFFFUL; - - union FP32 in; - in.f = value; - uint32_t sign = in.u & sign_mask; - in.u ^= sign; - - uint16_t out = 0; - - if (in.u >= f32inf.u) - { - out = (in.u > f32inf.u) ? (uint16_t) 0x7FFFU : (uint16_t) 0x7C00U; - } - else - { - in.u &= round_mask; - in.f *= magic.f; - in.u -= round_mask; - if (in.u > f16inf.u) - { - in.u = f16inf.u; - } - out = (uint16_t)(in.u >> 13U); - } - - out |= (uint16_t)(sign >> 16U); - - return out; -} - -float canardConvertFloat16ToNativeFloat(uint16_t value) -{ - union FP32 - { - uint32_t u; - float f; - }; - - const union FP32 magic = {(254UL - 15UL) << 23U}; - const union FP32 was_inf_nan = {(127UL + 16UL) << 23U}; - union FP32 out; - - out.u = (value & 0x7FFFU) << 13U; - out.f *= magic.f; - if (out.f >= was_inf_nan.f) - { - out.u |= 255UL << 23U; - } - out.u |= (value & 0x8000UL) << 16U; - - return out.f; -} - -CANARD_INTERNAL int16_t computeTransferIDForwardDistance(uint8_t a, uint8_t b) -{ - int16_t d = (int16_t)(b - a); - if (d < 0) - { - d = (int16_t)(d + (int16_t)(1U << TRANSFER_ID_BIT_LEN)); - } - return d; -} - -CANARD_INTERNAL void incrementTransferID(uint8_t* transfer_id) -{ - CANARD_ASSERT(transfer_id != NULL); - (*transfer_id)++; - if (*transfer_id >= 32) - { - *transfer_id = 0; - } -} - -CANARD_INTERNAL int16_t enqueueTxFrames(CanardInstance* ins, - uint32_t can_id, - const uint8_t* transfer_id, - uint16_t crc, - const uint8_t* payload, - uint16_t payload_len) -{ - CANARD_ASSERT(ins != NULL); - CANARD_ASSERT((can_id & CANARD_CAN_EXT_ID_MASK) == can_id); // Flags must be cleared - - if (transfer_id == NULL) - { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - - if ((payload_len > 0) && (payload == NULL)) - { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - - int16_t result = 0; + struct CanardInternalInputSession* next; - if (payload_len < CANARD_CAN_FRAME_MAX_DATA_LEN) // Single frame transfer - { - CanardTxQueueItem* queue_item = createTxItem(&ins->allocator); - if (queue_item == NULL) - { - return -CANARD_ERROR_OUT_OF_MEMORY; - } + uint8_t* payload; + size_t payload_length; ///< How many bytes received so far. + size_t payload_capacity; ///< Payload past this limit may be discarded by the library. - memcpy(queue_item->frame.data, payload, payload_len); + uint64_t timestamp_usec; ///< Time of last update of this session. Used for removal on timeout. + uint32_t transfer_id_timeout_usec; ///< When (current time - update timestamp) exceeds this, it's dead. - queue_item->frame.data_len = (uint8_t)(payload_len + 1); - queue_item->frame.data[payload_len] = (uint8_t)(0xE0U | (*transfer_id & 0x1FU)); - queue_item->frame.id = can_id | CANARD_CAN_FRAME_EFF; - - pushTxQueue(ins, queue_item); - result++; - } - else // Multi frame transfer - { - uint16_t data_index = 0; - uint8_t toggle = 1; - uint8_t sot_eot = 0x80; - - CanardTxQueueItem* queue_item = NULL; - - while (data_index < payload_len + CANARD_CAN_MULTI_FRAME_CRC_LENGTH) - { - queue_item = createTxItem(&ins->allocator); - if (queue_item == NULL) - { - return -CANARD_ERROR_OUT_OF_MEMORY; // TODO: Purge all frames enqueued so far - } - - uint8_t i = 0; - - while (i < (CANARD_CAN_FRAME_MAX_DATA_LEN - 1) && data_index < payload_len) - { - queue_item->frame.data[i] = payload[data_index]; - i++; - data_index++; - } - - if (i < (CANARD_CAN_FRAME_MAX_DATA_LEN - 1) && data_index == payload_len) - { - // add crc byte 1 - queue_item->frame.data[i] = (uint8_t)(crc); - i++; - data_index++; - } - - if (i < (CANARD_CAN_FRAME_MAX_DATA_LEN - 1) && data_index == payload_len + 1) - { - // add crc byte 2 - queue_item->frame.data[i] = (uint8_t)(crc >> 8U); - i++; - data_index++; - } - - // tail byte - sot_eot = (data_index == payload_len + CANARD_CAN_MULTI_FRAME_CRC_LENGTH) ? (uint8_t) 0x40 : sot_eot; - - queue_item->frame.data[i] = - (uint8_t)(sot_eot | ((uint32_t) toggle << 5U) | ((uint32_t) *transfer_id & 0x1FU)); - queue_item->frame.id = can_id | CANARD_CAN_FRAME_EFF; - queue_item->frame.data_len = (uint8_t)(i + 1); - pushTxQueue(ins, queue_item); - - result++; - toggle ^= 1U; - sot_eot = 0; - } - } - - return result; -} - -CANARD_INTERNAL void pushTxQueue(CanardInstance* ins, CanardTxQueueItem* item) -{ - CANARD_ASSERT(ins != NULL); - CANARD_ASSERT(item->frame.data_len > 0); // UAVCAN doesn't allow zero-payload frames - - if (ins->tx_queue == NULL) - { - ins->tx_queue = item; - return; - } - - CanardTxQueueItem* queue = ins->tx_queue; - CanardTxQueueItem* previous = ins->tx_queue; - - while (queue != NULL) - { - if (isPriorityHigher(queue->frame.id, item->frame.id)) // lower number wins - { - if (queue == ins->tx_queue) - { - item->next = queue; - ins->tx_queue = item; - } - else - { - previous->next = item; - item->next = queue; - } - break; - } - if (queue->next == NULL) - { - queue->next = item; - break; - } - previous = queue; - queue = queue->next; - } -} - -CANARD_INTERNAL CanardTxQueueItem* createTxItem(CanardPoolAllocator* allocator) -{ - CanardTxQueueItem* item = (CanardTxQueueItem*) allocateBlock(allocator); - if (item == NULL) - { - return NULL; - } - memset(item, 0, sizeof(*item)); - return item; -} - -CANARD_INTERNAL bool isPriorityHigher(uint32_t rhs, uint32_t id) -{ - const uint32_t clean_id = id & CANARD_CAN_EXT_ID_MASK; - const uint32_t rhs_clean_id = rhs & CANARD_CAN_EXT_ID_MASK; - - /* - * STD vs EXT - if 11 most significant bits are the same, EXT loses. - */ - const bool ext = (id & CANARD_CAN_FRAME_EFF) != 0; - const bool rhs_ext = (rhs & CANARD_CAN_FRAME_EFF) != 0; - if (ext != rhs_ext) - { - const uint32_t arb11 = ext ? (clean_id >> 18U) : clean_id; - const uint32_t rhs_arb11 = rhs_ext ? (rhs_clean_id >> 18U) : rhs_clean_id; - return (arb11 != rhs_arb11) ? (arb11 < rhs_arb11) : rhs_ext; - } - - /* - * RTR vs Data frame - if frame identifiers and frame types are the same, RTR loses. - */ - const bool rtr = (id & CANARD_CAN_FRAME_RTR) != 0; - const bool rhs_rtr = (rhs & CANARD_CAN_FRAME_RTR) != 0; - if (clean_id == rhs_clean_id && rtr != rhs_rtr) - { - return rhs_rtr; - } - - /* - * Plain ID arbitration - greater value loses. - */ - return clean_id < rhs_clean_id; -} - -CANARD_INTERNAL void prepareForNextTransfer(CanardRxState* state) -{ - CANARD_ASSERT(state->buffer_blocks == NULL); - state->transfer_id++; - state->payload_len = 0; - state->next_toggle = 1; -} - -CANARD_INTERNAL uint16_t extractDataType(uint32_t id) -{ - if (extractTransferKind(id) == CanardTransferKindMessagePublication) - { - return SUBJECT_TYPE_FROM_ID(id); - } - return SRV_TYPE_FROM_ID(id); -} - -CANARD_INTERNAL CanardTransferKind extractTransferKind(uint32_t id) -{ - const bool is_service = SERVICE_NOT_MSG_FROM_ID(id); - if (!is_service) - { - return CanardTransferKindMessagePublication; - } - if (REQUEST_NOT_RESPONSE_FROM_ID(id) == 1) - { - return CanardTransferKindServiceRequest; - } - return CanardTransferKindServiceResponse; -} - -CANARD_INTERNAL CanardRxState* traverseRxStates(CanardInstance* ins, uint32_t session_specifier) -{ - CanardRxState* states = ins->rx_states; - - if (states == NULL) // initialize CanardRxStates - { - states = createRxState(&ins->allocator, session_specifier); - if (states == NULL) - { - return NULL; - } - ins->rx_states = states; - return states; - } - - states = findRxState(states, session_specifier); - if (states != NULL) - { - return states; - } - return prependRxState(ins, session_specifier); -} - -CANARD_INTERNAL CanardRxState* findRxState(CanardRxState* state, uint32_t session_specifier) -{ - while (state != NULL) - { - if (state->session_specifier == session_specifier) - { - return state; - } - state = state->next; - } - return NULL; -} - -CANARD_INTERNAL CanardRxState* prependRxState(CanardInstance* ins, uint32_t session_specifier) -{ - CanardRxState* state = createRxState(&ins->allocator, session_specifier); - - if (state == NULL) - { - return NULL; - } - - state->next = ins->rx_states; - ins->rx_states = state; - return state; -} - -CANARD_INTERNAL CanardRxState* createRxState(CanardPoolAllocator* allocator, uint32_t session_specifier) -{ - CanardRxState init = {.next = NULL, .buffer_blocks = NULL, .session_specifier = session_specifier}; - - CanardRxState* state = (CanardRxState*) allocateBlock(allocator); - if (state == NULL) - { - return NULL; - } - memcpy(state, &init, sizeof(*state)); - - return state; -} - -CANARD_INTERNAL uint64_t releaseStatePayload(CanardInstance* ins, CanardRxState* rxstate) -{ - while (rxstate->buffer_blocks != NULL) - { - CanardBufferBlock* const temp = rxstate->buffer_blocks->next; - freeBlock(&ins->allocator, rxstate->buffer_blocks); - rxstate->buffer_blocks = temp; - } - rxstate->payload_len = 0; - return CANARD_OK; -} - -CANARD_INTERNAL int16_t bufferBlockPushBytes(CanardPoolAllocator* allocator, - CanardRxState* state, - const uint8_t* data, - uint8_t data_len) -{ - uint16_t data_index = 0; - - // if head is not full, add data to head - if ((CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE - state->payload_len) > 0) - { - for (uint16_t i = (uint16_t) state->payload_len; - i < CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE && data_index < data_len; - i++, data_index++) - { - state->buffer_head[i] = data[data_index]; - } - if (data_index >= data_len) - { - state->payload_len = - (uint16_t)((size_t) state->payload_len + data_len) & ((1U << CANARD_TRANSFER_PAYLOAD_LEN_BITS) - 1U); - return 1; - } - } // head is full. - - uint16_t index_at_nth_block = - (uint16_t)(((state->payload_len) - CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE) % CANARD_BUFFER_BLOCK_DATA_SIZE); - - // get to current block - CanardBufferBlock* block = NULL; - - // buffer blocks uninitialized - if (state->buffer_blocks == NULL) - { - state->buffer_blocks = createBufferBlock(allocator); - - if (state->buffer_blocks == NULL) - { - return -CANARD_ERROR_OUT_OF_MEMORY; - } - - block = state->buffer_blocks; - index_at_nth_block = 0; - } - else - { - uint16_t nth_block = 1; - - // get to block - block = state->buffer_blocks; - while (block->next != NULL) - { - nth_block++; - block = block->next; - } - - const uint16_t num_buffer_blocks = - (uint16_t)(((((uint32_t) state->payload_len + data_len) - CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE) / - CANARD_BUFFER_BLOCK_DATA_SIZE) + - 1U); - - if (num_buffer_blocks > nth_block && index_at_nth_block == 0) - { - block->next = createBufferBlock(allocator); - if (block->next == NULL) - { - return -CANARD_ERROR_OUT_OF_MEMORY; - } - block = block->next; - } - } - - // add data to current block until it becomes full, add new block if necessary - while (data_index < data_len) - { - for (uint16_t i = index_at_nth_block; i < CANARD_BUFFER_BLOCK_DATA_SIZE && data_index < data_len; - i++, data_index++) - { - block->data[i] = data[data_index]; - } - - if (data_index < data_len) - { - block->next = createBufferBlock(allocator); - if (block->next == NULL) - { - return -CANARD_ERROR_OUT_OF_MEMORY; - } - block = block->next; - index_at_nth_block = 0; - } - } - - state->payload_len = - (uint16_t)((size_t) state->payload_len + data_len) & ((1U << CANARD_TRANSFER_PAYLOAD_LEN_BITS) - 1U); - - return 1; -} - -CANARD_INTERNAL CanardBufferBlock* createBufferBlock(CanardPoolAllocator* allocator) -{ - CanardBufferBlock* block = (CanardBufferBlock*) allocateBlock(allocator); - if (block == NULL) - { - return NULL; - } - block->next = NULL; - return block; -} - -void copyBitArray(const uint8_t* src, uint32_t src_offset, uint32_t src_len, uint8_t* dst, uint32_t dst_offset) -{ - CANARD_ASSERT(src_len > 0U); - - src += src_offset / 8U; - dst += dst_offset / 8U; - - src_offset %= 8U; - dst_offset %= 8U; - - const size_t last_bit = src_offset + src_len; - while (last_bit - src_offset) - { - const uint8_t src_bit_offset = (uint8_t)(src_offset % 8U); - const uint8_t dst_bit_offset = (uint8_t)(dst_offset % 8U); - - const uint8_t max_offset = MAX(src_bit_offset, dst_bit_offset); - const uint32_t copy_bits = MIN(last_bit - src_offset, 8U - max_offset); - - const uint8_t write_mask = (uint8_t)((uint8_t)(0xFF00U >> copy_bits) >> dst_bit_offset); - const uint8_t src_data = (uint8_t)(((uint32_t) src[src_offset / 8U] << src_bit_offset) >> dst_bit_offset); - - dst[dst_offset / 8U] = - (uint8_t)(((uint32_t) dst[dst_offset / 8U] & (uint32_t) ~write_mask) | (uint32_t)(src_data & write_mask)); - - src_offset += copy_bits; - dst_offset += copy_bits; - } -} - -CANARD_INTERNAL int16_t descatterTransferPayload(const CanardRxTransfer* transfer, - uint32_t bit_offset, - uint8_t bit_length, - void* output) -{ - CANARD_ASSERT(transfer != 0); - - if (bit_offset >= transfer->payload_len * 8) - { - return 0; // Out of range, reading zero bits - } - - if (bit_offset + bit_length > transfer->payload_len * 8) - { - bit_length = (uint8_t)(transfer->payload_len * 8U - bit_offset); - } - - CANARD_ASSERT(bit_length > 0); - - if ((transfer->payload_middle != NULL) || (transfer->payload_tail != NULL)) // Multi frame - { - /* - * This part is hideously complicated and probably should be redesigned. - * The objective here is to copy the requested number of bits from scattered storage into the temporary - * local storage. We go through great pains to ensure that all corner cases are handled correctly. - */ - uint32_t input_bit_offset = bit_offset; - uint8_t output_bit_offset = 0; - uint8_t remaining_bit_length = bit_length; - - // Reading head - if (input_bit_offset < CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE * 8) - { - const uint8_t amount = - (uint8_t) MIN(remaining_bit_length, CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE * 8U - input_bit_offset); - - copyBitArray(&transfer->payload_head[0], input_bit_offset, amount, (uint8_t*) output, 0); - - input_bit_offset += amount; - output_bit_offset = (uint8_t)(output_bit_offset + amount); - remaining_bit_length = (uint8_t)(remaining_bit_length - amount); - } - - // Reading middle - uint32_t remaining_bits = transfer->payload_len * 8U - CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE * 8U; - uint32_t block_bit_offset = CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE * 8U; - const CanardBufferBlock* block = transfer->payload_middle; - - while ((block != NULL) && (remaining_bit_length > 0)) - { - CANARD_ASSERT(remaining_bits > 0); - const uint32_t block_end_bit_offset = - block_bit_offset + MIN(CANARD_BUFFER_BLOCK_DATA_SIZE * 8, remaining_bits); - - // Perform copy if we've reached the requested offset, otherwise jump over this block and try next - if (block_end_bit_offset > input_bit_offset) - { - const uint8_t amount = (uint8_t) MIN(remaining_bit_length, block_end_bit_offset - input_bit_offset); - - CANARD_ASSERT(input_bit_offset >= block_bit_offset); - const uint32_t bit_offset_within_block = input_bit_offset - block_bit_offset; - - copyBitArray(&block->data[0], bit_offset_within_block, amount, (uint8_t*) output, output_bit_offset); - - input_bit_offset += amount; - output_bit_offset = (uint8_t)(output_bit_offset + amount); - remaining_bit_length = (uint8_t)(remaining_bit_length - amount); - } - - CANARD_ASSERT(block_end_bit_offset > block_bit_offset); - remaining_bits -= block_end_bit_offset - block_bit_offset; - block_bit_offset = block_end_bit_offset; - block = block->next; - } - - CANARD_ASSERT(remaining_bit_length <= remaining_bits); - - // Reading tail - if ((transfer->payload_tail != NULL) && (remaining_bit_length > 0)) - { - CANARD_ASSERT(input_bit_offset >= block_bit_offset); - const uint32_t offset = input_bit_offset - block_bit_offset; - - copyBitArray(&transfer->payload_tail[0], - offset, - remaining_bit_length, - (uint8_t*) output, - output_bit_offset); - - input_bit_offset += remaining_bit_length; - output_bit_offset = (uint8_t)(output_bit_offset + remaining_bit_length); - remaining_bit_length = 0; - } - - CANARD_ASSERT(input_bit_offset <= transfer->payload_len * 8); - CANARD_ASSERT(output_bit_offset <= 64); - CANARD_ASSERT(remaining_bit_length == 0); - } - else // Single frame - { - copyBitArray(&transfer->payload_head[0], bit_offset, bit_length, (uint8_t*) output, 0); - } - - return bit_length; -} - -CANARD_INTERNAL bool isBigEndian(void) -{ -#if defined(BYTE_ORDER) && defined(BIG_ENDIAN) - return BYTE_ORDER == BIG_ENDIAN; // Some compilers offer this neat shortcut. -#else - union - { - uint16_t a; - uint8_t b[2]; - } u; - u.a = 1; - return u.b[1] == 1; // Some don't. -#endif -} - -CANARD_INTERNAL void swapByteOrder(void* data, size_t size) -{ - CANARD_ASSERT(data != NULL); - uint8_t* const bytes = (uint8_t*) data; - size_t fwd = 0; - size_t rev = size - 1; - while (fwd < rev) - { - const uint8_t x = bytes[fwd]; - bytes[fwd] = bytes[rev]; - bytes[rev] = x; - fwd++; - rev--; - } -} - -CANARD_INTERNAL uint16_t crcAddByte(uint16_t crc_val, uint8_t byte) -{ - crc_val ^= (uint16_t)((uint16_t)(byte) << 8U); - for (uint8_t j = 0; j < 8; j++) - { - if (crc_val & 0x8000U) - { - crc_val = (uint16_t)((uint16_t)(crc_val << 1U) ^ 0x1021U); - } - else - { - crc_val = (uint16_t)(crc_val << 1U); - } - } - return crc_val; -} + const uint32_t session_specifier; ///< Differentiates this session from other sessions. + uint16_t calculated_crc; ///< Updated with the received payload in real time. + uint8_t transfer_id; + bool next_toggle; +}; -CANARD_INTERNAL uint16_t crcAdd(uint16_t crc_val, const uint8_t* bytes, size_t len) -{ - while (len--) - { - crc_val = crcAddByte(crc_val, *bytes++); - } - return crc_val; -} +// ---------------------------------------- PRIVATE FUNCTIONS ---------------------------------------- -CANARD_INTERNAL void initPoolAllocator(CanardPoolAllocator* allocator, CanardPoolAllocatorBlock* buf, uint16_t buf_len) +CANARD_INTERNAL inline uint32_t makeMessageSessionSpecifier(const uint16_t subject_id, const uint8_t src_node_id) { - size_t current_index = 0; - CanardPoolAllocatorBlock** current_block = &(allocator->free_list); - while (current_index < buf_len) - { - *current_block = &buf[current_index]; - current_block = &((*current_block)->next); - current_index++; - } - *current_block = NULL; - - allocator->statistics.capacity_blocks = buf_len; - allocator->statistics.current_usage_blocks = 0; - allocator->statistics.peak_usage_blocks = 0; + CANARD_ASSERT(subject_id <= CANARD_SUBJECT_ID_MAX); + CANARD_ASSERT(src_node_id <= CANARD_NODE_ID_MAX); + return ((uint32_t) src_node_id) | ((uint32_t) subject_id << 8U); } -CANARD_INTERNAL void* allocateBlock(CanardPoolAllocator* allocator) +CANARD_INTERNAL inline uint32_t makeServiceSessionSpecifier(const uint16_t service_id, + const bool request_not_response, + const uint8_t src_node_id, + const uint8_t dst_node_id) { - // Check if there are any blocks available in the free list. - if (allocator->free_list == NULL) - { - return NULL; - } - - // Take first available block and prepares next block for use. - void* result = allocator->free_list; - allocator->free_list = allocator->free_list->next; - - // Update statistics - allocator->statistics.current_usage_blocks++; - if (allocator->statistics.peak_usage_blocks < allocator->statistics.current_usage_blocks) - { - allocator->statistics.peak_usage_blocks = allocator->statistics.current_usage_blocks; - } - - return result; + CANARD_ASSERT(service_id <= CANARD_SERVICE_ID_MAX); + CANARD_ASSERT(src_node_id <= CANARD_NODE_ID_MAX); + CANARD_ASSERT(dst_node_id <= CANARD_NODE_ID_MAX); + return ((uint32_t) src_node_id) | ((uint32_t) dst_node_id << 7U) | ((uint32_t) service_id << 14U) | + (request_not_response ? (1U << 24U) : 0U); } -CANARD_INTERNAL void freeBlock(CanardPoolAllocator* allocator, void* p) -{ - CanardPoolAllocatorBlock* block = (CanardPoolAllocatorBlock*) p; - - block->next = allocator->free_list; - allocator->free_list = block; - - CANARD_ASSERT(allocator->statistics.current_usage_blocks > 0); - allocator->statistics.current_usage_blocks--; -} +CANARD_STATIC_ASSERT(sizeof(float) == 4, "Native float format shall match IEEE 754 binary32"); diff --git a/libcanard/canard.h b/libcanard/canard.h index 158337cf..35c6d78e 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -1,61 +1,58 @@ -/* - * Copyright (c) 2016-2020 UAVCAN Development Team - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - * - * Contributors: https://github.com/UAVCAN/libcanard/contributors - */ - -#ifndef CANARD_H -#define CANARD_H +// Copyright (c) 2016-2020 UAVCAN Development Team +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +// Contributors: https://github.com/UAVCAN/libcanard/contributors + +#ifndef CANARD_H_INCLUDED +#define CANARD_H_INCLUDED -#include #include +#include #include -#include - -/// Build configuration header. Use it to provide your overrides. -#if defined(CANARD_ENABLE_CUSTOM_BUILD_CONFIG) && CANARD_ENABLE_CUSTOM_BUILD_CONFIG -# include "canard_build_config.h" -#endif #ifdef __cplusplus extern "C" { #endif -/// Semantic version numbers of this library (not the UAVCAN specification). -/// API will be backwards compatible within the same major version. -#define CANARD_VERSION_MAJOR 1 -#define CANARD_VERSION_MINOR 0 +// ---------------------------------------- BUILD CONFIGURATION ---------------------------------------- -/// The version number of the UAVCAN specification implemented by this library. -#define CANARD_UAVCAN_SPECIFICATION_VERSION_MAJOR 1 +/// Build configuration header. Use it to provide your overrides. +/// Alternatively, you can specify each option individually in the preprocessor flags when invoking the compiler. +#if defined(CANARD_CONFIG_HEADER_AVAILABLE) && CANARD_CONFIG_HEADER_AVAILABLE +# include "canard_build_config.h" +#endif + +/// By default, the library is built to support all versions of CAN. +/// The downside of such flexibility is that it increases the memory footprint of the library. +/// If the available CAN hardware supports only Classic CAN, then it might make sense to limit the maximum transmission +/// unit (MTU) supported by the library to reduce the memory footprint. +#ifndef CANARD_CONFIG_MTU_MAX +# define CANARD_CONFIG_MTU_MAX CANARD_MTU_CAN_FD +#endif -/// By default this macro resolves to the standard assert(). The user can redefine this if necessary. +/// By default, this macro resolves to the standard assert(). The user can redefine this if necessary. #ifndef CANARD_ASSERT # define CANARD_ASSERT(x) assert(x) #endif -#define CANARD_GLUE(a, b) CANARD_GLUE_IMPL_(a, b) -#define CANARD_GLUE_IMPL_(a, b) a##b - -/// By default this macro expands to static_assert if supported by the language (C11, C++11, or newer). +/// By default, this macro expands to the standard static_assert() if supported by the language (C11, C++11, or newer). /// The user can redefine this if necessary. #ifndef CANARD_STATIC_ASSERT # if (defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 201112L)) || \ @@ -66,6 +63,16 @@ extern "C" { # endif #endif +// ---------------------------------------- CONSTANTS ---------------------------------------- + +/// Semantic version numbers of this library (not the UAVCAN specification). +/// API will be backward compatible within the same major version. +#define CANARD_VERSION_MAJOR 1 +#define CANARD_VERSION_MINOR 0 + +/// The version number of the UAVCAN specification implemented by this library. +#define CANARD_UAVCAN_SPECIFICATION_VERSION_MAJOR 1 + /// Error code definitions; inverse of these values may be returned from API calls. #define CANARD_OK 0 // Value 1 is omitted intentionally since -1 is often used in 3rd party code @@ -73,456 +80,379 @@ extern "C" { #define CANARD_ERROR_OUT_OF_MEMORY 3 #define CANARD_ERROR_NODE_ID_NOT_SET 4 #define CANARD_ERROR_INTERNAL 9 -#define CANARD_ERROR_RX_INCOMPATIBLE_PACKET 10 -#define CANARD_ERROR_RX_WRONG_ADDRESS 11 -#define CANARD_ERROR_RX_NOT_WANTED 12 -#define CANARD_ERROR_RX_MISSED_START 13 -#define CANARD_ERROR_RX_WRONG_TOGGLE 14 -#define CANARD_ERROR_RX_UNEXPECTED_TID 15 -#define CANARD_ERROR_RX_SHORT_FRAME 16 -#define CANARD_ERROR_RX_BAD_CRC 17 - -/// The size of a memory block in bytes. -#define CANARD_MEM_BLOCK_SIZE 32U - -/// This will be changed when the support for CAN FD is added -#define CANARD_CAN_FRAME_MAX_DATA_LEN 8U - -#define CANARD_CAN_MULTI_FRAME_CRC_LENGTH 2U - -/// Node-ID values. Refer to the specification for more info. -#define CANARD_BROADCAST_NODE_ID 255 -#define CANARD_MIN_NODE_ID 0 -#define CANARD_MAX_NODE_ID 127 - -/// Refer to the type CanardRxTransfer -#define CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE (CANARD_MEM_BLOCK_SIZE - offsetof(CanardRxState, buffer_head)) - -/// Refer to the type CanardBufferBlock -#define CANARD_BUFFER_BLOCK_DATA_SIZE (CANARD_MEM_BLOCK_SIZE - offsetof(CanardBufferBlock, data)) - -/// Transfer priority definitions -#define CANARD_TRANSFER_PRIORITY_EXCEPTIONAL 0 -#define CANARD_TRANSFER_PRIORITY_IMMEDIATE 1 -#define CANARD_TRANSFER_PRIORITY_FAST 2 -#define CANARD_TRANSFER_PRIORITY_HIGH 3 -#define CANARD_TRANSFER_PRIORITY_NOMINAL 4 -#define CANARD_TRANSFER_PRIORITY_LOW 5 -#define CANARD_TRANSFER_PRIORITY_SLOW 6 -#define CANARD_TRANSFER_PRIORITY_OPTIONAL 7 - -/// Related to CanardCANFrame -#define CANARD_CAN_EXT_ID_MASK 0x1FFFFFFFU -#define CANARD_CAN_STD_ID_MASK 0x000007FFU -#define CANARD_CAN_FRAME_EFF (1UL << 31U) ///< Extended frame format -#define CANARD_CAN_FRAME_RTR (1UL << 30U) ///< Remote transmission (not used by UAVCAN) -#define CANARD_CAN_FRAME_ERR (1UL << 29U) ///< Error frame (not used by UAVCAN) - -#define CANARD_TRANSFER_PAYLOAD_LEN_BITS 10U -#define CANARD_MAX_TRANSFER_PAYLOAD_LEN ((1U << CANARD_TRANSFER_PAYLOAD_LEN_BITS) - 1U) - -/** - * This data type holds a standard CAN 2.0B data frame with 29-bit ID. - */ + +/// MTU values for supported protocols. +/// Per the recommendations given in the UAVCAN specification, other MTU values should not be used. +#define CANARD_MTU_CAN_CLASSIC 8U +#define CANARD_MTU_CAN_FD 64U + +#if CANARD_CONFIG_MTU_MAX == CANARD_MTU_CAN_FD +#elif CANARD_CONFIG_MTU_MAX == CANARD_MTU_CAN_CLASSIC +#else +# error "CANARD_CONFIG_MTU_MAX is invalid: one of the standard MTU values shall be used (8, 64...)" +#endif +#define CANARD_MTU_RUNTIME_CONFIGURABLE (CANARD_CONFIG_MTU_MAX > CANARD_MTU_CAN_CLASSIC) + +/// Parameter ranges are inclusive; the lower bound is zero for all. Refer to the specification for more info. +#define CANARD_SUBJECT_ID_MAX 32767U +#define CANARD_SERVICE_ID_MAX 511U +#define CANARD_NODE_ID_MAX 127U +#define CANARD_TRANSFER_ID_BIT_LENGTH 5U +#define CANARD_TRANSFER_ID_MAX ((1U << CANARD_TRANSFER_ID_BIT_LENGTH) - 1U) + +/// This value represents an undefined node-ID: broadcast destination or anonymous source. +/// Library functions treat all values above @ref CANARD_NODE_ID_MAX as anonymous. +#define CANARD_NODE_ID_UNSET 255U + +/// If not specified, the transfer-ID timeout will take this value for all new input sessions. +#define CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC 2000000UL + +// ---------------------------------------- TYPES ---------------------------------------- + +// Forward declarations. +typedef struct CanardInstance CanardInstance; +typedef struct CanardReceivedTransfer CanardReceivedTransfer; + +// These forward declarations are not a part of the library API, but they need to be here due to the limitations of C. +typedef struct CanardInternalInputSession CanardInternalInputSession; +typedef struct CanardInternalTxQueueItem CanardInternalTxQueueItem; + +/// Transfer priority level mnemonics per the recommendations given in the UAVCAN specification. +typedef enum +{ + CanardPriorityExceptional = 0, + CanardPriorityImmediate = 1, + CanardPriorityFast = 2, + CanardPriorityHigh = 3, + CanardPriorityNominal = 4, + CanardPriorityLow = 5, + CanardPrioritySlow = 6, + CanardPriorityOptional = 7, +} CanardPriority; + +/// CAN data frame with an extended 29-bit ID. typedef struct { - /** - * Refer to the following definitions: - * - CANARD_CAN_FRAME_EFF - * - CANARD_CAN_FRAME_RTR - * - CANARD_CAN_FRAME_ERR - */ - uint32_t id; - uint8_t data[CANARD_CAN_FRAME_MAX_DATA_LEN]; - uint8_t data_len; + uint32_t id; ///< 29-bit extended ID. The bits above 29-th are ignored. + uint8_t data_length; ///< The amount of useful data in the frame. Not to be confused with DLC! + uint8_t data[CANARD_CONFIG_MTU_MAX]; ///< The payload capacity depends on the compile-time MTU setting. } CanardCANFrame; -/** - * Transfer types are defined by the UAVCAN specification. - */ +/// Transfer kinds are defined by the UAVCAN specification. typedef enum { - CanardTransferKindServiceResponse = 0, - CanardTransferKindServiceRequest = 1, - CanardTransferKindMessagePublication = 2 + CanardTransferKindMessagePublication = 0, ///< Broadcast, from publisher to all subscribers. + CanardTransferKindServiceResponse = 1, ///< Point-to-point, from server to client. + CanardTransferKindServiceRequest = 2 ///< Point-to-point, from client to server. } CanardTransferKind; -/** - * Types of service transfers. These are not applicable to message transfers. - */ -typedef enum -{ - CanardResponse, - CanardRequest -} CanardRequestResponse; - -// Forward declarations. -typedef struct CanardInstance CanardInstance; -typedef struct CanardRxTransfer CanardRxTransfer; -typedef struct CanardRxState CanardRxState; -typedef struct CanardTxQueueItem CanardTxQueueItem; - -/** - * The application shall implement this function and supply a pointer to it to the library during initialization. - * The library calls this function to determine whether a transfer should be received. - * @param ins Library instance. - * @param port_id Subject-ID or service-ID of the transfer. - * @param transfer_kind Message or service transfer. - * @param source_node_id Node-ID of the origin; broadcast if anonymous. - * @returns True if the transfer should be received; false otherwise. - */ -typedef bool (*CanardShouldAcceptTransfer)(const CanardInstance* ins, - uint16_t port_id, - CanardTransferKind transfer_kind, - uint8_t source_node_id); - -/** - * This function will be invoked by the library every time a transfer is successfully received. - * If the application needs to send another transfer from this callback, it is highly recommended - * to call @ref canardReleaseRxTransferPayload() first, so that the memory that was used for the block - * buffer can be released and re-used by the TX queue. - * @param ins Library instance. - * @param transfer Pointer to the temporary transfer object. Invalidated upon return. - */ -typedef void (*CanardOnTransferReception)(CanardInstance* ins, CanardRxTransfer* transfer); - -/** - * This structure provides the usage statistics of the memory pool allocator. - * It indicates whether the allocated memory is sufficient for the application. - */ +/// The application supplies the library with this information when a new transfer should be received. typedef struct { - uint16_t capacity_blocks; ///< Pool capacity in number of blocks - uint16_t current_usage_blocks; ///< Number of blocks that are currently allocated by the library - uint16_t peak_usage_blocks; ///< Maximum number of blocks used since initialization -} CanardPoolAllocatorStatistics; - -typedef union CanardPoolAllocatorBlock_u -{ - char bytes[CANARD_MEM_BLOCK_SIZE]; - union CanardPoolAllocatorBlock_u* next; -} CanardPoolAllocatorBlock; - + bool should_accept; + uint32_t transfer_id_timeout_usec; + size_t payload_capacity; +} CanardTransferReceptionParameters; + +/// The application shall implement this function and supply a pointer to it to the library during initialization. +/// The library calls this function to determine whether a transfer should be received. +/// @param ins Library instance. +/// @param port_id Subject-ID or service-ID of the transfer. +/// @param transfer_kind Message or service transfer. +/// @param source_node_id Node-ID of the origin; broadcast if anonymous. +/// @returns @ref CanardTransferReceptionParameters. +typedef CanardTransferReceptionParameters (*CanardShouldAcceptTransfer)(const CanardInstance* ins, + uint16_t port_id, + CanardTransferKind transfer_kind, + uint8_t source_node_id); + +/// This function will be invoked by the library every time a transfer is successfully received. +/// If the application needs to send another transfer from this callback, it is highly recommended +/// to call @ref canardReleaseReceivedTransferPayload() first, so that the memory that was used for the block +/// buffer can be released and re-used by the TX queue. +/// @param ins Library instance. +/// @param transfer Pointer to the temporary transfer object. Invalidated upon return. +typedef void (*CanardProcessReceivedTransfer)(CanardInstance* ins, CanardReceivedTransfer* transfer); + +/// This structure provides the usage statistics of the dynamic memory allocator. +/// It can be used to determine whether the allocated memory is sufficient for the application. typedef struct { - CanardPoolAllocatorBlock* free_list; - CanardPoolAllocatorStatistics statistics; -} CanardPoolAllocator; + // TODO +} CanardMemoryAllocatorStatistics; -typedef struct CanardBufferBlock +typedef struct { - struct CanardBufferBlock* next; - uint8_t data[]; -} CanardBufferBlock; + // TODO + CanardMemoryAllocatorStatistics statistics; +} CanardInternalMemoryAllocator; -struct CanardRxState +/// This is the core structure that keeps all of the states and allocated resources of the library instance. +/// The application should never access any of the fields directly! Instead, the API functions should be used. +struct CanardInstance { - struct CanardRxState* next; - - CanardBufferBlock* buffer_blocks; + const CanardShouldAcceptTransfer should_accept_transfer; ///< Transfer acceptance predicate. + const CanardProcessReceivedTransfer process_received_transfer; ///< Received transfer handler. - uint64_t timestamp_usec; + CanardInternalMemoryAllocator allocator; ///< Deterministic constant-complexity dynamic memory allocator. + CanardInternalInputSession* input_sessions; ///< Rx session states. + CanardInternalTxQueueItem* tx_queue; ///< TX frames awaiting transmission. - const uint32_t session_specifier; + void* const user_reference; ///< User pointer that can link this instance with other objects - // We're using plain 'unsigned' here, because C99 doesn't permit explicit field type specification - unsigned calculated_crc : 16; - unsigned payload_len : CANARD_TRANSFER_PAYLOAD_LEN_BITS; - unsigned transfer_id : 5; - unsigned next_toggle : 1; // 16+10+5+1 = 32, aligned. + uint8_t node_id; ///< Local node-ID or @ref CANARD_NODE_ID_UNSET. - uint16_t payload_crc; - - uint8_t buffer_head[]; +#if CANARD_MTU_RUNTIME_CONFIGURABLE + uint8_t mtu_bytes; ///< Maximum number of data bytes per CAN frame. Range: [8, CANARD_CONFIG_MTU_MAX]. +#endif }; -CANARD_STATIC_ASSERT(offsetof(CanardRxState, buffer_head) <= 28, "Invalid memory layout"); -CANARD_STATIC_ASSERT(CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE >= 4, "Invalid memory layout"); -/** - * This is the core structure that keeps all of the states and allocated resources of the library instance. - * The application should never access any of the fields directly! Instead, the API functions should be used. - */ -struct CanardInstance +/// This structure represents a received transfer for the application. +/// An instance of it is passed to the application via the callback when a new transfer is received. +/// Pointers to the structure and all its fields are invalidated after the callback returns. +struct CanardReceivedTransfer { - uint8_t node_id; ///< Local node-ID; may be zero if the node is anonymous + /// Timestamp at which the first frame of this transfer was received. + const uint64_t timestamp_usec; - CanardShouldAcceptTransfer should_accept; ///< Function to decide whether the application wants this transfer - CanardOnTransferReception on_reception; ///< Function the library calls after RX transfer is complete + const uint8_t* const payload; + const size_t payload_length; - CanardPoolAllocator allocator; ///< Pool allocator + const CanardPriority priority; + const CanardTransferKind transfer_kind; + const uint16_t port_id; ///< Subject-ID or service-ID. + const uint8_t source_node_id; ///< For anonymous transfers it's @ref CANARD_NODE_ID_UNSET. + const uint8_t transfer_id; ///< Bits above @ref CANARD_TRANSFER_ID_BIT_LENGTH are always zero. +}; - CanardRxState* rx_states; ///< RX transfer states - CanardTxQueueItem* tx_queue; ///< TX frames awaiting transmission +/// Initialize a library instance. +/// The local node-ID will be initialized as @ref CANARD_NODE_ID_UNSET, i.e. anonymous by default. +/// +/// The size of the memory arena should be an integer power of two; otherwise, the size may be rounded down. +/// Typically, the size of the memory arena should not be less than 8 KiB, although it depends on the application. +/// The recommended way to detect the required memory size is to measure the peak pool usage after a stress-test. +/// Refer to the function @ref canardGetPoolAllocatorStatistics(). +/// +/// @param memory_arena Raw memory chunk used for the deterministic dynamic memory allocator. +/// @param memory_arena_size Size of the above, in bytes. +/// @param should_accept_transfer Callback, see @ref CanardShouldAcceptTransfer. +/// @param process_received_transfer Callback, see @ref CanardOnTransferReception. +/// @param user_reference Optional application-defined pointer; NULL if not needed. +CanardInstance canardInit(void* const memory_arena, + const size_t memory_arena_size, + const CanardShouldAcceptTransfer should_accept_transfer, + const CanardProcessReceivedTransfer process_received_transfer, + void* const user_reference); + +/// Read the value of the user pointer. +/// The user pointer is configured once during initialization. +/// It can be used to store references to any user-specific data, or to link the instance object with C++ objects. +/// @returns The application-defined pointer. +void* canardGetUserReference(const CanardInstance* const ins); + +/// Assign a new node-ID value to the current node. Node-ID can be assigned only once. +/// If the supplied value is invalid or the node-ID is already configured, nothing will be done. +void canardSetLocalNodeID(CanardInstance* const ins, const uint8_t self_node_id); + +/// @returns Node-ID of the local node; 255 (broadcast) if the node-ID is not set, i.e. if the local node is anonymous. +uint8_t canardGetLocalNodeID(const CanardInstance* const ins); + +#if CANARD_MTU_RUNTIME_CONFIGURABLE +/// Configure the maximum transmission unit. This can be done as many times as needed. +/// This setting defines the maximum number of bytes per CAN data frame in all outgoing transfers. +/// Regardless of this setting, CAN frames with any MTU up to CANARD_CONFIG_MTU_MAX bytes can always be accepted. +/// +/// Only the standard values should be used as recommended by the specification (8, 64 bytes); +/// otherwise, interoperability issues may arise. See "CANARD_MTU_*". +/// +/// Range: [8, CANARD_CONFIG_MTU_MAX]. The default is CANARD_CONFIG_MTU_MAX (i.e., the maximum). +/// Invalid values are rounded to the nearest valid value. +void canardSetMTU(const CanardInstance* const ins, const uint8_t mtu_bytes); +#endif - void* user_reference; ///< User pointer that can link this instance with other objects -}; +/// Send a broadcast message transfer. If the local node is anonymous, only single frame transfers are be allowed. +/// +/// The pointer to the transfer-ID shall point to a persistent variable (e.g. static or heap-allocated, not on the +/// stack); it will be updated by the library after every transmission. The transfer-ID value cannot be shared +/// between different sessions! Read the transport layer specification for details. +/// +/// @param ins Library instance. +/// @param subject_id Refer to the specification. +/// @param inout_transfer_id Pointer to a persistent variable containing the transfer-ID. +/// @param priority Refer to CANARD_TRANSFER_PRIORITY_*. +/// @param payload Transfer payload -- the serialized DSDL object. +/// @param payload_len Length of the above, in bytes. +/// @returns The number of frames enqueued, or a negative error code. +int32_t canardPublishMessage(CanardInstance* const ins, + const uint16_t subject_id, + uint8_t* const inout_transfer_id, + const uint8_t priority, + const void* const payload, + const size_t payload_length, + uint64_t deadline_usec); + +/// Send a request transfer. Fails if the local node is anonymous. +/// +/// The pointer to the transfer-ID shall point to a persistent variable (e.g., static- or heap-allocated, +/// not on the stack); it will be updated by the library after every request. The transfer-ID value +/// cannot be shared between different sessions! Read the transport layer specification for details. +/// +/// @param ins Library instance. +/// @param destination_node_id Node-ID of the destination server. +/// @param service_id Refer to the specification. +/// @param inout_transfer_id Pointer to a persistent variable containing the transfer-ID. +/// @param priority @ref CanardPriority. +/// @param payload Transfer payload -- the serialized DSDL object. +/// @param payload_length Length of the above, in bytes. +/// @returns The number of frames enqueued, or a negative error code. +int32_t canardSendRequest(CanardInstance* const ins, + const uint8_t server_node_id, + const uint16_t service_id, + uint8_t* const inout_transfer_id, + const CanardPriority priority, + const void* const payload, + const size_t payload_length, + uint64_t deadline_usec); + +/// Send a response transfer. Fails if the local node is anonymous. +/// +/// The transfer-ID shall be the same as in the corresponding service request. +/// +/// @param ins Library instance. +/// @param destination_node_id Node-ID of the destination client. +/// @param service_id Refer to the specification. +/// @param transfer_id Same as in the original request transfer. +/// @param priority @ref CanardPriority. +/// @param payload Transfer payload -- the serialized DSDL object. +/// @param payload_length Length of the above, in bytes. +/// @returns The number of frames enqueued, or a negative error code. +int32_t canardSendResponse(CanardInstance* const ins, + const uint8_t client_node_id, + const uint16_t service_id, + const uint8_t transfer_id, + const CanardPriority priority, + const void* const payload, + const size_t payload_length, + uint64_t deadline_usec); + +/// The application will call this function after @ref canardPublishMessage() or its service counterpart to transmit the +/// generated frames over the CAN bus. +/// +/// @returns A pointer to the top-priority frame in the TX queue; or NULL if the TX queue is empty. +const CanardCANFrame* canardPeekTxQueue(const CanardInstance* const ins, uint64_t* const out_deadline_usec); + +/// Remove the top priority frame from the TX queue. +/// The application will call this function after @ref canardPeekTxQueue() once the obtained frame has been processed. +/// Calling @ref canardPublishMessage() or its service counterpart between @ref canardPeekTxQueue() and this function +/// is NOT allowed, because it may change the frame at the top of the TX queue. +void canardPopTxQueue(CanardInstance* const ins); -/** - * This structure represents a received transfer for the application. - * An instance of it is passed to the application via the callback when a new transfer is received. - * Pointers to the structure and all its fields are invalidated after the callback returns. - */ -struct CanardRxTransfer +typedef enum { - /** - * Timestamp at which the first frame of this transfer was received. - */ - uint64_t timestamp_usec; - - /** - * Payload is scattered across three storages: - * - Head points to CanardRxState.buffer_head (length of which is up to CANARD_PAYLOAD_HEAD_SIZE), or to the - * payload field (possibly with offset) of the last received CAN frame. - * - * - Middle is located in the linked list of dynamic blocks (only for multi-frame transfers). - * - * - Tail points to the payload field (possibly with offset) of the last received CAN frame - * (only for multi-frame transfers). - * - * The tail offset depends on how much data of the last frame was accommodated in the last allocated block. - * - * For single-frame transfers, middle and tail will be NULL, and the head will point at first byte - * of the payload of the CAN frame. - * - * In simple cases it should be possible to get data directly from the head and/or tail pointers. - * Otherwise it is advised to use canardDecodePrimitive(). - */ - const uint8_t* payload_head; ///< Always valid, i.e. not NULL. - ///< For multi frame transfers, the maximum size is defined in the constant - ///< CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE. - ///< For single-frame transfers, the size is defined in the - ///< field payload_len. - CanardBufferBlock* payload_middle; ///< May be NULL if the buffer was not needed. Always NULL for single-frame - ///< transfers. - const uint8_t* payload_tail; ///< Last bytes of multi-frame transfers. Always NULL for single-frame - ///< transfers. - uint16_t payload_len; ///< Effective length of the payload in bytes. - - /** - * These fields identify the transfer for the application. - */ - uint16_t port_id; ///< 0 to 511 for services, 0 to 32767 for messages - uint8_t transfer_kind; ///< See CanardTransferKind - uint8_t transfer_id; ///< 0 to 31 - uint8_t priority; ///< 0 to 7 - uint8_t source_node_id; ///< 0 to 127, or 255 if the source is anonymous -}; - -/** - * Initialize a library instance. The local node-ID will be initialized as 255, i.e. anonymous by default. - * - * Typically, the size of the memory pool should not be less than 1K, although it depends on the application. - * The recommended way to detect the required pool size is to measure the peak pool usage after a stress-test. - * Refer to the function @ref canardGetPoolAllocatorStatistics(). - * - * @param out_ins Uninitialized library instance. - * @param mem_arena Raw memory chunk used for dynamic allocation. - * @param mem_arena_size Size of the above, in bytes. - * @param on_reception Callback, see @ref CanardOnTransferReception. - * @param should_accept Callback, see @ref CanardShouldAcceptTransfer. - * @param user_reference Optional application-defined pointer; NULL if not needed. - */ -void canardInit(CanardInstance* out_ins, - void* mem_arena, - size_t mem_arena_size, - CanardOnTransferReception on_reception, - CanardShouldAcceptTransfer should_accept, - void* user_reference); - -/** - * Read the value of the user pointer. - * The user pointer is configured once during initialization. - * It can be used to store references to any user-specific data, or to link the instance object with C++ objects. - * @returns The application-defined pointer. - */ -void* canardGetUserReference(CanardInstance* ins); - -/** - * Assign a new node-ID value to the current node. Node-ID can be assigned only once. - * @returns CANARD_OK on success; returns negative error code if the node-ID is outside of the valid range or is already - * configured. - */ -int16_t canardSetLocalNodeID(CanardInstance* ins, uint8_t self_node_id); - -/** - * @returns Node-ID of the local node; 255 (broadcast) if the node-ID is not set, i.e. if the local node is anonymous. - */ -uint8_t canardGetLocalNodeID(const CanardInstance* ins); - -/** - * Send a broadcast message transfer. If the local node is anonymous, only single frame transfers are be allowed. - * - * The pointer to the transfer-ID shall point to a persistent variable (e.g. static or heap-allocated, not on the - * stack); it will be updated by the library after every transmission. The transfer-ID value cannot be shared - * between different sessions! Read the transport layer specification for details. - * - * @param ins Library instance. - * @param subject_id Refer to the specification. - * @param inout_transfer_id Pointer to a persistent variable containing the transfer-ID. - * @param priority Refer to CANARD_TRANSFER_PRIORITY_*. - * @param payload Transfer payload -- the serialized DSDL object. - * @param payload_len Length of the above, in bytes. - * @returns The number of frames enqueued, or a negative error code. - */ -int16_t canardPublishMessage(CanardInstance* ins, - uint16_t subject_id, - uint8_t* inout_transfer_id, - uint8_t priority, - const void* payload, - uint16_t payload_len); - -/** - * Send a request or a response transfer. Fails if the local node is anonymous. - * - * For request transfers, the pointer to the transfer-ID shall point to a persistent variable (e.g., static- or - * heap-allocated, not on the stack); it will be updated by the library after every request. The transfer-ID value - * cannot be shared between different sessions! Read the transport layer specification for details. - * - * For response transfers, the transfer-ID pointer shall point to the transfer_id field of the request transfer - * structure @ref CanardRxTransfer. - * - * @param ins Library instance. - * @param destination_node_id Node-ID of the server/client. - * @param service_id Refer to the specification. - * @param inout_transfer_id Pointer to a persistent variable containing the transfer-ID. - * @param priority Refer to definitions CANARD_TRANSFER_PRIORITY_*. - * @param kind Refer to CanardRequestResponse. - * @param payload Transfer payload -- the serialized DSDL object. - * @param payload_len Length of the above, in bytes. - * @returns The number of frames enqueued, or a negative error code. - */ -int16_t canardRequestOrRespond(CanardInstance* ins, - uint8_t destination_node_id, - uint16_t service_id, - uint8_t* inout_transfer_id, - uint8_t priority, - CanardRequestResponse kind, - const void* payload, - uint16_t payload_len); - -/** - * The application will call this function after @ref canardPublishMessage() or its service counterpart to transmit the - * generated frames over the CAN bus. - * - * @returns A pointer to the top-priority frame in the TX queue; or NULL if the TX queue is empty. - */ -const CanardCANFrame* canardPeekTxQueue(const CanardInstance* ins); - -/** - * Remove the top priority frame from the TX queue. - * The application will call this function after @ref canardPeekTxQueue() once the obtained frame has been processed. - * Calling @ref canardPublishMessage() or its service counterpart between @ref canardPeekTxQueue() and this function - * is NOT allowed, because it may change the frame at the top of the TX queue. - */ -void canardPopTxQueue(CanardInstance* ins); - -/** - * Process a received CAN frame with a timestamp. - * The application will call this function upon reception of a new frame from the CAN bus. - * - * @param ins Library instance. - * @param frame The received CAN frame. - * @param timestamp_usec The timestamp. The time system may be arbitrary as long as the clock is monotonic (steady). - * @returns Zero or a negative error code. - */ -int16_t canardHandleRxFrame(CanardInstance* ins, const CanardCANFrame* frame, uint64_t timestamp_usec); - -/** - * This function may be used to extract values from received UAVCAN transfers. It decodes a scalar value -- - * boolean, integer, character, or floating point -- from the specified bit position in the RX transfer buffer. - * Simple single-frame transfers can also be parsed manually instead of using this function. - * - * Caveat: This function works correctly only on platforms that use two's complement signed integer representation. - * I am not aware of any modern microarchitecture that uses anything else than two's complement, so it should not - * limit the portability. - * - * The type of the value pointed to by 'out_value' is defined as follows: - * - * | bit_length | value_is_signed | out_value points to | - * |------------|-----------------|------------------------------------------| - * | 1 | false | bool (may be incompatible with uint8_t!) | - * | 1 | true | N/A | - * | [2, 8] | false | uint8_t, or char | - * | [2, 8] | true | int8_t, or char | - * | [9, 16] | false | uint16_t | - * | [9, 16] | true | int16_t | - * | [17, 32] | false | uint32_t | - * | [17, 32] | true | int32_t, or 32-bit float | - * | [33, 64] | false | uint64_t | - * | [33, 64] | true | int64_t, or 64-bit float | - * - * @param transfer The RX transfer where the data will be read from. - * @param bit_offset Offset, in bits, from the beginning of the transfer payload. - * @param bit_length Length of the value, in bits; see the table. - * @param value_is_signed True if the value can be negative (i.e., sign bit extension is needed); see the table. - * @param out_value Pointer to the output storage; see the table. - * @returns Same as bit_length, or a negated error code, such as invalid argument. - */ -int16_t canardDecodePrimitive(const CanardRxTransfer* transfer, - uint32_t bit_offset, - uint8_t bit_length, - bool value_is_signed, - void* out_value); - -/** - * This function may be used to encode values for later transmission in a UAVCAN transfer. It encodes a scalar value - * -- boolean, integer, character, or floating point -- and puts it at the specified bit offset in the specified - * contiguous buffer. Simple payloads can also be encoded manually instead of using this function. - * - * Caveat: This function works correctly only on platforms that use two's complement signed integer representation. - * I am not aware of any modern microarchitecture that uses anything else than two's complement, so it should not - * limit the portability. - * - * The type of the value pointed to by 'value' is defined as follows: - * - * | bit_length | value points to | - * |------------|------------------------------------------| - * | 1 | bool (may be incompatible with uint8_t!) | - * | [2, 8] | uint8_t, int8_t, or char | - * | [9, 16] | uint16_t, int16_t | - * | [17, 32] | uint32_t, int32_t, or 32-bit float | - * | [33, 64] | uint64_t, int64_t, or 64-bit float | - * - * @param destination Destination buffer where the result will be stored. - * @param bit_offset Offset, in bits, from the beginning of the destination buffer. - * @param bit_length Length of the value, in bits; see the table. - * @param value Pointer to the value; see the table. - */ -void canardEncodePrimitive(void* destination, uint32_t bit_offset, uint8_t bit_length, const void* value); - -/** - * This function may be invoked by the application to release the memory allocated for the received transfer payload. - * - * If the application needs to send new transfers from the transfer reception callback, this function should be - * invoked right before calling @ref canardPublishMessage() or its service counterpart. - * Not doing that may lead to a higher worst-case memory pool utilization. - * - * Failure to call this function will NOT lead to a memory leak because the library checks for it. - */ -void canardReleaseRxTransferPayload(CanardInstance* ins, CanardRxTransfer* transfer); - -/** - * Use this function to determine the worst case memory footprint of your application. - * See @ref CanardPoolAllocatorStatistics. - * @returns a copy of the pool allocator usage statistics. - */ -CanardPoolAllocatorStatistics canardGetPoolAllocatorStatistics(CanardInstance* ins); - -/** - * IEEE 754 binary16 marshaling helpers. - * These functions convert between the native float and the standard IEEE 754 binary16 float (a.k.a. half precision). - * It is assumed that the native float is IEEE 754 binary32, otherwise, the results may be unpredictable. - * Majority of modern computers and microcontrollers use IEEE 754, so this limitation should not limit the portability. - */ + CanardReceivedFrameProcessingResultOK = CANARD_OK, + CanardReceivedFrameProcessingResultWrongFormat = 1, + CanardReceivedFrameProcessingResultWrongDestination = 2, + CanardReceivedFrameProcessingResultWrongToggle = 3, + CanardReceivedFrameProcessingResultWrongTransferID = 4, + CanardReceivedFrameProcessingResultMissedStartOfTransfer = 6, + CanardReceivedFrameProcessingResultCRCMismatch = 7 +} CanardReceivedFrameProcessingResult; + +/// Process a received CAN frame with a timestamp. +/// The application will call this function upon reception of a new frame from the CAN bus. +/// +/// @param ins Library instance. +/// @param frame The received CAN frame. +/// @param timestamp_usec The timestamp. The time system may be arbitrary as long as the clock is monotonic (steady). +/// @returns Zero if accepted; negative error code; or a value from @ref CanardReceivedFrameProcessingResult. +int8_t canardProcessReceivedFrame(CanardInstance* const ins, + const CanardCANFrame* const frame, + const uint64_t timestamp_usec); + +/// This function may be used to extract values from received UAVCAN transfers. It decodes a scalar value -- +/// boolean, integer, character, or floating point -- from the specified bit position in the RX transfer buffer. +/// Simple single-frame transfers can also be parsed manually instead of using this function. +/// +/// Caveat: This function works correctly only on platforms that use two's complement signed integer representation. +/// I am not aware of any modern microarchitecture that uses anything else than two's complement, so it should not +/// limit the portability. +/// +/// The type of the value pointed to by 'out_value' is defined as follows: +/// +/// | bit_length | value_is_signed | out_value points to | +/// |------------|-----------------|------------------------------------------| +/// | 1 | false | bool (may be incompatible with uint8_t!) | +/// | 1 | true | N/A | +/// | [2, 8] | false | uint8_t, or char | +/// | [2, 8] | true | int8_t, or char | +/// | [9, 16] | false | uint16_t | +/// | [9, 16] | true | int16_t | +/// | [17, 32] | false | uint32_t | +/// | [17, 32] | true | int32_t, or 32-bit float IEEE 754 | +/// | [33, 64] | false | uint64_t | +/// | [33, 64] | true | int64_t, or 64-bit float IEEE 754 | +/// +/// @param transfer The RX transfer where the data will be read from. +/// @param bit_offset Offset, in bits, from the beginning of the transfer payload. +/// @param bit_length Length of the value, in bits; see the table. +/// @param value_is_signed True if the value can be negative (i.e., sign bit extension is needed); see the table. +/// @param out_value Pointer to the output storage; see the table. +void canardDeserializePrimitive(const CanardReceivedTransfer* const transfer, + const size_t bit_offset, + const uint8_t bit_length, + const bool value_is_signed, + void* const out_value); + +/// This function may be used to encode values for later transmission in a UAVCAN transfer. It encodes a scalar value +/// -- boolean, integer, character, or floating point -- and puts it at the specified bit offset in the specified +/// contiguous buffer. Simple payloads can also be encoded manually instead of using this function. +/// +/// Caveat: This function works correctly only on platforms that use two's complement signed integer representation. +/// I am not aware of any modern microarchitecture that uses anything else than two's complement, so it should not +/// limit the portability. +/// +/// The type of the value pointed to by 'value' is defined as follows: +/// +/// | bit_length | value points to | +/// |------------|------------------------------------------| +/// | 1 | bool (may be incompatible with uint8_t!) | +/// | [2, 8] | uint8_t, int8_t, or char | +/// | [9, 16] | uint16_t, int16_t | +/// | [17, 32] | uint32_t, int32_t, or 32-bit float | +/// | [33, 64] | uint64_t, int64_t, or 64-bit float | +/// +/// @param destination Destination buffer where the result will be stored. +/// @param bit_offset Offset, in bits, from the beginning of the destination buffer. +/// @param bit_length Length of the value, in bits; see the table. +/// @param value Pointer to the value; see the table. +void canardSerializePrimitive(void* const destination, + const size_t bit_offset, + const uint8_t bit_length, + const void* const value); + +/// This function may be invoked by the application to release the memory allocated for the received transfer payload. +/// +/// If the application needs to send new transfers from the transfer reception callback, this function should be +/// invoked right before calling @ref canardPublishMessage() or its service counterpart. +/// Not doing that may lead to a higher worst-case dynamic memory utilization. +/// +/// Failure to call this function will NOT lead to a memory leak because the library checks for it. +void canardReleaseReceivedTransferPayload(CanardInstance* const ins, CanardReceivedTransfer* const transfer); + +/// Use this function to determine the worst case memory footprint of your application. +/// See @ref CanardPoolAllocatorStatistics. +/// @returns a copy of the pool allocator usage statistics. +CanardMemoryAllocatorStatistics canardGetMemoryAllocatorStatistics(const CanardInstance* const ins); + +/// IEEE 754 binary16 marshaling helpers. +/// These functions convert between the native float and the standard IEEE 754 binary16 float (a.k.a. half precision). +/// It is assumed that the native float is IEEE 754 binary32, otherwise, the results may be unpredictable. +/// Majority of modern computers and microcontrollers use IEEE 754, so this limitation should not limit the portability. uint16_t canardConvertNativeFloatToFloat16(float value); float canardConvertFloat16ToNativeFloat(uint16_t value); -// Static checks. -CANARD_STATIC_ASSERT((((uint32_t) CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE) < 32) && - (((uint32_t) CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE) >= 6), - "Platforms where sizeof(void*) > 4 are not supported. " - "On AMD64 use 32-bit mode (e.g. GCC flag -m32)."); -CANARD_STATIC_ASSERT(sizeof(float) == 4, "Native float format shall match IEEE 754 binary32"); +#define CANARD_GLUE(a, b) CANARD_GLUE_IMPL_(a, b) +#define CANARD_GLUE_IMPL_(a, b) a##b #ifdef __cplusplus } diff --git a/libcanard/canard_internals.h b/libcanard/canard_internals.h deleted file mode 100644 index b5becf00..00000000 --- a/libcanard/canard_internals.h +++ /dev/null @@ -1,124 +0,0 @@ -/* - * The MIT License (MIT) - * - * Copyright (c) 2016-2017 UAVCAN Team - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - * - * Contributors: https://github.com/UAVCAN/libcanard/contributors - */ - -/* - * This file holds function declarations that expose the library's internal definitions for unit testing. - * It is NOT part of the library's API and should not even be looked at by the user. - */ - -#ifndef CANARD_INTERNALS_H -#define CANARD_INTERNALS_H - -#include "canard.h" - -#ifdef __cplusplus -extern "C" { -#endif - -/// This macro is needed only for testing and development. Do not redefine this in production. -#ifndef CANARD_INTERNAL -# define CANARD_INTERNAL static -#endif - -CANARD_INTERNAL CanardRxState* traverseRxStates(CanardInstance* ins, uint32_t session_specifier); - -CANARD_INTERNAL CanardRxState* createRxState(CanardPoolAllocator* allocator, uint32_t session_specifier); - -CANARD_INTERNAL CanardRxState* prependRxState(CanardInstance* ins, uint32_t session_specifier); - -CANARD_INTERNAL CanardRxState* findRxState(CanardRxState* state, uint32_t session_specifier); - -CANARD_INTERNAL int16_t bufferBlockPushBytes(CanardPoolAllocator* allocator, - CanardRxState* state, - const uint8_t* data, - uint8_t data_len); - -CANARD_INTERNAL CanardBufferBlock* createBufferBlock(CanardPoolAllocator* allocator); - -CANARD_INTERNAL CanardTransferKind extractTransferKind(uint32_t id); - -CANARD_INTERNAL uint16_t extractDataType(uint32_t id); - -CANARD_INTERNAL void pushTxQueue(CanardInstance* ins, CanardTxQueueItem* item); - -CANARD_INTERNAL bool isPriorityHigher(uint32_t id, uint32_t rhs); - -CANARD_INTERNAL CanardTxQueueItem* createTxItem(CanardPoolAllocator* allocator); - -CANARD_INTERNAL void prepareForNextTransfer(CanardRxState* state); - -CANARD_INTERNAL int16_t computeTransferIDForwardDistance(uint8_t a, uint8_t b); - -CANARD_INTERNAL void incrementTransferID(uint8_t* transfer_id); - -CANARD_INTERNAL uint64_t releaseStatePayload(CanardInstance* ins, CanardRxState* rxstate); - -/// Returns the number of frames enqueued. -CANARD_INTERNAL int16_t enqueueTxFrames(CanardInstance* ins, - uint32_t can_id, - const uint8_t* transfer_id, - uint16_t crc, - const uint8_t* payload, - uint16_t payload_len); - -CANARD_INTERNAL void copyBitArray(const uint8_t* src, - uint32_t src_offset, - uint32_t src_len, - uint8_t* dst, - uint32_t dst_offset); - -/** - * Moves the specified bits from a scattered transfer storage into a contiguous buffer. - * Returns the number of bits copied, or a negated error code. - */ -CANARD_INTERNAL int16_t descatterTransferPayload(const CanardRxTransfer* transfer, - uint32_t bit_offset, - uint8_t bit_length, - void* output); - -CANARD_INTERNAL bool isBigEndian(void); - -CANARD_INTERNAL void swapByteOrder(void* data, size_t size); - -CANARD_INTERNAL uint16_t crcAddByte(uint16_t crc_val, uint8_t byte); -CANARD_INTERNAL uint16_t crcAdd(uint16_t crc_val, const uint8_t* bytes, size_t len); - -/** - * Inits a memory allocator. - * - * @param allocator The memory allocator to initialize. - * @param buf The buffer used by the memory allocator. - * @param buf_len The number of blocks in buf. - */ -CANARD_INTERNAL void initPoolAllocator(CanardPoolAllocator* allocator, CanardPoolAllocatorBlock* buf, uint16_t buf_len); - -CANARD_INTERNAL void* allocateBlock(CanardPoolAllocator* allocator); -CANARD_INTERNAL void freeBlock(CanardPoolAllocator* allocator, void* p); - -#ifdef __cplusplus -} -#endif -#endif From 22400faf39d1ada5aa0c026e6f941d7d9d968ce5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 2 Feb 2020 02:16:06 +0200 Subject: [PATCH 002/100] WIP; tests are temporarily broken --- DESIGN.md | 535 ---------------------------- README.md | 53 +-- libcanard/canard.c | 45 ++- libcanard/canard.h | 366 ++++++------------- tests/test_crc.cpp | 39 -- tests/test_float16.cpp | 58 --- tests/test_framing.cpp | 612 -------------------------------- tests/test_init.cpp | 46 --- tests/test_memory_allocator.cpp | 104 ------ tests/test_rxerr.cpp | 381 -------------------- tests/test_scalar_encoding.cpp | 210 ----------- 11 files changed, 148 insertions(+), 2301 deletions(-) delete mode 100644 DESIGN.md delete mode 100644 tests/test_crc.cpp delete mode 100644 tests/test_float16.cpp delete mode 100644 tests/test_framing.cpp delete mode 100644 tests/test_init.cpp delete mode 100644 tests/test_memory_allocator.cpp delete mode 100644 tests/test_rxerr.cpp delete mode 100644 tests/test_scalar_encoding.cpp diff --git a/DESIGN.md b/DESIGN.md deleted file mode 100644 index a3daa54a..00000000 --- a/DESIGN.md +++ /dev/null @@ -1,535 +0,0 @@ -# libcanard design document - -## Design goals - -The following list contains main design goals in the order of importance. - -* *Small ROM footprint*. - A minimal node that periodically publishes `uavcan.protocol.NodeStatus` and responds to `uavcan.protocol.GetNodeInfo` should not require more than 4K of ROM. - For reference, a similar application based on libuavcan requires 19K of ROM (LPC11C24). -* *Small RAM footprint*. - A node like in the example above should not require more than 4K of RAM, including the stack and buffers. - For reference, a similar application based on libuavcan requires about 6K of RAM. -* *Determinism*. - Worst case execution time of all code paths should be predictable, which precludes use of heap. -* *Portability*. - The library should not present any specific requirements to the underlying hardware or OS, and it must be coded in standard C99. - However, the portability requirement can be superseded by other design goals, such as small memory footprint. -* *Simplicity*. - Unlike libuavcan, which somewhat resembles a framework rather than just a library, this project should not attempt to implement all of the high-level functionality of UAVCAN. - - -## Feature set - -According to the core design goals defined above, the functionality of the library should be restricted to the bare minimum. -The following features are considered to comprise the bare minimum: - -* Publication and reception of message transfers. -* Publication and reception of service request and service response transfers. -* Configurable Data Type ID of supported messages and services, at run time. -* Support for vendor-specific data types. - -The following features are intentionally not supported by the library: - -* Support for redundant physical interfaces. - Leaving out redundant interfaces allows to significantly simplify the implementation of the transport layer. -* RPC-like abstraction on top of service request/response exchanges. - This means that services will be supported simply as independent request and response transfers, unlike the way it is implemented in libuavcan, where a convenient high-level abstraction is provided. -* Time synchronization master. - This feature requires some special logic to be supported by the CAN driver and the library itself. - Applications that require this functionality are likely to be able to tolerate the memory requirements of libuavcan. -* Multithreaded nodes. - Again, this is implemented in libuavcan and is unlikely to be demanded by low-end applications. -* Support for platforms with 64-bit pointers. - Vast majority of deeply embedded systems use 32/16/8-bit CPU. - Systems that are based on AMD64 can still be supported by means of x86 compatibility mode (although it is recommended to use libuavcan instead). - -# Architecture - -## Memory management - -### Library state -Entire state of the library should be kept in one instance of a C structure. -Every API call of the library that depends on the state will be accepting the aforementioned instance as its first argument. - -### Dynamic memory pool -The library should implement a block memory allocator that will be used by the following subsystems (each is described below): - -* Incoming transfer buffers. -* Incoming transfer states. -* Prioritized TX queue. - -The number of blocks in the pool will be defined at compile time. -32 bytes is probably the optimal choice considering typical object sizes (see below). -For reference, libuavcan uses 64-byte blocks. - -Implementation of the block allocation algorithm can be borrowed from libuavcan. - - -### Transfer buffers -Transfer buffers should be implemented as a singly-linked lists of blocks, where every block is an instance of the following structure: - -```c -typedef struct CanardBufferBlock -{ - struct CanardBufferBlock* next; - uint8_t data[]; -} CanardBufferBlock; - -#define CANARD_BUFFER_BLOCK_DATA_SIZE (CANARD_MEM_BLOCK_SIZE - sizeof(CanardBufferBlock)) -``` - -Where `CANARD_MEM_BLOCK_SIZE` is the allocation size (32 bytes). - -According to the transport layer specification, the following operations must be defined for buffers: - -* Push bytes. - This operation adds a number of bytes (1 to 8 or 1 to 64, depending on the CAN standard used) to the end of the buffer. - It will be invoked during reception of multi-frame transfers. -* Pop bits. - This operation is used during deserialization of received payload. - It returns a number of bits (1 to 64, depending on the type of the field being deserialized) from the buffer to the caller. - -New blocks should be allocated ad hoc, however their removal should happen at once, after the data is processed upon completion of transfer reception. - -### RX transfer states - -The library will have to keep some state associated with every unique incoming transfer. -The concept of unique transfer is explained in the specification here. -Every unique incoming transfer can be identified with the following four values: - -* Data type ID -* Transfer type -* Source node ID -* Destination node ID (zero for broadcast transfers) - -Upon reception of a CAN frame, the library will have to check whether the frame should be accepted and how it should be processed. -Detailed description of the logic is available in the specification, here we just define a C structure that keeps all of the relevant states. -Such a structure will have to be instantiated and maintained for every unique incoming transfer that the library is interested in. -Since the number of unique incoming transfers cannot be determined statically, these structures will be allocated at run time using the memory pool. -Hence, size of the structure must not exceed the size of the allocatable block (32 bytes). - -```c -typedef struct CanardRxState -{ - struct CanardRxState* next; - CanardBufferBlock* buffer_blocks; - - uint64_t timestamp_usec; - - const uint32_t session_specifier; - - uint16_t payload_crc; - uint16_t payload_len : 10; - uint16_t transfer_id : 5; - uint16_t next_toggle : 1; - - uint8_t buffer_head[]; -} CanardRxState; - -#define CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE (CANARD_MEM_BLOCK_SIZE - sizeof(CanardRxState)) -``` - -Few things to note: - -* Size of the structure will exceed 32 bytes on platforms where size of the pointer is 64 bits. - This, however, is not a problem, because 64 bit platforms are not of interest (see the list of features). -* `timestamp_usec` keeps the timestamp at which the transfer that is currently being received was started, i.e. when the first frame of it was received. - This value is needed for detection and removal of stale RX state objects, and it is also passed to the application as a transfer reception timestamp. -* The last field of the structure is buffer_head, size of which is 32 - sizeof(CanardRxState). - It is intended for keeping first few bytes of incoming transfers. - -Value of the field `session_specifier` can be computed with the following helper macro: - -```c -#define CANARD_MAKE_SESSION_SPECIFIER(port_id, transfer_type, src_node_id, dst_node_id) \ - ((port_id) | ((transfer_type) << 16) | ((src_node_id) << 18) | ((dst_node_id) << 25)) -``` - -Keeping the transfer ID in a single scalar rather than in separate fields should be beneficial in terms of ROM footprint and linked list search speed. - -Using the concepts defined above, the frame reception procedure can be defined roughly as follows: - - -* Check if the application is interested in receiving a transfer with data type ID and transfer type of the newly received frame. - If not, exit. -* Check if there’s an RX state instance for this combination of (data type ID, transfer type, source node ID, destination node ID). - If not, instantiate one. If instantiation fails due to lack of memory, exit. -* Check if the frame matches the expectations of the RX state instance (e.g. toggle bit, transfer ID, payload size, etc - refer to the specification for details). - If not, exit. -* Update the RX state instance with the new frame, append the payload to the buffer if necessary. -* If the frame is the last one in the transfer, report to the application, then remove all buffered data and prepare the RX state instance for the next transfer. - Exit. - -### TX queue - -Frames that are scheduled for transmission should be stored in a prioritized TX queue. -When the CAN driver is ready to accept a frame, the application will take the highest priority frame from the TX queue and pass it to the driver. -The TX queue should be stored as a singly-linked list of CAN frames sorted in the descending order of priority (so that the highest priority frame is always at the root of the list). -There’s an implementation of a function that compares priorities of two CAN frames according to the [CAN specification](https://github.com/UAVCAN/libuavcan/blob/ec9006381b9ed0848eb12704a2beb9e74aea74bf/libuavcan/src/driver/uc_can.cpp). - -```c -#define CANARD_CAN_FRAME_MAX_DATA_LEN 8 - -/** - * CAN driver IO is based on this type. - */ -typedef struct -{ - uint32_t id; - uint8_t data[UC_CAN_FRAME_MAX_DATA_LEN]; - uint8_t data_len; -} CanardCANFrame; - -#define CANARD_CAN_FRAME_EFF (1U << 31) ///< Extended frame format -#define CANARD_CAN_FRAME_RTR (1U << 30) ///< Remote transmission request -#define CANARD_CAN_FRAME_ERR (1U << 29) ///< Error frame - -/** - * This type is internal to the library, it should not be exposed in the header file. - */ -typedef struct CanardTxQueueItem -{ - struct CanardTxQueueItem* next; - CanardCANFrame frame; -} CanardTxQueueItem; -``` - -### Threading model - -The library should be single-threaded, not thread-aware. -Hence the API will be not thread-safe, which is OK as most applications will likely be running all of the UAVCAN-related logic in one thread. - -The documentation should provide advices about how to integrate the library in a multithreaded environment. - -### API - -The following list provides a high-level description of the major use cases: - -* *Frame reception*. - On a reception of a frame, the application passes it to the library via a dedicated API function. - Upon reception of a frame, the library detects its transfer parameters, such as data type ID, transfer type and source node ID. - If this is the first frame of a transfer, the library then requests the application via a function pointer on whether the transfer should be received. - If the application reports that the transfer is not of interest, the frame will be ignored. - Otherwise, the library finds the receiver state instance for the transfer (or creates one if it couldn’t be found), then updates it according to the rules defined in the specification. - If the newly received frame was accepted and it was the last frame of the transfer, the library will invoke the appropriate callback (via a function pointer) so that the application can execute the associated business logic. - Upon return from the callback, all of the dynamic memory that was allocated for this transfer, if any, will be automatically released, although the RX state instance will be kept. -* *Transfer transmission*. - When the application needs to send a transfer, it invokes one of the corresponding functions depending on the type of the transfer (broadcast, request, or response). - The library breaks the transfer down into independent CAN frames and stores them into the internal TX queue, using the memory pool. - The application then unloads the frames from the TX queue into the CAN driver using two library calls: peek and pop. -* *Cleanup of stale transfers*. - The user should periodically invoke an API call that will traverse the list of receiver state instances and remove those that were last updated more than T seconds ago. - Note that the traversing is computationally inexpensive, as it requires only two operations: 1) switch to the next item; 2) check if the last update timestamp is lower than the current time minus T. - Recommended value of T is 3 seconds. -* *Data structure serialization and deserialization*. - Unlike in libuavcan, this functionality should be completely decoupled from the rest of the library. - Note that from the application side, transfer reception and transmission deals with raw binary chunks rather than with structured data. - This allows applications to operate on raw data, which is likely to be beneficial in terms of memory footprint and processing time. - Those applications that are interested in automatic serialization and deserialization should use independently autogenerated serialization and deserialization code, where serialization and deserialization of every data type will be implemented in a tiny header-only library of its own. - -The library should provide the following functions for the application: - -* *Send a transfer*. - Encoded transfer will be stored in the prioritized TX queue (as described above). - This function should accept serialized payload, i.e. not a message object. -* *Peek one frame from the TX queue*. - This function will be used by the application to move frames from the TX queue into the CAN driver. -* *Remove one frame from the TX queue*. - This function will be used by the application to remove the frame from the queue once the driver confirmed that it has been accepted for transmission. -* *Process one received frame*. - This function will be invoked by the application once the CAN driver reports about reception of a frame. - This function will also contain an argument for the RX timestamp of the frame. - -The application should provide the following functions for the library (the application will bind the library to these functions dynamically by means of function pointers): - -* *Process received transfer*. - The library will be passing information about the received transfer and the raw payload (non-deserialized) into the callback so the application can execute the associated business logic. -* *Determine if the transfer should be received*. - The library will be invoking this function on every reception of a CAN frame to check if it should proceed on reception of the transfer. - If the application does not need this transfer, the frame will be discarded. - -Note that the proposed API does not make any assumptions about the CAN driver interface or its implementation. -A rough draft of the API definitions is provided below: - -```c -#define CANARD_BROADCAST_NODE_ID 255 -#define CANARD_MIN_NODE_ID 0 -#define CANARD_MAX_NODE_ID 127 - -#define CANARD_MEM_BLOCK_SIZE 32 - -typedef enum -{ - CanardResponse, - CanardRequest -} CanardRequestResponse; - -typedef enum -{ - CanardTransferKindServiceResponse = 0, - CanardTransferKindServiceRequest = 1, - CanardTransferKindMessagePublication = 2 -} CanardTransferKind; - -/** - * This structure represents a received transfer for the application. - * An instance of it is passed to the application via callback when - * the library receives a new transfer. - */ -typedef struct -{ - /** - * Timestamp at which the first frame of this transfer was received. - */ - uint64_t timestamp_usec; - - /** - * Payload is scattered across three storages: - * - Head points to CanardRxState.buffer_head (length of which is up to Canard_PAYLOAD_HEAD_SIZE), or to the - * payload field (possibly with offset) of the last received CAN frame. - * - Middle is located in the linked list of dynamic blocks. - * - Tail points to the payload field (possibly with offset) of the last received CAN frame - * (only for multi-frame transfers). - * - * The tail offset depends on how much data of the last frame was accommodated in the last allocated - * block. - * - * For single-frame transfers, middle and tail will be NULL, and the head will point at first byte - * of the payload of the CAN frame. - * - * In simple cases it should be possible to get data directly from the head and/or tail pointers. - * Otherwise it is advised to use canardDecodePrimitive(). - */ - const uint8_t* payload_head; ///< Always valid, i.e. not NULL. - const CanardBufferBlock* payload_middle; ///< May be NULL if the buffer was not needed. Always NULL for single-frame transfers. - const uint8_t* payload_tail; ///< Last bytes of multi-frame transfers. Always NULL for single-frame transfers. - uint16_t payload_len; - - /** - * These fields identify the transfer for the application logic. - */ - uint16_t port_id; ///< 0 to 511 for services, 0 to 32767 for messages - uint8_t transfer_type; ///< See @ref CanardTransferKind - uint8_t transfer_id; ///< 0 to 31 - uint8_t priority; ///< 0 to 31 - uint8_t source_node_id; ///< 0 to 127, or 255 if the source is anonymous -} CanardRxTransfer; - -/** - * Definition is not provided in this draft. - * This structure should include at least the following: - * - Two callback function pointers - * - Local node ID - * - Memory pool and the associated state variables - * - List of RX state objects - * - An optional user-provided untyped pointer to user-specific data - */ -struct CanardInstance; - -/** - * Initializes the library state. - * Local node ID will be set to zero, i.e. the node will be anonymous. - */ -void canardInit(CanardInstance* out_ins, - void* mem_arena, - size_t mem_arena_size, - CanardOnTransferReception on_reception, - CanardShouldAcceptTransfer should_accept, - void* user_reference); - -/** - * Assigns a new node ID value to the current node. - */ -void canardSetLocalNodeID(CanardInstance* ins, uint8_t self_node_id); - -/** - * Returns node ID of the local node. - * Returns zero if the node ID has not been set. - */ -uint8_t canardGetLocalNodeID(const CanardInstance* ins); - -/** - * Sends a broadcast transfer. - * If the node is in passive mode, only single-frame transfers will be allowed. - */ -int canardPublishMessage(CanardInstance* ins, - uint16_t subject_id, - uint8_t* inout_transfer_id, - uint8_t priority, - const void* payload, - uint16_t payload_len); - -/** - * Sends a request or a response transfer. - * Fails if the node is in passive mode. - */ -int canardRequestOrRespond(CanardInstance* ins, - uint8_t destination_node_id, - uint16_t service_id, - uint8_t* inout_transfer_id, - uint8_t priority, - CanardRequestResponse kind, - const void* payload, - uint16_t payload_len); - -/** - * Returns a pointer to the top priority frame in the TX queue. - * Returns NULL if the TX queue is empty. - */ -const CanardCANFrame* canardPeekTxQueue(const CanardInstance* ins); - -/** - * Removes the top priority frame from the TX queue. - */ -void canardPopTxQueue(CanardInstance* ins); - -/** - * Processes a received CAN frame with a timestamp. - */ -void canardHandleRxFrame(CanardInstance* ins, - const CanardCANFrame* frame, - uint64_t timestamp_usec); - -/** - * The library calls this function when it receives first frame of a transfer to determine whether - * the transfer should be received. - */ -typedef bool (*CanardShouldAcceptTransfer)(const CanardInstance* ins, - uint16_t port_id, - CanardTransferKind transfer_type, - uint8_t source_node_id); - -/** - * This function will be invoked by the library every time a transfer is successfully received. - * If the application needs to send another transfer from this callback, it is recommended - * to call canardReleaseRxTransferPayload() first, so that the memory that was used for the block - * buffer can be released and re-used by the TX queue. - */ -typedef void (*CanardOnTransferReception)(CanardInstance* ins, - const CanardRxTransfer* transfer); - -/** - * This function can be used to extract values from received UAVCAN transfers. It decodes a scalar value - - * boolean, integer, character, or floating point - from the specified bit position in the RX transfer buffer. - * Simple single-frame transfers can also be parsed manually. - * - * Returns the number of bits successfully decoded, which may be less than requested if operation ran out of - * buffer boundaries, or negated error code, such as invalid argument. - * - * Caveat: This function works correctly only on platforms that use two's complement signed integer representation. - * I am not aware of any modern microarchitecture that uses anything else than two's complement, so it should - * not affect portability in any way. - * - * The type of value pointed to by 'out_value' is defined as follows: - * - * | bit_length | value_is_signed | out_value points to | - * |------------|-----------------|------------------------------------------| - * | 1 | false | bool (may be incompatible with uint8_t!) | - * | 1 | true | N/A | - * | [2, 8] | false | uint8_t, or char | - * | [2, 8] | true | int8_t, or char | - * | [9, 16] | false | uint16_t | - * | [9, 16] | true | int16_t | - * | [17, 32] | false | uint32_t | - * | [17, 32] | true | int32_t, or 32-bit float | - * | [33, 64] | false | uint64_t | - * | [33, 64] | true | int64_t, or 64-bit float | - */ -int canardDecodePrimitive(const CanardRxTransfer* transfer, ///< The RX transfer where the data will be copied from - uint32_t bit_offset, ///< Offset, in bits, from the beginning of the transfer - uint8_t bit_length, ///< Length of the value, in bits; see the table - bool value_is_signed, ///< True if the value can be negative; see the table - void* out_value); ///< Pointer to the output storage; see the table - -/** - * This function can be used to encode values for later transmission in a UAVCAN transfer. It encodes a scalar value - - * boolean, integer, character, or floating point - and puts it to the specified bit position in the specified - * contiguous buffer. - * Simple single-frame transfers can also be encoded manually. - * - * Caveat: This function works correctly only on platforms that use two's complement signed integer representation. - * I am not aware of any modern microarchitecture that uses anything else than two's complement, so it should - * not affect portability in any way. - * - * The type of value pointed to by 'value' is defined as follows: - * - * | bit_length | value points to | - * |------------|------------------------------------------| - * | 1 | bool (may be incompatible with uint8_t!) | - * | [2, 8] | uint8_t, int8_t, or char | - * | [9, 16] | uint16_t, int16_t | - * | [17, 32] | uint32_t, int32_t, or 32-bit float | - * | [33, 64] | uint64_t, int64_t, or 64-bit float | - */ -void canardEncodePrimitive(void* destination, ///< Destination buffer where the result will be stored - uint32_t bit_offset, ///< Offset, in bits, from the beginning of the destination buffer - uint8_t bit_length, ///< Length of the value, in bits; see the table - const void* value); ///< Pointer to the value; see the table - -/** - * This function can be invoked by the application to release pool blocks that are used to store - * the payload of this transfer. - */ -void canardReleaseRxTransferPayload(CanardInstance* ins, - CanardRxTransfer* transfer); -``` - -### Code generation - -As noted above, this shall remain an optional part of the library, as some applications may opt to implement serialization and deserialization manually. -The DSDL compiler should generate a tiny standalone single-header library for every data type the user needs to use in deserialized form. -The generated library should include the following entities: - -* A C structure that contains the fields of the target type (see below). -* A deserialization function that accepts a pointer to an CanardRxTransfer object and a pointer to the C structure above. -* A serialization function that accepts a pointer to a raw storage space and a pointer to the C structure above. -* References to other types this type depends on. - -The primitive DSDL types will be mapped to C types as follows: - -| DSDL type | C type | -|-----------|--------| -| `intX` | `int8_t, int16_t, int32_t, int64_t` | -| `uintX` | `uint8_t, uint16_t, uint32_t, uint64_t` | -| `bool` | `bool (from stdbool.h)` | -| `float16` | `float` | -| `float32` | `float` | -| `float64` | `double` | - -A dynamic DSDL array that contains values of type T and has maximum length N will be converted into two fields: one of them will be of type T mapped to the corresponding native C type as defined above; the second field will be of integer type uint16_t. -The first field will be given the same name as the original array, the second field’s name will be appended with “_len”. -An example is provided below. - - -| DSDL definition | C definition | -|-----------------|--------------| -| `Foo[<5] bars` | `Foo bars[4]; uint16_t bars_len;` - -# Implementation notes - -## Packaging - -The suggested approach is to keep all of the library’s functions in just one C file, and expose the entire API via just one header file. - -## Coding style - -Please refer to [the Zubax coding conventions](https://kb.zubax.com/x/84Ah). - -## Testing - -The library should be equipped with a testing suite (like libuavcan). -The testing suite should be based on the Google Test library (in which case it can be written in C++), or it can be just a dedicated application with a custom testing environment (in which case it is recommended to stick to C99). - -The testing suite does not have to be portable - it is quite acceptable to make it require an x86 or AMD64 machine running OS X or Linux. -Since 64-bit systems are not supported by the proposed design, on AMD64 systems the library should be compiled in 32-bit mode (on GCC use flag -m32). - -A continuous integration environment like Travis CI should be set up early in the project to run the test suite on each commit / pull request. - -## Name -It has been agreed to name the library “libcanard”. - -## License -The library must be released under the MIT open source license. -A short summary can be found [on tl;dr legal](http://www.tldrlegal.com/l/mit). -The license statement must declare that the code was developed by the UAVCAN project team. diff --git a/README.md b/README.md index 55f4c653..eccbf94f 100644 --- a/README.md +++ b/README.md @@ -13,51 +13,17 @@ Get help on the **[UAVCAN Forum](https://forum.uavcan.org)**. ## Usage -If you're not using Git, you can just copy the entire library into your project tree. -If you're using Git, it is recommended to add Libcanard to your project using submoduling, subtreeing, vendoring, -or whatever alternative suits your workflow best. - -The entire library is contained in three files under `libcanard/`: - -- `canard.c` - the only translation unit; add it to your build or compile it into a separate static library; -- `canard.h` - the API header; include it in your application; -- `canard_internals.h` - internal definitions of the library; -keep this file in the same directory with `canard.c`. - -Add `canard.c` to your application build, add `libcanard` directory to the include paths, -and you're ready to roll. - -Also you may want to use one of the available drivers for various CAN backends -that are distributed with Libcanard - check out the `drivers/` directory to find out more. - -If you wish to override some of the default options, e.g., assert macros' definition, -define the macro `CANARD_ENABLE_CUSTOM_BUILD_CONFIG` as a non-zero value -(e.g. for GCC or Clang: `-DCANARD_ENABLE_CUSTOM_BUILD_CONFIG=1`) -and provide your implementation in a file named `canard_build_config.h`. - -Example for Make: - -```make -# Adding the library. -INCLUDE += libcanard/libcanard/ -CSRC += libcanard/libcanard/canard.c - -# Adding drivers, unless you want to use your own. -# In this example we're using Linux SocketCAN drivers. -INCLUDE += libcanard_socketcan/ -CSRC += libcanard_socketcan/socketcan.c -``` +To integrate the library into your project, just copy the two files `canard.c` & `canard.h` +(find them under `libcanard/`) into your project tree. +Either keep them in the same directory, or make sure that the directory that contains the header +is added to the set of include look-up paths. +No special compiler options are needed to compile the source file (if you find this to be untrue, please open a ticket). There is no dedicated documentation for the library API, because it is simple enough to be self-documenting. Please check out the explanations provided in the comments in the header file to learn the basics. Most importantly, check out the demo application under `tests/demo.c`. Also use [code search to find real life usage examples](https://github.com/search?q=libcanard&type=Code&utf8=%E2%9C%93). -At the moment the library does not provide means to automate (de)serialization of UAVCAN data structures, -like other implementations (e.g. libuavcan for C++ or pyuavcan for Python) do. -Therefore, data structures need to be parsed and assembled manually. -The necessary examples are provided in the demo application. - ### Platform drivers The existing platform drivers should be used as a reference for implementation of one's own custom drivers. @@ -85,18 +51,11 @@ the library and the platform. This section is intended only for library developers and contributors. -The library design document can be found in [DESIGN.md](DESIGN.md) - Contributors, please follow the [Zubax C++ Coding Conventions](https://kb.zubax.com/x/84Ah). ### Testing -```bash -mkdir build && cd build -cmake ../libcanard/tests # Adjust path if necessary -make -./run_tests -``` +Please refer to the CI/CD automation scripts for instructions. ### Coverity Scan diff --git a/libcanard/canard.c b/libcanard/canard.c index b51d8699..26a3b60c 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -26,6 +26,13 @@ #include #include +// ---------------------------------------- BUILD CONFIGURATION ---------------------------------------- + +/// By default, this macro resolves to the standard assert(). The user can redefine this if necessary. +#ifndef CANARD_ASSERT +# define CANARD_ASSERT(x) assert(x) +#endif + /// This macro is needed only for testing and for library development. Do not redefine this in production. #ifndef CANARD_INTERNAL # define CANARD_INTERNAL static @@ -39,23 +46,39 @@ #define CAN_EXT_ID_MASK ((1UL << 29U) - 1U) -// ---------------------------------------- INTERNAL TYPES ---------------------------------------- +// ---------------------------------------- INTERNAL DEFINITIONS ---------------------------------------- + +#if !defined(__STDC_VERSION__) || (__STDC_VERSION__ < 199901L) +# error "Unsupported language: ISO C99 or a newer version is required." +#endif + +#if __STDC_VERSION__ < 201112L +// Intentional violation of MISRA: static assertion macro cannot be replaced with a function definition. +# define static_assert(x, ...) typedef char _static_assert_gl(_static_assertion_, __LINE__)[(x) ? 1 : -1] // NOSONAR +# define _static_assert_gl(a, b) _static_assert_gl_impl(a, b) // NOSONAR +// Intentional violation of MISRA: the paste operator ## cannot be avoided in this context. +# define _static_assert_gl_impl(a, b) a##b // NOSONAR +#endif -struct CanardInternalTxQueueItem +/// The fields are ordered to minimize padding on all platforms. +typedef struct CanardInternalTxQueueItem { - CanardInternalTxQueueItem* next; - uint64_t deadline_usec; - CanardCANFrame frame; -}; + struct CanardInternalTxQueueItem* next; + + uint32_t id; + uint64_t deadline_usec; + uint8_t data_length; + uint8_t data[]; +} CanardInternalTxQueueItem; /// The fields are ordered to minimize padding on all platforms. -struct CanardInternalInputSession +typedef struct CanardInternalInputSession { struct CanardInternalInputSession* next; - uint8_t* payload; - size_t payload_length; ///< How many bytes received so far. size_t payload_capacity; ///< Payload past this limit may be discarded by the library. + size_t payload_length; ///< How many bytes received so far. + uint8_t* payload; uint64_t timestamp_usec; ///< Time of last update of this session. Used for removal on timeout. uint32_t transfer_id_timeout_usec; ///< When (current time - update timestamp) exceeds this, it's dead. @@ -64,7 +87,7 @@ struct CanardInternalInputSession uint16_t calculated_crc; ///< Updated with the received payload in real time. uint8_t transfer_id; bool next_toggle; -}; +} CanardInternalInputSession; // ---------------------------------------- PRIVATE FUNCTIONS ---------------------------------------- @@ -87,4 +110,4 @@ CANARD_INTERNAL inline uint32_t makeServiceSessionSpecifier(const uint16_t servi (request_not_response ? (1U << 24U) : 0U); } -CANARD_STATIC_ASSERT(sizeof(float) == 4, "Native float format shall match IEEE 754 binary32"); +static_assert(sizeof(float) == 4, "Native float format shall match IEEE 754 binary32"); diff --git a/libcanard/canard.h b/libcanard/canard.h index 35c6d78e..abbe3060 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -31,39 +31,7 @@ extern "C" { #endif -// ---------------------------------------- BUILD CONFIGURATION ---------------------------------------- - -/// Build configuration header. Use it to provide your overrides. -/// Alternatively, you can specify each option individually in the preprocessor flags when invoking the compiler. -#if defined(CANARD_CONFIG_HEADER_AVAILABLE) && CANARD_CONFIG_HEADER_AVAILABLE -# include "canard_build_config.h" -#endif - -/// By default, the library is built to support all versions of CAN. -/// The downside of such flexibility is that it increases the memory footprint of the library. -/// If the available CAN hardware supports only Classic CAN, then it might make sense to limit the maximum transmission -/// unit (MTU) supported by the library to reduce the memory footprint. -#ifndef CANARD_CONFIG_MTU_MAX -# define CANARD_CONFIG_MTU_MAX CANARD_MTU_CAN_FD -#endif - -/// By default, this macro resolves to the standard assert(). The user can redefine this if necessary. -#ifndef CANARD_ASSERT -# define CANARD_ASSERT(x) assert(x) -#endif - -/// By default, this macro expands to the standard static_assert() if supported by the language (C11, C++11, or newer). -/// The user can redefine this if necessary. -#ifndef CANARD_STATIC_ASSERT -# if (defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 201112L)) || \ - (defined(__cplusplus) && (__cplusplus >= 201103L)) -# define CANARD_STATIC_ASSERT(...) static_assert(__VA_ARGS__) -# else -# define CANARD_STATIC_ASSERT(x, ...) typedef char CANARD_GLUE(_static_assertion_, __LINE__)[(x) ? 1 : -1] -# endif -#endif - -// ---------------------------------------- CONSTANTS ---------------------------------------- +// ---------------------------------------- CONSTANT DEFINITIONS ---------------------------------------- /// Semantic version numbers of this library (not the UAVCAN specification). /// API will be backward compatible within the same major version. @@ -79,20 +47,12 @@ extern "C" { #define CANARD_ERROR_INVALID_ARGUMENT 2 #define CANARD_ERROR_OUT_OF_MEMORY 3 #define CANARD_ERROR_NODE_ID_NOT_SET 4 -#define CANARD_ERROR_INTERNAL 9 /// MTU values for supported protocols. /// Per the recommendations given in the UAVCAN specification, other MTU values should not be used. #define CANARD_MTU_CAN_CLASSIC 8U #define CANARD_MTU_CAN_FD 64U -#if CANARD_CONFIG_MTU_MAX == CANARD_MTU_CAN_FD -#elif CANARD_CONFIG_MTU_MAX == CANARD_MTU_CAN_CLASSIC -#else -# error "CANARD_CONFIG_MTU_MAX is invalid: one of the standard MTU values shall be used (8, 64...)" -#endif -#define CANARD_MTU_RUNTIME_CONFIGURABLE (CANARD_CONFIG_MTU_MAX > CANARD_MTU_CAN_CLASSIC) - /// Parameter ranges are inclusive; the lower bound is zero for all. Refer to the specification for more info. #define CANARD_SUBJECT_ID_MAX 32767U #define CANARD_SERVICE_ID_MAX 511U @@ -107,16 +67,12 @@ extern "C" { /// If not specified, the transfer-ID timeout will take this value for all new input sessions. #define CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC 2000000UL -// ---------------------------------------- TYPES ---------------------------------------- +// ---------------------------------------- TYPE DEFINITIONS ---------------------------------------- // Forward declarations. typedef struct CanardInstance CanardInstance; typedef struct CanardReceivedTransfer CanardReceivedTransfer; -// These forward declarations are not a part of the library API, but they need to be here due to the limitations of C. -typedef struct CanardInternalInputSession CanardInternalInputSession; -typedef struct CanardInternalTxQueueItem CanardInternalTxQueueItem; - /// Transfer priority level mnemonics per the recommendations given in the UAVCAN specification. typedef enum { @@ -130,12 +86,20 @@ typedef enum CanardPriorityOptional = 7, } CanardPriority; -/// CAN data frame with an extended 29-bit ID. +/// CAN data frame with an extended 29-bit ID. RTR/Error frames are not used and therefore not modeled here. typedef struct { - uint32_t id; ///< 29-bit extended ID. The bits above 29-th are ignored. - uint8_t data_length; ///< The amount of useful data in the frame. Not to be confused with DLC! - uint8_t data[CANARD_CONFIG_MTU_MAX]; ///< The payload capacity depends on the compile-time MTU setting. + /// For RX frames: reception timestamp. + /// For TX frames: transmission deadline. + /// The time system may be arbitrary as long as the clock is monotonic (steady). + uint64_t time_usec; + + /// 29-bit extended ID. The bits above 29-th are ignored. + uint32_t id; + + /// The useful data in the frame. The length value is not to be confused with DLC! + uint8_t data_length; + uint8_t* data; } CanardCANFrame; /// Transfer kinds are defined by the UAVCAN specification. @@ -166,45 +130,19 @@ typedef CanardTransferReceptionParameters (*CanardShouldAcceptTransfer)(const Ca CanardTransferKind transfer_kind, uint8_t source_node_id); -/// This function will be invoked by the library every time a transfer is successfully received. -/// If the application needs to send another transfer from this callback, it is highly recommended -/// to call @ref canardReleaseReceivedTransferPayload() first, so that the memory that was used for the block -/// buffer can be released and re-used by the TX queue. -/// @param ins Library instance. -/// @param transfer Pointer to the temporary transfer object. Invalidated upon return. -typedef void (*CanardProcessReceivedTransfer)(CanardInstance* ins, CanardReceivedTransfer* transfer); - -/// This structure provides the usage statistics of the dynamic memory allocator. -/// It can be used to determine whether the allocated memory is sufficient for the application. -typedef struct -{ - // TODO -} CanardMemoryAllocatorStatistics; - -typedef struct -{ - // TODO - CanardMemoryAllocatorStatistics statistics; -} CanardInternalMemoryAllocator; - /// This is the core structure that keeps all of the states and allocated resources of the library instance. /// The application should never access any of the fields directly! Instead, the API functions should be used. struct CanardInstance { - const CanardShouldAcceptTransfer should_accept_transfer; ///< Transfer acceptance predicate. - const CanardProcessReceivedTransfer process_received_transfer; ///< Received transfer handler. - - CanardInternalMemoryAllocator allocator; ///< Deterministic constant-complexity dynamic memory allocator. - CanardInternalInputSession* input_sessions; ///< Rx session states. - CanardInternalTxQueueItem* tx_queue; ///< TX frames awaiting transmission. + CanardShouldAcceptTransfer should_accept_transfer; - void* const user_reference; ///< User pointer that can link this instance with other objects + struct CanardInternalInputSession* input_sessions; + struct CanardInternalTxQueueItem* tx_queue; - uint8_t node_id; ///< Local node-ID or @ref CANARD_NODE_ID_UNSET. + void* user_reference; ///< User pointer that can link this instance with other objects -#if CANARD_MTU_RUNTIME_CONFIGURABLE - uint8_t mtu_bytes; ///< Maximum number of data bytes per CAN frame. Range: [8, CANARD_CONFIG_MTU_MAX]. -#endif + uint8_t node_id; ///< Local node-ID or @ref CANARD_NODE_ID_UNSET. + uint8_t mtu_bytes; ///< Maximum number of data bytes per CAN frame. Range: [8, 64]. }; /// This structure represents a received transfer for the application. @@ -212,43 +150,28 @@ struct CanardInstance /// Pointers to the structure and all its fields are invalidated after the callback returns. struct CanardReceivedTransfer { - /// Timestamp at which the first frame of this transfer was received. - const uint64_t timestamp_usec; + /// Timestamp of the first frame of this transfer. + uint64_t timestamp_usec; - const uint8_t* const payload; - const size_t payload_length; + size_t payload_length; + uint8_t* payload; - const CanardPriority priority; - const CanardTransferKind transfer_kind; - const uint16_t port_id; ///< Subject-ID or service-ID. - const uint8_t source_node_id; ///< For anonymous transfers it's @ref CANARD_NODE_ID_UNSET. - const uint8_t transfer_id; ///< Bits above @ref CANARD_TRANSFER_ID_BIT_LENGTH are always zero. + CanardPriority priority; + CanardTransferKind transfer_kind; + uint16_t port_id; ///< Subject-ID or service-ID. + uint8_t source_node_id; ///< For anonymous transfers it's @ref CANARD_NODE_ID_UNSET. + uint8_t transfer_id; ///< Bits above @ref CANARD_TRANSFER_ID_BIT_LENGTH are always zero. }; +// ---------------------------------------- CONFIGURATION FUNCTIONS ---------------------------------------- + /// Initialize a library instance. /// The local node-ID will be initialized as @ref CANARD_NODE_ID_UNSET, i.e. anonymous by default. /// -/// The size of the memory arena should be an integer power of two; otherwise, the size may be rounded down. -/// Typically, the size of the memory arena should not be less than 8 KiB, although it depends on the application. -/// The recommended way to detect the required memory size is to measure the peak pool usage after a stress-test. -/// Refer to the function @ref canardGetPoolAllocatorStatistics(). -/// -/// @param memory_arena Raw memory chunk used for the deterministic dynamic memory allocator. -/// @param memory_arena_size Size of the above, in bytes. /// @param should_accept_transfer Callback, see @ref CanardShouldAcceptTransfer. -/// @param process_received_transfer Callback, see @ref CanardOnTransferReception. /// @param user_reference Optional application-defined pointer; NULL if not needed. -CanardInstance canardInit(void* const memory_arena, - const size_t memory_arena_size, - const CanardShouldAcceptTransfer should_accept_transfer, - const CanardProcessReceivedTransfer process_received_transfer, - void* const user_reference); - -/// Read the value of the user pointer. -/// The user pointer is configured once during initialization. -/// It can be used to store references to any user-specific data, or to link the instance object with C++ objects. -/// @returns The application-defined pointer. -void* canardGetUserReference(const CanardInstance* const ins); +CanardInstance canardInit(const CanardShouldAcceptTransfer should_accept_transfer, + void* const user_reference); /// Assign a new node-ID value to the current node. Node-ID can be assigned only once. /// If the supplied value is invalid or the node-ID is already configured, nothing will be done. @@ -257,7 +180,6 @@ void canardSetLocalNodeID(CanardInstance* const ins, const uint8_t self_node_id) /// @returns Node-ID of the local node; 255 (broadcast) if the node-ID is not set, i.e. if the local node is anonymous. uint8_t canardGetLocalNodeID(const CanardInstance* const ins); -#if CANARD_MTU_RUNTIME_CONFIGURABLE /// Configure the maximum transmission unit. This can be done as many times as needed. /// This setting defines the maximum number of bytes per CAN data frame in all outgoing transfers. /// Regardless of this setting, CAN frames with any MTU up to CANARD_CONFIG_MTU_MAX bytes can always be accepted. @@ -265,143 +187,55 @@ uint8_t canardGetLocalNodeID(const CanardInstance* const ins); /// Only the standard values should be used as recommended by the specification (8, 64 bytes); /// otherwise, interoperability issues may arise. See "CANARD_MTU_*". /// -/// Range: [8, CANARD_CONFIG_MTU_MAX]. The default is CANARD_CONFIG_MTU_MAX (i.e., the maximum). +/// Range: [8, 64]. The default is the maximum. /// Invalid values are rounded to the nearest valid value. void canardSetMTU(const CanardInstance* const ins, const uint8_t mtu_bytes); -#endif -/// Send a broadcast message transfer. If the local node is anonymous, only single frame transfers are be allowed. -/// -/// The pointer to the transfer-ID shall point to a persistent variable (e.g. static or heap-allocated, not on the -/// stack); it will be updated by the library after every transmission. The transfer-ID value cannot be shared -/// between different sessions! Read the transport layer specification for details. -/// -/// @param ins Library instance. -/// @param subject_id Refer to the specification. -/// @param inout_transfer_id Pointer to a persistent variable containing the transfer-ID. -/// @param priority Refer to CANARD_TRANSFER_PRIORITY_*. -/// @param payload Transfer payload -- the serialized DSDL object. -/// @param payload_len Length of the above, in bytes. -/// @returns The number of frames enqueued, or a negative error code. -int32_t canardPublishMessage(CanardInstance* const ins, - const uint16_t subject_id, - uint8_t* const inout_transfer_id, - const uint8_t priority, - const void* const payload, - const size_t payload_length, - uint64_t deadline_usec); - -/// Send a request transfer. Fails if the local node is anonymous. -/// -/// The pointer to the transfer-ID shall point to a persistent variable (e.g., static- or heap-allocated, -/// not on the stack); it will be updated by the library after every request. The transfer-ID value -/// cannot be shared between different sessions! Read the transport layer specification for details. -/// -/// @param ins Library instance. -/// @param destination_node_id Node-ID of the destination server. -/// @param service_id Refer to the specification. -/// @param inout_transfer_id Pointer to a persistent variable containing the transfer-ID. -/// @param priority @ref CanardPriority. -/// @param payload Transfer payload -- the serialized DSDL object. -/// @param payload_length Length of the above, in bytes. -/// @returns The number of frames enqueued, or a negative error code. -int32_t canardSendRequest(CanardInstance* const ins, - const uint8_t server_node_id, - const uint16_t service_id, - uint8_t* const inout_transfer_id, - const CanardPriority priority, - const void* const payload, - const size_t payload_length, - uint64_t deadline_usec); - -/// Send a response transfer. Fails if the local node is anonymous. -/// -/// The transfer-ID shall be the same as in the corresponding service request. -/// -/// @param ins Library instance. -/// @param destination_node_id Node-ID of the destination client. -/// @param service_id Refer to the specification. -/// @param transfer_id Same as in the original request transfer. -/// @param priority @ref CanardPriority. -/// @param payload Transfer payload -- the serialized DSDL object. -/// @param payload_length Length of the above, in bytes. -/// @returns The number of frames enqueued, or a negative error code. -int32_t canardSendResponse(CanardInstance* const ins, - const uint8_t client_node_id, - const uint16_t service_id, - const uint8_t transfer_id, - const CanardPriority priority, - const void* const payload, - const size_t payload_length, - uint64_t deadline_usec); - -/// The application will call this function after @ref canardPublishMessage() or its service counterpart to transmit the -/// generated frames over the CAN bus. -/// -/// @returns A pointer to the top-priority frame in the TX queue; or NULL if the TX queue is empty. -const CanardCANFrame* canardPeekTxQueue(const CanardInstance* const ins, uint64_t* const out_deadline_usec); +/// Read the value of the user pointer. +/// The user pointer is configured once during initialization. +/// It can be used to store references to any user-specific data, or to link the instance object with C++ objects. +/// @returns The application-defined pointer. +void* canardGetUserReference(const CanardInstance* const ins); -/// Remove the top priority frame from the TX queue. -/// The application will call this function after @ref canardPeekTxQueue() once the obtained frame has been processed. -/// Calling @ref canardPublishMessage() or its service counterpart between @ref canardPeekTxQueue() and this function -/// is NOT allowed, because it may change the frame at the top of the TX queue. -void canardPopTxQueue(CanardInstance* const ins); +// ---------------------------------------- OUTGOING TRANSFER FUNCTIONS ---------------------------------------- -typedef enum -{ - CanardReceivedFrameProcessingResultOK = CANARD_OK, - CanardReceivedFrameProcessingResultWrongFormat = 1, - CanardReceivedFrameProcessingResultWrongDestination = 2, - CanardReceivedFrameProcessingResultWrongToggle = 3, - CanardReceivedFrameProcessingResultWrongTransferID = 4, - CanardReceivedFrameProcessingResultMissedStartOfTransfer = 6, - CanardReceivedFrameProcessingResultCRCMismatch = 7 -} CanardReceivedFrameProcessingResult; - -/// Process a received CAN frame with a timestamp. -/// The application will call this function upon reception of a new frame from the CAN bus. -/// -/// @param ins Library instance. -/// @param frame The received CAN frame. -/// @param timestamp_usec The timestamp. The time system may be arbitrary as long as the clock is monotonic (steady). -/// @returns Zero if accepted; negative error code; or a value from @ref CanardReceivedFrameProcessingResult. -int8_t canardProcessReceivedFrame(CanardInstance* const ins, - const CanardCANFrame* const frame, - const uint64_t timestamp_usec); +int32_t canardPublish(CanardInstance* const ins, + const uint16_t subject_id, + uint8_t* const inout_transfer_id, + const uint8_t priority, + const size_t payload_length, + const void* const payload, + const uint64_t deadline_usec); -/// This function may be used to extract values from received UAVCAN transfers. It decodes a scalar value -- -/// boolean, integer, character, or floating point -- from the specified bit position in the RX transfer buffer. -/// Simple single-frame transfers can also be parsed manually instead of using this function. -/// -/// Caveat: This function works correctly only on platforms that use two's complement signed integer representation. -/// I am not aware of any modern microarchitecture that uses anything else than two's complement, so it should not -/// limit the portability. -/// -/// The type of the value pointed to by 'out_value' is defined as follows: -/// -/// | bit_length | value_is_signed | out_value points to | -/// |------------|-----------------|------------------------------------------| -/// | 1 | false | bool (may be incompatible with uint8_t!) | -/// | 1 | true | N/A | -/// | [2, 8] | false | uint8_t, or char | -/// | [2, 8] | true | int8_t, or char | -/// | [9, 16] | false | uint16_t | -/// | [9, 16] | true | int16_t | -/// | [17, 32] | false | uint32_t | -/// | [17, 32] | true | int32_t, or 32-bit float IEEE 754 | -/// | [33, 64] | false | uint64_t | -/// | [33, 64] | true | int64_t, or 64-bit float IEEE 754 | -/// -/// @param transfer The RX transfer where the data will be read from. -/// @param bit_offset Offset, in bits, from the beginning of the transfer payload. -/// @param bit_length Length of the value, in bits; see the table. -/// @param value_is_signed True if the value can be negative (i.e., sign bit extension is needed); see the table. -/// @param out_value Pointer to the output storage; see the table. -void canardDeserializePrimitive(const CanardReceivedTransfer* const transfer, - const size_t bit_offset, - const uint8_t bit_length, - const bool value_is_signed, - void* const out_value); +int32_t canardRequest(CanardInstance* const ins, + const uint8_t server_node_id, + const uint16_t service_id, + uint8_t* const inout_transfer_id, + const CanardPriority priority, + const size_t payload_length, + const void* const payload, + const uint64_t deadline_usec); + +int32_t canardRespond(CanardInstance* const ins, + const uint8_t client_node_id, + const uint16_t service_id, + const uint8_t transfer_id, + const CanardPriority priority, + const size_t payload_length, + const void* const payload, + const uint64_t deadline_usec); + +/// TODO: REVIEW NEEDED +CanardCANFrame canardPeekTxQueue(const CanardInstance* const ins); +void canardPopTxQueue(const CanardInstance* const ins); + +// ---------------------------------------- INCOMING TRANSFER FUNCTIONS ---------------------------------------- + +CanardReceivedTransfer canardReceive(CanardInstance* const ins, const CanardCANFrame frame); + +void canardDestroyTransfer(CanardInstance* const ins, CanardReceivedTransfer* const transfer); + +// ---------------------------------------- DATA SERIALIZATION FUNCTIONS ---------------------------------------- /// This function may be used to encode values for later transmission in a UAVCAN transfer. It encodes a scalar value /// -- boolean, integer, character, or floating point -- and puts it at the specified bit offset in the specified @@ -430,29 +264,45 @@ void canardSerializePrimitive(void* const destination, const uint8_t bit_length, const void* const value); -/// This function may be invoked by the application to release the memory allocated for the received transfer payload. +/// This function may be used to extract values from received UAVCAN transfers. It decodes a scalar value -- +/// boolean, integer, character, or floating point -- from the specified bit position in the source buffer. /// -/// If the application needs to send new transfers from the transfer reception callback, this function should be -/// invoked right before calling @ref canardPublishMessage() or its service counterpart. -/// Not doing that may lead to a higher worst-case dynamic memory utilization. +/// Caveat: This function works correctly only on platforms that use two's complement signed integer representation. +/// I am not aware of any modern microarchitecture that uses anything else than two's complement, so it should not +/// limit the portability. /// -/// Failure to call this function will NOT lead to a memory leak because the library checks for it. -void canardReleaseReceivedTransferPayload(CanardInstance* const ins, CanardReceivedTransfer* const transfer); - -/// Use this function to determine the worst case memory footprint of your application. -/// See @ref CanardPoolAllocatorStatistics. -/// @returns a copy of the pool allocator usage statistics. -CanardMemoryAllocatorStatistics canardGetMemoryAllocatorStatistics(const CanardInstance* const ins); +/// The type of the value pointed to by 'out_value' is defined as follows: +/// +/// | bit_length | value_is_signed | out_value points to | +/// |------------|-----------------|------------------------------------------| +/// | 1 | false | bool (may be incompatible with uint8_t!) | +/// | 1 | true | N/A | +/// | [2, 8] | false | uint8_t, or char | +/// | [2, 8] | true | int8_t, or char | +/// | [9, 16] | false | uint16_t | +/// | [9, 16] | true | int16_t | +/// | [17, 32] | false | uint32_t | +/// | [17, 32] | true | int32_t, or 32-bit float IEEE 754 | +/// | [33, 64] | false | uint64_t | +/// | [33, 64] | true | int64_t, or 64-bit float IEEE 754 | +/// +/// @param source The source buffer where the data will be read from. +/// @param bit_offset Offset, in bits, from the beginning of the source buffer. +/// @param bit_length Length of the value, in bits; see the table. +/// @param value_is_signed True if the value can be negative (i.e., sign bit extension is needed); see the table. +/// @param out_value Pointer to the output storage; see the table. +void canardDeserializePrimitive(const void* const source, + const size_t bit_offset, + const uint8_t bit_length, + const bool value_is_signed, + void* const out_value); /// IEEE 754 binary16 marshaling helpers. /// These functions convert between the native float and the standard IEEE 754 binary16 float (a.k.a. half precision). /// It is assumed that the native float is IEEE 754 binary32, otherwise, the results may be unpredictable. /// Majority of modern computers and microcontrollers use IEEE 754, so this limitation should not limit the portability. -uint16_t canardConvertNativeFloatToFloat16(float value); -float canardConvertFloat16ToNativeFloat(uint16_t value); - -#define CANARD_GLUE(a, b) CANARD_GLUE_IMPL_(a, b) -#define CANARD_GLUE_IMPL_(a, b) a##b +uint16_t canardComposeFloat16(float value); +float canardParseFloat16(uint16_t value); #ifdef __cplusplus } diff --git a/tests/test_crc.cpp b/tests/test_crc.cpp deleted file mode 100644 index 018bdd76..00000000 --- a/tests/test_crc.cpp +++ /dev/null @@ -1,39 +0,0 @@ -/* - * Copyright (c) 2016-2020 UAVCAN Development Team - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include "canard_internals.h" - -TEST_CASE("CRC, Correctness") -{ - uint16_t crc = 0xFFFFU; - - crc = crcAdd(crc, reinterpret_cast("1"), 1); - crc = crcAdd(crc, reinterpret_cast("2"), 1); - crc = crcAdd(crc, reinterpret_cast("3"), 1); - - REQUIRE(0x5BCE == crc); // Using Libuavcan as reference - - crc = crcAdd(crc, reinterpret_cast("456789"), 6); - - REQUIRE(0x29B1 == crc); -} diff --git a/tests/test_float16.cpp b/tests/test_float16.cpp deleted file mode 100644 index a27947c3..00000000 --- a/tests/test_float16.cpp +++ /dev/null @@ -1,58 +0,0 @@ -/* - * Copyright (c) 2016-2020 UAVCAN Development Team - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include -#include "canard.h" - -TEST_CASE("Float16, FromNative") -{ - // Reference values were generated manually with libuavcan and numpy.float16() - REQUIRE(0b0000000000000000 == canardConvertNativeFloatToFloat16(0)); - REQUIRE(0b0011110000000000 == canardConvertNativeFloatToFloat16(1)); - REQUIRE(0b1100000000000000 == canardConvertNativeFloatToFloat16(-2)); - REQUIRE(0b0111110000000000 == canardConvertNativeFloatToFloat16(999999)); // +inf - REQUIRE(0b1111101111111111 == canardConvertNativeFloatToFloat16(-65519)); // -max - REQUIRE(0b0111111111111111 == canardConvertNativeFloatToFloat16(std::nanf(""))); // nan -} - -TEST_CASE("Float16, ToNative") -{ - // Reference values were generated manually with libuavcan and numpy.float16() - REQUIRE(Approx(0.0F) == canardConvertFloat16ToNativeFloat(0b0000000000000000)); - REQUIRE(Approx(1.0F) == canardConvertFloat16ToNativeFloat(0b0011110000000000)); - REQUIRE(Approx(-2.0F) == canardConvertFloat16ToNativeFloat(0b1100000000000000)); - REQUIRE(Approx(-65504.0F) == canardConvertFloat16ToNativeFloat(0b1111101111111111)); - REQUIRE(std::isinf(canardConvertFloat16ToNativeFloat(0b0111110000000000))); - - REQUIRE(bool(std::isnan(canardConvertFloat16ToNativeFloat(0b0111111111111111)))); -} - -TEST_CASE("Float16, BackAndForth") -{ - float x = -1000.0F; - while (x <= 1000.0F) - { - REQUIRE(Approx(x) == canardConvertFloat16ToNativeFloat(canardConvertNativeFloatToFloat16(x))); - x += 0.5F; - } -} diff --git a/tests/test_framing.cpp b/tests/test_framing.cpp deleted file mode 100644 index 63e76d70..00000000 --- a/tests/test_framing.cpp +++ /dev/null @@ -1,612 +0,0 @@ -/* - * Copyright (c) 2016-2020 UAVCAN Development Team - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include "canard.h" -#include "canard_internals.h" - -static const uint8_t TEST_FRAMING_TOGGLE_BIT = 5; -static const uint8_t TEST_FRAMING_EOF_BIT = 6; -static const uint8_t TEST_FRAMING_SOF_BIT = 7; - -static const uint8_t TEST_FRAMING_TRANSFER_PRIORITY_BIT = 24; -static const uint8_t TEST_FRAMING_SUBJECT_ID_BIT = 8; -static const uint8_t TEST_FRAMING_NODE_ID_BIT = 1; - -static bool acceptAllTransfers(const CanardInstance*, uint16_t, CanardTransferKind, uint8_t) -{ - return true; -} - -static void onTransferReceptionMock(CanardInstance*, CanardRxTransfer*) {} - -TEST_CASE("Framing, SingleFrameBasicCan2") -{ - uint8_t node_id = 22; - - std::uint8_t memory_arena[1024]; - ::CanardInstance ins; - - uint16_t subject_id = 12; - uint8_t transfer_id = 2; - uint8_t transfer_priority = CANARD_TRANSFER_PRIORITY_NOMINAL; - uint8_t data[] = {1, 2, 3}; - - canardInit(&ins, - memory_arena, - sizeof(memory_arena), - onTransferReceptionMock, - acceptAllTransfers, - reinterpret_cast(12345)); - canardSetLocalNodeID(&ins, node_id); - - auto res = canardPublishMessage(&ins, subject_id, &transfer_id, transfer_priority, data, sizeof(data)); - REQUIRE(res >= 0); - - // compenaste for internally incrementing transfer_id in canardPublishMessage - transfer_id--; - - auto transfer_frame = canardPeekTxQueue(&ins); - REQUIRE(transfer_frame != nullptr); - canardPopTxQueue(&ins); - - // Only checks framing in this test - REQUIRE(transfer_frame->data_len == 4); - REQUIRE(transfer_frame->data[0] == 1); - REQUIRE(transfer_frame->data[1] == 2); - REQUIRE(transfer_frame->data[2] == 3); - REQUIRE(transfer_frame->data[3] == ((1U << TEST_FRAMING_SOF_BIT) | (1U << TEST_FRAMING_EOF_BIT) | - (1U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)); - - REQUIRE(canardPeekTxQueue(&ins) == nullptr); -} - -TEST_CASE("Deframing, SingleFrameBasicCan2") -{ - uint8_t node_id = 22; - - std::uint8_t memory_arena[1024]; - ::CanardInstance ins; - - uint16_t subject_id = 12; - uint8_t transfer_id = 2; - uint8_t transfer_priority = CANARD_TRANSFER_PRIORITY_NOMINAL; - - auto onTransferReception = [](CanardInstance*, CanardRxTransfer* transfer) { - // Only check (de)framing in this test - REQUIRE(transfer->payload_len == 3); - - uint8_t out_value = 0; - canardDecodePrimitive(transfer, 0, 8, false, &out_value); - REQUIRE(out_value == 1); - - canardDecodePrimitive(transfer, 8, 16, false, &out_value); - REQUIRE(out_value == 2); - - canardDecodePrimitive(transfer, 16, 24, false, &out_value); - REQUIRE(out_value == 3); - }; - - canardInit(&ins, - memory_arena, - sizeof(memory_arena), - onTransferReception, - acceptAllTransfers, - reinterpret_cast(12345)); - - CanardCANFrame frame = { - .id = (static_cast(CANARD_CAN_FRAME_EFF) | - static_cast(transfer_priority << TEST_FRAMING_TRANSFER_PRIORITY_BIT) | - static_cast(subject_id << TEST_FRAMING_SUBJECT_ID_BIT) | - static_cast(node_id << TEST_FRAMING_NODE_ID_BIT)), - .data = {1, - 2, - 3, - static_cast((1U << TEST_FRAMING_SOF_BIT) | (1U << TEST_FRAMING_EOF_BIT) | - (1U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)}, - .data_len = 4, - }; - - auto res = canardHandleRxFrame(&ins, &frame, 0); - REQUIRE(res >= 0); -} - -TEST_CASE("Framing, MultiFrameBasicCan2") -{ - uint8_t node_id = 22; - - std::uint8_t memory_arena[4096]; - ::CanardInstance ins; - - uint16_t subject_id = 12; - uint8_t transfer_id = 2; - uint8_t transfer_priority = CANARD_TRANSFER_PRIORITY_NOMINAL; - uint8_t data[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17}; - - canardInit(&ins, - memory_arena, - sizeof(memory_arena), - onTransferReceptionMock, - acceptAllTransfers, - reinterpret_cast(12345)); - canardSetLocalNodeID(&ins, node_id); - - auto res = canardPublishMessage(&ins, subject_id, &transfer_id, transfer_priority, data, sizeof(data)); - REQUIRE(res >= 0); - - // compenaste for internally incrementing transfer_id in canardPublishMessage - transfer_id--; - - // First frame (1) - auto transfer_frame = canardPeekTxQueue(&ins); - REQUIRE(transfer_frame != nullptr); - canardPopTxQueue(&ins); - - REQUIRE(transfer_frame->data_len == 8); - REQUIRE(transfer_frame->data[0] == 1); - REQUIRE(transfer_frame->data[1] == 2); - REQUIRE(transfer_frame->data[2] == 3); - REQUIRE(transfer_frame->data[3] == 4); - REQUIRE(transfer_frame->data[4] == 5); - REQUIRE(transfer_frame->data[5] == 6); - REQUIRE(transfer_frame->data[6] == 7); - REQUIRE(transfer_frame->data[7] == ((1U << TEST_FRAMING_SOF_BIT) | (0U << TEST_FRAMING_EOF_BIT) | - (1U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)); - - // Second frame (2) - transfer_frame = canardPeekTxQueue(&ins); - REQUIRE(transfer_frame != nullptr); - canardPopTxQueue(&ins); - - REQUIRE(transfer_frame->data_len == 8); - REQUIRE(transfer_frame->data[0] == 8); - REQUIRE(transfer_frame->data[1] == 9); - REQUIRE(transfer_frame->data[2] == 10); - REQUIRE(transfer_frame->data[3] == 11); - REQUIRE(transfer_frame->data[4] == 12); - REQUIRE(transfer_frame->data[5] == 13); - REQUIRE(transfer_frame->data[6] == 14); - REQUIRE(transfer_frame->data[7] == ((0U << TEST_FRAMING_SOF_BIT) | (0U << TEST_FRAMING_EOF_BIT) | - (0U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)); - - // Third and last frame (3) - transfer_frame = canardPeekTxQueue(&ins); - REQUIRE(transfer_frame != nullptr); - canardPopTxQueue(&ins); - - REQUIRE(transfer_frame->data_len == 6); - REQUIRE(transfer_frame->data[0] == 15); - REQUIRE(transfer_frame->data[1] == 16); - REQUIRE(transfer_frame->data[2] == 17); - // CRC correctness is to be checked in unrelated test - REQUIRE(transfer_frame->data[5] == ((0U << TEST_FRAMING_SOF_BIT) | (1U << TEST_FRAMING_EOF_BIT) | - (1U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)); - - // Make sure there are no frames after the last frame - REQUIRE(canardPeekTxQueue(&ins) == nullptr); -} - -TEST_CASE("Deframing, MultiFrameBasicCan2") -{ - uint8_t node_id = 22; - - std::uint8_t memory_arena[4096]; - ::CanardInstance ins; - - uint16_t subject_id = 12; - uint8_t transfer_id = 2; - uint8_t transfer_priority = CANARD_TRANSFER_PRIORITY_NOMINAL; - - static uint8_t data[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17}; - uint16_t crc = crcAdd(0xFFFFU, data, sizeof(data)); - - auto onTransferReception = [](CanardInstance*, CanardRxTransfer* transfer) { - // Only check (de)framing in this test - REQUIRE(transfer->payload_len == sizeof(data)); - - uint8_t out_value = 0; - - for (uint8_t i = 0; std::size_t(i) < sizeof(data); i++) - { - canardDecodePrimitive(transfer, i * 8U, 8, false, &out_value); - REQUIRE(out_value == data[i]); - } - }; - - canardInit(&ins, - memory_arena, - sizeof(memory_arena), - onTransferReception, - acceptAllTransfers, - reinterpret_cast(12345)); - - CanardCANFrame frame1 = { - .id = (static_cast(CANARD_CAN_FRAME_EFF) | - static_cast(transfer_priority << TEST_FRAMING_TRANSFER_PRIORITY_BIT) | - static_cast(subject_id << TEST_FRAMING_SUBJECT_ID_BIT) | - static_cast(node_id << TEST_FRAMING_NODE_ID_BIT)), - .data = {1, - 2, - 3, - 4, - 5, - 6, - 7, - static_cast((1U << TEST_FRAMING_SOF_BIT) | (0U << TEST_FRAMING_EOF_BIT) | - (1U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)}, - .data_len = 8, - }; - - auto res = canardHandleRxFrame(&ins, &frame1, 1); - REQUIRE(res >= 0); - - CanardCANFrame frame2 = { - .id = (static_cast(CANARD_CAN_FRAME_EFF) | - static_cast(transfer_priority << TEST_FRAMING_TRANSFER_PRIORITY_BIT) | - static_cast(subject_id << TEST_FRAMING_SUBJECT_ID_BIT) | - static_cast(node_id << TEST_FRAMING_NODE_ID_BIT)), - .data = {8, - 9, - 10, - 11, - 12, - 13, - 14, - static_cast((0U << TEST_FRAMING_SOF_BIT) | (0U << TEST_FRAMING_EOF_BIT) | - (0U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)}, - .data_len = 8, - }; - - res = canardHandleRxFrame(&ins, &frame2, 2); - REQUIRE(res >= 0); - - CanardCANFrame frame3 = { - .id = (static_cast(CANARD_CAN_FRAME_EFF) | - static_cast(transfer_priority << TEST_FRAMING_TRANSFER_PRIORITY_BIT) | - static_cast(subject_id << TEST_FRAMING_SUBJECT_ID_BIT) | - static_cast(node_id << TEST_FRAMING_NODE_ID_BIT)), - .data = {15, - 16, - 17, - static_cast(crc >> 8U), - static_cast(crc), - static_cast((0U << TEST_FRAMING_SOF_BIT) | (1U << TEST_FRAMING_EOF_BIT) | - (1U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)}, - .data_len = 6, - }; - - res = canardHandleRxFrame(&ins, &frame3, 3); - REQUIRE(res >= 0); -} - -/* This test is created to see that sending the CRC in a sepereate frame works fine. - * When all the data is sent with no free byte for CRC in the last frame, - * the CRC bytes must be sent in a separate frame. - */ -TEST_CASE("Framing, MultiFrameSeparateCRCCan2") -{ - uint8_t node_id = 22; - - std::uint8_t memory_arena[4096]; - ::CanardInstance ins; - - uint16_t subject_id = 12; - uint8_t transfer_id = 2; - uint8_t transfer_priority = CANARD_TRANSFER_PRIORITY_NOMINAL; - uint8_t data[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}; - - canardInit(&ins, - memory_arena, - sizeof(memory_arena), - onTransferReceptionMock, - acceptAllTransfers, - reinterpret_cast(12345)); - canardSetLocalNodeID(&ins, node_id); - - auto res = canardPublishMessage(&ins, subject_id, &transfer_id, transfer_priority, data, sizeof(data)); - REQUIRE(res >= 0); - - // compenaste for internally incrementing transfer_id in canardPublishMessage - transfer_id--; - - // First frame (1) - auto transfer_frame = canardPeekTxQueue(&ins); - REQUIRE(transfer_frame != nullptr); - canardPopTxQueue(&ins); - - REQUIRE(transfer_frame->data_len == 8); - // Data correctness is to be checked in unrelated test - REQUIRE(transfer_frame->data[7] == ((1U << TEST_FRAMING_SOF_BIT) | (0U << TEST_FRAMING_EOF_BIT) | - (1U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)); - - // Second frame (2) - Contains the ramining of the data - transfer_frame = canardPeekTxQueue(&ins); - REQUIRE(transfer_frame != nullptr); - canardPopTxQueue(&ins); - REQUIRE(transfer_frame->data_len == 8); - // Data correctness is to be checked in unrelated test - REQUIRE(transfer_frame->data[7] == ((0U << TEST_FRAMING_SOF_BIT) | (0U << TEST_FRAMING_EOF_BIT) | - (0U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)); - - // Third and last frame (3) - Only contains the last CRC byte and tail byte - transfer_frame = canardPeekTxQueue(&ins); - REQUIRE(transfer_frame != nullptr); - canardPopTxQueue(&ins); - - REQUIRE(transfer_frame->data_len == 3); - // CRC correctness is to be checked in unrelated test - REQUIRE(transfer_frame->data[2] == ((0U << TEST_FRAMING_SOF_BIT) | (1U << TEST_FRAMING_EOF_BIT) | - (1U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)); - - // Make sure there are no frames after the last frame - REQUIRE(canardPeekTxQueue(&ins) == nullptr); -} - -/* This test is created to see that sending the CRC in a sepereate frame works fine. - * When all the data is sent with no free byte for CRC in the last frame, - * the CRC bytes must be sent in a separate frame. - */ -TEST_CASE("Deframing, MultiFrameSeparateCRCCan2") -{ - uint8_t node_id = 22; - - std::uint8_t memory_arena[4096]; - ::CanardInstance ins; - - uint16_t subject_id = 12; - uint8_t transfer_id = 2; - uint8_t transfer_priority = CANARD_TRANSFER_PRIORITY_NOMINAL; - - static uint8_t data[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}; - uint16_t crc = crcAdd(0xFFFFU, data, sizeof(data)); - - auto onTransferReception = [](CanardInstance*, CanardRxTransfer* transfer) { - // Only check (de)framing in this test - REQUIRE(transfer->payload_len == sizeof(data)); - - uint8_t out_value = 0; - - for (uint8_t i = 0; std::size_t(i) < sizeof(data); i++) - { - canardDecodePrimitive(transfer, i * 8U, 8, false, &out_value); - REQUIRE(out_value == data[i]); - } - }; - - canardInit(&ins, - memory_arena, - sizeof(memory_arena), - onTransferReception, - acceptAllTransfers, - reinterpret_cast(12345)); - - CanardCANFrame frame1 = { - .id = (static_cast(CANARD_CAN_FRAME_EFF) | - static_cast(transfer_priority << TEST_FRAMING_TRANSFER_PRIORITY_BIT) | - static_cast(subject_id << TEST_FRAMING_SUBJECT_ID_BIT) | - static_cast(node_id << TEST_FRAMING_NODE_ID_BIT)), - .data = {1, - 2, - 3, - 4, - 5, - 6, - 7, - static_cast((1U << TEST_FRAMING_SOF_BIT) | (0U << TEST_FRAMING_EOF_BIT) | - (1U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)}, - .data_len = 8, - }; - - auto res = canardHandleRxFrame(&ins, &frame1, 1); - REQUIRE(res >= 0); - - CanardCANFrame frame2 = { - .id = (static_cast(CANARD_CAN_FRAME_EFF) | - static_cast(transfer_priority << TEST_FRAMING_TRANSFER_PRIORITY_BIT) | - static_cast(subject_id << TEST_FRAMING_SUBJECT_ID_BIT) | - static_cast(node_id << TEST_FRAMING_NODE_ID_BIT)), - .data = {8, - 9, - 10, - 11, - 12, - 13, - 14, - static_cast((0U << TEST_FRAMING_SOF_BIT) | (0U << TEST_FRAMING_EOF_BIT) | - (0U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)}, - .data_len = 8, - }; - - res = canardHandleRxFrame(&ins, &frame2, 2); - REQUIRE(res >= 0); - - CanardCANFrame frame3 = { - .id = (static_cast(CANARD_CAN_FRAME_EFF) | - static_cast(transfer_priority << TEST_FRAMING_TRANSFER_PRIORITY_BIT) | - static_cast(subject_id << TEST_FRAMING_SUBJECT_ID_BIT) | - static_cast(node_id << TEST_FRAMING_NODE_ID_BIT)), - .data = {static_cast(crc >> 8U), - static_cast(crc), - static_cast((0U << TEST_FRAMING_SOF_BIT) | (1U << TEST_FRAMING_EOF_BIT) | - (1U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)}, - .data_len = 3, - }; - - res = canardHandleRxFrame(&ins, &frame3, 3); - REQUIRE(res >= 0); -} - -/* This test is created to see that split CRC between frames works fine. - * When all the data is sent with only one free byte for CRC, - * the last CRC byte must be sent in a separate frame. - */ -TEST_CASE("Framing, MultiFrameSplitCRCCan2") -{ - uint8_t node_id = 22; - - std::uint8_t memory_arena[4096]; - ::CanardInstance ins; - - uint16_t subject_id = 12; - uint8_t transfer_id = 2; - uint8_t transfer_priority = CANARD_TRANSFER_PRIORITY_NOMINAL; - uint8_t data[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}; - - canardInit(&ins, - memory_arena, - sizeof(memory_arena), - onTransferReceptionMock, - acceptAllTransfers, - reinterpret_cast(12345)); - canardSetLocalNodeID(&ins, node_id); - - auto res = canardPublishMessage(&ins, subject_id, &transfer_id, transfer_priority, data, sizeof(data)); - REQUIRE(res >= 0); - - // compenaste for internally incrementing transfer_id in canardPublishMessage - transfer_id--; - - // First frame (1) - auto transfer_frame = canardPeekTxQueue(&ins); - REQUIRE(transfer_frame != nullptr); - canardPopTxQueue(&ins); - - REQUIRE(transfer_frame->data_len == 8); - // Data correctness is to be checked in unrelated test - REQUIRE(transfer_frame->data[7] == ((1U << TEST_FRAMING_SOF_BIT) | (0U << TEST_FRAMING_EOF_BIT) | - (1U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)); - - // Second frame (2) - Contains the first CRC byte - transfer_frame = canardPeekTxQueue(&ins); - REQUIRE(transfer_frame != nullptr); - canardPopTxQueue(&ins); - REQUIRE(transfer_frame->data_len == 8); - // Data correctness is to be checked in unrelated test - REQUIRE(transfer_frame->data[7] == ((0U << TEST_FRAMING_SOF_BIT) | (0U << TEST_FRAMING_EOF_BIT) | - (0U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)); - - // Third and last frame (3) - Only contains the last CRC byte and tail byte - transfer_frame = canardPeekTxQueue(&ins); - REQUIRE(transfer_frame != nullptr); - canardPopTxQueue(&ins); - - REQUIRE(transfer_frame->data_len == 2); - // CRC correctness is to be checked in unrelated test - REQUIRE(transfer_frame->data[1] == ((0U << TEST_FRAMING_SOF_BIT) | (1U << TEST_FRAMING_EOF_BIT) | - (1U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)); - - // Make sure there are no frames after the last frame - REQUIRE(canardPeekTxQueue(&ins) == nullptr); -} - -/* This test is created to see that split CRC between frames works fine. - * When all the data is sent with only one free byte for CRC, - * the last CRC byte must be sent in a separate frame. - */ -TEST_CASE("Deframing, MultiFrameSplitCRCCan2") -{ - uint8_t node_id = 22; - - std::uint8_t memory_arena[4096]; - ::CanardInstance ins; - - uint16_t subject_id = 12; - uint8_t transfer_id = 2; - uint8_t transfer_priority = CANARD_TRANSFER_PRIORITY_NOMINAL; - - static uint8_t data[] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}; - uint16_t crc = crcAdd(0xFFFFU, data, sizeof(data)); - - auto onTransferReception = [](CanardInstance*, CanardRxTransfer* transfer) { - // Only check (de)framing in this test - REQUIRE(transfer->payload_len == sizeof(data)); - - uint8_t out_value = 0; - - for (uint8_t i = 0; std::size_t(i) < sizeof(data); i++) - { - canardDecodePrimitive(transfer, i * 8U, 8, false, &out_value); - REQUIRE(out_value == data[i]); - } - }; - - canardInit(&ins, - memory_arena, - sizeof(memory_arena), - onTransferReception, - acceptAllTransfers, - reinterpret_cast(12345)); - - CanardCANFrame frame1 = { - .id = (static_cast(CANARD_CAN_FRAME_EFF) | - static_cast(transfer_priority << TEST_FRAMING_TRANSFER_PRIORITY_BIT) | - static_cast(subject_id << TEST_FRAMING_SUBJECT_ID_BIT) | - static_cast(node_id << TEST_FRAMING_NODE_ID_BIT)), - .data = {1, - 2, - 3, - 4, - 5, - 6, - 7, - static_cast((1U << TEST_FRAMING_SOF_BIT) | (0U << TEST_FRAMING_EOF_BIT) | - (1U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)}, - .data_len = 8, - }; - - auto res = canardHandleRxFrame(&ins, &frame1, 1); - REQUIRE(res >= 0); - - CanardCANFrame frame2 = { - .id = (static_cast(CANARD_CAN_FRAME_EFF) | - static_cast(transfer_priority << TEST_FRAMING_TRANSFER_PRIORITY_BIT) | - static_cast(subject_id << TEST_FRAMING_SUBJECT_ID_BIT) | - static_cast(node_id << TEST_FRAMING_NODE_ID_BIT)), - .data = {8, - 9, - 10, - 11, - 12, - 13, - static_cast(crc >> 8U), - static_cast((0U << TEST_FRAMING_SOF_BIT) | (0U << TEST_FRAMING_EOF_BIT) | - (0U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)}, - .data_len = 8, - }; - - res = canardHandleRxFrame(&ins, &frame2, 2); - REQUIRE(res >= 0); - - CanardCANFrame frame3 = { - .id = (static_cast(CANARD_CAN_FRAME_EFF) | - static_cast(transfer_priority << TEST_FRAMING_TRANSFER_PRIORITY_BIT) | - static_cast(subject_id << TEST_FRAMING_SUBJECT_ID_BIT) | - static_cast(node_id << TEST_FRAMING_NODE_ID_BIT)), - .data = {static_cast(crc), - static_cast((0U << TEST_FRAMING_SOF_BIT) | (1U << TEST_FRAMING_EOF_BIT) | - (1U << TEST_FRAMING_TOGGLE_BIT) | transfer_id)}, - .data_len = 2, - }; - - res = canardHandleRxFrame(&ins, &frame3, 3); - REQUIRE(res >= 0); -} diff --git a/tests/test_init.cpp b/tests/test_init.cpp deleted file mode 100644 index dca5a03a..00000000 --- a/tests/test_init.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/* - * Copyright (c) 2016-2020 UAVCAN Development Team - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include "canard.h" - -static bool shouldAcceptTransferMock(const CanardInstance*, uint16_t, CanardTransferKind, uint8_t) -{ - return false; -} - -static void onTransferReceptionMock(CanardInstance*, CanardRxTransfer*) {} - -TEST_CASE("Init, UserReference") -{ - std::uint8_t memory_arena[1024]; - - ::CanardInstance ins; - canardInit(&ins, - memory_arena, - sizeof(memory_arena), - &onTransferReceptionMock, - &shouldAcceptTransferMock, - reinterpret_cast(12345)); - - REQUIRE(12345 == reinterpret_cast(canardGetUserReference(&ins))); -} diff --git a/tests/test_memory_allocator.cpp b/tests/test_memory_allocator.cpp deleted file mode 100644 index 519b79b7..00000000 --- a/tests/test_memory_allocator.cpp +++ /dev/null @@ -1,104 +0,0 @@ -/* - * Copyright (c) 2016-2020 UAVCAN Development Team - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include "canard_internals.h" - -#define AVAILABLE_BLOCKS 3 - -TEST_CASE("MemoryAllocatorTestGroup, FreeListIsConstructedCorrectly") -{ - CanardPoolAllocator allocator; - CanardPoolAllocatorBlock buffer[AVAILABLE_BLOCKS]; - initPoolAllocator(&allocator, buffer, AVAILABLE_BLOCKS); - - // Check that the memory list is constructed correctly. - REQUIRE(&buffer[0] == allocator.free_list); - REQUIRE(&buffer[1] == allocator.free_list->next); - REQUIRE(&buffer[2] == allocator.free_list->next->next); - REQUIRE(NULL == allocator.free_list->next->next->next); - - // Check statistics - REQUIRE(AVAILABLE_BLOCKS == allocator.statistics.capacity_blocks); - REQUIRE(0 == allocator.statistics.current_usage_blocks); - REQUIRE(0 == allocator.statistics.peak_usage_blocks); -} - -TEST_CASE("MemoryAllocatorTestGroup, CanAllocateBlock") -{ - CanardPoolAllocator allocator; - CanardPoolAllocatorBlock buffer[AVAILABLE_BLOCKS]; - initPoolAllocator(&allocator, buffer, AVAILABLE_BLOCKS); - - void* block = allocateBlock(&allocator); - - // Check that the first free memory block was used and that the next block is ready. - REQUIRE(&buffer[0] == block); - REQUIRE(&buffer[1] == allocator.free_list); - - // Check statistics - REQUIRE(AVAILABLE_BLOCKS == allocator.statistics.capacity_blocks); - REQUIRE(1 == allocator.statistics.current_usage_blocks); - REQUIRE(1 == allocator.statistics.peak_usage_blocks); -} - -TEST_CASE("MemoryAllocatorTestGroup, ReturnsNullIfThereIsNoBlockLeft") -{ - CanardPoolAllocator allocator; - CanardPoolAllocatorBlock buffer[AVAILABLE_BLOCKS]; - initPoolAllocator(&allocator, buffer, AVAILABLE_BLOCKS); - - // First exhaust all availables block - for (int i = 0; i < AVAILABLE_BLOCKS; ++i) - { - allocateBlock(&allocator); - } - - // Try to allocate one extra block - void* block = allocateBlock(&allocator); - REQUIRE(NULL == block); - - // Check statistics - REQUIRE(AVAILABLE_BLOCKS == allocator.statistics.capacity_blocks); - REQUIRE(AVAILABLE_BLOCKS == allocator.statistics.current_usage_blocks); - REQUIRE(AVAILABLE_BLOCKS == allocator.statistics.peak_usage_blocks); -} - -TEST_CASE("MemoryAllocatorTestGroup, CanFreeBlock") -{ - CanardPoolAllocator allocator; - CanardPoolAllocatorBlock buffer[AVAILABLE_BLOCKS]; - initPoolAllocator(&allocator, buffer, AVAILABLE_BLOCKS); - - void* block = allocateBlock(&allocator); - - freeBlock(&allocator, block); - - // Check that the block was added back to the beginning - REQUIRE(&buffer[0] == allocator.free_list); - REQUIRE(&buffer[1] == allocator.free_list->next); - - // Check statistics - REQUIRE(AVAILABLE_BLOCKS == allocator.statistics.capacity_blocks); - REQUIRE(0 == allocator.statistics.current_usage_blocks); - REQUIRE(1 == allocator.statistics.peak_usage_blocks); -} diff --git a/tests/test_rxerr.cpp b/tests/test_rxerr.cpp deleted file mode 100644 index e2d732b9..00000000 --- a/tests/test_rxerr.cpp +++ /dev/null @@ -1,381 +0,0 @@ -/* - * Copyright (c) 2016-2020 UAVCAN Development Team - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include - -// Independent implementation of ID layout from the specification. -#define CONSTRUCT_PRIO(prio) ((std::uint8_t(prio) & 0x7U) << 26U) -#define CONSTRUCT_SUBJECT_ID(subject_id) ((std::uint16_t(subject_id) & 0xFFFFU) << 8U) -#define CONSTRUCT_SOURCE_ID(source_id) ((std::uint8_t(source_id) & 0x7FU) << 1U) -#define CONSTRUCT_SERVICE_ID(service_id) ((std::uint16_t(service_id) & 0x1FFU) << 15U) -#define CONSTRUCT_DEST_ID(dest_id) ((std::uint8_t(dest_id) & 0x7FU) << 8U) -#define CONSTRUCT_REQUEST(request) ((std::uint8_t(request) & 0x01U) << 24U) - -#define CONSTRUCT_ANON_BIT (1U << 24U) -#define CONSTRUCT_SNM_BIT (1U << 25U) -#define CONSTRUCT_PV_BIT (1U) - -#define CONSTRUCT_MSG_ID(prio, subject_id, source_id) \ - (CONSTRUCT_PRIO(prio) | CONSTRUCT_SUBJECT_ID(subject_id) | CONSTRUCT_SOURCE_ID(source_id) | CONSTRUCT_PV_BIT | \ - CANARD_CAN_FRAME_EFF) - -#define CONSTRUCT_ANON_MSG_ID(prio, subject_id, source_id) \ - (CONSTRUCT_ANON_BIT | CONSTRUCT_MSG_ID(prio, subject_id, source_id)) - -#define CONSTRUCT_SVC_ID(prio, service_id, request, dest_id, source_id) \ - (CONSTRUCT_PRIO(prio) | CONSTRUCT_SNM_BIT | CONSTRUCT_REQUEST(request) | CONSTRUCT_SERVICE_ID(service_id) | \ - CONSTRUCT_DEST_ID(dest_id) | CONSTRUCT_SOURCE_ID(source_id) | CONSTRUCT_PV_BIT | CANARD_CAN_FRAME_EFF) - -#define CONSTRUCT_START(start) ((std::uint8_t(start) & 0x1U) << 7U) -#define CONSTRUCT_END(end) ((std::uint8_t(end) & 0x1U) << 6U) -#define CONSTRUCT_TOGGLE(toggle) ((std::uint8_t(toggle) & 0x1U) << 5U) -#define CONSTRUCT_TID(tid) ((std::uint8_t(tid) & 0x1FU)) - -#define CONSTRUCT_TAIL_BYTE(start, end, toggle, tid) \ - (CONSTRUCT_START(start) | CONSTRUCT_END(end) | CONSTRUCT_TOGGLE(toggle) | CONSTRUCT_TID(tid)) - -static bool g_should_accept = true; - -/** - * This callback is invoked by the library when a new message or request or response is received. - */ -static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer) -{ - (void) ins; - (void) transfer; -} - -static bool shouldAcceptTransfer(const CanardInstance* ins, - uint16_t port_id, - CanardTransferKind transfer_kind, - uint8_t source_node_id) -{ - (void) ins; - (void) port_id; - (void) transfer_kind; - (void) source_node_id; - return g_should_accept; -} - -TEST_CASE("canardHandleRxFrame incompatible packet handling, Correctness") -{ - uint8_t canard_memory_pool[1024]; - CanardInstance canard; - CanardCANFrame frame; - int16_t err; - - // Setup frame data to be single frame transfer - frame.data[0] = CONSTRUCT_TAIL_BYTE(1, 1, 1, 0); - - canardInit(&canard, - canard_memory_pool, - sizeof(canard_memory_pool), - onTransferReceived, - shouldAcceptTransfer, - &canard); - g_should_accept = true; - - // Frame with good RTR/ERR/data_len bad EFF - frame.id = 0; - frame.data_len = 1; - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(-CANARD_ERROR_RX_INCOMPATIBLE_PACKET == err); - - // Frame with good EFF/ERR/data_len, bad RTR - frame.id = 0U | CANARD_CAN_FRAME_RTR | CANARD_CAN_FRAME_EFF; - frame.data_len = 1; - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(-CANARD_ERROR_RX_INCOMPATIBLE_PACKET == err); - - // Frame with good EFF/RTR/data_len, bad ERR - frame.id = 0U | CANARD_CAN_FRAME_ERR | CANARD_CAN_FRAME_EFF; - frame.data_len = 1; - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(-CANARD_ERROR_RX_INCOMPATIBLE_PACKET == err); - - // Frame with good EFF/RTR/ERR, bad data_len - frame.id = 0U | CANARD_CAN_FRAME_EFF; - frame.data_len = 0; - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(-CANARD_ERROR_RX_INCOMPATIBLE_PACKET == err); - - // Frame with good EFF/RTR/ERR/data_len - frame.id = 0U | CANARD_CAN_FRAME_EFF; - frame.data_len = 1; - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(CANARD_OK == err); -} - -TEST_CASE("canardHandleRxFrame wrong address handling, Correctness") -{ - uint8_t canard_memory_pool[1024]; - CanardInstance canard; - CanardCANFrame frame; - int16_t err; - - // Setup frame data to be single frame transfer - frame.data[0] = CONSTRUCT_TAIL_BYTE(1, 1, 1, 0); - - // Open canard to accept all transfers with a node ID of 20 - canardInit(&canard, - canard_memory_pool, - sizeof(canard_memory_pool), - onTransferReceived, - shouldAcceptTransfer, - &canard); - canardSetLocalNodeID(&canard, 20); - g_should_accept = true; - - // Send package with ID 24, should not be wanted - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 24, 0); - frame.data_len = 1; - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(-CANARD_ERROR_RX_WRONG_ADDRESS == err); - - // Send package with ID 20, should be OK - frame.id = 0U | CANARD_CAN_FRAME_EFF; // Set EFF - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 1; - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(CANARD_OK == err); -} - -TEST_CASE("canardHandleRxFrame shouldAccept handling, Correctness") -{ - uint8_t canard_memory_pool[1024]; - CanardInstance canard; - CanardCANFrame frame; - int16_t err; - - // Setup frame data to be single frame transfer - frame.data[0] = CONSTRUCT_TAIL_BYTE(1, 1, 1, 0); - - // Open canard to accept all transfers with a node ID of 20 - canardInit(&canard, - canard_memory_pool, - sizeof(canard_memory_pool), - onTransferReceived, - shouldAcceptTransfer, - &canard); - canardSetLocalNodeID(&canard, 20); - - // Send packet, don't accept - g_should_accept = false; - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 1; - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(-CANARD_ERROR_RX_NOT_WANTED == err); - - // Send packet, accept - g_should_accept = true; - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 1; - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(CANARD_OK == err); -} - -TEST_CASE("canardHandleRxFrame no state handling, Correctness") -{ - uint8_t canard_memory_pool[1024]; - CanardInstance canard; - CanardCANFrame frame; - int16_t err; - - g_should_accept = true; - - // Open canard to accept all transfers with a node ID of 20 - canardInit(&canard, - canard_memory_pool, - sizeof(canard_memory_pool), - onTransferReceived, - shouldAcceptTransfer, - &canard); - canardSetLocalNodeID(&canard, 20); - - // Not start or end of packet, should fail - frame.data[0] = CONSTRUCT_TAIL_BYTE(0, 0, 1, 0); - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 1; - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(-CANARD_ERROR_RX_MISSED_START == err); - - // End of packet, should fail - frame.data[0] = CONSTRUCT_TAIL_BYTE(0, 1, 1, 0); - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 1; - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(-CANARD_ERROR_RX_MISSED_START == err); - - // 1 Frame packet, should pass - frame.data[0] = CONSTRUCT_TAIL_BYTE(1, 1, 1, 0); - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 1; - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(CANARD_OK == err); - - // Send a start packet, should pass - frame.data[7] = CONSTRUCT_TAIL_BYTE(1, 0, 1, 1); - - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 8; // Data length MUST be full packet - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(CANARD_OK == err); - - // Send a middle packet, from the same ID, but don't toggle - frame.data[0] = CONSTRUCT_TAIL_BYTE(0, 0, 1, 1); - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 1; - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(-CANARD_ERROR_RX_WRONG_TOGGLE == err); - - // Send a middle packet, toggle, but use wrong ID - frame.data[7] = CONSTRUCT_TAIL_BYTE(0, 0, 0, 2); - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 8; - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(-CANARD_ERROR_RX_UNEXPECTED_TID == err); - - // Send a middle packet, toggle, and use correct ID - frame.data[7] = CONSTRUCT_TAIL_BYTE(0, 0, 0, 1); - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 8; - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(CANARD_OK == err); -} - -TEST_CASE("canardHandleRxFrame missed start handling, Correctness") -{ - uint8_t canard_memory_pool[1024]; - CanardInstance canard; - CanardCANFrame frame; - int16_t err; - - g_should_accept = true; - - // Open canard to accept all transfers with a node ID of 20 - canardInit(&canard, - canard_memory_pool, - sizeof(canard_memory_pool), - onTransferReceived, - shouldAcceptTransfer, - &canard); - canardSetLocalNodeID(&canard, 20); - - // Send a start packet, should pass - frame.data[7] = CONSTRUCT_TAIL_BYTE(1, 0, 1, 1); - - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 8; // Data length MUST be full packet - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(CANARD_OK == err); - - // Send a middle packet, toggle, and use correct ID - but timeout - frame.data[7] = CONSTRUCT_TAIL_BYTE(0, 0, 0, 1); - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 8; - err = canardHandleRxFrame(&canard, &frame, 4000000); - REQUIRE(-CANARD_ERROR_RX_MISSED_START == err); - - // Send a start packet, should pass - frame.data[7] = CONSTRUCT_TAIL_BYTE(1, 0, 1, 1); - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 8; // Data length MUST be full packet - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(CANARD_OK == err); - - // Send a middle packet, toggle, and use correct ID - but timestamp 0 - frame.data[7] = CONSTRUCT_TAIL_BYTE(0, 0, 0, 1); - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 8; - err = canardHandleRxFrame(&canard, &frame, 0); - REQUIRE(-CANARD_ERROR_RX_MISSED_START == err); - - // Send a start packet, should pass - frame.data[7] = CONSTRUCT_TAIL_BYTE(1, 0, 1, 1); - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 8; // Data length MUST be full packet - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(CANARD_OK == err); - - // Send a middle packet, toggle, and use an incorrect TID - frame.data[7] = CONSTRUCT_TAIL_BYTE(0, 0, 0, 3); - frame.data[7] = 0U | 3U | (1U << 5U); - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 8; - err = canardHandleRxFrame(&canard, &frame, 0); - REQUIRE(-CANARD_ERROR_RX_MISSED_START == err); -} - -TEST_CASE("canardHandleRxFrame short frame handling, Correctness") -{ - uint8_t canard_memory_pool[1024]; - CanardInstance canard; - CanardCANFrame frame; - int16_t err; - - g_should_accept = true; - - // Open canard to accept all transfers with a node ID of 20 - canardInit(&canard, - canard_memory_pool, - sizeof(canard_memory_pool), - onTransferReceived, - shouldAcceptTransfer, - &canard); - canardSetLocalNodeID(&canard, 20); - - // Send a start packet which is short, should fail - frame.data[1] = CONSTRUCT_TAIL_BYTE(1, 0, 1, 1); - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 2; // Data length MUST be full packet - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(-CANARD_ERROR_RX_SHORT_FRAME == err); - - // Send a start packet which is short, should fail - frame.data[2] = CONSTRUCT_TAIL_BYTE(1, 0, 1, 1); - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 3; // Data length MUST be full packet - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(-CANARD_ERROR_RX_SHORT_FRAME == err); -} - -TEST_CASE("canardHandleRxFrame OOM handling, Correctness") -{ - uint8_t dummy_buf; - CanardInstance canard; - CanardCANFrame frame; - int16_t err; - - g_should_accept = true; - - // Open canard to accept all transfers with a node ID of 20 - canardInit(&canard, &dummy_buf, 0, onTransferReceived, shouldAcceptTransfer, &canard); - canardSetLocalNodeID(&canard, 20); - - // Send a start packet - we shouldn't have any memory - frame.data[7] = CONSTRUCT_TAIL_BYTE(1, 0, 0, 1); - frame.id = CONSTRUCT_SVC_ID(0, 0, 1, 20, 0); - frame.data_len = 8; // Data length MUST be full packet - err = canardHandleRxFrame(&canard, &frame, 1); - REQUIRE(-CANARD_ERROR_OUT_OF_MEMORY == err); -} diff --git a/tests/test_scalar_encoding.cpp b/tests/test_scalar_encoding.cpp deleted file mode 100644 index 3ebafc06..00000000 --- a/tests/test_scalar_encoding.cpp +++ /dev/null @@ -1,210 +0,0 @@ -/* - * Copyright (c) 2016-2020 UAVCAN Development Team - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include -#include -#include -#include -#include "canard_internals.h" - -TEST_CASE("BigEndian, Check") -{ - // Assuming that unit tests can only be run on little-endian platforms! - REQUIRE_FALSE(isBigEndian()); -} - -template -static inline T read(CanardRxTransfer* transfer, uint32_t bit_offset, uint8_t bit_length) -{ - auto value = T(); - - const int res = canardDecodePrimitive(transfer, uint16_t(bit_offset), bit_length, std::is_signed::value, &value); - if (res != bit_length) - { - throw std::runtime_error("Unexpected return value; expected " + std::to_string(unsigned(bit_length)) + - ", got " + std::to_string(res)); - } - - return value; -} - -TEST_CASE("ScalarDecode, SingleFrame") -{ - auto transfer = CanardRxTransfer(); - - static const uint8_t buf[7] = {0b10100101, // 0 - 0b11000011, // 8 - 0b11100111, // 16 - 0b01111110, // 24 - 0b01010101, - 0b10101010, - 0b11101000}; - - transfer.payload_head = &buf[0]; - transfer.payload_len = sizeof(buf); - - REQUIRE(0b10100101 == read(&transfer, 0, 8)); - REQUIRE(0b01011100 == read(&transfer, 4, 8)); - REQUIRE(0b00000101 == read(&transfer, 4, 4)); - - REQUIRE(read(&transfer, 9, 1)); - REQUIRE_FALSE(read(&transfer, 10, 1)); - - REQUIRE(0b11101000101010100101010101111110 == read(&transfer, 24, 32)); - - /* - * Raw bit stream with offset 21: - * 111 01111110 01010101 10101010 11101 - * New byte segmentation: - * 11101111 11001010 10110101 01011101 - * Which is little endian representation of: - * 0b01011101101101011100101011101111 - */ - REQUIRE(0b01011101101101011100101011101111 == read(&transfer, 21, 32)); - - // Should fail - REQUIRE_THROWS_AS(read(&transfer, 25, 32), std::runtime_error); - - // Inexact size - REQUIRE(0b010111101101011100101011101111 == read(&transfer, 21, 30)); - - // Negatives; reference values taken from libuavcan test suite or computed manually - REQUIRE(-1 == read(&transfer, 16, 3)); // 0b111 - REQUIRE(-4 == read(&transfer, 2, 3)); // 0b100 - - REQUIRE(-91 == read(&transfer, 0, 8)); // 0b10100101 - REQUIRE(-15451 == read(&transfer, 0, 16)); // 0b1100001110100101 - REQUIRE(-7771 == read(&transfer, 0, 15)); // 0b100001110100101 -} - -TEST_CASE("ScalarDecode, MultiFrame") -{ - /* - * Configuring allocator - */ - CanardPoolAllocatorBlock allocator_blocks[2]; - CanardPoolAllocator allocator; - initPoolAllocator(&allocator, &allocator_blocks[0], 2); - - /* - * Configuring the transfer object - */ - auto transfer = CanardRxTransfer(); - - uint8_t head[CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE]; - for (auto& x : head) - { - x = 0b10100101; - } - static_assert(CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE == 6, - "Assumption is not met, are we on a 32-bit x86 machine?"); - - auto middle_a = createBufferBlock(&allocator); - auto middle_b = createBufferBlock(&allocator); - - std::fill_n(&middle_a->data[0], CANARD_BUFFER_BLOCK_DATA_SIZE, 0b01011010); - std::fill_n(&middle_b->data[0], CANARD_BUFFER_BLOCK_DATA_SIZE, 0b11001100); - - middle_a->next = middle_b; - middle_b->next = nullptr; - - const uint8_t tail[4] = {0b00010001, 0b00100010, 0b00110011, 0b01000100}; - - transfer.payload_head = &head[0]; - transfer.payload_middle = middle_a; - transfer.payload_tail = &tail[0]; - - transfer.payload_len = - uint16_t(CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE + CANARD_BUFFER_BLOCK_DATA_SIZE * 2 + sizeof(tail)); - - std::cout << "Payload size: " << transfer.payload_len << std::endl; - - /* - * Testing - */ - REQUIRE(0b10100101 == read(&transfer, 0, 8)); - REQUIRE(0b01011010 == read(&transfer, 4, 8)); - REQUIRE(0b00000101 == read(&transfer, 4, 4)); - - REQUIRE_FALSE(read(&transfer, CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE * 8, 1)); - REQUIRE(read(&transfer, CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE * 8 + 1, 1)); - - // 64 from beginning, 48 bits from head, 16 bits from the middle - REQUIRE(0b0101101001011010101001011010010110100101101001011010010110100101ULL == read(&transfer, 0, 64)); - - // 64 from two middle blocks, 32 from the first, 32 from the second - REQUIRE(0b1100110011001100110011001100110001011010010110100101101001011010ULL == - read(&transfer, - CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE * 8 + CANARD_BUFFER_BLOCK_DATA_SIZE * 8 - 32, - 64)); - - // Last 64 - REQUIRE(0b0100010000110011001000100001000111001100110011001100110011001100ULL == - read(&transfer, transfer.payload_len * 8U - 64U, 64)); - - /* - * Testing without the middle - */ - transfer.payload_middle = nullptr; - transfer.payload_len = uint16_t(transfer.payload_len - CANARD_BUFFER_BLOCK_DATA_SIZE * 2U); - - // Last 64 - REQUIRE(0b0100010000110011001000100001000110100101101001011010010110100101ULL == - read(&transfer, transfer.payload_len * 8U - 64U, 64)); -} - -TEST_CASE("ScalarEncode, Basic") -{ - uint8_t buffer[32]; - std::fill_n(std::begin(buffer), sizeof(buffer), 0); - - uint8_t u8 = 123; - canardEncodePrimitive(buffer, 0, 8, &u8); - REQUIRE(123 == buffer[0]); - REQUIRE(0 == buffer[1]); - - u8 = 0b1111; - canardEncodePrimitive(buffer, 5, 4, &u8); - REQUIRE((123U | 0b111U) == buffer[0]); - REQUIRE(0b10000000 == buffer[1]); - - int16_t s16 = -1; - canardEncodePrimitive(buffer, 9, 15, &s16); - REQUIRE((123U | 0b111U) == buffer[0]); - REQUIRE(0b11111111 == buffer[1]); - REQUIRE(0b11111111 == buffer[2]); - REQUIRE(0b00000000 == buffer[3]); - - auto s64 = int64_t(0b0000000100100011101111000110011110001001101010111100110111101111L); - canardEncodePrimitive(buffer, 16, 60, &s64); - REQUIRE((123U | 0b111U) == buffer[0]); // 0 - REQUIRE(0b11111111 == buffer[1]); // 8 - REQUIRE(0b11101111 == buffer[2]); // 16 - REQUIRE(0b11001101 == buffer[3]); - REQUIRE(0b10101011 == buffer[4]); - REQUIRE(0b10001001 == buffer[5]); - REQUIRE(0b01100111 == buffer[6]); - REQUIRE(0b10111100 == buffer[7]); - REQUIRE(0b00100011 == buffer[8]); - REQUIRE(0b00010000 == buffer[9]); -} From 165ef2de80e4f1a16d090da10f44d5b9e708beba Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 4 Feb 2020 06:16:43 +0200 Subject: [PATCH 003/100] API review WIP --- libcanard/canard.c | 6 +- libcanard/canard.h | 287 ++++++++++++++++++++------------------------- 2 files changed, 133 insertions(+), 160 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 26a3b60c..cc773a57 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -72,9 +72,9 @@ typedef struct CanardInternalTxQueueItem } CanardInternalTxQueueItem; /// The fields are ordered to minimize padding on all platforms. -typedef struct CanardInternalInputSession +typedef struct CanardInternalRxSession { - struct CanardInternalInputSession* next; + struct CanardInternalRxSession* next; size_t payload_capacity; ///< Payload past this limit may be discarded by the library. size_t payload_length; ///< How many bytes received so far. @@ -87,7 +87,7 @@ typedef struct CanardInternalInputSession uint16_t calculated_crc; ///< Updated with the received payload in real time. uint8_t transfer_id; bool next_toggle; -} CanardInternalInputSession; +} CanardInternalRxSession; // ---------------------------------------- PRIVATE FUNCTIONS ---------------------------------------- diff --git a/libcanard/canard.h b/libcanard/canard.h index abbe3060..41394700 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -31,23 +31,13 @@ extern "C" { #endif -// ---------------------------------------- CONSTANT DEFINITIONS ---------------------------------------- - /// Semantic version numbers of this library (not the UAVCAN specification). /// API will be backward compatible within the same major version. #define CANARD_VERSION_MAJOR 1 -#define CANARD_VERSION_MINOR 0 /// The version number of the UAVCAN specification implemented by this library. #define CANARD_UAVCAN_SPECIFICATION_VERSION_MAJOR 1 -/// Error code definitions; inverse of these values may be returned from API calls. -#define CANARD_OK 0 -// Value 1 is omitted intentionally since -1 is often used in 3rd party code -#define CANARD_ERROR_INVALID_ARGUMENT 2 -#define CANARD_ERROR_OUT_OF_MEMORY 3 -#define CANARD_ERROR_NODE_ID_NOT_SET 4 - /// MTU values for supported protocols. /// Per the recommendations given in the UAVCAN specification, other MTU values should not be used. #define CANARD_MTU_CAN_CLASSIC 8U @@ -67,11 +57,8 @@ extern "C" { /// If not specified, the transfer-ID timeout will take this value for all new input sessions. #define CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC 2000000UL -// ---------------------------------------- TYPE DEFINITIONS ---------------------------------------- - // Forward declarations. -typedef struct CanardInstance CanardInstance; -typedef struct CanardReceivedTransfer CanardReceivedTransfer; +typedef struct CanardInstance CanardInstance; /// Transfer priority level mnemonics per the recommendations given in the UAVCAN specification. typedef enum @@ -80,19 +67,28 @@ typedef enum CanardPriorityImmediate = 1, CanardPriorityFast = 2, CanardPriorityHigh = 3, - CanardPriorityNominal = 4, + CanardPriorityNominal = 4, ///< This should be the default. CanardPriorityLow = 5, CanardPrioritySlow = 6, CanardPriorityOptional = 7, } CanardPriority; +/// Transfer kinds are defined by the UAVCAN specification. +typedef enum +{ + CanardTransferKindMessagePublication = 0, ///< Multicast, from publisher to all subscribers. + CanardTransferKindServiceResponse = 1, ///< Point-to-point, from server to client. + CanardTransferKindServiceRequest = 2, ///< Point-to-point, from client to server. +} CanardTransferKind; + /// CAN data frame with an extended 29-bit ID. RTR/Error frames are not used and therefore not modeled here. typedef struct { /// For RX frames: reception timestamp. /// For TX frames: transmission deadline. - /// The time system may be arbitrary as long as the clock is monotonic (steady). - uint64_t time_usec; + /// The time system may be arbitrary as long as the clock is monotonic (steady) and 0 is not a valid timestamp. + /// Zero timestamp indicates that the instance is invalid (empty). + uint64_t timestamp_usec; /// 29-bit extended ID. The bits above 29-th are ignored. uint32_t id; @@ -100,142 +96,119 @@ typedef struct /// The useful data in the frame. The length value is not to be confused with DLC! uint8_t data_length; uint8_t* data; -} CanardCANFrame; +} CanardFrame; -/// Transfer kinds are defined by the UAVCAN specification. -typedef enum +typedef struct { - CanardTransferKindMessagePublication = 0, ///< Broadcast, from publisher to all subscribers. - CanardTransferKindServiceResponse = 1, ///< Point-to-point, from server to client. - CanardTransferKindServiceRequest = 2 ///< Point-to-point, from client to server. -} CanardTransferKind; + /// For RX transfers: reception timestamp. + /// For TX transfers: transmission deadline. + /// The time system may be arbitrary as long as the clock is monotonic (steady) and 0 is not a valid timestamp. + /// Zero timestamp indicates that the instance is invalid (empty). + uint64_t timestamp_usec; + + CanardPriority priority; + + CanardTransferKind transfer_kind; + + /// Subject-ID for message publications; service-ID for service requests/responses. + uint16_t port_id; + + uint8_t remote_node_id; + + uint8_t transfer_id; + + size_t payload_length; + uint8_t* payload; +} CanardTransfer; /// The application supplies the library with this information when a new transfer should be received. typedef struct { - bool should_accept; - uint32_t transfer_id_timeout_usec; - size_t payload_capacity; -} CanardTransferReceptionParameters; + /// The transfer-ID timeout for this session. If no specific requirements are defined, the default value + /// @ref CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC should be used. + /// Zero timeout indicates that this transfer should not be received (all its frames will be silently dropped). + uint64_t transfer_id_timeout_usec; + + /// The maximum payload size of the transfer (i.e., the maximum size of the serialized DSDL object). + /// Payloads larger than this will be silently truncated per the Implicit Truncation Rule defined in Specification. + /// Per Specification, the transfer CRC of multi-frame transfers is always validated regardless of the + /// implicit truncation rule. + /// Zero is also a valid value indicating that the transfer shall be accepted but the payload need not be stored. + size_t payload_length_max; +} CanardRxMetadata; /// The application shall implement this function and supply a pointer to it to the library during initialization. /// The library calls this function to determine whether a transfer should be received. /// @param ins Library instance. /// @param port_id Subject-ID or service-ID of the transfer. /// @param transfer_kind Message or service transfer. -/// @param source_node_id Node-ID of the origin; broadcast if anonymous. +/// @param source_node_id Node-ID of the origin; @ref CANARD_NODE_ID_UNSET if anonymous. /// @returns @ref CanardTransferReceptionParameters. -typedef CanardTransferReceptionParameters (*CanardShouldAcceptTransfer)(const CanardInstance* ins, - uint16_t port_id, - CanardTransferKind transfer_kind, - uint8_t source_node_id); +typedef CanardRxMetadata (*CanardRxFilter)(const CanardInstance* ins, + uint16_t port_id, + CanardTransferKind transfer_kind, + uint8_t source_node_id); + +typedef void* (*CanardHeapAllocate)(CanardInstance* ins, size_t amount); + +/// Free as in freedom. +typedef void (*CanardHeapFree)(CanardInstance* ins, void* pointer); /// This is the core structure that keeps all of the states and allocated resources of the library instance. -/// The application should never access any of the fields directly! Instead, the API functions should be used. +/// Fields whose names begin with an underscore SHALL NOT be accessed by the application, +/// they are for internal use only. struct CanardInstance { - CanardShouldAcceptTransfer should_accept_transfer; - - struct CanardInternalInputSession* input_sessions; - struct CanardInternalTxQueueItem* tx_queue; - - void* user_reference; ///< User pointer that can link this instance with other objects - - uint8_t node_id; ///< Local node-ID or @ref CANARD_NODE_ID_UNSET. - uint8_t mtu_bytes; ///< Maximum number of data bytes per CAN frame. Range: [8, 64]. + /// User pointer that can link this instance with other objects. + /// This field can be changed arbitrarily, the library does not access it after initialization. + /// The default value is NULL. + void* user_reference; + + /// The node-ID of the local node. The default value is @ref CANARD_NODE_ID_UNSET. + /// Per the UAVCAN Specification, the node-ID should not be assigned more than once. + /// Invalid values are treated as @ref CANARD_NODE_ID_UNSET. + uint8_t node_id; + + /// The maximum transmission unit. The value can be changed arbitrarily at any time. + /// This setting defines the maximum number of bytes per CAN data frame in all outgoing transfers. + /// Regardless of this setting, CAN frames with any MTU can always be accepted. + /// + /// Only the standard values should be used as recommended by the specification; otherwise, + /// networking interoperability issues may arise. See "CANARD_MTU_*". + /// Valid values are any valid CAN frame data length. The default is the maximum valid value. + /// Invalid values are treated as the nearest valid value. + uint8_t mtu_bytes; + + /// Callbacks invoked by the library. See their type documentation for details. + /// They SHALL be valid function pointers at all times. + CanardHeapAllocate heap_allocate; + CanardHeapFree heap_free; + CanardRxFilter rx_filter; + + /// These fields are for internal use only. Do not access from the application. + struct CanardInternalRxSession* _rx_sessions; + struct CanardInternalTxQueueItem* _tx_queue; }; -/// This structure represents a received transfer for the application. -/// An instance of it is passed to the application via the callback when a new transfer is received. -/// Pointers to the structure and all its fields are invalidated after the callback returns. -struct CanardReceivedTransfer -{ - /// Timestamp of the first frame of this transfer. - uint64_t timestamp_usec; +/// Initialize a new library instance. +/// The default values will be assigned as specified in the structure field documentation. +CanardInstance canardInit(const CanardHeapAllocate heap_allocate, + const CanardHeapFree heap_free, + const CanardRxFilter rx_filter); - size_t payload_length; - uint8_t* payload; +// ---------------------------------------- OUTGOING TRANSFERS ---------------------------------------- - CanardPriority priority; - CanardTransferKind transfer_kind; - uint16_t port_id; ///< Subject-ID or service-ID. - uint8_t source_node_id; ///< For anonymous transfers it's @ref CANARD_NODE_ID_UNSET. - uint8_t transfer_id; ///< Bits above @ref CANARD_TRANSFER_ID_BIT_LENGTH are always zero. -}; +void canardTxPush(CanardInstance* const ins, const CanardTransfer* transfer); -// ---------------------------------------- CONFIGURATION FUNCTIONS ---------------------------------------- +CanardFrame canardTxPeek(const CanardInstance* const ins); -/// Initialize a library instance. -/// The local node-ID will be initialized as @ref CANARD_NODE_ID_UNSET, i.e. anonymous by default. -/// -/// @param should_accept_transfer Callback, see @ref CanardShouldAcceptTransfer. -/// @param user_reference Optional application-defined pointer; NULL if not needed. -CanardInstance canardInit(const CanardShouldAcceptTransfer should_accept_transfer, - void* const user_reference); +void canardTxPop(CanardInstance* const ins); -/// Assign a new node-ID value to the current node. Node-ID can be assigned only once. -/// If the supplied value is invalid or the node-ID is already configured, nothing will be done. -void canardSetLocalNodeID(CanardInstance* const ins, const uint8_t self_node_id); +// ---------------------------------------- INCOMING TRANSFERS ---------------------------------------- -/// @returns Node-ID of the local node; 255 (broadcast) if the node-ID is not set, i.e. if the local node is anonymous. -uint8_t canardGetLocalNodeID(const CanardInstance* const ins); +CanardTransfer canardRxAcceptFrame(CanardInstance* const ins, const CanardFrame* frame); -/// Configure the maximum transmission unit. This can be done as many times as needed. -/// This setting defines the maximum number of bytes per CAN data frame in all outgoing transfers. -/// Regardless of this setting, CAN frames with any MTU up to CANARD_CONFIG_MTU_MAX bytes can always be accepted. -/// -/// Only the standard values should be used as recommended by the specification (8, 64 bytes); -/// otherwise, interoperability issues may arise. See "CANARD_MTU_*". -/// -/// Range: [8, 64]. The default is the maximum. -/// Invalid values are rounded to the nearest valid value. -void canardSetMTU(const CanardInstance* const ins, const uint8_t mtu_bytes); - -/// Read the value of the user pointer. -/// The user pointer is configured once during initialization. -/// It can be used to store references to any user-specific data, or to link the instance object with C++ objects. -/// @returns The application-defined pointer. -void* canardGetUserReference(const CanardInstance* const ins); - -// ---------------------------------------- OUTGOING TRANSFER FUNCTIONS ---------------------------------------- - -int32_t canardPublish(CanardInstance* const ins, - const uint16_t subject_id, - uint8_t* const inout_transfer_id, - const uint8_t priority, - const size_t payload_length, - const void* const payload, - const uint64_t deadline_usec); - -int32_t canardRequest(CanardInstance* const ins, - const uint8_t server_node_id, - const uint16_t service_id, - uint8_t* const inout_transfer_id, - const CanardPriority priority, - const size_t payload_length, - const void* const payload, - const uint64_t deadline_usec); - -int32_t canardRespond(CanardInstance* const ins, - const uint8_t client_node_id, - const uint16_t service_id, - const uint8_t transfer_id, - const CanardPriority priority, - const size_t payload_length, - const void* const payload, - const uint64_t deadline_usec); - -/// TODO: REVIEW NEEDED -CanardCANFrame canardPeekTxQueue(const CanardInstance* const ins); -void canardPopTxQueue(const CanardInstance* const ins); - -// ---------------------------------------- INCOMING TRANSFER FUNCTIONS ---------------------------------------- - -CanardReceivedTransfer canardReceive(CanardInstance* const ins, const CanardCANFrame frame); - -void canardDestroyTransfer(CanardInstance* const ins, CanardReceivedTransfer* const transfer); - -// ---------------------------------------- DATA SERIALIZATION FUNCTIONS ---------------------------------------- +// ---------------------------------------- DATA SERIALIZATION ---------------------------------------- /// This function may be used to encode values for later transmission in a UAVCAN transfer. It encodes a scalar value /// -- boolean, integer, character, or floating point -- and puts it at the specified bit offset in the specified @@ -256,13 +229,13 @@ void canardDestroyTransfer(CanardInstance* const ins, CanardReceivedTransfer* co /// | [33, 64] | uint64_t, int64_t, or 64-bit float | /// /// @param destination Destination buffer where the result will be stored. -/// @param bit_offset Offset, in bits, from the beginning of the destination buffer. -/// @param bit_length Length of the value, in bits; see the table. +/// @param offset_bit Offset, in bits, from the beginning of the destination buffer. +/// @param length_bit Length of the value, in bits; see the table. /// @param value Pointer to the value; see the table. -void canardSerializePrimitive(void* const destination, - const size_t bit_offset, - const uint8_t bit_length, - const void* const value); +void canardDSDLPrimitiveSerialize(void* const destination, + const size_t offset_bit, + const uint8_t length_bit, + const void* const value); /// This function may be used to extract values from received UAVCAN transfers. It decodes a scalar value -- /// boolean, integer, character, or floating point -- from the specified bit position in the source buffer. @@ -273,36 +246,36 @@ void canardSerializePrimitive(void* const destination, /// /// The type of the value pointed to by 'out_value' is defined as follows: /// -/// | bit_length | value_is_signed | out_value points to | -/// |------------|-----------------|------------------------------------------| -/// | 1 | false | bool (may be incompatible with uint8_t!) | -/// | 1 | true | N/A | -/// | [2, 8] | false | uint8_t, or char | -/// | [2, 8] | true | int8_t, or char | -/// | [9, 16] | false | uint16_t | -/// | [9, 16] | true | int16_t | -/// | [17, 32] | false | uint32_t | -/// | [17, 32] | true | int32_t, or 32-bit float IEEE 754 | -/// | [33, 64] | false | uint64_t | -/// | [33, 64] | true | int64_t, or 64-bit float IEEE 754 | +/// | bit_length | is_signed | out_value points to | +/// |------------|-------------|------------------------------------------| +/// | 1 | false | bool (may be incompatible with uint8_t!) | +/// | 1 | true | N/A | +/// | [2, 8] | false | uint8_t, or char | +/// | [2, 8] | true | int8_t, or char | +/// | [9, 16] | false | uint16_t | +/// | [9, 16] | true | int16_t | +/// | [17, 32] | false | uint32_t | +/// | [17, 32] | true | int32_t, or 32-bit float IEEE 754 | +/// | [33, 64] | false | uint64_t | +/// | [33, 64] | true | int64_t, or 64-bit float IEEE 754 | /// -/// @param source The source buffer where the data will be read from. -/// @param bit_offset Offset, in bits, from the beginning of the source buffer. -/// @param bit_length Length of the value, in bits; see the table. -/// @param value_is_signed True if the value can be negative (i.e., sign bit extension is needed); see the table. -/// @param out_value Pointer to the output storage; see the table. -void canardDeserializePrimitive(const void* const source, - const size_t bit_offset, - const uint8_t bit_length, - const bool value_is_signed, - void* const out_value); +/// @param source The source buffer where the data will be read from. +/// @param offset_bit Offset, in bits, from the beginning of the source buffer. +/// @param length_bit Length of the value, in bits; see the table. +/// @param is_signed True if the value can be negative (i.e., sign bit extension is needed); see the table. +/// @param out_value Pointer to the output storage; see the table. +void canardDSDLPrimitiveDeserialize(const void* const source, + const size_t offset_bit, + const uint8_t length_bit, + const bool is_signed, + void* const out_value); /// IEEE 754 binary16 marshaling helpers. /// These functions convert between the native float and the standard IEEE 754 binary16 float (a.k.a. half precision). /// It is assumed that the native float is IEEE 754 binary32, otherwise, the results may be unpredictable. /// Majority of modern computers and microcontrollers use IEEE 754, so this limitation should not limit the portability. -uint16_t canardComposeFloat16(float value); -float canardParseFloat16(uint16_t value); +uint16_t canardDSDLFloat16Serialize(float value); +float canardDSDLFloat16Deserialize(uint16_t value); #ifdef __cplusplus } From 5cf5b1bf90fa221be518fd19fed1b4e85fea6e35 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 4 Feb 2020 12:48:38 +0200 Subject: [PATCH 004/100] Test integration --- .travis.yml | 107 +++++++------ LICENSE | 2 +- format.sh | 20 --- .clang-tidy => libcanard/.clang-tidy | 7 +- libcanard/canard.c | 197 ++++++++++++++++++------ libcanard/canard.h | 77 ++++----- run_static_analysis.sh | 17 -- tests/.clang-tidy | 30 ++++ tests/CMakeLists.txt | 98 ++++++++---- tests/catch/{test_main.cpp => main.cpp} | 0 tests/internals.hpp | 20 +++ tests/test_float16.cpp | 38 +++++ tests/test_internals.cpp | 8 + 13 files changed, 408 insertions(+), 213 deletions(-) delete mode 100755 format.sh rename .clang-tidy => libcanard/.clang-tidy (71%) delete mode 100755 run_static_analysis.sh create mode 100644 tests/.clang-tidy rename tests/catch/{test_main.cpp => main.cpp} (100%) create mode 100644 tests/internals.hpp create mode 100644 tests/test_float16.cpp create mode 100644 tests/test_internals.cpp diff --git a/.travis.yml b/.travis.yml index b6bb7b08..57033011 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,60 +1,79 @@ dist: xenial +env: + - CTEST_OUTPUT_ON_FAILURE=1 matrix: include: # -------------------- GCC + SonarCloud -------------------- -# - language: cpp -# addons: -# sonarcloud: -# organization: uavcan -# token: -# secure: "LbaAOcguJspiraxAb9RNkwtGvWDh1aMTEPdz2MCbeICxIIiJ6LRoabD58rLvPsrNSWsR3AAep9Q551dDzT8c04UX+C67N3CZ/oof4DwBT+XQCSfB8FMj2QpbglJqwFPclYUorROjH31VwGcgIq3kfQq3Cw8G+nsL8vRaaJXsJb/KPp6MU698NGzHJIgRyn29VW76dW0NSxOv4ub3e6aKOnwfI+h1Ctx4p3hCdzd402PaZspv1VgEmirf5sVUJvE67PVIzlwov+CF+2PlrIpGUWI98Gl6HqYHv3hkvSP+4iLvCMD99Zmee4yLnCFY3xcJuZ8zKCRBBoquuUxdzK0f/4l9TZXePDXDMhaj3cXLlaAPWDw+emqTcm+hzP1mt/DaIqopAf54bQojVWELbL6QcjBNkphSvWBeIoyKWuUWU2LWJcJNPXFNUug//D99uXNurkzAIWR+lcsx6zO+cr4EN00N92W6hPt7mhKCF0prs7SvMleEi9mAbxvd4lOHFT56RvcB5ny6IapX9/q1+xm5iSoAzLhbvU1aUCnX74S/yFFejvClxxhW+P0bXYNtZ9RRfl8BdSgENTgA9RSnqdtIJGA4cU3OxIHDyJIC2cgmsE38u7QaMO49r1liJFH+xmDPa6bkGGHiPoHaPu9+g+wYFttK9FNt5ozyHY+VpjwTrY4=" -# apt: -# sources: -# - ubuntu-toolchain-r-test -# packages: -# - g++-9 -# - g++-9-multilib -# - gcc-9-multilib -# - linux-libc-dev:i386 -# script: -# - CC=gcc-9 && CXX=g++-9 && cmake tests -# - build-wrapper-linux-x86-64 --out-dir sonar-dump make all # The build wrapper comes from Sonar Cloud. -# - ./run_tests --rng-seed time -# - sonar-scanner + - language: cpp + addons: + sonarcloud: + organization: uavcan + token: + secure: "LbaAOcguJspiraxAb9RNkwtGvWDh1aMTEPdz2MCbeICxIIiJ6LRoabD58rLvPsrNSWsR3AAep9Q551dDzT8c04UX+C67N3CZ/oof4DwBT+XQCSfB8FMj2QpbglJqwFPclYUorROjH31VwGcgIq3kfQq3Cw8G+nsL8vRaaJXsJb/KPp6MU698NGzHJIgRyn29VW76dW0NSxOv4ub3e6aKOnwfI+h1Ctx4p3hCdzd402PaZspv1VgEmirf5sVUJvE67PVIzlwov+CF+2PlrIpGUWI98Gl6HqYHv3hkvSP+4iLvCMD99Zmee4yLnCFY3xcJuZ8zKCRBBoquuUxdzK0f/4l9TZXePDXDMhaj3cXLlaAPWDw+emqTcm+hzP1mt/DaIqopAf54bQojVWELbL6QcjBNkphSvWBeIoyKWuUWU2LWJcJNPXFNUug//D99uXNurkzAIWR+lcsx6zO+cr4EN00N92W6hPt7mhKCF0prs7SvMleEi9mAbxvd4lOHFT56RvcB5ny6IapX9/q1+xm5iSoAzLhbvU1aUCnX74S/yFFejvClxxhW+P0bXYNtZ9RRfl8BdSgENTgA9RSnqdtIJGA4cU3OxIHDyJIC2cgmsE38u7QaMO49r1liJFH+xmDPa6bkGGHiPoHaPu9+g+wYFttK9FNt5ozyHY+VpjwTrY4=" + apt: + sources: + - ubuntu-toolchain-r-test + packages: + - g++-9 + - g++-9-multilib + - gcc-9-multilib + - linux-libc-dev:i386 + script: + # DEBUG + - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Debug -DNO_STATIC_ANALYSIS=1 + - build-wrapper-linux-x86-64 --out-dir sonar-dump make all # The build wrapper comes from Sonar Cloud. + - make test + # TODO: COMBINE COVERAGE FROM ALL EXECUTABLES! + - find CMakeFiles/test_general_cov.dir/ -name 'canard.c.*' -type f -print -exec mv -f {} . \; + - gcov-9 libcanard/canard.c --object-file canard*gcda --object-file canard*gcno + + # RELEASE + - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Release -DNO_STATIC_ANALYSIS=1 + - make all VERBOSE=1 && make test + + # ANALYSIS + - sonar-scanner # -------------------- Clang 9 -------------------- -# - language: cpp -# addons: -# apt: -# sources: -# - ubuntu-toolchain-r-test -# packages: -# - libstdc++-7-dev:i386 -# - linux-libc-dev:i386 -# - libc6-dev-i386 -# script: -# - wget https://apt.llvm.org/llvm.sh && chmod +x llvm.sh && sudo ./llvm.sh 9 -# - clang++-9 -E -x c++ - -v < /dev/null # Print the Clang configuration for troubleshooting purposes -# - cmake -DCMAKE_C_COMPILER=clang-9 -DCMAKE_CXX_COMPILER=clang++-9 tests -# - make -# - ./run_tests --rng-seed time - - # -------------------- Static analysis -------------------- - language: cpp + addons: + apt: + sources: + - ubuntu-toolchain-r-test + packages: # Install a newer GCC because https://stackoverflow.com/a/51512150/1007777. + - g++-9 + - g++-9-multilib + - gcc-9-multilib + - linux-libc-dev:i386 + - libc6-dev-i386 + - libstdc++-7-dev:i386 script: - - sudo rm -rf /usr/local/clang* + # Set up the toolchain. - wget https://apt.llvm.org/llvm.sh && chmod +x llvm.sh && sudo ./llvm.sh 9 - - sudo apt install clang-format-9 clang-tidy-9 - - sudo ln -sf $(which clang-format-9) /usr/bin/clang-format - - sudo ln -sf $(which clang-tidy-9) /usr/bin/clang-tidy + - sudo apt install clang-tidy-9 clang-format-9 + - clang++-9 -E -x c++ - -v < /dev/null # Print the Clang configuration for troubleshooting purposes. + + # DEBUG + - cmake -DCMAKE_C_COMPILER=clang-9 -DCMAKE_CXX_COMPILER=clang++-9 tests -DCMAKE_BUILD_TYPE=Debug + - make VERBOSE=1 && make test + + # RELEASE + - cmake -DCMAKE_C_COMPILER=clang-9 -DCMAKE_CXX_COMPILER=clang++-9 tests -DCMAKE_BUILD_TYPE=Release + - make VERBOSE=1 && make test + + # RELWITHDEBINFO + - cmake -DCMAKE_C_COMPILER=clang-9 -DCMAKE_CXX_COMPILER=clang++-9 tests -DCMAKE_BUILD_TYPE=RelWithDebInfo + - make VERBOSE=1 && make test - # TODO: invoke clang-tidy. + # MINSIZEREL + - cmake -DCMAKE_C_COMPILER=clang-9 -DCMAKE_CXX_COMPILER=clang++-9 tests -DCMAKE_BUILD_TYPE=MinSizeRel + - make VERBOSE=1 && make test - # Code style enforcement. - - ./format.sh + # Format check + - make format VERBOSE=1 - 'modified="$(git status --porcelain --untracked-files=no)"' - - 'if [ -n "$modified" ]; then echo "Run format.sh to reformat the code."; exit 1; fi' + - 'if [ -n "$modified" ]; then echo "Run scripts/format.sh to reformat the code."; exit 1; fi' git: depth: false # Disable shallow clone because it is incompatible with SonarCloud diff --git a/LICENSE b/LICENSE index 9c6ac2ed..2749d71b 100644 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,6 @@ The MIT License (MIT) -Copyright (c) 2015 UAVCAN +Copyright (c) 2016-2020 UAVCAN Development Team Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/format.sh b/format.sh deleted file mode 100755 index 4f02018c..00000000 --- a/format.sh +++ /dev/null @@ -1,20 +0,0 @@ -#!/usr/bin/env bash - -MIN_CLANG_FORMAT_VERSION=9 - -die() -{ - echo "$@" >&2 - exit 1 -} - -command -v clang-format > /dev/null || die "clang-format not found" -clang_format_version=$(clang-format --version | sed -ne 's/[^0-9]*\([0-9]*\)\..*/\1/p') -[ "$clang_format_version" -ge $MIN_CLANG_FORMAT_VERSION ] || \ - die "clang-format v$MIN_CLANG_FORMAT_VERSION+ required; found v$clang_format_version" - -all_source_files="libcanard/* $(find tests -name 'test_*.cpp')" -for i in $all_source_files; do echo "$i"; done - -# shellcheck disable=SC2086 -clang-format -i -fallback-style=none -style=file $all_source_files || die "clang-format has failed" diff --git a/.clang-tidy b/libcanard/.clang-tidy similarity index 71% rename from .clang-tidy rename to libcanard/.clang-tidy index 68a8b6e4..e04e1951 100644 --- a/.clang-tidy +++ b/libcanard/.clang-tidy @@ -13,10 +13,11 @@ Checks: >- performance-*, portability-*, readability-*, - -*-magic-numbers, -google-readability-todo, -WarningsAsErrors: true -HeaderFilterRegex: '' + -readability-avoid-const-params-in-decls, + -llvm-header-guard, +WarningsAsErrors: '*' +HeaderFilterRegex: '.*' AnalyzeTemporaryDtors: false FormatStyle: file ... diff --git a/libcanard/canard.c b/libcanard/canard.c index cc773a57..7f016345 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -1,53 +1,25 @@ -/* - * Copyright (c) 2016-2019 UAVCAN Team - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - * - * Contributors: https://github.com/UAVCAN/libcanard/contributors - */ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. #include "canard.h" -#include #include +#include // ---------------------------------------- BUILD CONFIGURATION ---------------------------------------- /// By default, this macro resolves to the standard assert(). The user can redefine this if necessary. +/// To disable assertion checks completely, make it expand into `(void)(0)`. #ifndef CANARD_ASSERT # define CANARD_ASSERT(x) assert(x) #endif /// This macro is needed only for testing and for library development. Do not redefine this in production. -#ifndef CANARD_INTERNAL +#if defined(CANARD_EXPOSE_INTERNALS) && CANARD_EXPOSE_INTERNALS +# define CANARD_INTERNAL +#else # define CANARD_INTERNAL static #endif -// ---------------------------------------- CONSTANTS ---------------------------------------- - -#define TRANSFER_CRC_INITIAL 0xFFFFU - -#define TRANSFER_ID_BIT_LEN 5U - -#define CAN_EXT_ID_MASK ((1UL << 29U) - 1U) - -// ---------------------------------------- INTERNAL DEFINITIONS ---------------------------------------- - #if !defined(__STDC_VERSION__) || (__STDC_VERSION__ < 199901L) # error "Unsupported language: ISO C99 or a newer version is required." #endif @@ -60,6 +32,16 @@ # define _static_assert_gl_impl(a, b) a##b // NOSONAR #endif +// ---------------------------------------- TRANSPORT ---------------------------------------- + +#define TRANSFER_CRC_INITIAL 0xFFFFU + +#define TRANSFER_ID_BIT_LEN 5U + +#define CAN_EXT_ID_MASK ((((uint32_t) 1U) << 29U) - 1U) + +#define BITS_PER_BYTE 8U + /// The fields are ordered to minimize padding on all platforms. typedef struct CanardInternalTxQueueItem { @@ -77,37 +59,156 @@ typedef struct CanardInternalRxSession struct CanardInternalRxSession* next; size_t payload_capacity; ///< Payload past this limit may be discarded by the library. - size_t payload_length; ///< How many bytes received so far. + size_t payload_size; ///< How many bytes received so far. uint8_t* payload; uint64_t timestamp_usec; ///< Time of last update of this session. Used for removal on timeout. uint32_t transfer_id_timeout_usec; ///< When (current time - update timestamp) exceeds this, it's dead. const uint32_t session_specifier; ///< Differentiates this session from other sessions. - uint16_t calculated_crc; ///< Updated with the received payload in real time. - uint8_t transfer_id; - bool next_toggle; + + uint16_t calculated_crc; ///< Updated with the received payload in real time. + uint8_t transfer_id; + bool next_toggle; } CanardInternalRxSession; -// ---------------------------------------- PRIVATE FUNCTIONS ---------------------------------------- +const uint8_t CanardCANDLCToLength[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64}; +const uint8_t CanardCANLengthToDLC[65] = { + 0, 1, 2, 3, 4, 5, 6, 7, 8, // 0 + 12, 12, 12, 12, // 9 + 16, 16, 16, 16, // 13 + 20, 20, 20, 20, // 17 + 24, 24, 24, 24, // 21 + 32, 32, 32, 32, 32, 32, 32, 32, // 25 + 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, // 33 + 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, // 49 +}; + +CANARD_INTERNAL uint16_t crcAddByte(const uint16_t crc, const uint8_t byte) +{ + uint16_t out = crc ^ (uint16_t)((uint16_t)(byte) << BITS_PER_BYTE); + for (uint8_t i = 0; i < BITS_PER_BYTE; i++) // Should we use a table instead? Adds 512 bytes of ROM. + { + // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. + out = ((out & 0x8000U) != 0U) ? ((uint16_t)(out << 1U) ^ 0x1021U) : (uint16_t)(out << 1U); // NOLINT + } + return crc; +} + +CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const uint8_t* const bytes, const size_t size) +{ + uint16_t out = crc; + const uint8_t* p = bytes; + for (size_t i = 0; i < size; i++) + { + out = crcAddByte(out, *p++); + } + return out; +} -CANARD_INTERNAL inline uint32_t makeMessageSessionSpecifier(const uint16_t subject_id, const uint8_t src_node_id) +CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const uint16_t subject_id, const uint8_t src_node_id) { + // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. CANARD_ASSERT(subject_id <= CANARD_SUBJECT_ID_MAX); CANARD_ASSERT(src_node_id <= CANARD_NODE_ID_MAX); - return ((uint32_t) src_node_id) | ((uint32_t) subject_id << 8U); + return ((uint32_t) src_node_id) | ((uint32_t) subject_id << 8U); // NOLINT } -CANARD_INTERNAL inline uint32_t makeServiceSessionSpecifier(const uint16_t service_id, - const bool request_not_response, - const uint8_t src_node_id, - const uint8_t dst_node_id) +CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const uint16_t service_id, + const bool request_not_response, + const uint8_t src_node_id, + const uint8_t dst_node_id) { CANARD_ASSERT(service_id <= CANARD_SERVICE_ID_MAX); CANARD_ASSERT(src_node_id <= CANARD_NODE_ID_MAX); CANARD_ASSERT(dst_node_id <= CANARD_NODE_ID_MAX); - return ((uint32_t) src_node_id) | ((uint32_t) dst_node_id << 7U) | ((uint32_t) service_id << 14U) | - (request_not_response ? (1U << 24U) : 0U); + // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. + return ((uint32_t) src_node_id) | ((uint32_t) dst_node_id << 7U) | ((uint32_t) service_id << 14U) | // NOLINT + (request_not_response ? (1U << 24U) : 0U); // NOLINT +} + +CanardInstance canardInit(const CanardHeapAllocate heap_allocate, + const CanardHeapFree heap_free, + const CanardRxFilter rx_filter) +{ + CANARD_ASSERT(heap_allocate != NULL); + CANARD_ASSERT(heap_free != NULL); + CANARD_ASSERT(rx_filter != NULL); + const CanardInstance out = { + .user_reference = NULL, + .node_id = CANARD_NODE_ID_UNSET, + .mtu_bytes = CANARD_MTU_CAN_FD, + .heap_allocate = heap_allocate, + .heap_free = heap_free, + .rx_filter = rx_filter, + ._rx_sessions = NULL, + ._tx_queue = NULL, + }; + return out; +} + +void canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfer) +{ + CANARD_ASSERT(ins != NULL); + CANARD_ASSERT(transfer != NULL); + (void) ins; + (void) transfer; + (void) crcAdd; + (void) makeMessageSessionSpecifier; + (void) makeServiceSessionSpecifier; } +// ---------------------------------------- FLOAT16 SERIALIZATION ---------------------------------------- + +typedef union +{ + uint32_t bits; + float real; +} Float32Bits; static_assert(sizeof(float) == 4, "Native float format shall match IEEE 754 binary32"); +static_assert(sizeof(Float32Bits) == 4, "Native float format shall match IEEE 754 binary32"); + +uint16_t canardDSDLFloat16Serialize(const float value) +{ + // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. + const uint32_t round_mask = ~(uint32_t) 0x0FFFU; // NOLINT + const Float32Bits f32inf = {.bits = ((uint32_t) 255U) << 23U}; // NOLINT + const Float32Bits f16inf = {.bits = ((uint32_t) 31U) << 23U}; // NOLINT + const Float32Bits magic = {.bits = ((uint32_t) 15U) << 23U}; // NOLINT + Float32Bits in = {.real = value}; + const uint32_t sign = in.bits & (((uint32_t) 1U) << 31U); // NOLINT + in.bits ^= sign; + uint16_t out = 0; + if (in.bits >= f32inf.bits) + { + out = (in.bits > f32inf.bits) ? (uint16_t) 0x7FFFU : (uint16_t) 0x7C00U; // NOLINT + } + else + { + in.bits &= round_mask; + in.real *= magic.real; + in.bits -= round_mask; + if (in.bits > f16inf.bits) + { + in.bits = f16inf.bits; + } + out = (uint16_t)(in.bits >> 13U); // NOLINT + } + out |= (uint16_t)(sign >> 16U); // NOLINT + return out; +} + +float canardDSDLFloat16Deserialize(const uint16_t value) +{ + // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. + const Float32Bits magic = {.bits = ((uint32_t) 0xEFU) << 23U}; // NOLINT + const Float32Bits inf_nan = {.bits = ((uint32_t) 0x8FU) << 23U}; // NOLINT + Float32Bits out = {.bits = ((uint32_t)(value & 0x7FFFU)) << 13U}; // NOLINT + out.real *= magic.real; + if (out.real >= inf_nan.real) + { + out.bits |= ((uint32_t) 0xFFU) << 23U; // NOLINT + } + out.bits |= ((uint32_t)(value & 0x8000U)) << 16U; // NOLINT + return out.real; +} diff --git a/libcanard/canard.h b/libcanard/canard.h index 41394700..8954ab21 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -1,37 +1,20 @@ -// Copyright (c) 2016-2020 UAVCAN Development Team -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to deal -// in the Software without restriction, including without limitation the rights -// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -// copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -// -// Contributors: https://github.com/UAVCAN/libcanard/contributors +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. +// Contributors: https://github.com/UAVCAN/libcanard/contributors. +// READ THE DOCUMENTATION IN README.md. #ifndef CANARD_H_INCLUDED #define CANARD_H_INCLUDED +#include #include #include -#include #ifdef __cplusplus extern "C" { #endif -/// Semantic version numbers of this library (not the UAVCAN specification). +/// Semantic version of this library (not the UAVCAN specification). /// API will be backward compatible within the same major version. #define CANARD_VERSION_MAJOR 1 @@ -67,7 +50,7 @@ typedef enum CanardPriorityImmediate = 1, CanardPriorityFast = 2, CanardPriorityHigh = 3, - CanardPriorityNominal = 4, ///< This should be the default. + CanardPriorityNominal = 4, ///< Nominal priority level should be the default. CanardPriorityLow = 5, CanardPrioritySlow = 6, CanardPriorityOptional = 7, @@ -90,13 +73,17 @@ typedef struct /// Zero timestamp indicates that the instance is invalid (empty). uint64_t timestamp_usec; - /// 29-bit extended ID. The bits above 29-th are ignored. - uint32_t id; + /// 29-bit extended ID. The bits above 29-th are zero/ignored. + uint32_t extended_can_id; /// The useful data in the frame. The length value is not to be confused with DLC! - uint8_t data_length; - uint8_t* data; -} CanardFrame; + uint8_t payload_size; + void* payload; +} CanardCANFrame; + +/// Conversion look-up tables between CAN DLC and data length. +extern const uint8_t CanardCANDLCToLength[16]; +extern const uint8_t CanardCANLengthToDLC[65]; typedef struct { @@ -117,8 +104,8 @@ typedef struct uint8_t transfer_id; - size_t payload_length; - uint8_t* payload; + size_t payload_size; + void* payload; } CanardTransfer; /// The application supplies the library with this information when a new transfer should be received. @@ -129,12 +116,12 @@ typedef struct /// Zero timeout indicates that this transfer should not be received (all its frames will be silently dropped). uint64_t transfer_id_timeout_usec; - /// The maximum payload size of the transfer (i.e., the maximum size of the serialized DSDL object). + /// The maximum payload size of the transfer (i.e., the maximum size of the serialized DSDL object), in bytes. /// Payloads larger than this will be silently truncated per the Implicit Truncation Rule defined in Specification. /// Per Specification, the transfer CRC of multi-frame transfers is always validated regardless of the /// implicit truncation rule. /// Zero is also a valid value indicating that the transfer shall be accepted but the payload need not be stored. - size_t payload_length_max; + size_t payload_size_max; } CanardRxMetadata; /// The application shall implement this function and supply a pointer to it to the library during initialization. @@ -192,27 +179,23 @@ struct CanardInstance /// Initialize a new library instance. /// The default values will be assigned as specified in the structure field documentation. +/// If any of the pointers are NULL, the behavior is undefined. CanardInstance canardInit(const CanardHeapAllocate heap_allocate, const CanardHeapFree heap_free, const CanardRxFilter rx_filter); -// ---------------------------------------- OUTGOING TRANSFERS ---------------------------------------- +void canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfer); -void canardTxPush(CanardInstance* const ins, const CanardTransfer* transfer); - -CanardFrame canardTxPeek(const CanardInstance* const ins); +CanardCANFrame canardTxPeek(const CanardInstance* const ins); void canardTxPop(CanardInstance* const ins); -// ---------------------------------------- INCOMING TRANSFERS ---------------------------------------- - -CanardTransfer canardRxAcceptFrame(CanardInstance* const ins, const CanardFrame* frame); - -// ---------------------------------------- DATA SERIALIZATION ---------------------------------------- +CanardTransfer canardRxPush(CanardInstance* const ins, const CanardCANFrame* const frame); -/// This function may be used to encode values for later transmission in a UAVCAN transfer. It encodes a scalar value -/// -- boolean, integer, character, or floating point -- and puts it at the specified bit offset in the specified -/// contiguous buffer. Simple payloads can also be encoded manually instead of using this function. +/// This function may be used to encode values for later transmission in a UAVCAN transfer. +/// It serializes a primitive value -- boolean, integer, character, or floating point -- and puts it at the +/// specified bit offset in the specified contiguous buffer. +/// Simple objects can also be serialized manually instead of using this function. /// /// Caveat: This function works correctly only on platforms that use two's complement signed integer representation. /// I am not aware of any modern microarchitecture that uses anything else than two's complement, so it should not @@ -274,8 +257,8 @@ void canardDSDLPrimitiveDeserialize(const void* const source, /// These functions convert between the native float and the standard IEEE 754 binary16 float (a.k.a. half precision). /// It is assumed that the native float is IEEE 754 binary32, otherwise, the results may be unpredictable. /// Majority of modern computers and microcontrollers use IEEE 754, so this limitation should not limit the portability. -uint16_t canardDSDLFloat16Serialize(float value); -float canardDSDLFloat16Deserialize(uint16_t value); +uint16_t canardDSDLFloat16Serialize(const float value); +float canardDSDLFloat16Deserialize(const uint16_t value); #ifdef __cplusplus } diff --git a/run_static_analysis.sh b/run_static_analysis.sh deleted file mode 100755 index 402b880d..00000000 --- a/run_static_analysis.sh +++ /dev/null @@ -1,17 +0,0 @@ -#!/usr/bin/env bash - -MIN_CLANG_TIDY_VERSION=9 - -die() -{ - echo "$@" >&2 - exit 1 -} - -command -v clang-tidy > /dev/null || die "clang-tidy not found" -clang_tidy_version=$(clang-tidy --version | sed -ne 's/[^0-9]*\([0-9]*\)\..*/\1/p') -[ "$clang_tidy_version" -ge $MIN_CLANG_TIDY_VERSION ] || \ - die "clang-tidy v$MIN_CLANG_TIDY_VERSION+ required; found v$clang_tidy_version" - -set -e -clang-tidy libcanard/canard.c diff --git a/tests/.clang-tidy b/tests/.clang-tidy new file mode 100644 index 00000000..aae3d865 --- /dev/null +++ b/tests/.clang-tidy @@ -0,0 +1,30 @@ +--- +# The tests are held to somewhat lower quality standards than production code. +# The tests are also written in somewhat non-idiomatic C++ because they are tightly related to the C codebase. +Checks: >- + boost-*, + bugprone-*, + cert-*, + clang-analyzer-*, + cppcoreguidelines-*, + google-*, + hicpp-*, + llvm-*, + misc-*, + modernize-*, + performance-*, + portability-*, + readability-*, + -google-readability-todo, + -readability-avoid-const-params-in-decls, + -llvm-header-guard, + -cppcoreguidelines-pro-type-reinterpret-cast, + -misc-non-private-member-variables-in-classes, + -cppcoreguidelines-pro-bounds-pointer-arithmetic, + -cppcoreguidelines-avoid-magic-numbers, + -readability-magic-numbers, +WarningsAsErrors: '*' +HeaderFilterRegex: 'internal.hpp' +AnalyzeTemporaryDtors: false +FormatStyle: file +... diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index caf963ec..dfdfaf2e 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,47 +1,79 @@ -# -# Copyright (c) 2016-2017 UAVCAN Team -# +# This software is distributed under the terms of the MIT License. +# Copyright (c) 2016-2020 UAVCAN Development Team. -cmake_minimum_required(VERSION 3.1) -project(libcanard) +cmake_minimum_required(VERSION 3.12) +project(canard_tests C CXX) +enable_testing() -# Catch library for unit testing -include_directories(catch) +if (CMAKE_BUILD_TYPE STREQUAL "RelWithDebInfo") + # assert() shall be disabled in release builds to enable testing of bad free() calls. + add_definitions(-DNDEBUG=1) +endif () -# Libcanard -set(LIBCANARD_DIR "${CMAKE_SOURCE_DIR}/../libcanard/") -include_directories(${LIBCANARD_DIR}) +set(library_dir "${CMAKE_SOURCE_DIR}/../libcanard") -# Compiler configuration - supporting only Clang and GCC -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall -Wextra -Werror -m32") -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c99 -Wall -Wextra -Werror -m32 -pedantic") +# Use -DNO_STATIC_ANALYSIS=1 to suppress static analysis. +# If not suppressed, the tools used here shall be available, otherwise the build will fail. +if (NOT NO_STATIC_ANALYSIS) + # clang-tidy (separate config files per directory) + find_program(clang_tidy NAMES clang-tidy-10 clang-tidy-9 clang-tidy) + if (NOT clang_tidy) + message(FATAL_ERROR "Could not locate clang-tidy") + endif () + message(STATUS "Using clang-tidy: ${clang_tidy}") + set(CMAKE_C_CLANG_TIDY ${clang_tidy}) + set(CMAKE_CXX_CLANG_TIDY ${clang_tidy}) -# C warnings + # clang-format + find_program(clang_format NAMES clang-format-10 clang-format-9 clang-format) + if (NOT clang_format) + message(FATAL_ERROR "Could not locate clang-format") + endif () + file(GLOB format_files + ${library_dir}/*.[ch] + ${CMAKE_SOURCE_DIR}/*.[ch]pp) + message(STATUS "Using clang-format: ${clang_format}; files: ${format_files}") + add_custom_target(format COMMAND ${clang_format} -i -fallback-style=none -style=file --verbose ${format_files}) +endif () + +# C options +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wextra -Werror -pedantic -fstrict-aliasing") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wdouble-promotion -Wswitch-enum -Wfloat-equal -Wundef") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wconversion -Wtype-limits") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wsign-conversion -Wcast-align -Wmissing-declarations") -# C++ warnings +# C++ options +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Werror -pedantic -fstrict-aliasing") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wdouble-promotion -Wswitch-enum -Wfloat-equal -Wundef") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wconversion -Wsign-promo") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wsign-conversion -Wcast-align -Wmissing-declarations") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wtype-limits -Wzero-as-null-pointer-constant -Wnon-virtual-dtor") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Woverloaded-virtual -Wsign-promo -Wold-style-cast") -# We allow the following warnings for compatibility with the C codebase: -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-error=old-style-cast -Wno-error=zero-as-null-pointer-constant") - -# Expose internal API for unit testing -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DCANARD_INTERNAL=''") -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DCANARD_INTERNAL=''") - -# Unit tests -file(GLOB tests_src - RELATIVE "${CMAKE_SOURCE_DIR}" - "*.cpp" - "catch/*.cpp") -message(STATUS "Unit test source files: ${tests_src}") -add_executable(run_tests - ${tests_src} - ${LIBCANARD_DIR}/canard.c) -target_link_libraries(run_tests - pthread) + +include_directories(catch ${library_dir}) + +set(common_sources catch/main.cpp ${library_dir}/canard.c) + +function(gen_test name files compile_definitions compile_features compile_flags link_flags) + add_executable(${name} ${common_sources} ${files}) + target_compile_definitions(${name} PUBLIC ${compile_definitions}) + target_compile_features(${name} PUBLIC ${compile_features}) + set_target_properties(${name} PROPERTIES COMPILE_FLAGS "${compile_flags}" LINK_FLAGS "${link_flags}") + add_test("run_${name}" "${name}" --rng-seed time) +endfunction() + +function(gen_test_matrix name files compile_definitions) + gen_test("${name}_c99_x64" "${files}" "${compile_definitions}" c_std_99 "-m64" "-m64") + gen_test("${name}_c99_x32" "${files}" "${compile_definitions}" c_std_99 "-m32" "-m32") + gen_test("${name}_c11_x64" "${files}" "${compile_definitions}" c_std_11 "-m64" "-m64") + gen_test("${name}_c11_x32" "${files}" "${compile_definitions}" c_std_11 "-m32" "-m32") + # Coverage is only available for GCC builds. + if ((CMAKE_CXX_COMPILER_ID STREQUAL "GNU") AND (CMAKE_BUILD_TYPE STREQUAL "Debug")) + gen_test("${name}_cov" "${files}" "${compile_definitions}" c_std_11 "-g -O0 --coverage" "--coverage") + endif () +endfunction() + +gen_test_matrix(test_internals test_internals.cpp CANARD_EXPOSE_INTERNALS=1) + +gen_test_matrix(test_general test_float16.cpp "") diff --git a/tests/catch/test_main.cpp b/tests/catch/main.cpp similarity index 100% rename from tests/catch/test_main.cpp rename to tests/catch/main.cpp diff --git a/tests/internals.hpp b/tests/internals.hpp new file mode 100644 index 00000000..753fb2f1 --- /dev/null +++ b/tests/internals.hpp @@ -0,0 +1,20 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. + +#pragma once + +#include "canard.h" +#include "catch.hpp" +#include + +namespace internals +{ +extern "C" { +std::uint32_t makeMessageSessionSpecifier(const std::uint16_t subject_id, const std::uint8_t src_node_id); +std::uint32_t makeServiceSessionSpecifier(const std::uint16_t service_id, + const bool request_not_response, + const std::uint8_t src_node_id, + const std::uint8_t dst_node_id); +} + +} // namespace internals diff --git a/tests/test_float16.cpp b/tests/test_float16.cpp new file mode 100644 index 00000000..3a674794 --- /dev/null +++ b/tests/test_float16.cpp @@ -0,0 +1,38 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. + +#include "internals.hpp" +#include + +TEST_CASE("canardDSDLFloat16Serialize") +{ + // Reference values were generated manually with libuavcan and numpy.float16(). + REQUIRE(0b0000000000000000 == canardDSDLFloat16Serialize(0.0F)); + REQUIRE(0b0011110000000000 == canardDSDLFloat16Serialize(1.0F)); + REQUIRE(0b1100000000000000 == canardDSDLFloat16Serialize(-2.0F)); + REQUIRE(0b0111110000000000 == canardDSDLFloat16Serialize(999999.0F)); // +inf + REQUIRE(0b1111101111111111 == canardDSDLFloat16Serialize(-65519.0F)); // -max + REQUIRE(0b0111111111111111 == canardDSDLFloat16Serialize(std::nanf(""))); // nan +} + +TEST_CASE("canardDSDLFloat16Deserialize") +{ + // Reference values were generated manually with libuavcan and numpy.float16(). + REQUIRE(Approx(0.0F) == canardDSDLFloat16Deserialize(0b0000000000000000)); + REQUIRE(Approx(1.0F) == canardDSDLFloat16Deserialize(0b0011110000000000)); + REQUIRE(Approx(-2.0F) == canardDSDLFloat16Deserialize(0b1100000000000000)); + REQUIRE(Approx(-65504.0F) == canardDSDLFloat16Deserialize(0b1111101111111111)); + REQUIRE(std::isinf(canardDSDLFloat16Deserialize(0b0111110000000000))); + + REQUIRE(bool(std::isnan(canardDSDLFloat16Deserialize(0b0111111111111111)))); +} + +TEST_CASE("canardDSDLFloat16") +{ + float x = -1000.0F; + while (x <= 1000.0F) + { + REQUIRE(Approx(x) == canardDSDLFloat16Deserialize(canardDSDLFloat16Serialize(x))); + x += 0.5F; + } +} diff --git a/tests/test_internals.cpp b/tests/test_internals.cpp new file mode 100644 index 00000000..599072e4 --- /dev/null +++ b/tests/test_internals.cpp @@ -0,0 +1,8 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. + +#include "internals.hpp" + +TEST_CASE("makeMessageSessionSpecifier") {} + +TEST_CASE("makeServiceSessionSpecifier") {} From 3c7188c1d7995c9cabc972ed721949ce1533500c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 4 Feb 2020 12:58:10 +0200 Subject: [PATCH 005/100] CI test fix --- libcanard/canard.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libcanard/canard.c b/libcanard/canard.c index 7f016345..4716bd95 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -84,6 +84,7 @@ const uint8_t CanardCANLengthToDLC[65] = { 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, // 49 }; +CANARD_INTERNAL uint16_t crcAddByte(const uint16_t crc, const uint8_t byte); CANARD_INTERNAL uint16_t crcAddByte(const uint16_t crc, const uint8_t byte) { uint16_t out = crc ^ (uint16_t)((uint16_t)(byte) << BITS_PER_BYTE); @@ -95,6 +96,7 @@ CANARD_INTERNAL uint16_t crcAddByte(const uint16_t crc, const uint8_t byte) return crc; } +CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const uint8_t* const bytes, const size_t size); CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const uint8_t* const bytes, const size_t size) { uint16_t out = crc; @@ -106,6 +108,7 @@ CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const uint8_t* const bytes, return out; } +CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const uint16_t subject_id, const uint8_t src_node_id); CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const uint16_t subject_id, const uint8_t src_node_id) { // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. @@ -114,6 +117,10 @@ CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const uint16_t subject_id, return ((uint32_t) src_node_id) | ((uint32_t) subject_id << 8U); // NOLINT } +CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const uint16_t service_id, + const bool request_not_response, + const uint8_t src_node_id, + const uint8_t dst_node_id); CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const uint16_t service_id, const bool request_not_response, const uint8_t src_node_id, From ab480af5375f9c514d5be98e2700ef19d683a4b4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 4 Feb 2020 13:39:58 +0200 Subject: [PATCH 006/100] Linter fixes --- libcanard/canard.c | 29 +++++++++++++++++++---------- libcanard/canard.h | 7 +++++-- 2 files changed, 24 insertions(+), 12 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 4716bd95..9c026234 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -10,7 +10,8 @@ /// By default, this macro resolves to the standard assert(). The user can redefine this if necessary. /// To disable assertion checks completely, make it expand into `(void)(0)`. #ifndef CANARD_ASSERT -# define CANARD_ASSERT(x) assert(x) +// Intentional violation of MISRA: assertion macro cannot be replaced with a function definition. +# define CANARD_ASSERT(x) assert(x) // NOSONAR #endif /// This macro is needed only for testing and for library development. Do not redefine this in production. @@ -50,7 +51,12 @@ typedef struct CanardInternalTxQueueItem uint32_t id; uint64_t deadline_usec; uint8_t data_length; - uint8_t data[]; + + // Intentional violation of MISRA: this flex array is the lesser of three evils. The other two are: + // - Use pointer, make it point to the remainder of the allocated memory following this structure. + // The pointer is bad because it requires us to use pointer arithmetics and adds sizeof(void*) of waste per item. + // - Use a separate memory allocation for data. This is terribly wasteful. + uint8_t data[]; // NOSONAR } CanardInternalTxQueueItem; /// The fields are ordered to minimize padding on all platforms. @@ -103,7 +109,8 @@ CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const uint8_t* const bytes, const uint8_t* p = bytes; for (size_t i = 0; i < size; i++) { - out = crcAddByte(out, *p++); + out = crcAddByte(out, *p); + ++p; } return out; } @@ -167,15 +174,17 @@ void canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfe // ---------------------------------------- FLOAT16 SERIALIZATION ---------------------------------------- -typedef union +// Intentional violation of MISRA: we need this union because the alternative is far more error prone. +// We have to rely on low-level data representation details to do the conversion; unions are helpful. +typedef union // NOSONAR { - uint32_t bits; - float real; + uint32_t bits; + CanardIEEE754Binary32 real; } Float32Bits; -static_assert(sizeof(float) == 4, "Native float format shall match IEEE 754 binary32"); -static_assert(sizeof(Float32Bits) == 4, "Native float format shall match IEEE 754 binary32"); +static_assert(4 == sizeof(CanardIEEE754Binary32), "Native float format shall match IEEE 754 binary32"); +static_assert(4 == sizeof(Float32Bits), "Native float format shall match IEEE 754 binary32"); -uint16_t canardDSDLFloat16Serialize(const float value) +uint16_t canardDSDLFloat16Serialize(const CanardIEEE754Binary32 value) { // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. const uint32_t round_mask = ~(uint32_t) 0x0FFFU; // NOLINT @@ -205,7 +214,7 @@ uint16_t canardDSDLFloat16Serialize(const float value) return out; } -float canardDSDLFloat16Deserialize(const uint16_t value) +CanardIEEE754Binary32 canardDSDLFloat16Deserialize(const uint16_t value) { // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. const Float32Bits magic = {.bits = ((uint32_t) 0xEFU) << 23U}; // NOLINT diff --git a/libcanard/canard.h b/libcanard/canard.h index 8954ab21..7cd6df7b 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -40,6 +40,9 @@ extern "C" { /// If not specified, the transfer-ID timeout will take this value for all new input sessions. #define CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC 2000000UL +/// It is assumed that the native float type follows the IEEE 754 binary32 representation. +typedef float CanardIEEE754Binary32; + // Forward declarations. typedef struct CanardInstance CanardInstance; @@ -257,8 +260,8 @@ void canardDSDLPrimitiveDeserialize(const void* const source, /// These functions convert between the native float and the standard IEEE 754 binary16 float (a.k.a. half precision). /// It is assumed that the native float is IEEE 754 binary32, otherwise, the results may be unpredictable. /// Majority of modern computers and microcontrollers use IEEE 754, so this limitation should not limit the portability. -uint16_t canardDSDLFloat16Serialize(const float value); -float canardDSDLFloat16Deserialize(const uint16_t value); +uint16_t canardDSDLFloat16Serialize(const CanardIEEE754Binary32 value); +CanardIEEE754Binary32 canardDSDLFloat16Deserialize(const uint16_t value); #ifdef __cplusplus } From 501955d4c41fe25bd50ce318b08ab80efbb35866 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 4 Feb 2020 14:51:35 +0200 Subject: [PATCH 007/100] Moving on --- libcanard/canard.c | 114 ++++++++++++++++++++++----------------- libcanard/canard.h | 10 ++-- tests/internals.hpp | 6 +++ tests/test_internals.cpp | 22 +++++++- 4 files changed, 94 insertions(+), 58 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 9c026234..685e8299 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -33,9 +33,7 @@ # define _static_assert_gl_impl(a, b) a##b // NOSONAR #endif -// ---------------------------------------- TRANSPORT ---------------------------------------- - -#define TRANSFER_CRC_INITIAL 0xFFFFU +// ---------------------------------------- COMMON CONSTANTS ---------------------------------------- #define TRANSFER_ID_BIT_LEN 5U @@ -43,52 +41,9 @@ #define BITS_PER_BYTE 8U -/// The fields are ordered to minimize padding on all platforms. -typedef struct CanardInternalTxQueueItem -{ - struct CanardInternalTxQueueItem* next; - - uint32_t id; - uint64_t deadline_usec; - uint8_t data_length; - - // Intentional violation of MISRA: this flex array is the lesser of three evils. The other two are: - // - Use pointer, make it point to the remainder of the allocated memory following this structure. - // The pointer is bad because it requires us to use pointer arithmetics and adds sizeof(void*) of waste per item. - // - Use a separate memory allocation for data. This is terribly wasteful. - uint8_t data[]; // NOSONAR -} CanardInternalTxQueueItem; - -/// The fields are ordered to minimize padding on all platforms. -typedef struct CanardInternalRxSession -{ - struct CanardInternalRxSession* next; - - size_t payload_capacity; ///< Payload past this limit may be discarded by the library. - size_t payload_size; ///< How many bytes received so far. - uint8_t* payload; +// ---------------------------------------- TRANSFER CRC ---------------------------------------- - uint64_t timestamp_usec; ///< Time of last update of this session. Used for removal on timeout. - uint32_t transfer_id_timeout_usec; ///< When (current time - update timestamp) exceeds this, it's dead. - - const uint32_t session_specifier; ///< Differentiates this session from other sessions. - - uint16_t calculated_crc; ///< Updated with the received payload in real time. - uint8_t transfer_id; - bool next_toggle; -} CanardInternalRxSession; - -const uint8_t CanardCANDLCToLength[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64}; -const uint8_t CanardCANLengthToDLC[65] = { - 0, 1, 2, 3, 4, 5, 6, 7, 8, // 0 - 12, 12, 12, 12, // 9 - 16, 16, 16, 16, // 13 - 20, 20, 20, 20, // 17 - 24, 24, 24, 24, // 21 - 32, 32, 32, 32, 32, 32, 32, 32, // 25 - 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, // 33 - 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, // 49 -}; +#define TRANSFER_CRC_INITIAL 0xFFFFU CANARD_INTERNAL uint16_t crcAddByte(const uint16_t crc, const uint8_t byte); CANARD_INTERNAL uint16_t crcAddByte(const uint16_t crc, const uint8_t byte) @@ -99,7 +54,7 @@ CANARD_INTERNAL uint16_t crcAddByte(const uint16_t crc, const uint8_t byte) // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. out = ((out & 0x8000U) != 0U) ? ((uint16_t)(out << 1U) ^ 0x1021U) : (uint16_t)(out << 1U); // NOLINT } - return crc; + return out; } CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const uint8_t* const bytes, const size_t size); @@ -115,6 +70,12 @@ CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const uint8_t* const bytes, return out; } +// ---------------------------------------- SESSION SPECIFIER ---------------------------------------- + +#define SERVICE_NOT_MESSAGE (((uint32_t) 1U) << 25U) +#define ANONYMOUS_MESSAGE (((uint32_t) 1U) << 24U) +#define REQUEST_NOT_RESPONSE (((uint32_t) 1U) << 24U) + CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const uint16_t subject_id, const uint8_t src_node_id); CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const uint16_t subject_id, const uint8_t src_node_id) { @@ -138,9 +99,62 @@ CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const uint16_t service_id, CANARD_ASSERT(dst_node_id <= CANARD_NODE_ID_MAX); // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. return ((uint32_t) src_node_id) | ((uint32_t) dst_node_id << 7U) | ((uint32_t) service_id << 14U) | // NOLINT - (request_not_response ? (1U << 24U) : 0U); // NOLINT + (request_not_response ? REQUEST_NOT_RESPONSE : 0U) | SERVICE_NOT_MESSAGE; } +// ---------------------------------------- TRANSMISSION ---------------------------------------- + +/// The fields are ordered to minimize padding on all platforms. +typedef struct CanardInternalTxQueueItem +{ + struct CanardInternalTxQueueItem* next; + + uint32_t id; + uint64_t deadline_usec; + uint8_t data_length; + + // Intentional violation of MISRA: this flex array is the lesser of three evils. The other two are: + // - Use pointer, make it point to the remainder of the allocated memory following this structure. + // The pointer is bad because it requires us to use pointer arithmetics and adds sizeof(void*) of waste per item. + // - Use a separate memory allocation for data. This is terribly wasteful. + uint8_t data[]; // NOSONAR +} CanardInternalTxQueueItem; + +// ---------------------------------------- RECEPTION ---------------------------------------- + +/// The fields are ordered to minimize padding on all platforms. +typedef struct CanardInternalRxSession +{ + struct CanardInternalRxSession* next; + + size_t payload_capacity; ///< Payload past this limit may be discarded by the library. + size_t payload_size; ///< How many bytes received so far. + uint8_t* payload; + + uint64_t timestamp_usec; ///< Time of last update of this session. Used for removal on timeout. + uint32_t transfer_id_timeout_usec; ///< When (current time - update timestamp) exceeds this, it's dead. + + const uint32_t session_specifier; ///< Differentiates this session from other sessions. + + uint16_t calculated_crc; ///< Updated with the received payload in real time. + uint8_t transfer_id; + bool next_toggle; +} CanardInternalRxSession; + +// ---------------------------------------- PUBLIC API ---------------------------------------- + +const uint8_t CanardCANDLCToLength[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64}; +const uint8_t CanardCANLengthToDLC[65] = { + 0, 1, 2, 3, 4, 5, 6, 7, 8, // 0 + 12, 12, 12, 12, // 9 + 16, 16, 16, 16, // 13 + 20, 20, 20, 20, // 17 + 24, 24, 24, 24, // 21 + 32, 32, 32, 32, 32, 32, 32, 32, // 25 + 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, // 33 + 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, // 49 +}; + CanardInstance canardInit(const CanardHeapAllocate heap_allocate, const CanardHeapFree heap_free, const CanardRxFilter rx_filter) diff --git a/libcanard/canard.h b/libcanard/canard.h index 7cd6df7b..cbe48315 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -40,9 +40,6 @@ extern "C" { /// If not specified, the transfer-ID timeout will take this value for all new input sessions. #define CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC 2000000UL -/// It is assumed that the native float type follows the IEEE 754 binary32 representation. -typedef float CanardIEEE754Binary32; - // Forward declarations. typedef struct CanardInstance CanardInstance; @@ -62,9 +59,9 @@ typedef enum /// Transfer kinds are defined by the UAVCAN specification. typedef enum { - CanardTransferKindMessagePublication = 0, ///< Multicast, from publisher to all subscribers. - CanardTransferKindServiceResponse = 1, ///< Point-to-point, from server to client. - CanardTransferKindServiceRequest = 2, ///< Point-to-point, from client to server. + CanardTransferKindMessage = 0, ///< Multicast, from publisher to all subscribers. + CanardTransferKindResponse = 1, ///< Point-to-point, from server to client. + CanardTransferKindRequest = 2, ///< Point-to-point, from client to server. } CanardTransferKind; /// CAN data frame with an extended 29-bit ID. RTR/Error frames are not used and therefore not modeled here. @@ -260,6 +257,7 @@ void canardDSDLPrimitiveDeserialize(const void* const source, /// These functions convert between the native float and the standard IEEE 754 binary16 float (a.k.a. half precision). /// It is assumed that the native float is IEEE 754 binary32, otherwise, the results may be unpredictable. /// Majority of modern computers and microcontrollers use IEEE 754, so this limitation should not limit the portability. +typedef float CanardIEEE754Binary32; uint16_t canardDSDLFloat16Serialize(const CanardIEEE754Binary32 value); CanardIEEE754Binary32 canardDSDLFloat16Deserialize(const uint16_t value); diff --git a/tests/internals.hpp b/tests/internals.hpp index 753fb2f1..8ed25ffa 100644 --- a/tests/internals.hpp +++ b/tests/internals.hpp @@ -5,16 +5,22 @@ #include "canard.h" #include "catch.hpp" +#include #include +/// Definitions that are not exposed by the library but that are needed for testing. +/// Please keep them in sync with the library by manually updating as necessary. namespace internals { +// Extern C effectively discards the outer namespaces. extern "C" { std::uint32_t makeMessageSessionSpecifier(const std::uint16_t subject_id, const std::uint8_t src_node_id); std::uint32_t makeServiceSessionSpecifier(const std::uint16_t service_id, const bool request_not_response, const std::uint8_t src_node_id, const std::uint8_t dst_node_id); + +std::uint16_t crcAdd(const std::uint16_t crc, const std::uint8_t* const bytes, const std::size_t size); } } // namespace internals diff --git a/tests/test_internals.cpp b/tests/test_internals.cpp index 599072e4..7d1718da 100644 --- a/tests/test_internals.cpp +++ b/tests/test_internals.cpp @@ -3,6 +3,24 @@ #include "internals.hpp" -TEST_CASE("makeMessageSessionSpecifier") {} +TEST_CASE("SessionSpecifier") +{ + REQUIRE(0b000'00'0011001100110011'0'1010101 == + internals::makeMessageSessionSpecifier(0b0011001100110011, 0b1010101)); + REQUIRE(0b000'11'0100110011'0101010'1010101 == + internals::makeServiceSessionSpecifier(0b0100110011, true, 0b1010101, 0b0101010)); + REQUIRE(0b000'10'0100110011'1010101'0101010 == + internals::makeServiceSessionSpecifier(0b0100110011, false, 0b0101010, 0b1010101)); +} -TEST_CASE("makeServiceSessionSpecifier") {} +TEST_CASE("TransferCRC") +{ + using internals::crcAdd; + std::uint16_t crc = 0xFFFFU; + crc = crcAdd(crc, reinterpret_cast("1"), 1); + crc = crcAdd(crc, reinterpret_cast("2"), 1); + crc = crcAdd(crc, reinterpret_cast("3"), 1); + REQUIRE(0x5BCEU == crc); // Using Libuavcan as reference + crc = crcAdd(crc, reinterpret_cast("456789"), 6); + REQUIRE(0x29B1U == crc); +} From 4cfa339e40d2b78c35502e9d45e357b2774f6ebd Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 5 Feb 2020 10:55:12 +0200 Subject: [PATCH 008/100] Compile-time platform assumption checks --- libcanard/canard.c | 20 ++++++++++++-------- libcanard/canard.h | 17 +++++++++++++++++ 2 files changed, 29 insertions(+), 8 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 685e8299..3c0a82c8 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -145,14 +145,14 @@ typedef struct CanardInternalRxSession const uint8_t CanardCANDLCToLength[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64}; const uint8_t CanardCANLengthToDLC[65] = { - 0, 1, 2, 3, 4, 5, 6, 7, 8, // 0 - 12, 12, 12, 12, // 9 - 16, 16, 16, 16, // 13 - 20, 20, 20, 20, // 17 - 24, 24, 24, 24, // 21 - 32, 32, 32, 32, 32, 32, 32, 32, // 25 - 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, 48, // 33 - 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, // 49 + 0, 1, 2, 3, 4, 5, 6, 7, 8, // 0-8 + 9, 9, 9, 9, // 9-12 + 10, 10, 10, 10, // 13-16 + 11, 11, 11, 11, // 17-20 + 12, 12, 12, 12, // 21-24 + 13, 13, 13, 13, 13, 13, 13, 13, // 25-32 + 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, 14, // 33-48 + 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, // 49-64 }; CanardInstance canardInit(const CanardHeapAllocate heap_allocate, @@ -188,6 +188,8 @@ void canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfe // ---------------------------------------- FLOAT16 SERIALIZATION ---------------------------------------- +#if CANARD_PLATFORM_IEEE754 + // Intentional violation of MISRA: we need this union because the alternative is far more error prone. // We have to rely on low-level data representation details to do the conversion; unions are helpful. typedef union // NOSONAR @@ -242,3 +244,5 @@ CanardIEEE754Binary32 canardDSDLFloat16Deserialize(const uint16_t value) out.bits |= ((uint32_t)(value & 0x8000U)) << 16U; // NOLINT return out.real; } + +#endif // CANARD_PLATFORM_IEEE754 diff --git a/libcanard/canard.h b/libcanard/canard.h index cbe48315..5bbb76c9 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -6,6 +6,7 @@ #ifndef CANARD_H_INCLUDED #define CANARD_H_INCLUDED +#include #include #include #include @@ -40,6 +41,14 @@ extern "C" { /// If not specified, the transfer-ID timeout will take this value for all new input sessions. #define CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC 2000000UL +/// Auto-detected properties of the target platform. +#define CANARD_PLATFORM_TWOS_COMPLEMENT \ + ((INT8_MIN == -128) && (INT8_MAX == 127) && (INT16_MIN == -32768) && (INT16_MAX == 32767) && \ + (INT32_MIN == -0x80000000LL) && (INT32_MAX == 0x7FFFFFFFLL) && (INT64_MAX == 0x7FFFFFFFFFFFFFFFLL)) +#define CANARD_PLATFORM_IEEE754 \ + ((FLT_RADIX == 2) && (FLT_MANT_DIG == 24) && (FLT_HAS_SUBNORM == 1) && (FLT_MIN_EXP == -125) && \ + (FLT_MAX_EXP == 128)) + // Forward declarations. typedef struct CanardInstance CanardInstance; @@ -192,6 +201,8 @@ void canardTxPop(CanardInstance* const ins); CanardTransfer canardRxPush(CanardInstance* const ins, const CanardCANFrame* const frame); +#if CANARD_PLATFORM_TWOS_COMPLEMENT + /// This function may be used to encode values for later transmission in a UAVCAN transfer. /// It serializes a primitive value -- boolean, integer, character, or floating point -- and puts it at the /// specified bit offset in the specified contiguous buffer. @@ -253,6 +264,10 @@ void canardDSDLPrimitiveDeserialize(const void* const source, const bool is_signed, void* const out_value); +#endif // CANARD_PLATFORM_TWOS_COMPLEMENT + +#if CANARD_PLATFORM_IEEE754 + /// IEEE 754 binary16 marshaling helpers. /// These functions convert between the native float and the standard IEEE 754 binary16 float (a.k.a. half precision). /// It is assumed that the native float is IEEE 754 binary32, otherwise, the results may be unpredictable. @@ -261,6 +276,8 @@ typedef float CanardIEEE754Binary32; uint16_t canardDSDLFloat16Serialize(const CanardIEEE754Binary32 value); CanardIEEE754Binary32 canardDSDLFloat16Deserialize(const uint16_t value); +#endif // CANARD_PLATFORM_IEEE754 + #ifdef __cplusplus } #endif From bcca48493514bc87e4795a2db5db113c5d275e29 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 5 Feb 2020 14:58:41 +0200 Subject: [PATCH 009/100] Transmission WIP --- libcanard/canard.c | 350 +++++++++++++++++++++++++++++++++++---- libcanard/canard.h | 3 + tests/internals.hpp | 2 +- tests/test_internals.cpp | 11 +- 4 files changed, 332 insertions(+), 34 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 3c0a82c8..24ee7c58 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -18,7 +18,7 @@ #if defined(CANARD_EXPOSE_INTERNALS) && CANARD_EXPOSE_INTERNALS # define CANARD_INTERNAL #else -# define CANARD_INTERNAL static +# define CANARD_INTERNAL static inline #endif #if !defined(__STDC_VERSION__) || (__STDC_VERSION__ < 199901L) @@ -35,15 +35,21 @@ // ---------------------------------------- COMMON CONSTANTS ---------------------------------------- -#define TRANSFER_ID_BIT_LEN 5U +#define TAIL_START_OF_TRANSFER 128U +#define TAIL_END_OF_TRANSFER 64U +#define TAIL_TOGGLE 32U -#define CAN_EXT_ID_MASK ((((uint32_t) 1U) << 29U) - 1U) +#define CAN_EXT_ID_MASK ((UINT32_C(1) << 29U) - 1U) #define BITS_PER_BYTE 8U +#define BYTE_MAX 0xFFU + +#define PADDING_BYTE 0U // ---------------------------------------- TRANSFER CRC ---------------------------------------- -#define TRANSFER_CRC_INITIAL 0xFFFFU +#define CRC_INITIAL 0xFFFFU +#define CRC_SIZE_BYTES 2U CANARD_INTERNAL uint16_t crcAddByte(const uint16_t crc, const uint8_t byte); CANARD_INTERNAL uint16_t crcAddByte(const uint16_t crc, const uint8_t byte) @@ -57,11 +63,11 @@ CANARD_INTERNAL uint16_t crcAddByte(const uint16_t crc, const uint8_t byte) return out; } -CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const uint8_t* const bytes, const size_t size); -CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const uint8_t* const bytes, const size_t size) +CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const size_t size, const void* const data); +CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const size_t size, const void* const data) { uint16_t out = crc; - const uint8_t* p = bytes; + const uint8_t* p = (const uint8_t*) data; for (size_t i = 0; i < size; i++) { out = crcAddByte(out, *p); @@ -72,17 +78,20 @@ CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const uint8_t* const bytes, // ---------------------------------------- SESSION SPECIFIER ---------------------------------------- -#define SERVICE_NOT_MESSAGE (((uint32_t) 1U) << 25U) -#define ANONYMOUS_MESSAGE (((uint32_t) 1U) << 24U) -#define REQUEST_NOT_RESPONSE (((uint32_t) 1U) << 24U) +#define OFFSET_PRIORITY 26U +#define OFFSET_SUBJECT_ID 8U +#define OFFSET_SERVICE_ID 14U +#define OFFSET_DST_NODE_ID 7U + +#define SERVICE_NOT_MESSAGE (UINT32_C(1) << 25U) +#define ANONYMOUS_MESSAGE (UINT32_C(1) << 24U) +#define REQUEST_NOT_RESPONSE (UINT32_C(1) << 24U) CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const uint16_t subject_id, const uint8_t src_node_id); CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const uint16_t subject_id, const uint8_t src_node_id) { - // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. - CANARD_ASSERT(subject_id <= CANARD_SUBJECT_ID_MAX); - CANARD_ASSERT(src_node_id <= CANARD_NODE_ID_MAX); - return ((uint32_t) src_node_id) | ((uint32_t) subject_id << 8U); // NOLINT + return ((uint32_t)(src_node_id & CANARD_NODE_ID_MAX)) | + ((uint32_t)(subject_id & CANARD_SUBJECT_ID_MAX) << OFFSET_SUBJECT_ID); } CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const uint16_t service_id, @@ -94,11 +103,9 @@ CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const uint16_t service_id, const uint8_t src_node_id, const uint8_t dst_node_id) { - CANARD_ASSERT(service_id <= CANARD_SERVICE_ID_MAX); - CANARD_ASSERT(src_node_id <= CANARD_NODE_ID_MAX); - CANARD_ASSERT(dst_node_id <= CANARD_NODE_ID_MAX); - // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. - return ((uint32_t) src_node_id) | ((uint32_t) dst_node_id << 7U) | ((uint32_t) service_id << 14U) | // NOLINT + return ((uint32_t)(src_node_id & CANARD_NODE_ID_MAX)) | + ((uint32_t)(dst_node_id & CANARD_NODE_ID_MAX) << OFFSET_DST_NODE_ID) | + ((uint32_t)(service_id & CANARD_SERVICE_ID_MAX) << OFFSET_SERVICE_ID) | (request_not_response ? REQUEST_NOT_RESPONSE : 0U) | SERVICE_NOT_MESSAGE; } @@ -111,15 +118,278 @@ typedef struct CanardInternalTxQueueItem uint32_t id; uint64_t deadline_usec; - uint8_t data_length; + uint8_t payload_size; // Intentional violation of MISRA: this flex array is the lesser of three evils. The other two are: // - Use pointer, make it point to the remainder of the allocated memory following this structure. // The pointer is bad because it requires us to use pointer arithmetics and adds sizeof(void*) of waste per item. // - Use a separate memory allocation for data. This is terribly wasteful. - uint8_t data[]; // NOSONAR + uint8_t payload[]; // NOSONAR } CanardInternalTxQueueItem; +CANARD_INTERNAL uint8_t getPresentationLayerMTU(const CanardInstance* const ins); +CANARD_INTERNAL uint8_t getPresentationLayerMTU(const CanardInstance* const ins) +{ + uint8_t out = 0U; + if (ins->mtu_bytes < CANARD_MTU_MIN) + { + out = CANARD_MTU_MIN - 1U; + } + else if (ins->mtu_bytes > CANARD_MTU_MAX) + { + out = CANARD_MTU_MAX - 1U; + } + else + { + out = ins->mtu_bytes - 1U; + } + return out; +} + +/// Returns a value above CAN_EXT_ID_MASK to indicate failure. +CANARD_INTERNAL uint32_t makeCANID(const CanardTransfer* const transfer, + const uint8_t local_node_id, + const uint8_t presentation_layer_mtu); +CANARD_INTERNAL uint32_t makeCANID(const CanardTransfer* const transfer, + const uint8_t local_node_id, + const uint8_t presentation_layer_mtu) +{ + uint32_t out = UINT32_MAX; + if (transfer->transfer_kind == CanardTransferKindMessage) + { + if (local_node_id <= CANARD_NODE_ID_MAX) + { + out = makeMessageSessionSpecifier(transfer->port_id, local_node_id); + } + else if (transfer->payload_size <= presentation_layer_mtu) + { + const uint8_t c = (uint8_t) crcAdd(CRC_INITIAL, transfer->payload_size, transfer->payload); + out = makeMessageSessionSpecifier(transfer->port_id, c) | ANONYMOUS_MESSAGE; + } + else + { + CANARD_ASSERT(false); // Anonymous multi-frame message transfers are not allowed. + } + } + else if ((transfer->transfer_kind == CanardTransferKindRequest) || + (transfer->transfer_kind == CanardTransferKindResponse)) + { + if (local_node_id <= CANARD_NODE_ID_MAX) + { + out = makeServiceSessionSpecifier(transfer->port_id, + transfer->transfer_kind == CanardTransferKindRequest, + local_node_id, + transfer->remote_node_id); + } + else + { + CANARD_ASSERT(false); // Anonymous service transfers are not allowed. + } + } + else + { + CANARD_ASSERT(false); // Invalid transfer kind. + } + + if (out < UINT32_MAX) + { + out |= ((uint32_t) transfer->priority & CANARD_PRIORITY_MAX) << OFFSET_PRIORITY; + CANARD_ASSERT(out <= CAN_EXT_ID_MASK); + } + return out; +} + +CANARD_INTERNAL uint8_t makeTailByte(const bool start_of_transfer, + const bool end_of_transfer, + const bool toggle, + const uint8_t transfer_id); +CANARD_INTERNAL uint8_t makeTailByte(const bool start_of_transfer, + const bool end_of_transfer, + const bool toggle, + const uint8_t transfer_id) +{ + return (uint8_t)((start_of_transfer ? TAIL_START_OF_TRANSFER : 0U) | (end_of_transfer ? TAIL_END_OF_TRANSFER : 0U) | + (toggle ? TAIL_TOGGLE : 0U) | (transfer_id & CANARD_TRANSFER_ID_MAX)); +} + +CANARD_INTERNAL CanardInternalTxQueueItem* allocateTxQueueItem(CanardInstance* const ins, + const uint32_t id, + const uint64_t deadline_usec, + const uint8_t payload_size); +CANARD_INTERNAL CanardInternalTxQueueItem* allocateTxQueueItem(CanardInstance* const ins, + const uint32_t id, + const uint64_t deadline_usec, + const uint8_t payload_size) +{ + CANARD_ASSERT(ins != NULL); + CANARD_ASSERT(id <= CAN_EXT_ID_MASK); + CANARD_ASSERT(payload_size > 0U); // UAVCAN/CAN doesn't allow zero-payload frames. + CanardInternalTxQueueItem* const out = + (CanardInternalTxQueueItem*) ins->heap_allocate(ins, sizeof(CanardInternalTxQueueItem) + payload_size); + if (out != NULL) + { + out->next = NULL; + out->id = id; + out->deadline_usec = deadline_usec; + out->payload_size = payload_size; + } + return out; +} + +/// Returns the element after which new elements with the specified CAN ID should be inserted. +/// Returns NULL if the element shall be inserted in the beginning of the list (i.e., no prior elements). +CANARD_INTERNAL CanardInternalTxQueueItem* findTxQueueSupremum(CanardInstance* const ins, const uint32_t can_id); +CANARD_INTERNAL CanardInternalTxQueueItem* findTxQueueSupremum(CanardInstance* const ins, const uint32_t can_id) +{ + CANARD_ASSERT(ins != NULL); + CANARD_ASSERT(can_id <= CAN_EXT_ID_MASK); + CanardInternalTxQueueItem* out = ins->_tx_queue; // The linear search should be replaced with O(log n) at least. + // TODO: INCOMPLETE. + if ((out != NULL) && (out->id <= can_id)) + { + while ((out != NULL) && (out->next != NULL) && (out->next->id <= can_id)) + { + out = out->next; + } + } + CANARD_ASSERT((out == NULL) || (out->id <= can_id)); + return out; +} + +CANARD_INTERNAL void pushSingleFrameTransfer(CanardInstance* const ins, + const uint64_t deadline_usec, + const uint32_t can_id, + const uint8_t transfer_id, + const size_t payload_size, + const void* const payload); +CANARD_INTERNAL void pushSingleFrameTransfer(CanardInstance* const ins, + const uint64_t deadline_usec, + const uint32_t can_id, + const uint8_t transfer_id, + const size_t payload_size, + const void* const payload) +{ + CANARD_ASSERT(ins != NULL); + CANARD_ASSERT(can_id <= CAN_EXT_ID_MASK); + CANARD_ASSERT((payload_size == 0U) || (payload != NULL)); + CanardInternalTxQueueItem* const tqi = + allocateTxQueueItem(ins, can_id, deadline_usec, (uint8_t)(payload_size + 1U)); + if (tqi != NULL) + { + (void) memmove(&tqi->payload[0], payload, payload_size); + tqi->payload[payload_size] = makeTailByte(true, true, true, transfer_id); + CanardInternalTxQueueItem* sup = findTxQueueSupremum(ins, can_id); + if (sup != NULL) + { + tqi->next = sup->next; + sup->next = tqi; + } + else + { + tqi->next = ins->_tx_queue; + ins->_tx_queue = tqi; + } + } +} + +CANARD_INTERNAL void pushMultiFrameTransfer(CanardInstance* const ins, + const uint8_t presentation_layer_mtu, + const uint64_t deadline_usec, + const uint32_t can_id, + const uint8_t transfer_id, + const size_t payload_size, + const void* const payload); +CANARD_INTERNAL void pushMultiFrameTransfer(CanardInstance* const ins, + const uint8_t presentation_layer_mtu, + const uint64_t deadline_usec, + const uint32_t can_id, + const uint8_t transfer_id, + const size_t payload_size, + const void* const payload) +{ + CANARD_ASSERT(ins != NULL); + CANARD_ASSERT(presentation_layer_mtu > 1U); + CANARD_ASSERT(can_id <= CAN_EXT_ID_MASK); + CANARD_ASSERT(payload_size > presentation_layer_mtu); + CANARD_ASSERT(payload != NULL); + + // TODO: QUEUE PRE-ALLOCATION. + + const size_t payload_size_with_crc = payload_size + CRC_SIZE_BYTES; + size_t offset = 0U; + uint16_t crc = crcAdd(CRC_INITIAL, payload_size, payload); + bool start_of_transfer = true; + bool toggle = true; + const uint8_t* payload_ptr = (const uint8_t*) payload; + + while (offset < payload_size_with_crc) + { + // Compute the required size of the frame payload for this frame, including CRC and padding. + uint8_t frame_payload_size_with_tail = (uint8_t)(presentation_layer_mtu + 1U); + { + const size_t remaining_payload_with_crc = payload_size_with_crc - offset; + if (remaining_payload_with_crc < presentation_layer_mtu) + { + const size_t index = remaining_payload_with_crc + 1U; + CANARD_ASSERT(index < sizeof(CanardCANLengthToDLC)); + // Round up to accommodate padding. + frame_payload_size_with_tail = CanardCANDLCToLength[CanardCANLengthToDLC[index]]; + } + } + + // Allocate the storage using the above computed size. + CanardInternalTxQueueItem* const tqi = + allocateTxQueueItem(ins, can_id, deadline_usec, frame_payload_size_with_tail); + if (tqi == NULL) + { + break; + } + + // Copy the payload into the frame. + const uint8_t frame_payload_size = (uint8_t)(frame_payload_size_with_tail - 1U); + uint8_t frame_offset = 0U; + while ((offset < payload_size) && (frame_offset < frame_payload_size)) + { + tqi->payload[frame_offset] = *payload_ptr; + ++frame_offset; + ++offset; + ++payload_ptr; + } + + // Handle the last frame of the transfer: it is special because it also contains padding and CRC. + const bool end_of_transfer = offset >= payload_size; + if (end_of_transfer) + { + // Insert padding -- only in the last frame. Don't forget to include padding into the CRC. + while ((frame_offset + CRC_SIZE_BYTES) < frame_payload_size) + { + tqi->payload[frame_offset] = PADDING_BYTE; + ++frame_offset; + crc = crcAddByte(crc, PADDING_BYTE); + } + + // Insert the CRC. + if ((frame_offset < frame_payload_size) && (offset == payload_size)) + { + tqi->payload[frame_offset] = (uint8_t)(crc & BYTE_MAX); + ++frame_offset; + } + if ((frame_offset < frame_payload_size) && (offset > payload_size)) + { + tqi->payload[frame_offset] = (uint8_t)(crc >> BITS_PER_BYTE); + ++frame_offset; + } + } + + // Finalize the frame. + tqi->payload[frame_offset] = makeTailByte(start_of_transfer, end_of_transfer, toggle, transfer_id); + start_of_transfer = false; + toggle = !toggle; + } + + // TODO: INSERTION. +} + // ---------------------------------------- RECEPTION ---------------------------------------- /// The fields are ordered to minimize padding on all platforms. @@ -177,13 +447,37 @@ CanardInstance canardInit(const CanardHeapAllocate heap_allocate, void canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfer) { - CANARD_ASSERT(ins != NULL); - CANARD_ASSERT(transfer != NULL); - (void) ins; - (void) transfer; - (void) crcAdd; - (void) makeMessageSessionSpecifier; - (void) makeServiceSessionSpecifier; + if ((ins != NULL) && (transfer != NULL)) + { + const uint8_t pl_mtu = getPresentationLayerMTU(ins); + const uint32_t can_id = makeCANID(transfer, ins->node_id, pl_mtu); + if (can_id <= CAN_EXT_ID_MASK) + { + if (transfer->payload_size <= pl_mtu) + { + pushSingleFrameTransfer(ins, + transfer->timestamp_usec, + can_id, + transfer->transfer_id, + transfer->payload_size, + transfer->payload); + } + else + { + pushMultiFrameTransfer(ins, + pl_mtu, + transfer->timestamp_usec, + can_id, + transfer->transfer_id, + transfer->payload_size, + transfer->payload); + } + } + } + else + { + CANARD_ASSERT(false); + } } // ---------------------------------------- FLOAT16 SERIALIZATION ---------------------------------------- diff --git a/libcanard/canard.h b/libcanard/canard.h index 5bbb76c9..252f7963 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -26,11 +26,14 @@ extern "C" { /// Per the recommendations given in the UAVCAN specification, other MTU values should not be used. #define CANARD_MTU_CAN_CLASSIC 8U #define CANARD_MTU_CAN_FD 64U +#define CANARD_MTU_MIN 8U +#define CANARD_MTU_MAX 64U /// Parameter ranges are inclusive; the lower bound is zero for all. Refer to the specification for more info. #define CANARD_SUBJECT_ID_MAX 32767U #define CANARD_SERVICE_ID_MAX 511U #define CANARD_NODE_ID_MAX 127U +#define CANARD_PRIORITY_MAX 7U #define CANARD_TRANSFER_ID_BIT_LENGTH 5U #define CANARD_TRANSFER_ID_MAX ((1U << CANARD_TRANSFER_ID_BIT_LENGTH) - 1U) diff --git a/tests/internals.hpp b/tests/internals.hpp index 8ed25ffa..d552f624 100644 --- a/tests/internals.hpp +++ b/tests/internals.hpp @@ -20,7 +20,7 @@ std::uint32_t makeServiceSessionSpecifier(const std::uint16_t service_id, const std::uint8_t src_node_id, const std::uint8_t dst_node_id); -std::uint16_t crcAdd(const std::uint16_t crc, const std::uint8_t* const bytes, const std::size_t size); +std::uint16_t crcAdd(const std::uint16_t crc, const std::size_t size, const void* const bytes); } } // namespace internals diff --git a/tests/test_internals.cpp b/tests/test_internals.cpp index 7d1718da..ee906fea 100644 --- a/tests/test_internals.cpp +++ b/tests/test_internals.cpp @@ -17,10 +17,11 @@ TEST_CASE("TransferCRC") { using internals::crcAdd; std::uint16_t crc = 0xFFFFU; - crc = crcAdd(crc, reinterpret_cast("1"), 1); - crc = crcAdd(crc, reinterpret_cast("2"), 1); - crc = crcAdd(crc, reinterpret_cast("3"), 1); - REQUIRE(0x5BCEU == crc); // Using Libuavcan as reference - crc = crcAdd(crc, reinterpret_cast("456789"), 6); + + crc = crcAdd(crc, 1, "1"); + crc = crcAdd(crc, 1, "2"); + crc = crcAdd(crc, 1, "3"); + REQUIRE(0x5BCEU == crc); // Using Libuavcan as reference + crc = crcAdd(crc, 6, "456789"); REQUIRE(0x29B1U == crc); } From 0989c310d55c7b4c2a5c4e14f49b96d6c3897785 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 5 Feb 2020 17:01:16 +0200 Subject: [PATCH 010/100] Should be ready for testing now --- libcanard/canard.c | 105 +++++++++++++++++++++++++++++++++------------ libcanard/canard.h | 13 +++++- 2 files changed, 89 insertions(+), 29 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 24ee7c58..7457936e 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -127,26 +127,27 @@ typedef struct CanardInternalTxQueueItem uint8_t payload[]; // NOSONAR } CanardInternalTxQueueItem; +/// This is the transport MTU minus the tail byte. CANARD_INTERNAL uint8_t getPresentationLayerMTU(const CanardInstance* const ins); CANARD_INTERNAL uint8_t getPresentationLayerMTU(const CanardInstance* const ins) { uint8_t out = 0U; if (ins->mtu_bytes < CANARD_MTU_MIN) { - out = CANARD_MTU_MIN - 1U; + out = (uint8_t)(CANARD_MTU_MIN - 1U); } else if (ins->mtu_bytes > CANARD_MTU_MAX) { - out = CANARD_MTU_MAX - 1U; + out = (uint8_t)(CANARD_MTU_MAX - 1U); } else { - out = ins->mtu_bytes - 1U; + out = (uint8_t)(ins->mtu_bytes - 1U); } return out; } -/// Returns a value above CAN_EXT_ID_MASK to indicate failure. +/// Returns an invalid value above CAN_EXT_ID_MASK to indicate failure. CANARD_INTERNAL uint32_t makeCANID(const CanardTransfer* const transfer, const uint8_t local_node_id, const uint8_t presentation_layer_mtu); @@ -243,10 +244,14 @@ CANARD_INTERNAL CanardInternalTxQueueItem* findTxQueueSupremum(CanardInstance* c { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(can_id <= CAN_EXT_ID_MASK); - CanardInternalTxQueueItem* out = ins->_tx_queue; // The linear search should be replaced with O(log n) at least. - // TODO: INCOMPLETE. - if ((out != NULL) && (out->id <= can_id)) + CanardInternalTxQueueItem* out = ins->_tx_queue; + if ((out == NULL) || (out->id > can_id)) { + out = NULL; + } + else + { + // The linear search should be replaced with O(log n) at least. Please help us here. while ((out != NULL) && (out->next != NULL) && (out->next->id <= can_id)) { out = out->next; @@ -276,9 +281,11 @@ CANARD_INTERNAL void pushSingleFrameTransfer(CanardInstance* const ins, allocateTxQueueItem(ins, can_id, deadline_usec, (uint8_t)(payload_size + 1U)); if (tqi != NULL) { - (void) memmove(&tqi->payload[0], payload, payload_size); - tqi->payload[payload_size] = makeTailByte(true, true, true, transfer_id); - CanardInternalTxQueueItem* sup = findTxQueueSupremum(ins, can_id); + // Clang-Tidy raises an error recommending the use of memcpy_s() instead. + // We ignore this recommendation because it is not available in C99. + (void) memcpy(&tqi->payload[0], payload, payload_size); // NOLINT + tqi->payload[payload_size] = makeTailByte(true, true, true, transfer_id); + CanardInternalTxQueueItem* const sup = findTxQueueSupremum(ins, can_id); if (sup != NULL) { tqi->next = sup->next; @@ -313,7 +320,8 @@ CANARD_INTERNAL void pushMultiFrameTransfer(CanardInstance* const ins, CANARD_ASSERT(payload_size > presentation_layer_mtu); CANARD_ASSERT(payload != NULL); - // TODO: QUEUE PRE-ALLOCATION. + CanardInternalTxQueueItem* head = NULL; // Head and tail of the linked list of frames of this transfer. + CanardInternalTxQueueItem* tail = NULL; const size_t payload_size_with_crc = payload_size + CRC_SIZE_BYTES; size_t offset = 0U; @@ -337,10 +345,21 @@ CANARD_INTERNAL void pushMultiFrameTransfer(CanardInstance* const ins, } } - // Allocate the storage using the above computed size. - CanardInternalTxQueueItem* const tqi = - allocateTxQueueItem(ins, can_id, deadline_usec, frame_payload_size_with_tail); - if (tqi == NULL) + // Allocate the storage using the above computed size. Link all frames into a list. + { + CanardInternalTxQueueItem* const tqi = + allocateTxQueueItem(ins, can_id, deadline_usec, frame_payload_size_with_tail); + if (head == NULL) + { + head = tqi; + } + else + { + tail->next = tqi; + } + tail = tqi; + } + if (tail == NULL) { break; } @@ -348,12 +367,19 @@ CANARD_INTERNAL void pushMultiFrameTransfer(CanardInstance* const ins, // Copy the payload into the frame. const uint8_t frame_payload_size = (uint8_t)(frame_payload_size_with_tail - 1U); uint8_t frame_offset = 0U; - while ((offset < payload_size) && (frame_offset < frame_payload_size)) + if (offset < payload_size) { - tqi->payload[frame_offset] = *payload_ptr; - ++frame_offset; - ++offset; - ++payload_ptr; + size_t move_size = payload_size - offset; + if (move_size > frame_payload_size) + { + move_size = frame_payload_size; + } + // Clang-Tidy raises an error recommending the use of memcpy_s() instead. + // We ignore this recommendation because it is not available in C99. + (void) memcpy(&tail->payload[0], payload_ptr, move_size); // NOLINT + frame_offset = (uint8_t)(frame_offset + move_size); + offset += move_size; + payload_ptr += move_size; } // Handle the last frame of the transfer: it is special because it also contains padding and CRC. @@ -363,7 +389,7 @@ CANARD_INTERNAL void pushMultiFrameTransfer(CanardInstance* const ins, // Insert padding -- only in the last frame. Don't forget to include padding into the CRC. while ((frame_offset + CRC_SIZE_BYTES) < frame_payload_size) { - tqi->payload[frame_offset] = PADDING_BYTE; + tail->payload[frame_offset] = PADDING_BYTE; ++frame_offset; crc = crcAddByte(crc, PADDING_BYTE); } @@ -371,23 +397,48 @@ CANARD_INTERNAL void pushMultiFrameTransfer(CanardInstance* const ins, // Insert the CRC. if ((frame_offset < frame_payload_size) && (offset == payload_size)) { - tqi->payload[frame_offset] = (uint8_t)(crc & BYTE_MAX); + tail->payload[frame_offset] = (uint8_t)(crc & BYTE_MAX); ++frame_offset; } if ((frame_offset < frame_payload_size) && (offset > payload_size)) { - tqi->payload[frame_offset] = (uint8_t)(crc >> BITS_PER_BYTE); + tail->payload[frame_offset] = (uint8_t)(crc >> BITS_PER_BYTE); ++frame_offset; } } // Finalize the frame. - tqi->payload[frame_offset] = makeTailByte(start_of_transfer, end_of_transfer, toggle, transfer_id); - start_of_transfer = false; - toggle = !toggle; + CANARD_ASSERT((frame_offset + 1U) == tail->payload_size); + tail->payload[frame_offset] = makeTailByte(start_of_transfer, end_of_transfer, toggle, transfer_id); + start_of_transfer = false; + toggle = !toggle; } - // TODO: INSERTION. + if (tail != NULL) + { + CANARD_ASSERT(head->next != NULL); // This is not a single-frame transfer so at least two frames shall exist. + CANARD_ASSERT(tail->next == NULL); // The list shall be properly terminated. + CanardInternalTxQueueItem* const sup = findTxQueueSupremum(ins, can_id); + if (sup == NULL) // Once the insertion point is located, we insert the entire frame sequence in constant time. + { + tail->next = ins->_tx_queue; + ins->_tx_queue = head; + } + else + { + tail->next = sup->next; + sup->next = head; + } + } + else // Failed to allocate at least one frame in the queue! Remove all frames and abort. + { + while (head != NULL) + { + CanardInternalTxQueueItem* const next = head->next; + ins->heap_free(ins, head); + head = next; + } + } } // ---------------------------------------- RECEPTION ---------------------------------------- diff --git a/libcanard/canard.h b/libcanard/canard.h index 252f7963..b465aea4 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -196,6 +196,15 @@ CanardInstance canardInit(const CanardHeapAllocate heap_allocate, const CanardHeapFree heap_free, const CanardRxFilter rx_filter); +/// Values of remote node-ID above @ref CANARD_NODE_ID_MAX are treated as @ref CANARD_NODE_ID_UNSET. +/// In message transfers, the remote node-ID is ignored. +/// Excessive bits in priority, subject-ID, service-ID, and transfer-ID are silently masked away. +/// +/// Either all frames of the transfer are enqueued successfully, or none are. +/// Partial enqueueing is guaranteed to never happen. +/// +/// The time complexity is O(s+t), where s is the amount of payload in the transfer, and t is the number of +/// frames already enqueued in the transmission queue. void canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfer); CanardCANFrame canardTxPeek(const CanardInstance* const ins); @@ -206,7 +215,7 @@ CanardTransfer canardRxPush(CanardInstance* const ins, const CanardCANFrame* con #if CANARD_PLATFORM_TWOS_COMPLEMENT -/// This function may be used to encode values for later transmission in a UAVCAN transfer. +/// This function may be used to serialize values for later transmission in a UAVCAN transfer. /// It serializes a primitive value -- boolean, integer, character, or floating point -- and puts it at the /// specified bit offset in the specified contiguous buffer. /// Simple objects can also be serialized manually instead of using this function. @@ -234,7 +243,7 @@ void canardDSDLPrimitiveSerialize(void* const destination, const uint8_t length_bit, const void* const value); -/// This function may be used to extract values from received UAVCAN transfers. It decodes a scalar value -- +/// This function may be used to extract values from received UAVCAN transfers. It deserializes a scalar value -- /// boolean, integer, character, or floating point -- from the specified bit position in the source buffer. /// /// Caveat: This function works correctly only on platforms that use two's complement signed integer representation. From aa64c18bf53a0d7e5bbd422599f6acd6a02ecf4e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 5 Feb 2020 17:08:52 +0200 Subject: [PATCH 011/100] Remove float subnorm test to fix GCC build --- libcanard/canard.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/libcanard/canard.h b/libcanard/canard.h index b465aea4..cfc9d6a9 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -48,9 +48,8 @@ extern "C" { #define CANARD_PLATFORM_TWOS_COMPLEMENT \ ((INT8_MIN == -128) && (INT8_MAX == 127) && (INT16_MIN == -32768) && (INT16_MAX == 32767) && \ (INT32_MIN == -0x80000000LL) && (INT32_MAX == 0x7FFFFFFFLL) && (INT64_MAX == 0x7FFFFFFFFFFFFFFFLL)) -#define CANARD_PLATFORM_IEEE754 \ - ((FLT_RADIX == 2) && (FLT_MANT_DIG == 24) && (FLT_HAS_SUBNORM == 1) && (FLT_MIN_EXP == -125) && \ - (FLT_MAX_EXP == 128)) +#define CANARD_PLATFORM_IEEE754 \ + ((FLT_RADIX == 2) && (FLT_MANT_DIG == 24) && (FLT_MIN_EXP == -125) && (FLT_MAX_EXP == 128)) // Forward declarations. typedef struct CanardInstance CanardInstance; From 716b5b20468b1db16dcdd3e8d6491e379ab2ff2e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 5 Feb 2020 19:59:57 +0200 Subject: [PATCH 012/100] Moving on --- libcanard/canard.h | 4 ++-- tests/.clang-tidy | 2 +- tests/internals.hpp | 47 ++++++++++++++++++++++++++++++++++------ tests/test_internals.cpp | 14 ++++++++++++ 4 files changed, 57 insertions(+), 10 deletions(-) diff --git a/libcanard/canard.h b/libcanard/canard.h index cfc9d6a9..7b6ad822 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -115,8 +115,8 @@ typedef struct uint8_t transfer_id; - size_t payload_size; - void* payload; + size_t payload_size; + const void* payload; // TODO incompatible with free(). } CanardTransfer; /// The application supplies the library with this information when a new transfer should be received. diff --git a/tests/.clang-tidy b/tests/.clang-tidy index aae3d865..7653b14b 100644 --- a/tests/.clang-tidy +++ b/tests/.clang-tidy @@ -24,7 +24,7 @@ Checks: >- -cppcoreguidelines-avoid-magic-numbers, -readability-magic-numbers, WarningsAsErrors: '*' -HeaderFilterRegex: 'internal.hpp' +HeaderFilterRegex: '.*\.hpp' AnalyzeTemporaryDtors: false FormatStyle: file ... diff --git a/tests/internals.hpp b/tests/internals.hpp index d552f624..545e65f3 100644 --- a/tests/internals.hpp +++ b/tests/internals.hpp @@ -14,13 +14,46 @@ namespace internals { // Extern C effectively discards the outer namespaces. extern "C" { -std::uint32_t makeMessageSessionSpecifier(const std::uint16_t subject_id, const std::uint8_t src_node_id); -std::uint32_t makeServiceSessionSpecifier(const std::uint16_t service_id, - const bool request_not_response, - const std::uint8_t src_node_id, - const std::uint8_t dst_node_id); -std::uint16_t crcAdd(const std::uint16_t crc, const std::size_t size, const void* const bytes); -} +auto crcAdd(const std::uint16_t crc, const std::size_t size, const void* const bytes) -> std::uint16_t; + +auto makeMessageSessionSpecifier(const std::uint16_t subject_id, const std::uint8_t src_node_id) -> std::uint32_t; +auto makeServiceSessionSpecifier(const std::uint16_t service_id, + const bool request_not_response, + const std::uint8_t src_node_id, + const std::uint8_t dst_node_id) -> std::uint32_t; + +auto getPresentationLayerMTU(const CanardInstance* const ins) -> std::uint8_t; + +auto makeCANID(const CanardTransfer* const transfer, + const std::uint8_t local_node_id, + const std::uint8_t presentation_layer_mtu) -> std::uint32_t; + +auto makeTailByte(const bool start_of_transfer, + const bool end_of_transfer, + const bool toggle, + const std::uint8_t transfer_id) -> std::uint8_t; +auto allocateTxQueueItem(CanardInstance* const ins, + const std::uint32_t id, + const std::uint64_t deadline_usec, + const std::uint8_t payload_size) -> CanardInternalTxQueueItem*; + +auto findTxQueueSupremum(CanardInstance* const ins, const std::uint32_t can_id) -> CanardInternalTxQueueItem*; + +void pushSingleFrameTransfer(CanardInstance* const ins, + const std::uint64_t deadline_usec, + const std::uint32_t can_id, + const std::uint8_t transfer_id, + const std::size_t payload_size, + const void* const payload); + +void pushMultiFrameTransfer(CanardInstance* const ins, + const std::uint8_t presentation_layer_mtu, + const std::uint64_t deadline_usec, + const std::uint32_t can_id, + const std::uint8_t transfer_id, + const std::size_t payload_size, + const void* const payload); +} } // namespace internals diff --git a/tests/test_internals.cpp b/tests/test_internals.cpp index ee906fea..7f741fd7 100644 --- a/tests/test_internals.cpp +++ b/tests/test_internals.cpp @@ -2,6 +2,7 @@ // Copyright (c) 2016-2020 UAVCAN Development Team. #include "internals.hpp" +#include "helpers.hpp" TEST_CASE("SessionSpecifier") { @@ -25,3 +26,16 @@ TEST_CASE("TransferCRC") crc = crcAdd(crc, 6, "456789"); REQUIRE(0x29B1U == crc); } + +TEST_CASE("getPresentationLayerMTU") +{ + auto ins = + canardInit(&helpers::dummy_allocator::allocate, &helpers::dummy_allocator::free, &helpers::rejectAllRxFilter); + REQUIRE(63 == internals::getPresentationLayerMTU(&ins)); // This is the default. + ins.mtu_bytes = 0; + REQUIRE(7 == internals::getPresentationLayerMTU(&ins)); + ins.mtu_bytes = 255; + REQUIRE(63 == internals::getPresentationLayerMTU(&ins)); + ins.mtu_bytes = 32; + REQUIRE(31 == internals::getPresentationLayerMTU(&ins)); +} From 93ad49f706dd9d51acf64cd8fc06fabf93c2014c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 9 Feb 2020 14:56:24 +0200 Subject: [PATCH 013/100] Moving on with the API. Improved error handling and typing. --- libcanard/canard.c | 246 +++++++++++++++++++++++++------------------- libcanard/canard.h | 48 ++++++--- tests/helpers.hpp | 39 +++++++ tests/internals.hpp | 16 +-- 4 files changed, 221 insertions(+), 128 deletions(-) create mode 100644 tests/helpers.hpp diff --git a/libcanard/canard.c b/libcanard/canard.c index 7457936e..38838e2a 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -83,15 +83,16 @@ CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const size_t size, const voi #define OFFSET_SERVICE_ID 14U #define OFFSET_DST_NODE_ID 7U -#define SERVICE_NOT_MESSAGE (UINT32_C(1) << 25U) -#define ANONYMOUS_MESSAGE (UINT32_C(1) << 24U) -#define REQUEST_NOT_RESPONSE (UINT32_C(1) << 24U) +#define FLAG_SERVICE_NOT_MESSAGE (UINT32_C(1) << 25U) +#define FLAG_ANONYMOUS_MESSAGE (UINT32_C(1) << 24U) +#define FLAG_REQUEST_NOT_RESPONSE (UINT32_C(1) << 24U) CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const uint16_t subject_id, const uint8_t src_node_id); CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const uint16_t subject_id, const uint8_t src_node_id) { - return ((uint32_t)(src_node_id & CANARD_NODE_ID_MAX)) | - ((uint32_t)(subject_id & CANARD_SUBJECT_ID_MAX) << OFFSET_SUBJECT_ID); + CANARD_ASSERT(src_node_id <= CANARD_NODE_ID_MAX); + CANARD_ASSERT(subject_id <= CANARD_SUBJECT_ID_MAX); + return src_node_id | ((uint32_t) subject_id << OFFSET_SUBJECT_ID); } CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const uint16_t service_id, @@ -103,10 +104,12 @@ CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const uint16_t service_id, const uint8_t src_node_id, const uint8_t dst_node_id) { - return ((uint32_t)(src_node_id & CANARD_NODE_ID_MAX)) | - ((uint32_t)(dst_node_id & CANARD_NODE_ID_MAX) << OFFSET_DST_NODE_ID) | - ((uint32_t)(service_id & CANARD_SERVICE_ID_MAX) << OFFSET_SERVICE_ID) | - (request_not_response ? REQUEST_NOT_RESPONSE : 0U) | SERVICE_NOT_MESSAGE; + CANARD_ASSERT(src_node_id <= CANARD_NODE_ID_MAX); + CANARD_ASSERT(dst_node_id <= CANARD_NODE_ID_MAX); + CANARD_ASSERT(service_id <= CANARD_SERVICE_ID_MAX); + return src_node_id | (((uint32_t) dst_node_id) << OFFSET_DST_NODE_ID) | // + (((uint32_t) service_id) << OFFSET_SERVICE_ID) | // + (request_not_response ? FLAG_REQUEST_NOT_RESPONSE : 0U) | FLAG_SERVICE_NOT_MESSAGE; } // ---------------------------------------- TRANSMISSION ---------------------------------------- @@ -118,7 +121,7 @@ typedef struct CanardInternalTxQueueItem uint32_t id; uint64_t deadline_usec; - uint8_t payload_size; + size_t payload_size; // Intentional violation of MISRA: this flex array is the lesser of three evils. The other two are: // - Use pointer, make it point to the remainder of the allocated memory following this structure. @@ -127,75 +130,88 @@ typedef struct CanardInternalTxQueueItem uint8_t payload[]; // NOSONAR } CanardInternalTxQueueItem; -/// This is the transport MTU minus the tail byte. -CANARD_INTERNAL uint8_t getPresentationLayerMTU(const CanardInstance* const ins); -CANARD_INTERNAL uint8_t getPresentationLayerMTU(const CanardInstance* const ins) +/// This is the transport MTU rounded up to next full DLC minus the tail byte. +CANARD_INTERNAL size_t getPresentationLayerMTU(const CanardInstance* const ins); +CANARD_INTERNAL size_t getPresentationLayerMTU(const CanardInstance* const ins) { - uint8_t out = 0U; - if (ins->mtu_bytes < CANARD_MTU_MIN) + const size_t max_index = (sizeof(CanardCANLengthToDLC) / sizeof(CanardCANLengthToDLC[0])) - 1U; + size_t mtu = 0U; + if (ins->mtu_bytes < CANARD_MTU_CAN_CLASSIC) { - out = (uint8_t)(CANARD_MTU_MIN - 1U); + mtu = CANARD_MTU_CAN_CLASSIC; } - else if (ins->mtu_bytes > CANARD_MTU_MAX) + else if (ins->mtu_bytes <= max_index) { - out = (uint8_t)(CANARD_MTU_MAX - 1U); + mtu = CanardCANDLCToLength[CanardCANLengthToDLC[ins->mtu_bytes]]; // Round up to nearest valid length. } else { - out = (uint8_t)(ins->mtu_bytes - 1U); + mtu = CanardCANDLCToLength[CanardCANLengthToDLC[max_index]]; } - return out; + return mtu - 1U; } -/// Returns an invalid value above CAN_EXT_ID_MASK to indicate failure. -CANARD_INTERNAL uint32_t makeCANID(const CanardTransfer* const transfer, - const uint8_t local_node_id, - const uint8_t presentation_layer_mtu); -CANARD_INTERNAL uint32_t makeCANID(const CanardTransfer* const transfer, - const uint8_t local_node_id, - const uint8_t presentation_layer_mtu) +CANARD_INTERNAL int32_t makeCANID(const CanardTransfer* const tr, + const uint8_t local_node_id, + const size_t presentation_layer_mtu); +CANARD_INTERNAL int32_t makeCANID(const CanardTransfer* const tr, + const uint8_t local_node_id, + const size_t presentation_layer_mtu) { - uint32_t out = UINT32_MAX; - if (transfer->transfer_kind == CanardTransferKindMessage) + CANARD_ASSERT(tr != NULL); + CANARD_ASSERT(presentation_layer_mtu > 0); + int32_t out = -CANARD_ERROR_INVALID_ARGUMENT; + if ((tr->transfer_kind == CanardTransferKindMessage) && (tr->remote_node_id == CANARD_NODE_ID_UNSET) && + (tr->port_id <= CANARD_SUBJECT_ID_MAX)) { if (local_node_id <= CANARD_NODE_ID_MAX) { - out = makeMessageSessionSpecifier(transfer->port_id, local_node_id); + out = (int32_t) makeMessageSessionSpecifier(tr->port_id, local_node_id); + CANARD_ASSERT(out >= 0); } - else if (transfer->payload_size <= presentation_layer_mtu) + else if (tr->payload_size <= presentation_layer_mtu) { - const uint8_t c = (uint8_t) crcAdd(CRC_INITIAL, transfer->payload_size, transfer->payload); - out = makeMessageSessionSpecifier(transfer->port_id, c) | ANONYMOUS_MESSAGE; + const uint8_t c = (uint8_t)(crcAdd(CRC_INITIAL, tr->payload_size, tr->payload) & CANARD_NODE_ID_MAX); + out = (int32_t)(makeMessageSessionSpecifier(tr->port_id, c) | FLAG_ANONYMOUS_MESSAGE); + CANARD_ASSERT(out >= 0); } else { - CANARD_ASSERT(false); // Anonymous multi-frame message transfers are not allowed. + out = -CANARD_ERROR_INVALID_ARGUMENT; // Anonymous multi-frame message trs are not allowed. } } - else if ((transfer->transfer_kind == CanardTransferKindRequest) || - (transfer->transfer_kind == CanardTransferKindResponse)) + else if (((tr->transfer_kind == CanardTransferKindRequest) || (tr->transfer_kind == CanardTransferKindResponse)) && + (tr->remote_node_id <= CANARD_NODE_ID_MAX) && (tr->port_id <= CANARD_SERVICE_ID_MAX)) { if (local_node_id <= CANARD_NODE_ID_MAX) { - out = makeServiceSessionSpecifier(transfer->port_id, - transfer->transfer_kind == CanardTransferKindRequest, - local_node_id, - transfer->remote_node_id); + out = (int32_t) makeServiceSessionSpecifier(tr->port_id, + tr->transfer_kind == CanardTransferKindRequest, + local_node_id, + tr->remote_node_id); + CANARD_ASSERT(out >= 0); } else { - CANARD_ASSERT(false); // Anonymous service transfers are not allowed. + out = -CANARD_ERROR_INVALID_ARGUMENT; // Anonymous service transfers are not allowed. } } else { - CANARD_ASSERT(false); // Invalid transfer kind. + out = -CANARD_ERROR_INVALID_ARGUMENT; } - if (out < UINT32_MAX) + if (out >= 0) { - out |= ((uint32_t) transfer->priority & CANARD_PRIORITY_MAX) << OFFSET_PRIORITY; - CANARD_ASSERT(out <= CAN_EXT_ID_MASK); + const uint32_t prio = (uint32_t) tr->priority; + if (prio <= CANARD_PRIORITY_MAX) + { + out = (int32_t)(((uint32_t) out) | (prio << OFFSET_PRIORITY)); + } + else + { + out = -CANARD_ERROR_INVALID_ARGUMENT; + } } return out; } @@ -216,14 +232,13 @@ CANARD_INTERNAL uint8_t makeTailByte(const bool start_of_transfer, CANARD_INTERNAL CanardInternalTxQueueItem* allocateTxQueueItem(CanardInstance* const ins, const uint32_t id, const uint64_t deadline_usec, - const uint8_t payload_size); + const size_t payload_size); CANARD_INTERNAL CanardInternalTxQueueItem* allocateTxQueueItem(CanardInstance* const ins, const uint32_t id, const uint64_t deadline_usec, - const uint8_t payload_size) + const size_t payload_size) { CANARD_ASSERT(ins != NULL); - CANARD_ASSERT(id <= CAN_EXT_ID_MASK); CANARD_ASSERT(payload_size > 0U); // UAVCAN/CAN doesn't allow zero-payload frames. CanardInternalTxQueueItem* const out = (CanardInternalTxQueueItem*) ins->heap_allocate(ins, sizeof(CanardInternalTxQueueItem) + payload_size); @@ -261,24 +276,24 @@ CANARD_INTERNAL CanardInternalTxQueueItem* findTxQueueSupremum(CanardInstance* c return out; } -CANARD_INTERNAL void pushSingleFrameTransfer(CanardInstance* const ins, - const uint64_t deadline_usec, - const uint32_t can_id, - const uint8_t transfer_id, - const size_t payload_size, - const void* const payload); -CANARD_INTERNAL void pushSingleFrameTransfer(CanardInstance* const ins, - const uint64_t deadline_usec, - const uint32_t can_id, - const uint8_t transfer_id, - const size_t payload_size, - const void* const payload) +/// Returns the number of frames enqueued or error (i.e., =1 or <0). +CANARD_INTERNAL int32_t pushSingleFrameTransfer(CanardInstance* const ins, + const uint64_t deadline_usec, + const uint32_t can_id, + const uint8_t transfer_id, + const size_t payload_size, + const void* const payload); +CANARD_INTERNAL int32_t pushSingleFrameTransfer(CanardInstance* const ins, + const uint64_t deadline_usec, + const uint32_t can_id, + const uint8_t transfer_id, + const size_t payload_size, + const void* const payload) { CANARD_ASSERT(ins != NULL); - CANARD_ASSERT(can_id <= CAN_EXT_ID_MASK); - CANARD_ASSERT((payload_size == 0U) || (payload != NULL)); - CanardInternalTxQueueItem* const tqi = - allocateTxQueueItem(ins, can_id, deadline_usec, (uint8_t)(payload_size + 1U)); + CANARD_ASSERT((payload != NULL) || (payload_size == 0)); + int32_t out = 0; + CanardInternalTxQueueItem* const tqi = allocateTxQueueItem(ins, can_id, deadline_usec, payload_size + 1U); if (tqi != NULL) { // Clang-Tidy raises an error recommending the use of memcpy_s() instead. @@ -296,30 +311,39 @@ CANARD_INTERNAL void pushSingleFrameTransfer(CanardInstance* const ins, tqi->next = ins->_tx_queue; ins->_tx_queue = tqi; } + out = 1; // One frame enqueued. + } + else + { + out = -CANARD_ERROR_OUT_OF_MEMORY; } + CANARD_ASSERT((out < 0) || (out == 1)); + return out; } -CANARD_INTERNAL void pushMultiFrameTransfer(CanardInstance* const ins, - const uint8_t presentation_layer_mtu, - const uint64_t deadline_usec, - const uint32_t can_id, - const uint8_t transfer_id, - const size_t payload_size, - const void* const payload); -CANARD_INTERNAL void pushMultiFrameTransfer(CanardInstance* const ins, - const uint8_t presentation_layer_mtu, - const uint64_t deadline_usec, - const uint32_t can_id, - const uint8_t transfer_id, - const size_t payload_size, - const void* const payload) +/// Returns the number of frames enqueued or error. +CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, + const size_t presentation_layer_mtu, + const uint64_t deadline_usec, + const uint32_t can_id, + const uint8_t transfer_id, + const size_t payload_size, + const void* const payload); +CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, + const size_t presentation_layer_mtu, + const uint64_t deadline_usec, + const uint32_t can_id, + const uint8_t transfer_id, + const size_t payload_size, + const void* const payload) { CANARD_ASSERT(ins != NULL); - CANARD_ASSERT(presentation_layer_mtu > 1U); - CANARD_ASSERT(can_id <= CAN_EXT_ID_MASK); - CANARD_ASSERT(payload_size > presentation_layer_mtu); + CANARD_ASSERT(presentation_layer_mtu > 0U); + CANARD_ASSERT(payload_size > presentation_layer_mtu); // Otherwise, a single-frame transfer should be used. CANARD_ASSERT(payload != NULL); + int32_t out = 0; // The number of frames enqueued or negated error. + CanardInternalTxQueueItem* head = NULL; // Head and tail of the linked list of frames of this transfer. CanardInternalTxQueueItem* tail = NULL; @@ -332,8 +356,10 @@ CANARD_INTERNAL void pushMultiFrameTransfer(CanardInstance* const ins, while (offset < payload_size_with_crc) { + ++out; // Count the number of generated frames. + // Compute the required size of the frame payload for this frame, including CRC and padding. - uint8_t frame_payload_size_with_tail = (uint8_t)(presentation_layer_mtu + 1U); + size_t frame_payload_size_with_tail = presentation_layer_mtu + 1U; { const size_t remaining_payload_with_crc = payload_size_with_crc - offset; if (remaining_payload_with_crc < presentation_layer_mtu) @@ -365,8 +391,8 @@ CANARD_INTERNAL void pushMultiFrameTransfer(CanardInstance* const ins, } // Copy the payload into the frame. - const uint8_t frame_payload_size = (uint8_t)(frame_payload_size_with_tail - 1U); - uint8_t frame_offset = 0U; + const size_t frame_payload_size = frame_payload_size_with_tail - 1U; + size_t frame_offset = 0U; if (offset < payload_size) { size_t move_size = payload_size - offset; @@ -377,7 +403,7 @@ CANARD_INTERNAL void pushMultiFrameTransfer(CanardInstance* const ins, // Clang-Tidy raises an error recommending the use of memcpy_s() instead. // We ignore this recommendation because it is not available in C99. (void) memcpy(&tail->payload[0], payload_ptr, move_size); // NOLINT - frame_offset = (uint8_t)(frame_offset + move_size); + frame_offset = frame_offset + move_size; offset += move_size; payload_ptr += move_size; } @@ -432,6 +458,7 @@ CANARD_INTERNAL void pushMultiFrameTransfer(CanardInstance* const ins, } else // Failed to allocate at least one frame in the queue! Remove all frames and abort. { + out = -CANARD_ERROR_OUT_OF_MEMORY; while (head != NULL) { CanardInternalTxQueueItem* const next = head->next; @@ -439,6 +466,9 @@ CANARD_INTERNAL void pushMultiFrameTransfer(CanardInstance* const ins, head = next; } } + + CANARD_ASSERT((out < 0) || (out >= 2)); + return out; } // ---------------------------------------- RECEPTION ---------------------------------------- @@ -496,39 +526,41 @@ CanardInstance canardInit(const CanardHeapAllocate heap_allocate, return out; } -void canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfer) +int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfer) { - if ((ins != NULL) && (transfer != NULL)) + int32_t out = -CANARD_ERROR_INVALID_ARGUMENT; + if ((ins != NULL) && (transfer != NULL) && ((transfer->payload != NULL) || (transfer->payload_size == 0U))) { - const uint8_t pl_mtu = getPresentationLayerMTU(ins); - const uint32_t can_id = makeCANID(transfer, ins->node_id, pl_mtu); - if (can_id <= CAN_EXT_ID_MASK) + const size_t pl_mtu = getPresentationLayerMTU(ins); + const int32_t maybe_can_id = makeCANID(transfer, ins->node_id, pl_mtu); + if (maybe_can_id >= 0) { if (transfer->payload_size <= pl_mtu) { - pushSingleFrameTransfer(ins, - transfer->timestamp_usec, - can_id, - transfer->transfer_id, - transfer->payload_size, - transfer->payload); + out = pushSingleFrameTransfer(ins, + transfer->timestamp_usec, + (uint32_t) maybe_can_id, + transfer->transfer_id, + transfer->payload_size, + transfer->payload); } else { - pushMultiFrameTransfer(ins, - pl_mtu, - transfer->timestamp_usec, - can_id, - transfer->transfer_id, - transfer->payload_size, - transfer->payload); + out = pushMultiFrameTransfer(ins, + pl_mtu, + transfer->timestamp_usec, + (uint32_t) maybe_can_id, + transfer->transfer_id, + transfer->payload_size, + transfer->payload); } } + else + { + out = maybe_can_id; + } } - else - { - CANARD_ASSERT(false); - } + return out; } // ---------------------------------------- FLOAT16 SERIALIZATION ---------------------------------------- diff --git a/libcanard/canard.h b/libcanard/canard.h index 7b6ad822..38217a31 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -22,12 +22,19 @@ extern "C" { /// The version number of the UAVCAN specification implemented by this library. #define CANARD_UAVCAN_SPECIFICATION_VERSION_MAJOR 1 +/// These error codes may be returned from the library API calls whose return type is a signed integer +/// in the negated form (i.e., code 2 returned as -2). +/// API calls whose return type is not a signer integer cannot fail by contract. +/// No other error states may occur in the library. +/// By contract, a deterministic application with a properly sized heap will never encounter any of the listed errors. +/// The error code 1 is not used because -1 is often used as a generic error code in 3rd-party code. +#define CANARD_ERROR_INVALID_ARGUMENT 2 +#define CANARD_ERROR_OUT_OF_MEMORY 3 + /// MTU values for supported protocols. /// Per the recommendations given in the UAVCAN specification, other MTU values should not be used. #define CANARD_MTU_CAN_CLASSIC 8U #define CANARD_MTU_CAN_FD 64U -#define CANARD_MTU_MIN 8U -#define CANARD_MTU_MAX 64U /// Parameter ranges are inclusive; the lower bound is zero for all. Refer to the specification for more info. #define CANARD_SUBJECT_ID_MAX 32767U @@ -171,9 +178,9 @@ struct CanardInstance /// This setting defines the maximum number of bytes per CAN data frame in all outgoing transfers. /// Regardless of this setting, CAN frames with any MTU can always be accepted. /// - /// Only the standard values should be used as recommended by the specification; otherwise, - /// networking interoperability issues may arise. See "CANARD_MTU_*". - /// Valid values are any valid CAN frame data length. The default is the maximum valid value. + /// Only the standard values should be used as recommended by the specification; + /// otherwise, networking interoperability issues may arise. See "CANARD_MTU_*". + /// Valid values are any valid CAN frame data length not smaller than 8. The default is the maximum valid value. /// Invalid values are treated as the nearest valid value. uint8_t mtu_bytes; @@ -195,22 +202,37 @@ CanardInstance canardInit(const CanardHeapAllocate heap_allocate, const CanardHeapFree heap_free, const CanardRxFilter rx_filter); -/// Values of remote node-ID above @ref CANARD_NODE_ID_MAX are treated as @ref CANARD_NODE_ID_UNSET. -/// In message transfers, the remote node-ID is ignored. -/// Excessive bits in priority, subject-ID, service-ID, and transfer-ID are silently masked away. +/// Takes a transfer, serializes it into a sequence of CAN frames which are injected into the prioritized TX queue +/// at the appropriate position. +/// Returns the number of frames enqueued (which is always a positive number) in case of success. +/// Returns a negated error code in case of failure. +/// +/// An invalid argument error may be returned in the following cases: +/// - Any of the input arguments are NULL. +/// - The remote node-ID is not @ref CANARD_NODE_ID_UNSET and the transfer is a message transfer. +/// - The remote node-ID is above @ref CANARD_NODE_ID_MAX and the transfer is a service transfer. +/// - The priority, subject-ID, or service-ID exceed their respective maximums. +/// - The transfer kind is invalid. +/// - The payload pointer is NULL while the payload size is nonzero. +/// - The local node is anonymous and a message transfer is requested that requires a multi-frame transfer. +/// - The local node is anonymous and a service transfer is requested. +/// The following cases are handled without raising an invalid argument error: +/// - If the transfer-ID is above the maximum, the excessive bits are silently masked away +/// (i.e., the modulo is computed automatically, so the caller doesn't have to bother). /// -/// Either all frames of the transfer are enqueued successfully, or none are. -/// Partial enqueueing is guaranteed to never happen. +/// An out-of-memory error is returned if a TX frame could not be allocated due to the heap being exhausted. +/// In that case, all previously allocated frames will be purged automatically. +/// In other words, either all frames of the transfer are enqueued successfully, or none are. /// /// The time complexity is O(s+t), where s is the amount of payload in the transfer, and t is the number of /// frames already enqueued in the transmission queue. -void canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfer); +int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfer); -CanardCANFrame canardTxPeek(const CanardInstance* const ins); +int8_t canardTxPeek(const CanardInstance* const ins, CanardCANFrame* const out_frame); void canardTxPop(CanardInstance* const ins); -CanardTransfer canardRxPush(CanardInstance* const ins, const CanardCANFrame* const frame); +int32_t canardRxPush(CanardInstance* const ins, const CanardCANFrame* const frame, CanardTransfer* const out_transfer); #if CANARD_PLATFORM_TWOS_COMPLEMENT diff --git a/tests/helpers.hpp b/tests/helpers.hpp new file mode 100644 index 00000000..64b13b0f --- /dev/null +++ b/tests/helpers.hpp @@ -0,0 +1,39 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. + +#pragma once + +#include "canard.h" +#include + +namespace helpers +{ +namespace dummy_allocator +{ +inline auto allocate(CanardInstance* const ins, std::size_t const amount) -> void* +{ + (void) ins; + (void) amount; + return nullptr; +} + +inline void free(CanardInstance* const ins, void* const pointer) +{ + (void) ins; + (void) pointer; +} +} // namespace dummy_allocator + +inline auto rejectAllRxFilter(const CanardInstance* ins, + std::uint16_t port_id, + CanardTransferKind transfer_kind, + std::uint8_t source_node_id) -> CanardRxMetadata +{ + (void) ins; + (void) port_id; + (void) transfer_kind; + (void) source_node_id; + return CanardRxMetadata{}; +} + +} // namespace helpers diff --git a/tests/internals.hpp b/tests/internals.hpp index 545e65f3..ec402791 100644 --- a/tests/internals.hpp +++ b/tests/internals.hpp @@ -23,11 +23,11 @@ auto makeServiceSessionSpecifier(const std::uint16_t service_id, const std::uint8_t src_node_id, const std::uint8_t dst_node_id) -> std::uint32_t; -auto getPresentationLayerMTU(const CanardInstance* const ins) -> std::uint8_t; +auto getPresentationLayerMTU(const CanardInstance* const ins) -> std::size_t; auto makeCANID(const CanardTransfer* const transfer, const std::uint8_t local_node_id, - const std::uint8_t presentation_layer_mtu) -> std::uint32_t; + const std::size_t presentation_layer_mtu) -> std::int32_t; auto makeTailByte(const bool start_of_transfer, const bool end_of_transfer, @@ -37,23 +37,23 @@ auto makeTailByte(const bool start_of_transfer, auto allocateTxQueueItem(CanardInstance* const ins, const std::uint32_t id, const std::uint64_t deadline_usec, - const std::uint8_t payload_size) -> CanardInternalTxQueueItem*; + const std::size_t payload_size) -> CanardInternalTxQueueItem*; auto findTxQueueSupremum(CanardInstance* const ins, const std::uint32_t can_id) -> CanardInternalTxQueueItem*; -void pushSingleFrameTransfer(CanardInstance* const ins, +auto pushSingleFrameTransfer(CanardInstance* const ins, const std::uint64_t deadline_usec, const std::uint32_t can_id, const std::uint8_t transfer_id, const std::size_t payload_size, - const void* const payload); + const void* const payload) -> std::int32_t; -void pushMultiFrameTransfer(CanardInstance* const ins, - const std::uint8_t presentation_layer_mtu, +auto pushMultiFrameTransfer(CanardInstance* const ins, + const std::size_t presentation_layer_mtu, const std::uint64_t deadline_usec, const std::uint32_t can_id, const std::uint8_t transfer_id, const std::size_t payload_size, - const void* const payload); + const void* const payload) -> std::int32_t; } } // namespace internals From ff822fd7978f6f5cdee63f214afb5ffdd59428a6 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 9 Feb 2020 15:18:35 +0200 Subject: [PATCH 014/100] Formatting & clarifications --- .travis.yml | 2 +- libcanard/canard.h | 13 ++++++++----- tests/test_internals.cpp | 28 ++++++++++++++++++---------- 3 files changed, 27 insertions(+), 16 deletions(-) diff --git a/.travis.yml b/.travis.yml index 57033011..df941027 100644 --- a/.travis.yml +++ b/.travis.yml @@ -73,7 +73,7 @@ matrix: # Format check - make format VERBOSE=1 - 'modified="$(git status --porcelain --untracked-files=no)"' - - 'if [ -n "$modified" ]; then echo "Run scripts/format.sh to reformat the code."; exit 1; fi' + - 'if [ -n "$modified" ]; then echo "Run make format to reformat the code properly."; exit 1; fi' git: depth: false # Disable shallow clone because it is incompatible with SonarCloud diff --git a/libcanard/canard.h b/libcanard/canard.h index 38217a31..8160b2cd 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -95,8 +95,8 @@ typedef struct uint32_t extended_can_id; /// The useful data in the frame. The length value is not to be confused with DLC! - uint8_t payload_size; - void* payload; + size_t payload_size; + const void* payload; } CanardCANFrame; /// Conversion look-up tables between CAN DLC and data length. @@ -122,8 +122,9 @@ typedef struct uint8_t transfer_id; + /// The const pointer makes it incompatible with free(), but we have to tolerate that due to the limitations of C. size_t payload_size; - const void* payload; // TODO incompatible with free(). + const void* payload; } CanardTransfer; /// The application supplies the library with this information when a new transfer should be received. @@ -202,8 +203,10 @@ CanardInstance canardInit(const CanardHeapAllocate heap_allocate, const CanardHeapFree heap_free, const CanardRxFilter rx_filter); -/// Takes a transfer, serializes it into a sequence of CAN frames which are injected into the prioritized TX queue -/// at the appropriate position. +/// Takes a transfer, serializes it into a sequence of CAN frames, and inserts them into the prioritized TX queue +/// at the appropriate position. The application is supposed to take the enqueued frames from the TX buffer and +/// transmit them afterwards. +/// /// Returns the number of frames enqueued (which is always a positive number) in case of success. /// Returns a negated error code in case of failure. /// diff --git a/tests/test_internals.cpp b/tests/test_internals.cpp index 7f741fd7..aec1adca 100644 --- a/tests/test_internals.cpp +++ b/tests/test_internals.cpp @@ -4,16 +4,6 @@ #include "internals.hpp" #include "helpers.hpp" -TEST_CASE("SessionSpecifier") -{ - REQUIRE(0b000'00'0011001100110011'0'1010101 == - internals::makeMessageSessionSpecifier(0b0011001100110011, 0b1010101)); - REQUIRE(0b000'11'0100110011'0101010'1010101 == - internals::makeServiceSessionSpecifier(0b0100110011, true, 0b1010101, 0b0101010)); - REQUIRE(0b000'10'0100110011'1010101'0101010 == - internals::makeServiceSessionSpecifier(0b0100110011, false, 0b0101010, 0b1010101)); -} - TEST_CASE("TransferCRC") { using internals::crcAdd; @@ -27,6 +17,16 @@ TEST_CASE("TransferCRC") REQUIRE(0x29B1U == crc); } +TEST_CASE("SessionSpecifier") +{ + REQUIRE(0b000'00'0011001100110011'0'1010101 == + internals::makeMessageSessionSpecifier(0b0011001100110011, 0b1010101)); + REQUIRE(0b000'11'0100110011'0101010'1010101 == + internals::makeServiceSessionSpecifier(0b0100110011, true, 0b1010101, 0b0101010)); + REQUIRE(0b000'10'0100110011'1010101'0101010 == + internals::makeServiceSessionSpecifier(0b0100110011, false, 0b0101010, 0b1010101)); +} + TEST_CASE("getPresentationLayerMTU") { auto ins = @@ -38,4 +38,12 @@ TEST_CASE("getPresentationLayerMTU") REQUIRE(63 == internals::getPresentationLayerMTU(&ins)); ins.mtu_bytes = 32; REQUIRE(31 == internals::getPresentationLayerMTU(&ins)); + ins.mtu_bytes = 30; // Round up. + REQUIRE(31 == internals::getPresentationLayerMTU(&ins)); } + +TEST_CASE("makeCANID") {} + +TEST_CASE("makeTailByte") {} + +TEST_CASE("findTxQueueSupremum") {} From 8410701681630b2c49683fbd42a641f47ae5167f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 9 Feb 2020 17:31:56 +0200 Subject: [PATCH 015/100] Finalized makeTailByte() & makeCANID() --- libcanard/canard.c | 4 ++ libcanard/canard.h | 5 +- tests/test_internals.cpp | 108 ++++++++++++++++++++++++++++++++++++++- 3 files changed, 114 insertions(+), 3 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 38838e2a..2d2397ec 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -66,6 +66,7 @@ CANARD_INTERNAL uint16_t crcAddByte(const uint16_t crc, const uint8_t byte) CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const size_t size, const void* const data); CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const size_t size, const void* const data) { + CANARD_ASSERT((data != NULL) || (size == 0U)); uint16_t out = crc; const uint8_t* p = (const uint8_t*) data; for (size_t i = 0; i < size; i++) @@ -171,6 +172,7 @@ CANARD_INTERNAL int32_t makeCANID(const CanardTransfer* const tr, } else if (tr->payload_size <= presentation_layer_mtu) { + CANARD_ASSERT((tr->payload != NULL) || (tr->payload_size == 0U)); const uint8_t c = (uint8_t)(crcAdd(CRC_INITIAL, tr->payload_size, tr->payload) & CANARD_NODE_ID_MAX); out = (int32_t)(makeMessageSessionSpecifier(tr->port_id, c) | FLAG_ANONYMOUS_MESSAGE); CANARD_ASSERT(out >= 0); @@ -225,6 +227,7 @@ CANARD_INTERNAL uint8_t makeTailByte(const bool start_of_transfer, const bool toggle, const uint8_t transfer_id) { + CANARD_ASSERT(start_of_transfer ? toggle : true); return (uint8_t)((start_of_transfer ? TAIL_START_OF_TRANSFER : 0U) | (end_of_transfer ? TAIL_END_OF_TRANSFER : 0U) | (toggle ? TAIL_TOGGLE : 0U) | (transfer_id & CANARD_TRANSFER_ID_MAX)); } @@ -488,6 +491,7 @@ typedef struct CanardInternalRxSession const uint32_t session_specifier; ///< Differentiates this session from other sessions. uint16_t calculated_crc; ///< Updated with the received payload in real time. + uint8_t iface_index; uint8_t transfer_id; bool next_toggle; } CanardInternalRxSession; diff --git a/libcanard/canard.h b/libcanard/canard.h index 8160b2cd..4021706f 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -235,7 +235,10 @@ int8_t canardTxPeek(const CanardInstance* const ins, CanardCANFrame* const out_f void canardTxPop(CanardInstance* const ins); -int32_t canardRxPush(CanardInstance* const ins, const CanardCANFrame* const frame, CanardTransfer* const out_transfer); +int32_t canardRxPush(CanardInstance* const ins, + const CanardCANFrame* const frame, + const uint8_t iface_index, + CanardTransfer* const out_transfer); #if CANARD_PLATFORM_TWOS_COMPLEMENT diff --git a/tests/test_internals.cpp b/tests/test_internals.cpp index aec1adca..6135a480 100644 --- a/tests/test_internals.cpp +++ b/tests/test_internals.cpp @@ -42,8 +42,112 @@ TEST_CASE("getPresentationLayerMTU") REQUIRE(31 == internals::getPresentationLayerMTU(&ins)); } -TEST_CASE("makeCANID") {} +TEST_CASE("makeCANID") +{ + using internals::makeCANID; + + CanardTransfer transfer{}; + std::vector transfer_payload; + + const auto mk_transfer = [&](const CanardPriority priority, + const CanardTransferKind kind, + const std::uint16_t port_id, + const std::uint8_t remote_node_id, + const std::vector& payload = {}) { + transfer_payload = payload; + transfer.priority = priority; + transfer.transfer_kind = kind; + transfer.port_id = port_id; + transfer.remote_node_id = remote_node_id; + transfer.payload = transfer_payload.data(); + transfer.payload_size = transfer_payload.size(); + return &transfer; + }; + + const auto crc123 = internals::crcAdd(0xFFFFU, 3, "\x01\x02\x03"); + + // MESSAGE TRANSFERS + REQUIRE(0b000'00'0011001100110011'0'1010101 == // Regular message. + makeCANID(mk_transfer(CanardPriorityExceptional, + CanardTransferKindMessage, + 0b0011001100110011, + CANARD_NODE_ID_UNSET), + 0b1010101, + 7U)); + REQUIRE(0b111'00'0011001100110011'0'1010101 == // Regular message. + makeCANID(mk_transfer(CanardPriorityOptional, + CanardTransferKindMessage, + 0b0011001100110011, + CANARD_NODE_ID_UNSET), + 0b1010101, + 7U)); + REQUIRE((0b010'01'0011001100110011'0'0000000U | (crc123 & CANARD_NODE_ID_MAX)) == // Anonymous message. + makeCANID(mk_transfer(CanardPriorityFast, + CanardTransferKindMessage, + 0b0011001100110011, + CANARD_NODE_ID_UNSET, + {1, 2, 3}), + 128U, // Invalid local node-ID. + 7U)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Multi-frame anonymous messages are not allowed. + makeCANID(mk_transfer(CanardPriorityFast, + CanardTransferKindMessage, + 0b0011001100110011, + CANARD_NODE_ID_UNSET, + {1, 2, 3, 4, 5, 6, 7, 8}), + 128U, // Invalid local node-ID is treated as anonymous/unset. + 7U)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Bad remote node-ID -- unicast messages not supported. + makeCANID(mk_transfer(CanardPriorityFast, CanardTransferKindMessage, 0b0011001100110011, 123U), 0U, 7U)); + REQUIRE( + -CANARD_ERROR_INVALID_ARGUMENT == // Bad subject-ID. + makeCANID(mk_transfer(CanardPriorityFast, CanardTransferKindMessage, 0xFFFFU, CANARD_NODE_ID_UNSET), 0U, 7U)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Bad priority. + makeCANID(mk_transfer(static_cast(123), + CanardTransferKindMessage, + 0b0011001100110011, + CANARD_NODE_ID_UNSET), + 0b1010101, + 7U)); -TEST_CASE("makeTailByte") {} + // SERVICE TRANSFERS + REQUIRE(0b000'11'0100110011'0101010'1010101 == // Request. + makeCANID(mk_transfer(CanardPriorityExceptional, CanardTransferKindRequest, 0b0100110011, 0b0101010), + 0b1010101, + 7U)); + REQUIRE(0b111'10'0100110011'0101010'1010101 == // Response. + makeCANID(mk_transfer(CanardPriorityOptional, CanardTransferKindResponse, 0b0100110011, 0b0101010), + 0b1010101, + 7U)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Anonymous service transfers not permitted. + makeCANID(mk_transfer(CanardPriorityExceptional, CanardTransferKindRequest, 0b0100110011, 0b0101010), + CANARD_NODE_ID_UNSET, + 7U)); + REQUIRE( + -CANARD_ERROR_INVALID_ARGUMENT == // Broadcast service transfers not permitted. + makeCANID(mk_transfer(CanardPriorityOptional, CanardTransferKindResponse, 0b0100110011, CANARD_NODE_ID_UNSET), + 0b1010101, + 7U)); + REQUIRE( + -CANARD_ERROR_INVALID_ARGUMENT == // Bad service-ID. + makeCANID(mk_transfer(CanardPriorityOptional, CanardTransferKindResponse, 0xFFFFU, 0b0101010), 0b1010101, 7U)); + REQUIRE( + -CANARD_ERROR_INVALID_ARGUMENT == // Bad priority. + makeCANID(mk_transfer(static_cast(123), CanardTransferKindResponse, 0b0100110011, 0b0101010), + 0b1010101, + 7U)); +} + +TEST_CASE("makeTailByte") +{ + using internals::makeTailByte; + REQUIRE(0b111'00000 == makeTailByte(true, true, true, 0U)); + REQUIRE(0b111'00000 == makeTailByte(true, true, true, 32U)); + REQUIRE(0b111'11111 == makeTailByte(true, true, true, 31U)); + REQUIRE(0b011'11111 == makeTailByte(false, true, true, 31U)); + REQUIRE(0b101'11110 == makeTailByte(true, false, true, 30U)); + REQUIRE(0b001'11101 == makeTailByte(false, false, true, 29U)); + REQUIRE(0b010'00001 == makeTailByte(false, true, false, 1U)); +} TEST_CASE("findTxQueueSupremum") {} From 70e5322fbfcb8977e3aa987be7d3671610d7cdb5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 9 Feb 2020 18:17:11 +0200 Subject: [PATCH 016/100] findTxQueueSupremum() --- tests/internals.hpp | 12 ++++++++++ tests/test_internals.cpp | 51 +++++++++++++++++++++++++++++++++++++++- 2 files changed, 62 insertions(+), 1 deletion(-) diff --git a/tests/internals.hpp b/tests/internals.hpp index ec402791..9faa67e9 100644 --- a/tests/internals.hpp +++ b/tests/internals.hpp @@ -12,6 +12,18 @@ /// Please keep them in sync with the library by manually updating as necessary. namespace internals { + +struct CanardInternalTxQueueItem +{ + CanardInternalTxQueueItem* next = nullptr; + + std::uint32_t id = 0; + std::uint64_t deadline_usec = 0; + std::size_t payload_size = 0; + + std::array payload{}; // The real definition has a flex array here. +}; + // Extern C effectively discards the outer namespaces. extern "C" { diff --git a/tests/test_internals.cpp b/tests/test_internals.cpp index 6135a480..ca806235 100644 --- a/tests/test_internals.cpp +++ b/tests/test_internals.cpp @@ -150,4 +150,53 @@ TEST_CASE("makeTailByte") REQUIRE(0b010'00001 == makeTailByte(false, true, false, 1U)); } -TEST_CASE("findTxQueueSupremum") {} +TEST_CASE("findTxQueueSupremum") +{ + using internals::findTxQueueSupremum; + using TxQueueItem = internals::CanardInternalTxQueueItem; + + auto ins = + canardInit(&helpers::dummy_allocator::allocate, &helpers::dummy_allocator::free, &helpers::rejectAllRxFilter); + + const auto find = [&](std::uint32_t x) -> TxQueueItem* { return findTxQueueSupremum(&ins, x); }; + + REQUIRE(nullptr == find(0)); + REQUIRE(nullptr == find((1UL << 29U) - 1U)); + + TxQueueItem a{}; + a.id = 1000; + ins._tx_queue = reinterpret_cast(&a); + + REQUIRE(nullptr == find(999)); + REQUIRE(&a == find(1000)); + REQUIRE(&a == find(1001)); + + TxQueueItem b{}; + b.id = 1010; + a.next = &b; + + REQUIRE(nullptr == find(999)); + REQUIRE(&a == find(1000)); + REQUIRE(&a == find(1001)); + REQUIRE(&a == find(1009)); + REQUIRE(&b == find(1010)); + REQUIRE(&b == find(1011)); + + TxQueueItem c{}; + c.id = 990; + c.next = &a; + ins._tx_queue = reinterpret_cast(&c); + REQUIRE(reinterpret_cast(ins._tx_queue)->id == 990); // Make sure the list is assembled correctly. + REQUIRE(reinterpret_cast(ins._tx_queue)->next->id == 1000); + REQUIRE(reinterpret_cast(ins._tx_queue)->next->next->id == 1010); + REQUIRE(reinterpret_cast(ins._tx_queue)->next->next->next == nullptr); + + REQUIRE(nullptr == find(989)); + REQUIRE(&c == find(990)); + REQUIRE(&c == find(999)); + REQUIRE(&a == find(1000)); + REQUIRE(&a == find(1001)); + REQUIRE(&a == find(1009)); + REQUIRE(&b == find(1010)); + REQUIRE(&b == find(1011)); +} From 5274db50faa7851df645d635324e2efb4a685bce Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 9 Feb 2020 18:36:25 +0200 Subject: [PATCH 017/100] Started work on the TX test suite --- tests/CMakeLists.txt | 2 +- tests/internals.hpp | 14 -------------- tests/test_internals.cpp | 10 +++++----- tests/test_tx.cpp | 7 +++++++ 4 files changed, 13 insertions(+), 20 deletions(-) create mode 100644 tests/test_tx.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index dfdfaf2e..f4ad5b42 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -76,4 +76,4 @@ endfunction() gen_test_matrix(test_internals test_internals.cpp CANARD_EXPOSE_INTERNALS=1) -gen_test_matrix(test_general test_float16.cpp "") +gen_test_matrix(test_general "test_float16.cpp;test_tx.cpp" "") diff --git a/tests/internals.hpp b/tests/internals.hpp index 9faa67e9..59a14d9a 100644 --- a/tests/internals.hpp +++ b/tests/internals.hpp @@ -53,19 +53,5 @@ auto allocateTxQueueItem(CanardInstance* const ins, auto findTxQueueSupremum(CanardInstance* const ins, const std::uint32_t can_id) -> CanardInternalTxQueueItem*; -auto pushSingleFrameTransfer(CanardInstance* const ins, - const std::uint64_t deadline_usec, - const std::uint32_t can_id, - const std::uint8_t transfer_id, - const std::size_t payload_size, - const void* const payload) -> std::int32_t; - -auto pushMultiFrameTransfer(CanardInstance* const ins, - const std::size_t presentation_layer_mtu, - const std::uint64_t deadline_usec, - const std::uint32_t can_id, - const std::uint8_t transfer_id, - const std::size_t payload_size, - const void* const payload) -> std::int32_t; } } // namespace internals diff --git a/tests/test_internals.cpp b/tests/test_internals.cpp index ca806235..f623c906 100644 --- a/tests/test_internals.cpp +++ b/tests/test_internals.cpp @@ -90,7 +90,7 @@ TEST_CASE("makeCANID") 128U, // Invalid local node-ID. 7U)); REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Multi-frame anonymous messages are not allowed. - makeCANID(mk_transfer(CanardPriorityFast, + makeCANID(mk_transfer(CanardPriorityImmediate, CanardTransferKindMessage, 0b0011001100110011, CANARD_NODE_ID_UNSET, @@ -98,10 +98,10 @@ TEST_CASE("makeCANID") 128U, // Invalid local node-ID is treated as anonymous/unset. 7U)); REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Bad remote node-ID -- unicast messages not supported. - makeCANID(mk_transfer(CanardPriorityFast, CanardTransferKindMessage, 0b0011001100110011, 123U), 0U, 7U)); + makeCANID(mk_transfer(CanardPriorityHigh, CanardTransferKindMessage, 0b0011001100110011, 123U), 0U, 7U)); REQUIRE( -CANARD_ERROR_INVALID_ARGUMENT == // Bad subject-ID. - makeCANID(mk_transfer(CanardPriorityFast, CanardTransferKindMessage, 0xFFFFU, CANARD_NODE_ID_UNSET), 0U, 7U)); + makeCANID(mk_transfer(CanardPriorityLow, CanardTransferKindMessage, 0xFFFFU, CANARD_NODE_ID_UNSET), 0U, 7U)); REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Bad priority. makeCANID(mk_transfer(static_cast(123), CanardTransferKindMessage, @@ -125,12 +125,12 @@ TEST_CASE("makeCANID") 7U)); REQUIRE( -CANARD_ERROR_INVALID_ARGUMENT == // Broadcast service transfers not permitted. - makeCANID(mk_transfer(CanardPriorityOptional, CanardTransferKindResponse, 0b0100110011, CANARD_NODE_ID_UNSET), + makeCANID(mk_transfer(CanardPrioritySlow, CanardTransferKindResponse, 0b0100110011, CANARD_NODE_ID_UNSET), 0b1010101, 7U)); REQUIRE( -CANARD_ERROR_INVALID_ARGUMENT == // Bad service-ID. - makeCANID(mk_transfer(CanardPriorityOptional, CanardTransferKindResponse, 0xFFFFU, 0b0101010), 0b1010101, 7U)); + makeCANID(mk_transfer(CanardPriorityNominal, CanardTransferKindResponse, 0xFFFFU, 0b0101010), 0b1010101, 7U)); REQUIRE( -CANARD_ERROR_INVALID_ARGUMENT == // Bad priority. makeCANID(mk_transfer(static_cast(123), CanardTransferKindResponse, 0b0100110011, 0b0101010), diff --git a/tests/test_tx.cpp b/tests/test_tx.cpp new file mode 100644 index 00000000..4ce4df06 --- /dev/null +++ b/tests/test_tx.cpp @@ -0,0 +1,7 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. + +#include "internals.hpp" +#include "helpers.hpp" + +TEST_CASE("TxMessageSingleFrame") {} From f4d9a44977b7f797c050120fa2934e124778d004 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 9 Feb 2020 18:52:58 +0200 Subject: [PATCH 018/100] Fixed a documentation error --- libcanard/canard.h | 6 ++---- tests/test_internals.cpp | 8 +++++++- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/libcanard/canard.h b/libcanard/canard.h index 4021706f..76a0e136 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -87,8 +87,7 @@ typedef struct { /// For RX frames: reception timestamp. /// For TX frames: transmission deadline. - /// The time system may be arbitrary as long as the clock is monotonic (steady) and 0 is not a valid timestamp. - /// Zero timestamp indicates that the instance is invalid (empty). + /// The time system may be arbitrary as long as the clock is monotonic (steady). uint64_t timestamp_usec; /// 29-bit extended ID. The bits above 29-th are zero/ignored. @@ -107,8 +106,7 @@ typedef struct { /// For RX transfers: reception timestamp. /// For TX transfers: transmission deadline. - /// The time system may be arbitrary as long as the clock is monotonic (steady) and 0 is not a valid timestamp. - /// Zero timestamp indicates that the instance is invalid (empty). + /// The time system may be arbitrary as long as the clock is monotonic (steady). uint64_t timestamp_usec; CanardPriority priority; diff --git a/tests/test_internals.cpp b/tests/test_internals.cpp index f623c906..e78ea505 100644 --- a/tests/test_internals.cpp +++ b/tests/test_internals.cpp @@ -66,6 +66,12 @@ TEST_CASE("makeCANID") const auto crc123 = internals::crcAdd(0xFFFFU, 3, "\x01\x02\x03"); + union PriorityAlias + { + std::uint8_t bits; + CanardPriority prio; + }; + // MESSAGE TRANSFERS REQUIRE(0b000'00'0011001100110011'0'1010101 == // Regular message. makeCANID(mk_transfer(CanardPriorityExceptional, @@ -133,7 +139,7 @@ TEST_CASE("makeCANID") makeCANID(mk_transfer(CanardPriorityNominal, CanardTransferKindResponse, 0xFFFFU, 0b0101010), 0b1010101, 7U)); REQUIRE( -CANARD_ERROR_INVALID_ARGUMENT == // Bad priority. - makeCANID(mk_transfer(static_cast(123), CanardTransferKindResponse, 0b0100110011, 0b0101010), + makeCANID(mk_transfer(PriorityAlias{123}.prio, CanardTransferKindResponse, 0b0100110011, 0b0101010), 0b1010101, 7U)); } From c7ed98382105c2de49dd74c88f226ac4321a871b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 9 Feb 2020 18:57:51 +0200 Subject: [PATCH 019/100] Ci build fix --- libcanard/canard.h | 12 ++++++------ tests/test_internals.cpp | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/libcanard/canard.h b/libcanard/canard.h index 76a0e136..ab7987d4 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -205,8 +205,8 @@ CanardInstance canardInit(const CanardHeapAllocate heap_allocate, /// at the appropriate position. The application is supposed to take the enqueued frames from the TX buffer and /// transmit them afterwards. /// -/// Returns the number of frames enqueued (which is always a positive number) in case of success. -/// Returns a negated error code in case of failure. +/// Returns the number of frames enqueued into the prioritized TX queue (which is always a positive number) +/// in case of success. Returns a negated error code in case of failure. /// /// An invalid argument error may be returned in the following cases: /// - Any of the input arguments are NULL. @@ -233,10 +233,10 @@ int8_t canardTxPeek(const CanardInstance* const ins, CanardCANFrame* const out_f void canardTxPop(CanardInstance* const ins); -int32_t canardRxPush(CanardInstance* const ins, - const CanardCANFrame* const frame, - const uint8_t iface_index, - CanardTransfer* const out_transfer); +int8_t canardRxPush(CanardInstance* const ins, + const CanardCANFrame* const frame, + const uint8_t iface_index, + CanardTransfer* const out_transfer); #if CANARD_PLATFORM_TWOS_COMPLEMENT diff --git a/tests/test_internals.cpp b/tests/test_internals.cpp index e78ea505..bb6399fb 100644 --- a/tests/test_internals.cpp +++ b/tests/test_internals.cpp @@ -109,7 +109,7 @@ TEST_CASE("makeCANID") -CANARD_ERROR_INVALID_ARGUMENT == // Bad subject-ID. makeCANID(mk_transfer(CanardPriorityLow, CanardTransferKindMessage, 0xFFFFU, CANARD_NODE_ID_UNSET), 0U, 7U)); REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Bad priority. - makeCANID(mk_transfer(static_cast(123), + makeCANID(mk_transfer(PriorityAlias{123}.prio, CanardTransferKindMessage, 0b0011001100110011, CANARD_NODE_ID_UNSET), From f75eff2a837d769e981fd8b90f6348ce0a15437d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 9 Feb 2020 19:05:01 +0200 Subject: [PATCH 020/100] Formatting --- tests/test_internals.cpp | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/tests/test_internals.cpp b/tests/test_internals.cpp index bb6399fb..0e9f2d1a 100644 --- a/tests/test_internals.cpp +++ b/tests/test_internals.cpp @@ -68,7 +68,7 @@ TEST_CASE("makeCANID") union PriorityAlias { - std::uint8_t bits; + std::uint8_t bits; CanardPriority prio; }; @@ -129,19 +129,17 @@ TEST_CASE("makeCANID") makeCANID(mk_transfer(CanardPriorityExceptional, CanardTransferKindRequest, 0b0100110011, 0b0101010), CANARD_NODE_ID_UNSET, 7U)); - REQUIRE( - -CANARD_ERROR_INVALID_ARGUMENT == // Broadcast service transfers not permitted. - makeCANID(mk_transfer(CanardPrioritySlow, CanardTransferKindResponse, 0b0100110011, CANARD_NODE_ID_UNSET), - 0b1010101, - 7U)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Broadcast service transfers not permitted. + makeCANID(mk_transfer(CanardPrioritySlow, CanardTransferKindResponse, 0b0100110011, CANARD_NODE_ID_UNSET), + 0b1010101, + 7U)); REQUIRE( -CANARD_ERROR_INVALID_ARGUMENT == // Bad service-ID. makeCANID(mk_transfer(CanardPriorityNominal, CanardTransferKindResponse, 0xFFFFU, 0b0101010), 0b1010101, 7U)); - REQUIRE( - -CANARD_ERROR_INVALID_ARGUMENT == // Bad priority. - makeCANID(mk_transfer(PriorityAlias{123}.prio, CanardTransferKindResponse, 0b0100110011, 0b0101010), - 0b1010101, - 7U)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Bad priority. + makeCANID(mk_transfer(PriorityAlias{123}.prio, CanardTransferKindResponse, 0b0100110011, 0b0101010), + 0b1010101, + 7U)); } TEST_CASE("makeTailByte") From 534f31799db634457592ea09c1bf9596155caec8 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 10 Feb 2020 23:48:09 +0200 Subject: [PATCH 021/100] TX test WIP --- libcanard/canard.c | 2 +- libcanard/canard.h | 20 ++--- tests/.clang-tidy | 1 + tests/CMakeLists.txt | 2 +- tests/helpers.hpp | 167 +++++++++++++++++++++++++++++++++++++-- tests/internals.hpp | 13 +-- tests/test_internals.cpp | 2 +- tests/test_self.cpp | 51 ++++++++++++ tests/test_tx.cpp | 11 ++- 9 files changed, 240 insertions(+), 29 deletions(-) create mode 100644 tests/test_self.cpp diff --git a/libcanard/canard.c b/libcanard/canard.c index 2d2397ec..0da97657 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -519,8 +519,8 @@ CanardInstance canardInit(const CanardHeapAllocate heap_allocate, CANARD_ASSERT(rx_filter != NULL); const CanardInstance out = { .user_reference = NULL, - .node_id = CANARD_NODE_ID_UNSET, .mtu_bytes = CANARD_MTU_CAN_FD, + .node_id = CANARD_NODE_ID_UNSET, .heap_allocate = heap_allocate, .heap_free = heap_free, .rx_filter = rx_filter, diff --git a/libcanard/canard.h b/libcanard/canard.h index ab7987d4..652b4ba6 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -168,11 +168,6 @@ struct CanardInstance /// The default value is NULL. void* user_reference; - /// The node-ID of the local node. The default value is @ref CANARD_NODE_ID_UNSET. - /// Per the UAVCAN Specification, the node-ID should not be assigned more than once. - /// Invalid values are treated as @ref CANARD_NODE_ID_UNSET. - uint8_t node_id; - /// The maximum transmission unit. The value can be changed arbitrarily at any time. /// This setting defines the maximum number of bytes per CAN data frame in all outgoing transfers. /// Regardless of this setting, CAN frames with any MTU can always be accepted. @@ -181,7 +176,12 @@ struct CanardInstance /// otherwise, networking interoperability issues may arise. See "CANARD_MTU_*". /// Valid values are any valid CAN frame data length not smaller than 8. The default is the maximum valid value. /// Invalid values are treated as the nearest valid value. - uint8_t mtu_bytes; + size_t mtu_bytes; + + /// The node-ID of the local node. The default value is @ref CANARD_NODE_ID_UNSET. + /// Per the UAVCAN Specification, the node-ID should not be assigned more than once. + /// Invalid values are treated as @ref CANARD_NODE_ID_UNSET. + uint8_t node_id; /// Callbacks invoked by the library. See their type documentation for details. /// They SHALL be valid function pointers at all times. @@ -233,10 +233,10 @@ int8_t canardTxPeek(const CanardInstance* const ins, CanardCANFrame* const out_f void canardTxPop(CanardInstance* const ins); -int8_t canardRxPush(CanardInstance* const ins, - const CanardCANFrame* const frame, - const uint8_t iface_index, - CanardTransfer* const out_transfer); +int8_t canardRxAccept(CanardInstance* const ins, + const CanardCANFrame* const frame, + const uint8_t iface_index, + CanardTransfer* const out_transfer); #if CANARD_PLATFORM_TWOS_COMPLEMENT diff --git a/tests/.clang-tidy b/tests/.clang-tidy index 7653b14b..57a22cf9 100644 --- a/tests/.clang-tidy +++ b/tests/.clang-tidy @@ -23,6 +23,7 @@ Checks: >- -cppcoreguidelines-pro-bounds-pointer-arithmetic, -cppcoreguidelines-avoid-magic-numbers, -readability-magic-numbers, + -google-runtime-references, WarningsAsErrors: '*' HeaderFilterRegex: '.*\.hpp' AnalyzeTemporaryDtors: false diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index f4ad5b42..29d926c7 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -76,4 +76,4 @@ endfunction() gen_test_matrix(test_internals test_internals.cpp CANARD_EXPOSE_INTERNALS=1) -gen_test_matrix(test_general "test_float16.cpp;test_tx.cpp" "") +gen_test_matrix(test_general "test_float16.cpp;test_tx.cpp;test_self.cpp" "") diff --git a/tests/helpers.hpp b/tests/helpers.hpp index 64b13b0f..ab4fb91b 100644 --- a/tests/helpers.hpp +++ b/tests/helpers.hpp @@ -4,13 +4,17 @@ #pragma once #include "canard.h" +#include "internals.hpp" +#include #include +#include +#include namespace helpers { namespace dummy_allocator { -inline auto allocate(CanardInstance* const ins, std::size_t const amount) -> void* +inline auto allocate(CanardInstance* const ins, const std::size_t amount) -> void* { (void) ins; (void) amount; @@ -24,10 +28,10 @@ inline void free(CanardInstance* const ins, void* const pointer) } } // namespace dummy_allocator -inline auto rejectAllRxFilter(const CanardInstance* ins, - std::uint16_t port_id, - CanardTransferKind transfer_kind, - std::uint8_t source_node_id) -> CanardRxMetadata +inline auto rejectAllRxFilter(const CanardInstance* const ins, + const std::uint16_t port_id, + const CanardTransferKind transfer_kind, + const std::uint8_t source_node_id) -> CanardRxMetadata { (void) ins; (void) port_id; @@ -36,4 +40,157 @@ inline auto rejectAllRxFilter(const CanardInstance* ins, return CanardRxMetadata{}; } +class TestAllocator +{ + std::unordered_map allocated_; + std::size_t ceiling_ = std::numeric_limits::max(); + +public: + TestAllocator() = default; + TestAllocator(const TestAllocator&) = delete; + TestAllocator(const TestAllocator&&) = delete; + auto operator=(const TestAllocator&) -> TestAllocator& = delete; + auto operator=(const TestAllocator &&) -> TestAllocator& = delete; + + virtual ~TestAllocator() + { + for (auto [ptr, _] : allocated_) + { + // Clang-tidy complains about manual memory management. Suppressed because we need it for testing purposes. + std::free(ptr); // NOLINT + } + } + + [[nodiscard]] auto allocate(const std::size_t amount) + { + void* p = nullptr; + if ((getTotalAllocatedAmount() + amount) <= ceiling_) + { + // Clang-tidy complains about manual memory management. Suppressed because we need it for testing purposes. + p = std::malloc(amount); // NOLINT + if (p == nullptr) + { + throw std::bad_alloc(); // This is a test suite failure, not a failed test. Mind the difference. + } + allocated_.emplace(p, amount); + } + return p; + } + + void deallocate(void* const pointer) + { + const auto it = allocated_.find(pointer); + if (it == std::end(allocated_)) + { + throw std::logic_error("Heap corruption: an attempt to deallocate memory that is not allocated"); + } + // Clang-tidy complains about manual memory management. Suppressed because we need it for testing purposes. + std::free(it->first); // NOLINT + allocated_.erase(it); + } + + [[nodiscard]] auto getNumAllocatedFragments() const { return std::size(allocated_); } + + [[nodiscard]] auto getTotalAllocatedAmount() const -> std::size_t + { + std::size_t out = 0U; + for (auto [ptr, size] : allocated_) + { + out += size; + } + return out; + } + + [[nodiscard]] auto getAllocationCeiling() const { return ceiling_; } + void setAllocationCeiling(const std::size_t amount) { ceiling_ = amount; } +}; + +/// An enhancing wrapper over the library to remove boilerplate from the tests. +class Instance +{ + CanardInstance canard_ = canardInit(&Instance::trampolineAllocate, // + &Instance::trampolineDeallocate, + &Instance::trampolineApplyRxFilter); + TestAllocator allocator_; + + static auto trampolineAllocate(CanardInstance* const ins, const std::size_t amount) -> void* + { + auto p = reinterpret_cast(ins->user_reference); + return p->allocator_.allocate(amount); + } + + static void trampolineDeallocate(CanardInstance* const ins, void* const pointer) + { + auto p = reinterpret_cast(ins->user_reference); + p->allocator_.deallocate(pointer); + } + + static auto trampolineApplyRxFilter(const CanardInstance* const ins, + const std::uint16_t port_id, + const CanardTransferKind transfer_kind, + const std::uint8_t source_node_id) -> CanardRxMetadata + { + auto p = reinterpret_cast(ins->user_reference); + return p->applyRxFilter(port_id, transfer_kind, source_node_id); + } + +protected: + /// The default behavior is to discard everything. Override to change. + [[nodiscard]] virtual auto applyRxFilter(const std::uint16_t port_id, + const CanardTransferKind transfer_kind, + const std::uint8_t source_node_id) const -> CanardRxMetadata + { + (void) port_id; + (void) transfer_kind; + (void) source_node_id; + return {}; + } + +public: + Instance() { canard_.user_reference = this; } + + virtual ~Instance() = default; + + Instance(const Instance&) = delete; + Instance(const Instance&&) = delete; + auto operator=(const Instance&) -> Instance& = delete; + auto operator=(const Instance &&) -> Instance& = delete; + + [[nodiscard]] auto txPush(const CanardTransfer& transfer) { return canardTxPush(&canard_, &transfer); } + + [[nodiscard]] auto txPeek(CanardCANFrame& out_frame) const { return canardTxPeek(&canard_, &out_frame); } + + void txPop() { canardTxPop(&canard_); } + + [[nodiscard]] auto rxAccept(const CanardCANFrame& frame, const uint8_t iface_index, CanardTransfer& out_transfer) + { + return canardRxAccept(&canard_, &frame, iface_index, &out_transfer); + } + + [[nodiscard]] auto getNodeID() const { return canard_.node_id; } + void setNodeID(const std::uint8_t x) { canard_.node_id = x; } + + [[nodiscard]] auto getMTU() const { return canard_.mtu_bytes; } + void setMTU(const std::size_t x) { canard_.mtu_bytes = x; } + + [[nodiscard]] auto getTxQueueRoot() const + { + return reinterpret_cast(canard_._tx_queue); + } + + [[nodiscard]] auto getTxQueueLength() const + { + std::size_t out = 0U; + auto p = getTxQueueRoot(); + while (p != nullptr) + { + ++out; + p = p->next; + } + return out; + } + + [[nodiscard]] auto getAllocator() -> TestAllocator& { return allocator_; } +}; + } // namespace helpers diff --git a/tests/internals.hpp b/tests/internals.hpp index 59a14d9a..73255a85 100644 --- a/tests/internals.hpp +++ b/tests/internals.hpp @@ -12,10 +12,9 @@ /// Please keep them in sync with the library by manually updating as necessary. namespace internals { - -struct CanardInternalTxQueueItem +struct TxQueueItem { - CanardInternalTxQueueItem* next = nullptr; + TxQueueItem* next = nullptr; std::uint32_t id = 0; std::uint64_t deadline_usec = 0; @@ -46,12 +45,6 @@ auto makeTailByte(const bool start_of_transfer, const bool toggle, const std::uint8_t transfer_id) -> std::uint8_t; -auto allocateTxQueueItem(CanardInstance* const ins, - const std::uint32_t id, - const std::uint64_t deadline_usec, - const std::size_t payload_size) -> CanardInternalTxQueueItem*; - -auto findTxQueueSupremum(CanardInstance* const ins, const std::uint32_t can_id) -> CanardInternalTxQueueItem*; - +auto findTxQueueSupremum(CanardInstance* const ins, const std::uint32_t can_id) -> TxQueueItem*; } } // namespace internals diff --git a/tests/test_internals.cpp b/tests/test_internals.cpp index 0e9f2d1a..21dc2f84 100644 --- a/tests/test_internals.cpp +++ b/tests/test_internals.cpp @@ -157,7 +157,7 @@ TEST_CASE("makeTailByte") TEST_CASE("findTxQueueSupremum") { using internals::findTxQueueSupremum; - using TxQueueItem = internals::CanardInternalTxQueueItem; + using TxQueueItem = internals::TxQueueItem; auto ins = canardInit(&helpers::dummy_allocator::allocate, &helpers::dummy_allocator::free, &helpers::rejectAllRxFilter); diff --git a/tests/test_self.cpp b/tests/test_self.cpp new file mode 100644 index 00000000..31951604 --- /dev/null +++ b/tests/test_self.cpp @@ -0,0 +1,51 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. + +#include "internals.hpp" +#include "helpers.hpp" + +TEST_CASE("TestAllocator") +{ + helpers::TestAllocator al; + + REQUIRE(0 == al.getNumAllocatedFragments()); + REQUIRE(std::numeric_limits::max() == al.getAllocationCeiling()); + + auto a = al.allocate(123); + REQUIRE(1 == al.getNumAllocatedFragments()); + REQUIRE(123 == al.getTotalAllocatedAmount()); + + auto b = al.allocate(456); + REQUIRE(2 == al.getNumAllocatedFragments()); + REQUIRE(579 == al.getTotalAllocatedAmount()); + + al.setAllocationCeiling(600); + + REQUIRE(nullptr == al.allocate(100)); + REQUIRE(2 == al.getNumAllocatedFragments()); + REQUIRE(579 == al.getTotalAllocatedAmount()); + + auto c = al.allocate(21); + REQUIRE(3 == al.getNumAllocatedFragments()); + REQUIRE(600 == al.getTotalAllocatedAmount()); + + al.deallocate(a); + REQUIRE(2 == al.getNumAllocatedFragments()); + REQUIRE(477 == al.getTotalAllocatedAmount()); + + auto d = al.allocate(100); + REQUIRE(3 == al.getNumAllocatedFragments()); + REQUIRE(577 == al.getTotalAllocatedAmount()); + + al.deallocate(c); + REQUIRE(2 == al.getNumAllocatedFragments()); + REQUIRE(556 == al.getTotalAllocatedAmount()); + + al.deallocate(d); + REQUIRE(1 == al.getNumAllocatedFragments()); + REQUIRE(456 == al.getTotalAllocatedAmount()); + + al.deallocate(b); + REQUIRE(0 == al.getNumAllocatedFragments()); + REQUIRE(0 == al.getTotalAllocatedAmount()); +} diff --git a/tests/test_tx.cpp b/tests/test_tx.cpp index 4ce4df06..8c5e8cad 100644 --- a/tests/test_tx.cpp +++ b/tests/test_tx.cpp @@ -4,4 +4,13 @@ #include "internals.hpp" #include "helpers.hpp" -TEST_CASE("TxMessageSingleFrame") {} +TEST_CASE("TxMessageSingleFrame") +{ + helpers::Instance ins; + + REQUIRE(CANARD_NODE_ID_UNSET == ins.getNodeID()); + REQUIRE(CANARD_MTU_CAN_FD == ins.getMTU()); + REQUIRE(0 == ins.getAllocator().getNumAllocatedFragments()); + REQUIRE(nullptr == ins.getTxQueueRoot()); + REQUIRE(0 == ins.getTxQueueLength()); +} From 91d85899e05ee1c22e1b71931ed886a1bbaa800e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 11 Feb 2020 04:53:25 +0200 Subject: [PATCH 022/100] TX test, partial --- libcanard/canard.c | 2 + libcanard/canard.h | 2 +- tests/.clang-tidy | 7 ++-- tests/helpers.hpp | 2 + tests/internals.hpp | 25 ++++++++++- tests/test_self.cpp | 7 ++++ tests/test_tx.cpp | 100 +++++++++++++++++++++++++++++++++++++++++++- 7 files changed, 138 insertions(+), 7 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 0da97657..d54c1d5e 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -428,11 +428,13 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, { tail->payload[frame_offset] = (uint8_t)(crc & BYTE_MAX); ++frame_offset; + ++offset; } if ((frame_offset < frame_payload_size) && (offset > payload_size)) { tail->payload[frame_offset] = (uint8_t)(crc >> BITS_PER_BYTE); ++frame_offset; + ++offset; } } diff --git a/libcanard/canard.h b/libcanard/canard.h index 652b4ba6..3b7bc884 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -206,7 +206,7 @@ CanardInstance canardInit(const CanardHeapAllocate heap_allocate, /// transmit them afterwards. /// /// Returns the number of frames enqueued into the prioritized TX queue (which is always a positive number) -/// in case of success. Returns a negated error code in case of failure. +/// in case of success. Returns a negated error code in case of failure. Zero cannot be returned. /// /// An invalid argument error may be returned in the following cases: /// - Any of the input arguments are NULL. diff --git a/tests/.clang-tidy b/tests/.clang-tidy index 57a22cf9..dbad0065 100644 --- a/tests/.clang-tidy +++ b/tests/.clang-tidy @@ -16,14 +16,15 @@ Checks: >- portability-*, readability-*, -google-readability-todo, + -google-runtime-references, -readability-avoid-const-params-in-decls, + -readability-magic-numbers, -llvm-header-guard, - -cppcoreguidelines-pro-type-reinterpret-cast, -misc-non-private-member-variables-in-classes, -cppcoreguidelines-pro-bounds-pointer-arithmetic, -cppcoreguidelines-avoid-magic-numbers, - -readability-magic-numbers, - -google-runtime-references, + -cppcoreguidelines-pro-type-union-access, + -cppcoreguidelines-pro-type-reinterpret-cast, WarningsAsErrors: '*' HeaderFilterRegex: '.*\.hpp' AnalyzeTemporaryDtors: false diff --git a/tests/helpers.hpp b/tests/helpers.hpp index ab4fb91b..278c183a 100644 --- a/tests/helpers.hpp +++ b/tests/helpers.hpp @@ -40,6 +40,8 @@ inline auto rejectAllRxFilter(const CanardInstance* const ins, return CanardRxMetadata{}; } +/// An allocator that sits on top of the standard malloc() providing additional testing capabilities. +/// It allows the user to specify the maximum amount of memory that can be allocated; further requests will emulate OOM. class TestAllocator { std::unordered_map allocated_; diff --git a/tests/internals.hpp b/tests/internals.hpp index 73255a85..05c7c9ae 100644 --- a/tests/internals.hpp +++ b/tests/internals.hpp @@ -12,7 +12,7 @@ /// Please keep them in sync with the library by manually updating as necessary. namespace internals { -struct TxQueueItem +struct TxQueueItem final { TxQueueItem* next = nullptr; @@ -21,6 +21,29 @@ struct TxQueueItem std::size_t payload_size = 0; std::array payload{}; // The real definition has a flex array here. + + [[nodiscard]] auto getPayloadByte(const std::size_t offset) const -> std::uint8_t + { + return *(payload.data() + offset); + } + + [[nodiscard]] auto getTailByte() const + { + REQUIRE(payload_size >= 1U); + return getPayloadByte(payload_size - 1U); + } + + [[nodiscard]] auto isStartOfTransfer() const { return (getTailByte() & 128U) != 0; } + [[nodiscard]] auto isEndOfTransfer() const { return (getTailByte() & 64U) != 0; } + [[nodiscard]] auto isToggleBitSet() const { return (getTailByte() & 32U) != 0; } + + // Instances of this type cannot be constructed by the test suite. It is designed for aliasing only. + TxQueueItem() = delete; + ~TxQueueItem() = delete; + TxQueueItem(const TxQueueItem&) = delete; + TxQueueItem(const TxQueueItem&&) = delete; + auto operator=(const TxQueueItem&) -> TxQueueItem& = delete; + auto operator=(const TxQueueItem &&) -> TxQueueItem& = delete; }; // Extern C effectively discards the outer namespaces. diff --git a/tests/test_self.cpp b/tests/test_self.cpp index 31951604..f08e3013 100644 --- a/tests/test_self.cpp +++ b/tests/test_self.cpp @@ -37,6 +37,8 @@ TEST_CASE("TestAllocator") REQUIRE(3 == al.getNumAllocatedFragments()); REQUIRE(577 == al.getTotalAllocatedAmount()); + REQUIRE_THROWS_AS(al.deallocate(a), std::logic_error); + al.deallocate(c); REQUIRE(2 == al.getNumAllocatedFragments()); REQUIRE(556 == al.getTotalAllocatedAmount()); @@ -48,4 +50,9 @@ TEST_CASE("TestAllocator") al.deallocate(b); REQUIRE(0 == al.getNumAllocatedFragments()); REQUIRE(0 == al.getTotalAllocatedAmount()); + + REQUIRE_THROWS_AS(al.deallocate(a), std::logic_error); + REQUIRE_THROWS_AS(al.deallocate(b), std::logic_error); + REQUIRE_THROWS_AS(al.deallocate(c), std::logic_error); + REQUIRE_THROWS_AS(al.deallocate(d), std::logic_error); } diff --git a/tests/test_tx.cpp b/tests/test_tx.cpp index 8c5e8cad..3e84a24f 100644 --- a/tests/test_tx.cpp +++ b/tests/test_tx.cpp @@ -4,13 +4,109 @@ #include "internals.hpp" #include "helpers.hpp" -TEST_CASE("TxMessageSingleFrame") +TEST_CASE("TxBasic") { + using internals::TxQueueItem; + helpers::Instance ins; + auto& alloc = ins.getAllocator(); + + std::array payload{}; + for (std::size_t i = 0; i < std::size(payload); i++) + { + payload.at(i) = static_cast(i & 0xFFU); + } + REQUIRE(CANARD_NODE_ID_UNSET == ins.getNodeID()); REQUIRE(CANARD_MTU_CAN_FD == ins.getMTU()); - REQUIRE(0 == ins.getAllocator().getNumAllocatedFragments()); REQUIRE(nullptr == ins.getTxQueueRoot()); REQUIRE(0 == ins.getTxQueueLength()); + REQUIRE(0 == alloc.getNumAllocatedFragments()); + + alloc.setAllocationCeiling(200); + + CanardTransfer transfer{}; + transfer.payload = payload.data(); + + // Single-frame, success. + transfer.timestamp_usec = 1'000'000'000'000ULL; + transfer.priority = CanardPriorityNominal; + transfer.transfer_kind = CanardTransferKindMessage; + transfer.port_id = 321; + transfer.remote_node_id = CANARD_NODE_ID_UNSET; + transfer.transfer_id = 21; + transfer.payload_size = 8; + REQUIRE(1 == ins.txPush(transfer)); + REQUIRE(1 == ins.getTxQueueLength()); + REQUIRE(1 == alloc.getNumAllocatedFragments()); + REQUIRE(10 < alloc.getTotalAllocatedAmount()); + REQUIRE(80 > alloc.getTotalAllocatedAmount()); + REQUIRE(ins.getTxQueueRoot()->deadline_usec == 1'000'000'000'000ULL); + REQUIRE(ins.getTxQueueRoot()->payload_size == 9); + REQUIRE(ins.getTxQueueRoot()->isStartOfTransfer()); + REQUIRE(ins.getTxQueueRoot()->isEndOfTransfer()); + REQUIRE(ins.getTxQueueRoot()->isToggleBitSet()); + + // Multi-frame, success. Priority low, inserted at the end of the TX queue. + transfer.timestamp_usec = 1'000'000'000'100ULL; + transfer.priority = CanardPriorityLow; + transfer.transfer_id = 22; + transfer.payload_size = 8; + ins.setMTU(CANARD_MTU_CAN_CLASSIC); + ins.setNodeID(42); + REQUIRE(2 == ins.txPush(transfer)); // 8 bytes --> 2 frames + REQUIRE(3 == ins.getTxQueueLength()); + REQUIRE(3 == alloc.getNumAllocatedFragments()); + REQUIRE(20 < alloc.getTotalAllocatedAmount()); + REQUIRE(200 > alloc.getTotalAllocatedAmount()); + + // Check the TX queue. + { + auto q = ins.getTxQueueRoot(); + REQUIRE(q != nullptr); + REQUIRE(q->deadline_usec == 1'000'000'000'000ULL); + REQUIRE(q->payload_size == 9); + REQUIRE(q->isStartOfTransfer()); + REQUIRE(q->isEndOfTransfer()); + REQUIRE(q->isToggleBitSet()); + q = q->next; + REQUIRE(q != nullptr); + REQUIRE(q->deadline_usec == 1'000'000'000'100ULL); + REQUIRE(q->payload_size == 8); + REQUIRE(q->isStartOfTransfer()); + REQUIRE(!q->isEndOfTransfer()); + REQUIRE(q->isToggleBitSet()); + q = q->next; + REQUIRE(q != nullptr); + REQUIRE(q->deadline_usec == 1'000'000'000'100ULL); + REQUIRE(q->payload_size == 4); // One leftover, two CRC, one tail. + REQUIRE(!q->isStartOfTransfer()); + REQUIRE(q->isEndOfTransfer()); + REQUIRE(!q->isToggleBitSet()); + q = q->next; + REQUIRE(q == nullptr); + } + + // Single-frame, OOM. + alloc.setAllocationCeiling(alloc.getTotalAllocatedAmount()); // Seal up the heap at this level. + transfer.timestamp_usec = 1'000'000'000'200ULL; + transfer.priority = CanardPriorityLow; + transfer.transfer_id = 23; + transfer.payload_size = 1; + REQUIRE(-CANARD_ERROR_OUT_OF_MEMORY == ins.txPush(transfer)); + REQUIRE(3 == ins.getTxQueueLength()); + REQUIRE(3 == alloc.getNumAllocatedFragments()); + + // Multi-frame, first frame added successfully, then OOM. The entire transaction rejected. + alloc.setAllocationCeiling(alloc.getTotalAllocatedAmount() + sizeof(TxQueueItem) + 10U); + transfer.timestamp_usec = 1'000'000'000'300ULL; + transfer.priority = CanardPriorityHigh; + transfer.transfer_id = 24; + transfer.payload_size = 100; + REQUIRE(-CANARD_ERROR_OUT_OF_MEMORY == ins.txPush(transfer)); + REQUIRE(3 == ins.getTxQueueLength()); + REQUIRE(3 == alloc.getNumAllocatedFragments()); + REQUIRE(20 < alloc.getTotalAllocatedAmount()); + REQUIRE(200 > alloc.getTotalAllocatedAmount()); } From 45f8b404633bb16a7b1a85e64e8896ba46ae9a0f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 11 Feb 2020 05:46:24 +0200 Subject: [PATCH 023/100] TX queue popping --- libcanard/canard.c | 31 +++++++++++++++++++++++++ libcanard/canard.h | 56 ++++++++++++++++++++++++++++++++++++++++++--- tests/internals.hpp | 4 +--- tests/test_tx.cpp | 3 +++ 4 files changed, 88 insertions(+), 6 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index d54c1d5e..5a2a00fc 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -569,6 +569,37 @@ int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const tran return out; } +int8_t canardTxPeek(const CanardInstance* const ins, CanardCANFrame* const out_frame) +{ + int8_t out = -CANARD_ERROR_INVALID_ARGUMENT; + if ((ins != NULL) && (out_frame != NULL)) + { + CanardInternalTxQueueItem* const tqi = ins->_tx_queue; + if (tqi != NULL) + { + out_frame->timestamp_usec = tqi->deadline_usec; + out_frame->extended_can_id = tqi->id; + out_frame->payload_size = tqi->payload_size; + out_frame->payload = &tqi->payload[0]; + out = 1; + } + else + { + out = 0; + } + } + return out; +} + +void canardTxPop(CanardInstance* const ins) +{ + if ((ins != NULL) && (ins->_tx_queue != NULL)) + { + ins->heap_free(ins, ins->_tx_queue); + ins->_tx_queue = ins->_tx_queue->next; + } +} + // ---------------------------------------- FLOAT16 SERIALIZATION ---------------------------------------- #if CANARD_PLATFORM_IEEE754 diff --git a/libcanard/canard.h b/libcanard/canard.h index 3b7bc884..be24782f 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -185,6 +185,14 @@ struct CanardInstance /// Callbacks invoked by the library. See their type documentation for details. /// They SHALL be valid function pointers at all times. + /// + /// The time complexity parameters given in the API documentation are made on the assumption that + /// the heap management functions (allocate and free) have constant complexity. + /// + /// There are only two API functions that may lead to allocation of heap memory: + /// - @ref canardTxPush() + /// - @ref canardRxAccept() + /// Their exact memory requirement model is specified in their documentation. CanardHeapAllocate heap_allocate; CanardHeapFree heap_free; CanardRxFilter rx_filter; @@ -196,14 +204,20 @@ struct CanardInstance /// Initialize a new library instance. /// The default values will be assigned as specified in the structure field documentation. +/// The time complexity parameters given in the API documentation are made on the assumption that the heap management +/// functions (allocate and free) have constant complexity. /// If any of the pointers are NULL, the behavior is undefined. CanardInstance canardInit(const CanardHeapAllocate heap_allocate, const CanardHeapFree heap_free, const CanardRxFilter rx_filter); -/// Takes a transfer, serializes it into a sequence of CAN frames, and inserts them into the prioritized TX queue -/// at the appropriate position. The application is supposed to take the enqueued frames from the TX buffer and -/// transmit them afterwards. +/// Takes a transfer, serializes it into a sequence of transport frames, and inserts them into the prioritized +/// transmission queue at the appropriate position. Afterwards, the application is supposed to take the enqueued +/// frames from the transmission queue using the function @ref canardTxPeek() and transmit them. Each transmitted +/// (or otherwise discarded, e.g., due to timeout) frame should be removed from the queue using @ref canardTxPop(). +/// +/// The MTU of the generated frames is dependent on the value of the MTU setting at the time when this function +/// is invoked. /// /// Returns the number of frames enqueued into the prioritized TX queue (which is always a positive number) /// in case of success. Returns a negated error code in case of failure. Zero cannot be returned. @@ -227,10 +241,46 @@ CanardInstance canardInit(const CanardHeapAllocate heap_allocate, /// /// The time complexity is O(s+t), where s is the amount of payload in the transfer, and t is the number of /// frames already enqueued in the transmission queue. +/// +/// The heap memory requirement is one allocation per transport frame. In other words, a single-frame transfer takes +/// one allocation; a multi-frame transfer of N frames takes N allocations. The maximum size of each allocation is +/// sizeof(CanardInternalTxQueueItem) plus MTU. int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfer); +/// Access the top element of the prioritized transmission queue. The queue itself is not modified (i.e., the +/// accessed element is not removed). The application should invoke this function to collect the transport frames +/// of serialized transfers stored into the prioritized transmission queue by @ref canardTxPush(). +/// +/// If the queue is empty, the return value is zero and the out_frame is not modified. +/// +/// If the queue is non-empty, the return value is 1 (one) and the out_frame is populated with the data from +/// the top element (i.e., the next frame awaiting transmission). +/// The payload pointer of the out_frame will point to the data buffer of the accessed frame; +/// the pointer retains validity until the element is removed from the queue by calling @ref canardTxPop(). +/// The payload pointer retains validity even if more frames are added to the transmission queue. +/// If the returned frame instance is not needed, it can be dropped -- no deinitialization procedures are needed +/// since it does not own any memory itself. +/// +/// If either of the arguments are NULL, the negated invalid argument error code is returned and no other +/// actions are performed. +/// +/// The time complexity is constant. int8_t canardTxPeek(const CanardInstance* const ins, CanardCANFrame* const out_frame); +/// Remove and free the top element from the prioritized transmission queue. +/// The application should invoke this function after the top frame obtained through @ref canardTxPeek() has been +/// processed and need not be kept anymore (e.g., transmitted successfully, timed out, errored, etc.). +/// +/// WARNING: +/// Invocation of @ref canardTxPush() may add new elements at the top of the prioritized transmission queue. +/// The calling code shall take that into account to eliminate the possibility of data loss due to the frame +/// at the top of the queue being unexpectedly replaced between calls of @ref canardTxPeek() and this function. +/// +/// Invocation of this function invalidates the payload pointer of the top frame because the underlying buffer is freed. +/// +/// If the input argument is NULL or if the transmission queue is empty, the function has no effect. +/// +/// The time complexity is constant. void canardTxPop(CanardInstance* const ins); int8_t canardRxAccept(CanardInstance* const ins, diff --git a/tests/internals.hpp b/tests/internals.hpp index 05c7c9ae..bd4b3a1f 100644 --- a/tests/internals.hpp +++ b/tests/internals.hpp @@ -37,9 +37,7 @@ struct TxQueueItem final [[nodiscard]] auto isEndOfTransfer() const { return (getTailByte() & 64U) != 0; } [[nodiscard]] auto isToggleBitSet() const { return (getTailByte() & 32U) != 0; } - // Instances of this type cannot be constructed by the test suite. It is designed for aliasing only. - TxQueueItem() = delete; - ~TxQueueItem() = delete; + ~TxQueueItem() = default; TxQueueItem(const TxQueueItem&) = delete; TxQueueItem(const TxQueueItem&&) = delete; auto operator=(const TxQueueItem&) -> TxQueueItem& = delete; diff --git a/tests/test_tx.cpp b/tests/test_tx.cpp index 3e84a24f..559f3f13 100644 --- a/tests/test_tx.cpp +++ b/tests/test_tx.cpp @@ -109,4 +109,7 @@ TEST_CASE("TxBasic") REQUIRE(3 == alloc.getNumAllocatedFragments()); REQUIRE(20 < alloc.getTotalAllocatedAmount()); REQUIRE(200 > alloc.getTotalAllocatedAmount()); + + // Pop the queue. + // TODO } From a6679b6591ee70d4a0e00a58333b2ca88eac14fa Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 11 Feb 2020 10:47:18 +0200 Subject: [PATCH 024/100] Fix SonarQube warnings --- libcanard/canard.c | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 5a2a00fc..60475b7d 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -299,9 +299,13 @@ CANARD_INTERNAL int32_t pushSingleFrameTransfer(CanardInstance* const ins, CanardInternalTxQueueItem* const tqi = allocateTxQueueItem(ins, can_id, deadline_usec, payload_size + 1U); if (tqi != NULL) { - // Clang-Tidy raises an error recommending the use of memcpy_s() instead. - // We ignore this recommendation because it is not available in C99. - (void) memcpy(&tqi->payload[0], payload, payload_size); // NOLINT + if (payload_size > 0U) // The check is needed to avoid calling memcpy() with a NULL pointer, it's an UB. + { + CANARD_ASSERT(payload != NULL); + // Clang-Tidy raises an error recommending the use of memcpy_s() instead. + // We ignore this recommendation because it is not available in C99. + (void) memcpy(&tqi->payload[0], payload, payload_size); // NOLINT + } tqi->payload[payload_size] = makeTailByte(true, true, true, transfer_id); CanardInternalTxQueueItem* const sup = findTxQueueSupremum(ins, can_id); if (sup != NULL) @@ -367,10 +371,11 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, const size_t remaining_payload_with_crc = payload_size_with_crc - offset; if (remaining_payload_with_crc < presentation_layer_mtu) { - const size_t index = remaining_payload_with_crc + 1U; + size_t index = remaining_payload_with_crc + 1U; // Round up to accommodate padding. CANARD_ASSERT(index < sizeof(CanardCANLengthToDLC)); - // Round up to accommodate padding. - frame_payload_size_with_tail = CanardCANDLCToLength[CanardCANLengthToDLC[index]]; + index = CanardCANLengthToDLC[index]; + CANARD_ASSERT(index < sizeof(CanardCANDLCToLength)); + frame_payload_size_with_tail = CanardCANDLCToLength[index]; } } From aedd4f81338f9f5171b058b5dc544468c5ef727d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 11 Feb 2020 10:59:45 +0200 Subject: [PATCH 025/100] Fix minor SonarQube warnings --- libcanard/canard.c | 72 ++++++++++++++++++++++----------------------- tests/internals.hpp | 2 +- 2 files changed, 37 insertions(+), 37 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 60475b7d..b9689ec6 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -162,7 +162,7 @@ CANARD_INTERNAL int32_t makeCANID(const CanardTransfer* const tr, CANARD_ASSERT(tr != NULL); CANARD_ASSERT(presentation_layer_mtu > 0); int32_t out = -CANARD_ERROR_INVALID_ARGUMENT; - if ((tr->transfer_kind == CanardTransferKindMessage) && (tr->remote_node_id == CANARD_NODE_ID_UNSET) && + if ((tr->transfer_kind == CanardTransferKindMessage) && (CANARD_NODE_ID_UNSET == tr->remote_node_id) && (tr->port_id <= CANARD_SUBJECT_ID_MAX)) { if (local_node_id <= CANARD_NODE_ID_MAX) @@ -173,9 +173,10 @@ CANARD_INTERNAL int32_t makeCANID(const CanardTransfer* const tr, else if (tr->payload_size <= presentation_layer_mtu) { CANARD_ASSERT((tr->payload != NULL) || (tr->payload_size == 0U)); - const uint8_t c = (uint8_t)(crcAdd(CRC_INITIAL, tr->payload_size, tr->payload) & CANARD_NODE_ID_MAX); - out = (int32_t)(makeMessageSessionSpecifier(tr->port_id, c) | FLAG_ANONYMOUS_MESSAGE); - CANARD_ASSERT(out >= 0); + const uint8_t c = (uint8_t)(crcAdd(CRC_INITIAL, tr->payload_size, tr->payload) & CANARD_NODE_ID_MAX); + const uint32_t spec = makeMessageSessionSpecifier(tr->port_id, c) | FLAG_ANONYMOUS_MESSAGE; + CANARD_ASSERT(spec <= CAN_EXT_ID_MASK); + out = (int32_t) spec; } else { @@ -208,7 +209,8 @@ CANARD_INTERNAL int32_t makeCANID(const CanardTransfer* const tr, const uint32_t prio = (uint32_t) tr->priority; if (prio <= CANARD_PRIORITY_MAX) { - out = (int32_t)(((uint32_t) out) | (prio << OFFSET_PRIORITY)); + const uint32_t id = ((uint32_t) out) | (prio << OFFSET_PRIORITY); + out = (int32_t) id; } else { @@ -257,13 +259,13 @@ CANARD_INTERNAL CanardInternalTxQueueItem* allocateTxQueueItem(CanardInstance* c /// Returns the element after which new elements with the specified CAN ID should be inserted. /// Returns NULL if the element shall be inserted in the beginning of the list (i.e., no prior elements). -CANARD_INTERNAL CanardInternalTxQueueItem* findTxQueueSupremum(CanardInstance* const ins, const uint32_t can_id); -CANARD_INTERNAL CanardInternalTxQueueItem* findTxQueueSupremum(CanardInstance* const ins, const uint32_t can_id) +CANARD_INTERNAL CanardInternalTxQueueItem* findTxQueueSupremum(const CanardInstance* const ins, const uint32_t can_id); +CANARD_INTERNAL CanardInternalTxQueueItem* findTxQueueSupremum(const CanardInstance* const ins, const uint32_t can_id) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(can_id <= CAN_EXT_ID_MASK); CanardInternalTxQueueItem* out = ins->_tx_queue; - if ((out == NULL) || (out->id > can_id)) + if ((NULL == out) || (out->id > can_id)) { out = NULL; } @@ -367,33 +369,29 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, // Compute the required size of the frame payload for this frame, including CRC and padding. size_t frame_payload_size_with_tail = presentation_layer_mtu + 1U; + if ((payload_size_with_crc - offset) < presentation_layer_mtu) { - const size_t remaining_payload_with_crc = payload_size_with_crc - offset; - if (remaining_payload_with_crc < presentation_layer_mtu) - { - size_t index = remaining_payload_with_crc + 1U; // Round up to accommodate padding. - CANARD_ASSERT(index < sizeof(CanardCANLengthToDLC)); - index = CanardCANLengthToDLC[index]; - CANARD_ASSERT(index < sizeof(CanardCANDLCToLength)); - frame_payload_size_with_tail = CanardCANDLCToLength[index]; - } + size_t index = payload_size_with_crc - offset + 1U; + CANARD_ASSERT(index < sizeof(CanardCANLengthToDLC)); + index = CanardCANLengthToDLC[index]; + CANARD_ASSERT(index < sizeof(CanardCANDLCToLength)); + frame_payload_size_with_tail = CanardCANDLCToLength[index]; // Round up to accommodate padding. } // Allocate the storage using the above computed size. Link all frames into a list. + + CanardInternalTxQueueItem* const tqi = + allocateTxQueueItem(ins, can_id, deadline_usec, frame_payload_size_with_tail); + if (NULL == head) { - CanardInternalTxQueueItem* const tqi = - allocateTxQueueItem(ins, can_id, deadline_usec, frame_payload_size_with_tail); - if (head == NULL) - { - head = tqi; - } - else - { - tail->next = tqi; - } - tail = tqi; + head = tqi; + } + else + { + tail->next = tqi; } - if (tail == NULL) + tail = tqi; + if (NULL == tail) { break; } @@ -455,7 +453,7 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, CANARD_ASSERT(head->next != NULL); // This is not a single-frame transfer so at least two frames shall exist. CANARD_ASSERT(tail->next == NULL); // The list shall be properly terminated. CanardInternalTxQueueItem* const sup = findTxQueueSupremum(ins, can_id); - if (sup == NULL) // Once the insertion point is located, we insert the entire frame sequence in constant time. + if (NULL == sup) // Once the insertion point is located, we insert the entire frame sequence in constant time. { tail->next = ins->_tx_queue; ins->_tx_queue = head; @@ -622,10 +620,11 @@ static_assert(4 == sizeof(Float32Bits), "Native float format shall match IEEE 75 uint16_t canardDSDLFloat16Serialize(const CanardIEEE754Binary32 value) { // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. + // The no-lint statements suppress the warning about the use of union. This is required for low-level bit access. const uint32_t round_mask = ~(uint32_t) 0x0FFFU; // NOLINT - const Float32Bits f32inf = {.bits = ((uint32_t) 255U) << 23U}; // NOLINT - const Float32Bits f16inf = {.bits = ((uint32_t) 31U) << 23U}; // NOLINT - const Float32Bits magic = {.bits = ((uint32_t) 15U) << 23U}; // NOLINT + const Float32Bits f32inf = {.bits = ((uint32_t) 255U) << 23U}; // NOLINT NOSONAR + const Float32Bits f16inf = {.bits = ((uint32_t) 31U) << 23U}; // NOLINT NOSONAR + const Float32Bits magic = {.bits = ((uint32_t) 15U) << 23U}; // NOLINT NOSONAR Float32Bits in = {.real = value}; const uint32_t sign = in.bits & (((uint32_t) 1U) << 31U); // NOLINT in.bits ^= sign; @@ -652,9 +651,10 @@ uint16_t canardDSDLFloat16Serialize(const CanardIEEE754Binary32 value) CanardIEEE754Binary32 canardDSDLFloat16Deserialize(const uint16_t value) { // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. - const Float32Bits magic = {.bits = ((uint32_t) 0xEFU) << 23U}; // NOLINT - const Float32Bits inf_nan = {.bits = ((uint32_t) 0x8FU) << 23U}; // NOLINT - Float32Bits out = {.bits = ((uint32_t)(value & 0x7FFFU)) << 13U}; // NOLINT + // The no-lint statements suppress the warning about the use of union. This is required for low-level bit access. + const Float32Bits magic = {.bits = ((uint32_t) 0xEFU) << 23U}; // NOLINT NOSONAR + const Float32Bits inf_nan = {.bits = ((uint32_t) 0x8FU) << 23U}; // NOLINT NOSONAR + Float32Bits out = {.bits = ((uint32_t)(value & 0x7FFFU)) << 13U}; // NOLINT NOSONAR out.real *= magic.real; if (out.real >= inf_nan.real) { diff --git a/tests/internals.hpp b/tests/internals.hpp index bd4b3a1f..fc954e5f 100644 --- a/tests/internals.hpp +++ b/tests/internals.hpp @@ -66,6 +66,6 @@ auto makeTailByte(const bool start_of_transfer, const bool toggle, const std::uint8_t transfer_id) -> std::uint8_t; -auto findTxQueueSupremum(CanardInstance* const ins, const std::uint32_t can_id) -> TxQueueItem*; +auto findTxQueueSupremum(const CanardInstance* const ins, const std::uint32_t can_id) -> TxQueueItem*; } } // namespace internals From a4aac16eee6fdb4185dbea302ef0d836a222bb4f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 11 Feb 2020 11:56:57 +0200 Subject: [PATCH 026/100] More TX testing --- libcanard/canard.c | 19 ++++--- tests/.clang-tidy | 3 ++ tests/helpers.hpp | 11 ++++ tests/test_tx.cpp | 126 ++++++++++++++++++++++++++++++++++++++++++++- 4 files changed, 150 insertions(+), 9 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index b9689ec6..fc1ba6f0 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -372,9 +372,9 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, if ((payload_size_with_crc - offset) < presentation_layer_mtu) { size_t index = payload_size_with_crc - offset + 1U; - CANARD_ASSERT(index < sizeof(CanardCANLengthToDLC)); + CANARD_ASSERT(index < (sizeof(CanardCANLengthToDLC) / sizeof(CanardCANLengthToDLC[0]))); index = CanardCANLengthToDLC[index]; - CANARD_ASSERT(index < sizeof(CanardCANDLCToLength)); + CANARD_ASSERT(index < (sizeof(CanardCANDLCToLength) / sizeof(CanardCANDLCToLength[0]))); frame_payload_size_with_tail = CanardCANDLCToLength[index]; // Round up to accommodate padding. } @@ -415,8 +415,7 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, } // Handle the last frame of the transfer: it is special because it also contains padding and CRC. - const bool end_of_transfer = offset >= payload_size; - if (end_of_transfer) + if (offset >= payload_size) { // Insert padding -- only in the last frame. Don't forget to include padding into the CRC. while ((frame_offset + CRC_SIZE_BYTES) < frame_payload_size) @@ -429,13 +428,13 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, // Insert the CRC. if ((frame_offset < frame_payload_size) && (offset == payload_size)) { - tail->payload[frame_offset] = (uint8_t)(crc & BYTE_MAX); + tail->payload[frame_offset] = (uint8_t)(crc >> BITS_PER_BYTE); ++frame_offset; ++offset; } if ((frame_offset < frame_payload_size) && (offset > payload_size)) { - tail->payload[frame_offset] = (uint8_t)(crc >> BITS_PER_BYTE); + tail->payload[frame_offset] = (uint8_t)(crc & BYTE_MAX); ++frame_offset; ++offset; } @@ -443,7 +442,10 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, // Finalize the frame. CANARD_ASSERT((frame_offset + 1U) == tail->payload_size); - tail->payload[frame_offset] = makeTailByte(start_of_transfer, end_of_transfer, toggle, transfer_id); + tail->payload[frame_offset] = makeTailByte(start_of_transfer, // + offset >= payload_size_with_crc, + toggle, + transfer_id); start_of_transfer = false; toggle = !toggle; } @@ -598,8 +600,9 @@ void canardTxPop(CanardInstance* const ins) { if ((ins != NULL) && (ins->_tx_queue != NULL)) { + CanardInternalTxQueueItem* const next = ins->_tx_queue->next; ins->heap_free(ins, ins->_tx_queue); - ins->_tx_queue = ins->_tx_queue->next; + ins->_tx_queue = next; } } diff --git a/tests/.clang-tidy b/tests/.clang-tidy index dbad0065..0c248721 100644 --- a/tests/.clang-tidy +++ b/tests/.clang-tidy @@ -15,10 +15,13 @@ Checks: >- performance-*, portability-*, readability-*, + -hicpp-function-size, -google-readability-todo, -google-runtime-references, + -google-readability-function-size, -readability-avoid-const-params-in-decls, -readability-magic-numbers, + -readability-function-size, -llvm-header-guard, -misc-non-private-member-variables-in-classes, -cppcoreguidelines-pro-bounds-pointer-arithmetic, diff --git a/tests/helpers.hpp b/tests/helpers.hpp index 278c183a..ee586d02 100644 --- a/tests/helpers.hpp +++ b/tests/helpers.hpp @@ -8,6 +8,7 @@ #include #include #include +#include #include namespace helpers @@ -47,6 +48,14 @@ class TestAllocator std::unordered_map allocated_; std::size_t ceiling_ = std::numeric_limits::max(); + static auto getRandomByte() + { + static std::random_device rd; + static std::mt19937 gen(rd()); + static std::uniform_int_distribution dis(0, 255U); + return static_cast(dis(gen)); + } + public: TestAllocator() = default; TestAllocator(const TestAllocator&) = delete; @@ -86,6 +95,8 @@ class TestAllocator { throw std::logic_error("Heap corruption: an attempt to deallocate memory that is not allocated"); } + // Damage the memory to make sure it's not used after deallocation. + std::generate_n(reinterpret_cast(pointer), it->second, &TestAllocator::getRandomByte); // Clang-tidy complains about manual memory management. Suppressed because we need it for testing purposes. std::free(it->first); // NOLINT allocated_.erase(it); diff --git a/tests/test_tx.cpp b/tests/test_tx.cpp index 559f3f13..e4f772fd 100644 --- a/tests/test_tx.cpp +++ b/tests/test_tx.cpp @@ -3,6 +3,7 @@ #include "internals.hpp" #include "helpers.hpp" +#include TEST_CASE("TxBasic") { @@ -111,5 +112,128 @@ TEST_CASE("TxBasic") REQUIRE(200 > alloc.getTotalAllocatedAmount()); // Pop the queue. - // TODO + // hex(pyuavcan.transport.commons.crc.CRC16CCITT.new(list(range(8))).value) + constexpr std::uint16_t CRC8 = 0x178DU; + CanardCANFrame frame{}; + REQUIRE(1 == ins.txPeek(frame)); + REQUIRE(frame.payload_size == 9); + REQUIRE(0 == std::memcmp(frame.payload, payload.data(), 8)); + REQUIRE((0b11100000U | 21U) == reinterpret_cast(frame.payload)[8]); + REQUIRE(frame.timestamp_usec == 1'000'000'000'000ULL); + frame = {}; + REQUIRE(1 == ins.txPeek(frame)); // Make sure we get the same frame again. + REQUIRE(frame.payload_size == 9); + REQUIRE(0 == std::memcmp(frame.payload, payload.data(), 8)); + REQUIRE((0b11100000U | 21U) == reinterpret_cast(frame.payload)[8]); + REQUIRE(frame.timestamp_usec == 1'000'000'000'000ULL); + ins.txPop(); + REQUIRE(2 == ins.getTxQueueLength()); + REQUIRE(2 == alloc.getNumAllocatedFragments()); + REQUIRE(1 == ins.txPeek(frame)); + REQUIRE(frame.payload_size == 8); + REQUIRE(0 == std::memcmp(frame.payload, payload.data(), 7)); + REQUIRE((0b10100000U | 22U) == reinterpret_cast(frame.payload)[7]); + REQUIRE(frame.timestamp_usec == 1'000'000'000'100ULL); + ins.txPop(); + REQUIRE(1 == ins.getTxQueueLength()); + REQUIRE(1 == alloc.getNumAllocatedFragments()); + REQUIRE(1 == ins.txPeek(frame)); + REQUIRE(frame.payload_size == 4); + REQUIRE(0 == std::memcmp(frame.payload, payload.data() + 7U, 1)); + REQUIRE((CRC8 >> 8U) == reinterpret_cast(frame.payload)[1]); + REQUIRE((CRC8 & 0xFFU) == reinterpret_cast(frame.payload)[2]); + REQUIRE((0b01000000U | 22U) == reinterpret_cast(frame.payload)[3]); + REQUIRE(frame.timestamp_usec == 1'000'000'000'100ULL); + ins.txPop(); + REQUIRE(0 == ins.getTxQueueLength()); + REQUIRE(nullptr == ins.getTxQueueRoot()); + REQUIRE(0 == alloc.getNumAllocatedFragments()); + REQUIRE(0 == ins.txPeek(frame)); + ins.txPop(); // Invocation when empty has no effect. + ins.txPop(); + REQUIRE(0 == ins.getTxQueueLength()); + REQUIRE(nullptr == ins.getTxQueueRoot()); + REQUIRE(0 == alloc.getNumAllocatedFragments()); + REQUIRE(0 == ins.txPeek(frame)); + + alloc.setAllocationCeiling(1000); + ins.setMTU(32); + + // Multi-frame, success. CRC split over the frame boundary. + // hex(pyuavcan.transport.commons.crc.CRC16CCITT.new(list(range(61))).value) + constexpr std::uint16_t CRC61 = 0x554EU; + transfer.timestamp_usec = 1'000'000'001'000ULL; + transfer.priority = CanardPriorityFast; + transfer.transfer_id = 25; + transfer.payload_size = 31 + 30; // CRC takes 2 bytes at the end; 3 frames: (31+1) + (30+1+1) + (1+1) + REQUIRE(3 == ins.txPush(transfer)); + REQUIRE(3 == ins.getTxQueueLength()); + REQUIRE(3 == alloc.getNumAllocatedFragments()); + REQUIRE(40 < alloc.getTotalAllocatedAmount()); + REQUIRE(200 > alloc.getTotalAllocatedAmount()); + // Read the generated frames. + REQUIRE(1 == ins.txPeek(frame)); + REQUIRE(frame.payload_size == 32); + REQUIRE(0 == std::memcmp(frame.payload, payload.data(), 31)); + REQUIRE((0b10100000U | 25U) == reinterpret_cast(frame.payload)[31]); + REQUIRE(frame.timestamp_usec == 1'000'000'001'000ULL); + ins.txPop(); + REQUIRE(2 == ins.getTxQueueLength()); + REQUIRE(2 == alloc.getNumAllocatedFragments()); + REQUIRE(1 == ins.txPeek(frame)); + REQUIRE(frame.payload_size == 32); + REQUIRE(0 == std::memcmp(frame.payload, payload.data() + 31U, 30)); + REQUIRE((CRC61 >> 8U) == reinterpret_cast(frame.payload)[30]); + REQUIRE((0b00000000U | 25U) == reinterpret_cast(frame.payload)[31]); + REQUIRE(frame.timestamp_usec == 1'000'000'001'000ULL); + ins.txPop(); + REQUIRE(1 == ins.getTxQueueLength()); + REQUIRE(1 == alloc.getNumAllocatedFragments()); + REQUIRE(1 == ins.txPeek(frame)); + REQUIRE(frame.payload_size == 2); // The last byte of CRC plus the tail byte. + REQUIRE((CRC61 & 0xFFU) == reinterpret_cast(frame.payload)[0]); + REQUIRE((0b01100000U | 25U) == reinterpret_cast(frame.payload)[1]); + REQUIRE(frame.timestamp_usec == 1'000'000'001'000ULL); + ins.txPop(); + REQUIRE(0 == ins.getTxQueueLength()); + REQUIRE(0 == alloc.getNumAllocatedFragments()); + + // Multi-frame, success. CRC is in the last frame. + // hex(pyuavcan.transport.commons.crc.CRC16CCITT.new(list(range(62))).value) + constexpr std::uint16_t CRC62 = 0xA3AEU; + transfer.timestamp_usec = 1'000'000'002'000ULL; + transfer.priority = CanardPrioritySlow; + transfer.transfer_id = 26; + transfer.payload_size = 31 + 31; // CRC takes 2 bytes at the end; 3 frames: (31+1) + (31+1) + (2+1) + REQUIRE(3 == ins.txPush(transfer)); + REQUIRE(3 == ins.getTxQueueLength()); + REQUIRE(3 == alloc.getNumAllocatedFragments()); + REQUIRE(40 < alloc.getTotalAllocatedAmount()); + REQUIRE(200 > alloc.getTotalAllocatedAmount()); + // Read the generated frames. + REQUIRE(1 == ins.txPeek(frame)); + REQUIRE(frame.payload_size == 32); + REQUIRE(0 == std::memcmp(frame.payload, payload.data(), 31)); + REQUIRE((0b10100000U | 26U) == reinterpret_cast(frame.payload)[31]); + REQUIRE(frame.timestamp_usec == 1'000'000'002'000ULL); + ins.txPop(); + REQUIRE(2 == ins.getTxQueueLength()); + REQUIRE(2 == alloc.getNumAllocatedFragments()); + REQUIRE(1 == ins.txPeek(frame)); + REQUIRE(frame.payload_size == 32); + REQUIRE(0 == std::memcmp(frame.payload, payload.data() + 31U, 31)); + REQUIRE((0b00000000U | 26U) == reinterpret_cast(frame.payload)[31]); + REQUIRE(frame.timestamp_usec == 1'000'000'002'000ULL); + ins.txPop(); + REQUIRE(1 == ins.getTxQueueLength()); + REQUIRE(1 == alloc.getNumAllocatedFragments()); + REQUIRE(1 == ins.txPeek(frame)); + REQUIRE(frame.payload_size == 3); // The CRC plus the tail byte. + REQUIRE((CRC62 >> 8U) == reinterpret_cast(frame.payload)[0]); + REQUIRE((CRC62 & 0xFFU) == reinterpret_cast(frame.payload)[1]); + REQUIRE((0b01100000U | 26U) == reinterpret_cast(frame.payload)[2]); + REQUIRE(frame.timestamp_usec == 1'000'000'002'000ULL); + ins.txPop(); + REQUIRE(0 == ins.getTxQueueLength()); + REQUIRE(0 == alloc.getNumAllocatedFragments()); } From 70e84114cb457efc0280142c14a2382a78412834 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 11 Feb 2020 13:32:12 +0200 Subject: [PATCH 027/100] Random-fill allocated memory --- tests/helpers.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/tests/helpers.hpp b/tests/helpers.hpp index ee586d02..a48067ba 100644 --- a/tests/helpers.hpp +++ b/tests/helpers.hpp @@ -83,6 +83,8 @@ class TestAllocator { throw std::bad_alloc(); // This is a test suite failure, not a failed test. Mind the difference. } + // Random-fill the memory to make sure no assumptions are made about its contents. + std::generate_n(reinterpret_cast(p), amount, &TestAllocator::getRandomByte); allocated_.emplace(p, amount); } return p; @@ -107,7 +109,7 @@ class TestAllocator [[nodiscard]] auto getTotalAllocatedAmount() const -> std::size_t { std::size_t out = 0U; - for (auto [ptr, size] : allocated_) + for (auto [_, size] : allocated_) { out += size; } From 65aab43d3866bdec009ead80d427234c0a5ed9ba Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 11 Feb 2020 17:44:54 +0200 Subject: [PATCH 028/100] More TX testing --- libcanard/canard.c | 49 ++++++++++------- tests/internals.hpp | 2 + tests/test_internals.cpp | 30 +++++++++++ tests/test_tx.cpp | 110 ++++++++++++++++++++++++++++++++------- 4 files changed, 153 insertions(+), 38 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index fc1ba6f0..80e0d3ef 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -234,6 +234,16 @@ CANARD_INTERNAL uint8_t makeTailByte(const bool start_of_transfer, (toggle ? TAIL_TOGGLE : 0U) | (transfer_id & CANARD_TRANSFER_ID_MAX)); } +/// Takes a frame payload size, returns a new size that is >=x and is rounded up to the nearest valid DLC. +CANARD_INTERNAL size_t roundFramePayloadSizeUp(const size_t x); +CANARD_INTERNAL size_t roundFramePayloadSizeUp(const size_t x) +{ + CANARD_ASSERT(x < (sizeof(CanardCANLengthToDLC) / sizeof(CanardCANLengthToDLC[0]))); + const size_t y = CanardCANLengthToDLC[x]; + CANARD_ASSERT(y < (sizeof(CanardCANDLCToLength) / sizeof(CanardCANDLCToLength[0]))); + return CanardCANDLCToLength[y]; +} + CANARD_INTERNAL CanardInternalTxQueueItem* allocateTxQueueItem(CanardInstance* const ins, const uint32_t id, const uint64_t deadline_usec, @@ -297,8 +307,14 @@ CANARD_INTERNAL int32_t pushSingleFrameTransfer(CanardInstance* const ins, { CANARD_ASSERT(ins != NULL); CANARD_ASSERT((payload != NULL) || (payload_size == 0)); - int32_t out = 0; - CanardInternalTxQueueItem* const tqi = allocateTxQueueItem(ins, can_id, deadline_usec, payload_size + 1U); + + const size_t frame_payload_size = roundFramePayloadSizeUp(payload_size + 1U); + CANARD_ASSERT(frame_payload_size > payload_size); + const size_t padding_size = frame_payload_size - payload_size - 1U; + CANARD_ASSERT((padding_size + payload_size + 1U) == frame_payload_size); + int32_t out = 0; + + CanardInternalTxQueueItem* const tqi = allocateTxQueueItem(ins, can_id, deadline_usec, frame_payload_size); if (tqi != NULL) { if (payload_size > 0U) // The check is needed to avoid calling memcpy() with a NULL pointer, it's an UB. @@ -308,8 +324,13 @@ CANARD_INTERNAL int32_t pushSingleFrameTransfer(CanardInstance* const ins, // We ignore this recommendation because it is not available in C99. (void) memcpy(&tqi->payload[0], payload, payload_size); // NOLINT } - tqi->payload[payload_size] = makeTailByte(true, true, true, transfer_id); - CanardInternalTxQueueItem* const sup = findTxQueueSupremum(ins, can_id); + + // Clang-Tidy raises an error recommending the use of memset_s() instead. + // We ignore this recommendation because it is not available in C99. + (void) memset(&tqi->payload[payload_size], PADDING_BYTE, padding_size); // NOLINT + + tqi->payload[frame_payload_size - 1U] = makeTailByte(true, true, true, transfer_id); + CanardInternalTxQueueItem* const sup = findTxQueueSupremum(ins, can_id); if (sup != NULL) { tqi->next = sup->next; @@ -365,21 +386,11 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, while (offset < payload_size_with_crc) { - ++out; // Count the number of generated frames. - - // Compute the required size of the frame payload for this frame, including CRC and padding. - size_t frame_payload_size_with_tail = presentation_layer_mtu + 1U; - if ((payload_size_with_crc - offset) < presentation_layer_mtu) - { - size_t index = payload_size_with_crc - offset + 1U; - CANARD_ASSERT(index < (sizeof(CanardCANLengthToDLC) / sizeof(CanardCANLengthToDLC[0]))); - index = CanardCANLengthToDLC[index]; - CANARD_ASSERT(index < (sizeof(CanardCANDLCToLength) / sizeof(CanardCANDLCToLength[0]))); - frame_payload_size_with_tail = CanardCANDLCToLength[index]; // Round up to accommodate padding. - } - - // Allocate the storage using the above computed size. Link all frames into a list. - + ++out; + const size_t frame_payload_size_with_tail = + ((payload_size_with_crc - offset) < presentation_layer_mtu) + ? roundFramePayloadSizeUp(payload_size_with_crc - offset + 1U) // Add padding only in the last frame. + : (presentation_layer_mtu + 1U); CanardInternalTxQueueItem* const tqi = allocateTxQueueItem(ins, can_id, deadline_usec, frame_payload_size_with_tail); if (NULL == head) diff --git a/tests/internals.hpp b/tests/internals.hpp index fc954e5f..818c7b8a 100644 --- a/tests/internals.hpp +++ b/tests/internals.hpp @@ -66,6 +66,8 @@ auto makeTailByte(const bool start_of_transfer, const bool toggle, const std::uint8_t transfer_id) -> std::uint8_t; +auto roundFramePayloadSizeUp(const std::size_t x) -> std::size_t; + auto findTxQueueSupremum(const CanardInstance* const ins, const std::uint32_t can_id) -> TxQueueItem*; } } // namespace internals diff --git a/tests/test_internals.cpp b/tests/test_internals.cpp index 21dc2f84..7647907d 100644 --- a/tests/test_internals.cpp +++ b/tests/test_internals.cpp @@ -154,6 +154,36 @@ TEST_CASE("makeTailByte") REQUIRE(0b010'00001 == makeTailByte(false, true, false, 1U)); } +TEST_CASE("roundFramePayloadSizeUp") +{ + using internals::roundFramePayloadSizeUp; + REQUIRE(0 == roundFramePayloadSizeUp(0)); + REQUIRE(1 == roundFramePayloadSizeUp(1)); + REQUIRE(2 == roundFramePayloadSizeUp(2)); + REQUIRE(3 == roundFramePayloadSizeUp(3)); + REQUIRE(4 == roundFramePayloadSizeUp(4)); + REQUIRE(5 == roundFramePayloadSizeUp(5)); + REQUIRE(6 == roundFramePayloadSizeUp(6)); + REQUIRE(7 == roundFramePayloadSizeUp(7)); + REQUIRE(8 == roundFramePayloadSizeUp(8)); + REQUIRE(12 == roundFramePayloadSizeUp(9)); + REQUIRE(12 == roundFramePayloadSizeUp(10)); + REQUIRE(12 == roundFramePayloadSizeUp(11)); + REQUIRE(12 == roundFramePayloadSizeUp(12)); + REQUIRE(16 == roundFramePayloadSizeUp(13)); + REQUIRE(16 == roundFramePayloadSizeUp(14)); + REQUIRE(16 == roundFramePayloadSizeUp(15)); + REQUIRE(16 == roundFramePayloadSizeUp(16)); + REQUIRE(20 == roundFramePayloadSizeUp(17)); + REQUIRE(20 == roundFramePayloadSizeUp(20)); + REQUIRE(32 == roundFramePayloadSizeUp(30)); + REQUIRE(32 == roundFramePayloadSizeUp(32)); + REQUIRE(48 == roundFramePayloadSizeUp(40)); + REQUIRE(48 == roundFramePayloadSizeUp(48)); + REQUIRE(64 == roundFramePayloadSizeUp(50)); + REQUIRE(64 == roundFramePayloadSizeUp(64)); +} + TEST_CASE("findTxQueueSupremum") { using internals::findTxQueueSupremum; diff --git a/tests/test_tx.cpp b/tests/test_tx.cpp index e4f772fd..5e278ee9 100644 --- a/tests/test_tx.cpp +++ b/tests/test_tx.cpp @@ -30,7 +30,7 @@ TEST_CASE("TxBasic") CanardTransfer transfer{}; transfer.payload = payload.data(); - // Single-frame, success. + // Single-frame with padding. transfer.timestamp_usec = 1'000'000'000'000ULL; transfer.priority = CanardPriorityNominal; transfer.transfer_kind = CanardTransferKindMessage; @@ -44,12 +44,23 @@ TEST_CASE("TxBasic") REQUIRE(10 < alloc.getTotalAllocatedAmount()); REQUIRE(80 > alloc.getTotalAllocatedAmount()); REQUIRE(ins.getTxQueueRoot()->deadline_usec == 1'000'000'000'000ULL); - REQUIRE(ins.getTxQueueRoot()->payload_size == 9); - REQUIRE(ins.getTxQueueRoot()->isStartOfTransfer()); + REQUIRE(ins.getTxQueueRoot()->payload_size == 12); // Three bytes of padding. + REQUIRE(ins.getTxQueueRoot()->getPayloadByte(0) == 0); // Payload start. + REQUIRE(ins.getTxQueueRoot()->getPayloadByte(1) == 1); + REQUIRE(ins.getTxQueueRoot()->getPayloadByte(2) == 2); + REQUIRE(ins.getTxQueueRoot()->getPayloadByte(3) == 3); + REQUIRE(ins.getTxQueueRoot()->getPayloadByte(4) == 4); + REQUIRE(ins.getTxQueueRoot()->getPayloadByte(5) == 5); + REQUIRE(ins.getTxQueueRoot()->getPayloadByte(6) == 6); + REQUIRE(ins.getTxQueueRoot()->getPayloadByte(7) == 7); // Payload end. + REQUIRE(ins.getTxQueueRoot()->getPayloadByte(8) == 0); // Padding. + REQUIRE(ins.getTxQueueRoot()->getPayloadByte(9) == 0); // Padding. + REQUIRE(ins.getTxQueueRoot()->getPayloadByte(10) == 0); // Padding. + REQUIRE(ins.getTxQueueRoot()->isStartOfTransfer()); // Tail byte at the end. REQUIRE(ins.getTxQueueRoot()->isEndOfTransfer()); REQUIRE(ins.getTxQueueRoot()->isToggleBitSet()); - // Multi-frame, success. Priority low, inserted at the end of the TX queue. + // Multi-frame. Priority low, inserted at the end of the TX queue. transfer.timestamp_usec = 1'000'000'000'100ULL; transfer.priority = CanardPriorityLow; transfer.transfer_id = 22; @@ -67,7 +78,7 @@ TEST_CASE("TxBasic") auto q = ins.getTxQueueRoot(); REQUIRE(q != nullptr); REQUIRE(q->deadline_usec == 1'000'000'000'000ULL); - REQUIRE(q->payload_size == 9); + REQUIRE(q->payload_size == 12); REQUIRE(q->isStartOfTransfer()); REQUIRE(q->isEndOfTransfer()); REQUIRE(q->isToggleBitSet()); @@ -114,17 +125,20 @@ TEST_CASE("TxBasic") // Pop the queue. // hex(pyuavcan.transport.commons.crc.CRC16CCITT.new(list(range(8))).value) constexpr std::uint16_t CRC8 = 0x178DU; - CanardCANFrame frame{}; + CanardCANFrame frame{}; REQUIRE(1 == ins.txPeek(frame)); - REQUIRE(frame.payload_size == 9); + REQUIRE(frame.payload_size == 12); REQUIRE(0 == std::memcmp(frame.payload, payload.data(), 8)); - REQUIRE((0b11100000U | 21U) == reinterpret_cast(frame.payload)[8]); + REQUIRE(0 == reinterpret_cast(frame.payload)[8]); // Padding. + REQUIRE(0 == reinterpret_cast(frame.payload)[9]); // Padding. + REQUIRE(0 == reinterpret_cast(frame.payload)[10]); // Padding. + REQUIRE((0b11100000U | 21U) == reinterpret_cast(frame.payload)[11]); REQUIRE(frame.timestamp_usec == 1'000'000'000'000ULL); frame = {}; REQUIRE(1 == ins.txPeek(frame)); // Make sure we get the same frame again. - REQUIRE(frame.payload_size == 9); + REQUIRE(frame.payload_size == 12); REQUIRE(0 == std::memcmp(frame.payload, payload.data(), 8)); - REQUIRE((0b11100000U | 21U) == reinterpret_cast(frame.payload)[8]); + REQUIRE((0b11100000U | 21U) == reinterpret_cast(frame.payload)[11]); REQUIRE(frame.timestamp_usec == 1'000'000'000'000ULL); ins.txPop(); REQUIRE(2 == ins.getTxQueueLength()); @@ -157,15 +171,15 @@ TEST_CASE("TxBasic") REQUIRE(0 == ins.txPeek(frame)); alloc.setAllocationCeiling(1000); - ins.setMTU(32); // Multi-frame, success. CRC split over the frame boundary. // hex(pyuavcan.transport.commons.crc.CRC16CCITT.new(list(range(61))).value) constexpr std::uint16_t CRC61 = 0x554EU; - transfer.timestamp_usec = 1'000'000'001'000ULL; - transfer.priority = CanardPriorityFast; - transfer.transfer_id = 25; - transfer.payload_size = 31 + 30; // CRC takes 2 bytes at the end; 3 frames: (31+1) + (30+1+1) + (1+1) + ins.setMTU(32); + transfer.timestamp_usec = 1'000'000'001'000ULL; + transfer.priority = CanardPriorityFast; + transfer.transfer_id = 25; + transfer.payload_size = 31 + 30; // CRC takes 2 bytes at the end; 3 frames: (31+1) + (30+1+1) + (1+1) REQUIRE(3 == ins.txPush(transfer)); REQUIRE(3 == ins.getTxQueueLength()); REQUIRE(3 == alloc.getNumAllocatedFragments()); @@ -201,10 +215,11 @@ TEST_CASE("TxBasic") // Multi-frame, success. CRC is in the last frame. // hex(pyuavcan.transport.commons.crc.CRC16CCITT.new(list(range(62))).value) constexpr std::uint16_t CRC62 = 0xA3AEU; - transfer.timestamp_usec = 1'000'000'002'000ULL; - transfer.priority = CanardPrioritySlow; - transfer.transfer_id = 26; - transfer.payload_size = 31 + 31; // CRC takes 2 bytes at the end; 3 frames: (31+1) + (31+1) + (2+1) + ins.setMTU(32); + transfer.timestamp_usec = 1'000'000'002'000ULL; + transfer.priority = CanardPrioritySlow; + transfer.transfer_id = 26; + transfer.payload_size = 31 + 31; // CRC takes 2 bytes at the end; 3 frames: (31+1) + (31+1) + (2+1) REQUIRE(3 == ins.txPush(transfer)); REQUIRE(3 == ins.getTxQueueLength()); REQUIRE(3 == alloc.getNumAllocatedFragments()); @@ -236,4 +251,61 @@ TEST_CASE("TxBasic") ins.txPop(); REQUIRE(0 == ins.getTxQueueLength()); REQUIRE(0 == alloc.getNumAllocatedFragments()); + + // Multi-frame with padding. + // hex(pyuavcan.transport.commons.crc.CRC16CCITT.new(list(range(112)) + [0] * 12).value) + constexpr std::uint16_t CRC112Padding12 = 0xE7A5U; + ins.setMTU(64); + transfer.timestamp_usec = 1'000'000'003'000ULL; + transfer.priority = CanardPriorityImmediate; + transfer.transfer_id = 27; + transfer.payload_size = 112; // 63 + 63 - 2 = 124 bytes; 124 - 112 = 12 bytes of padding. + REQUIRE(2 == ins.txPush(transfer)); + REQUIRE(2 == ins.getTxQueueLength()); + REQUIRE(2 == alloc.getNumAllocatedFragments()); + // Read the generated frames. + REQUIRE(1 == ins.txPeek(frame)); + REQUIRE(frame.payload_size == 64); + REQUIRE(0 == std::memcmp(frame.payload, payload.data(), 63)); + REQUIRE((0b10100000U | 27U) == reinterpret_cast(frame.payload)[63]); + REQUIRE(frame.timestamp_usec == 1'000'000'003'000ULL); + ins.txPop(); + REQUIRE(1 == ins.getTxQueueLength()); + REQUIRE(1 == alloc.getNumAllocatedFragments()); + REQUIRE(1 == ins.txPeek(frame)); + REQUIRE(frame.payload_size == 64); + REQUIRE(0 == std::memcmp(frame.payload, payload.data() + 63U, 49)); + REQUIRE(std::all_of(reinterpret_cast(frame.payload) + 49, // Check padding. + reinterpret_cast(frame.payload) + 61, + [](auto x) { return x == 0U; })); + REQUIRE((CRC112Padding12 >> 8U) == reinterpret_cast(frame.payload)[61]); // CRC + REQUIRE((CRC112Padding12 & 0xFFU) == reinterpret_cast(frame.payload)[62]); // CRC + REQUIRE((0b01000000U | 27U) == reinterpret_cast(frame.payload)[63]); // Tail + REQUIRE(frame.timestamp_usec == 1'000'000'003'000ULL); + ins.txPop(); + REQUIRE(0 == ins.getTxQueueLength()); + REQUIRE(0 == alloc.getNumAllocatedFragments()); + + // Single-frame empty. + transfer.timestamp_usec = 1'000'000'004'000ULL; + transfer.transfer_id = 28; + transfer.payload_size = 0; + REQUIRE(1 == ins.txPush(transfer)); + REQUIRE(1 == ins.getTxQueueLength()); + REQUIRE(1 == alloc.getNumAllocatedFragments()); + REQUIRE(40 > alloc.getTotalAllocatedAmount()); + REQUIRE(ins.getTxQueueRoot()->deadline_usec == 1'000'000'004'000ULL); + REQUIRE(ins.getTxQueueRoot()->payload_size == 1); + REQUIRE(ins.getTxQueueRoot()->isStartOfTransfer()); + REQUIRE(ins.getTxQueueRoot()->isEndOfTransfer()); + REQUIRE(ins.getTxQueueRoot()->isToggleBitSet()); + REQUIRE(1 == ins.txPeek(frame)); + REQUIRE(frame.payload_size == 1); + REQUIRE((0b11100000U | 28U) == reinterpret_cast(frame.payload)[0]); + REQUIRE(frame.timestamp_usec == 1'000'000'004'000ULL); + ins.txPop(); + REQUIRE(0 == ins.getTxQueueLength()); + REQUIRE(0 == alloc.getNumAllocatedFragments()); } + +TEST_CASE("TxPrioritization") {} From 5a3d960e0cf34d1bb0b0c4e386284fba8e8eb4c3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 11 Feb 2020 20:17:53 +0200 Subject: [PATCH 029/100] Finished the TX test and added some docs --- libcanard/canard.c | 13 ++++----- libcanard/canard.h | 72 ++++++++++++++++++++++++++++++++-------------- tests/helpers.hpp | 3 ++ tests/test_tx.cpp | 22 ++++++++++++-- 4 files changed, 79 insertions(+), 31 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 80e0d3ef..631e0132 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -239,7 +239,8 @@ CANARD_INTERNAL size_t roundFramePayloadSizeUp(const size_t x); CANARD_INTERNAL size_t roundFramePayloadSizeUp(const size_t x) { CANARD_ASSERT(x < (sizeof(CanardCANLengthToDLC) / sizeof(CanardCANLengthToDLC[0]))); - const size_t y = CanardCANLengthToDLC[x]; + // Suppressing a false-positive out-of-bounds access error from Sonar. Its control flow analyser is misbehaving. + const size_t y = CanardCANLengthToDLC[x]; // NOSONAR CANARD_ASSERT(y < (sizeof(CanardCANDLCToLength) / sizeof(CanardCANDLCToLength[0]))); return CanardCANDLCToLength[y]; } @@ -453,12 +454,10 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, // Finalize the frame. CANARD_ASSERT((frame_offset + 1U) == tail->payload_size); - tail->payload[frame_offset] = makeTailByte(start_of_transfer, // - offset >= payload_size_with_crc, - toggle, - transfer_id); - start_of_transfer = false; - toggle = !toggle; + tail->payload[frame_offset] = + makeTailByte(start_of_transfer, offset >= payload_size_with_crc, toggle, transfer_id); + start_of_transfer = false; + toggle = !toggle; } if (tail != NULL) diff --git a/libcanard/canard.h b/libcanard/canard.h index be24782f..86cb89dc 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -45,7 +45,7 @@ extern "C" { #define CANARD_TRANSFER_ID_MAX ((1U << CANARD_TRANSFER_ID_BIT_LENGTH) - 1U) /// This value represents an undefined node-ID: broadcast destination or anonymous source. -/// Library functions treat all values above @ref CANARD_NODE_ID_MAX as anonymous. +/// Library functions treat all values above CANARD_NODE_ID_MAX as anonymous. #define CANARD_NODE_ID_UNSET 255U /// If not specified, the transfer-ID timeout will take this value for all new input sessions. @@ -116,6 +116,7 @@ typedef struct /// Subject-ID for message publications; service-ID for service requests/responses. uint16_t port_id; + /// For outgoing message transfers or for incoming anonymous transfers, the value is CANARD_NODE_ID_UNSET. uint8_t remote_node_id; uint8_t transfer_id; @@ -128,8 +129,8 @@ typedef struct /// The application supplies the library with this information when a new transfer should be received. typedef struct { - /// The transfer-ID timeout for this session. If no specific requirements are defined, the default value - /// @ref CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC should be used. + /// The transfer-ID timeout for this session. + /// If no specific requirements are defined, the default CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC should be used. /// Zero timeout indicates that this transfer should not be received (all its frames will be silently dropped). uint64_t transfer_id_timeout_usec; @@ -146,8 +147,8 @@ typedef struct /// @param ins Library instance. /// @param port_id Subject-ID or service-ID of the transfer. /// @param transfer_kind Message or service transfer. -/// @param source_node_id Node-ID of the origin; @ref CANARD_NODE_ID_UNSET if anonymous. -/// @returns @ref CanardTransferReceptionParameters. +/// @param source_node_id Node-ID of the origin; CANARD_NODE_ID_UNSET if anonymous. +/// @returns CanardTransferReceptionParameters. typedef CanardRxMetadata (*CanardRxFilter)(const CanardInstance* ins, uint16_t port_id, CanardTransferKind transfer_kind, @@ -178,9 +179,9 @@ struct CanardInstance /// Invalid values are treated as the nearest valid value. size_t mtu_bytes; - /// The node-ID of the local node. The default value is @ref CANARD_NODE_ID_UNSET. + /// The node-ID of the local node. The default value is CANARD_NODE_ID_UNSET. /// Per the UAVCAN Specification, the node-ID should not be assigned more than once. - /// Invalid values are treated as @ref CANARD_NODE_ID_UNSET. + /// Invalid values are treated as CANARD_NODE_ID_UNSET. uint8_t node_id; /// Callbacks invoked by the library. See their type documentation for details. @@ -190,8 +191,8 @@ struct CanardInstance /// the heap management functions (allocate and free) have constant complexity. /// /// There are only two API functions that may lead to allocation of heap memory: - /// - @ref canardTxPush() - /// - @ref canardRxAccept() + /// - canardTxPush() + /// - canardRxAccept() /// Their exact memory requirement model is specified in their documentation. CanardHeapAllocate heap_allocate; CanardHeapFree heap_free; @@ -211,21 +212,42 @@ CanardInstance canardInit(const CanardHeapAllocate heap_allocate, const CanardHeapFree heap_free, const CanardRxFilter rx_filter); -/// Takes a transfer, serializes it into a sequence of transport frames, and inserts them into the prioritized -/// transmission queue at the appropriate position. Afterwards, the application is supposed to take the enqueued -/// frames from the transmission queue using the function @ref canardTxPeek() and transmit them. Each transmitted -/// (or otherwise discarded, e.g., due to timeout) frame should be removed from the queue using @ref canardTxPop(). +/// Serializes a transfer into a sequence of transport frames, and inserts them into the prioritized transmission +/// queue at the appropriate position. Afterwards, the application is supposed to take the enqueued frames from +/// the transmission queue using the function canardTxPeek() and transmit them. Each transmitted (or otherwise +/// discarded, e.g., due to timeout) frame should be removed from the queue using canardTxPop(). /// /// The MTU of the generated frames is dependent on the value of the MTU setting at the time when this function -/// is invoked. +/// is invoked. The MTU setting can be changed arbitrarily between invocations. No other functions rely on that +/// parameter. +/// +/// The timestamp value of the transfer will be used to populate the timestamp values of the resulting transport +/// frames (so all frames will have the same timestamp value). This feature is intended to facilitate transmission +/// deadline tracking, i.e., aborting frames that could not be transmitted before the specified deadline. +/// Therefore, normally, the timestamp value should be in the future. +/// The library itself, however, does not use or check this value itself, so it can be zero if not needed. +/// +/// It is the responsibility of the application to ensure that the transfer parameters are managed correctly. +/// In particular, the application shall ensure that the transfer-ID computation rules, as defined in the +/// UAVCAN Specification, are being followed; here is a relevant excerpt: +/// - For service response transfers the transfer-ID value shall be directly copied from the corresponding +/// service request transfer. +/// - A node that is interested in emitting message transfers or service request transfers under a particular +/// session specifier, whether periodically or on an ad-hoc basis, shall allocate a transfer-ID counter state +/// associated with said session specifier exclusively. The transfer-ID value of every emitted transfer is +/// determined by sampling the corresponding counter keyed by the session specifier of the transfer; afterwards, +/// the counter is incremented by one. +/// The recommended approach to storing the transfer-ID counters is to use static or member variables. +/// Sophisticated applications may find this approach unsuitable, in which case O(1) static look-up tables +/// or heap-allocated data structures should be considered. /// /// Returns the number of frames enqueued into the prioritized TX queue (which is always a positive number) /// in case of success. Returns a negated error code in case of failure. Zero cannot be returned. /// /// An invalid argument error may be returned in the following cases: /// - Any of the input arguments are NULL. -/// - The remote node-ID is not @ref CANARD_NODE_ID_UNSET and the transfer is a message transfer. -/// - The remote node-ID is above @ref CANARD_NODE_ID_MAX and the transfer is a service transfer. +/// - The remote node-ID is not CANARD_NODE_ID_UNSET and the transfer is a message transfer. +/// - The remote node-ID is above CANARD_NODE_ID_MAX and the transfer is a service transfer. /// - The priority, subject-ID, or service-ID exceed their respective maximums. /// - The transfer kind is invalid. /// - The payload pointer is NULL while the payload size is nonzero. @@ -249,14 +271,22 @@ int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const tran /// Access the top element of the prioritized transmission queue. The queue itself is not modified (i.e., the /// accessed element is not removed). The application should invoke this function to collect the transport frames -/// of serialized transfers stored into the prioritized transmission queue by @ref canardTxPush(). +/// of serialized transfers stored into the prioritized transmission queue by canardTxPush(). +/// +/// Nodes with redundant transports should replicate every frame into each of the transport interfaces. +/// Such replication may require additional buffering in the driver layer, depending on the implementation. +/// +/// The timestamp values of returned frames are initialized with the timestamp value of the transfer instance they +/// originate from. Timestamps are used to specify the transmission deadline. It is up to the application and/or +/// the driver layer to implement the discardment of timed-out transport frames. The library does not check it, +/// so a frame that is already timed out may be returned here. /// /// If the queue is empty, the return value is zero and the out_frame is not modified. /// /// If the queue is non-empty, the return value is 1 (one) and the out_frame is populated with the data from /// the top element (i.e., the next frame awaiting transmission). /// The payload pointer of the out_frame will point to the data buffer of the accessed frame; -/// the pointer retains validity until the element is removed from the queue by calling @ref canardTxPop(). +/// the pointer retains validity until the element is removed from the queue by calling canardTxPop(). /// The payload pointer retains validity even if more frames are added to the transmission queue. /// If the returned frame instance is not needed, it can be dropped -- no deinitialization procedures are needed /// since it does not own any memory itself. @@ -268,13 +298,13 @@ int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const tran int8_t canardTxPeek(const CanardInstance* const ins, CanardCANFrame* const out_frame); /// Remove and free the top element from the prioritized transmission queue. -/// The application should invoke this function after the top frame obtained through @ref canardTxPeek() has been +/// The application should invoke this function after the top frame obtained through canardTxPeek() has been /// processed and need not be kept anymore (e.g., transmitted successfully, timed out, errored, etc.). /// /// WARNING: -/// Invocation of @ref canardTxPush() may add new elements at the top of the prioritized transmission queue. +/// Invocation of canardTxPush() may add new elements at the top of the prioritized transmission queue. /// The calling code shall take that into account to eliminate the possibility of data loss due to the frame -/// at the top of the queue being unexpectedly replaced between calls of @ref canardTxPeek() and this function. +/// at the top of the queue being unexpectedly replaced between calls of canardTxPeek() and this function. /// /// Invocation of this function invalidates the payload pointer of the top frame because the underlying buffer is freed. /// diff --git a/tests/helpers.hpp b/tests/helpers.hpp index a48067ba..e82d4567 100644 --- a/tests/helpers.hpp +++ b/tests/helpers.hpp @@ -206,6 +206,9 @@ class Instance } [[nodiscard]] auto getAllocator() -> TestAllocator& { return allocator_; } + + [[nodiscard]] auto getInstance() -> CanardInstance& { return canard_; } + [[nodiscard]] auto getInstance() const -> const CanardInstance& { return canard_; } }; } // namespace helpers diff --git a/tests/test_tx.cpp b/tests/test_tx.cpp index 5e278ee9..3e118378 100644 --- a/tests/test_tx.cpp +++ b/tests/test_tx.cpp @@ -5,7 +5,7 @@ #include "helpers.hpp" #include -TEST_CASE("TxBasic") +TEST_CASE("TxBasic0") { using internals::TxQueueItem; @@ -290,6 +290,7 @@ TEST_CASE("TxBasic") transfer.timestamp_usec = 1'000'000'004'000ULL; transfer.transfer_id = 28; transfer.payload_size = 0; + transfer.payload = nullptr; // Null is OK if size is 0. REQUIRE(1 == ins.txPush(transfer)); REQUIRE(1 == ins.getTxQueueLength()); REQUIRE(1 == alloc.getNumAllocatedFragments()); @@ -306,6 +307,21 @@ TEST_CASE("TxBasic") ins.txPop(); REQUIRE(0 == ins.getTxQueueLength()); REQUIRE(0 == alloc.getNumAllocatedFragments()); -} -TEST_CASE("TxPrioritization") {} + // Nothing left to peek at. + REQUIRE(0 == ins.txPeek(frame)); + + // Error handling. + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardTxPush(nullptr, nullptr)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardTxPush(nullptr, &transfer)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardTxPush(&ins.getInstance(), nullptr)); + transfer.payload_size = 1; + transfer.payload = nullptr; + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == ins.txPush(transfer)); + + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardTxPeek(nullptr, nullptr)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardTxPeek(nullptr, &frame)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardTxPeek(&ins.getInstance(), nullptr)); + + canardTxPop(&ins.getInstance()); // No effect. +} From 25f210a0e94e16191cb25b2f6c7d8bf899f291d4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 14 Feb 2020 16:32:10 +0200 Subject: [PATCH 030/100] Fix tests & static analysis --- libcanard/canard.c | 8 ++++---- tests/test_self.cpp | 3 +-- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 631e0132..b6c4c95b 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -390,7 +390,7 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, ++out; const size_t frame_payload_size_with_tail = ((payload_size_with_crc - offset) < presentation_layer_mtu) - ? roundFramePayloadSizeUp(payload_size_with_crc - offset + 1U) // Add padding only in the last frame. + ? roundFramePayloadSizeUp((payload_size_with_crc - offset) + 1U) // Add padding only in the last frame. : (presentation_layer_mtu + 1U); CanardInternalTxQueueItem* const tqi = allocateTxQueueItem(ins, can_id, deadline_usec, frame_payload_size_with_tail); @@ -550,7 +550,7 @@ CanardInstance canardInit(const CanardHeapAllocate heap_allocate, int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfer) { int32_t out = -CANARD_ERROR_INVALID_ARGUMENT; - if ((ins != NULL) && (transfer != NULL) && ((transfer->payload != NULL) || (transfer->payload_size == 0U))) + if ((ins != NULL) && (transfer != NULL) && ((transfer->payload != NULL) || (0U == transfer->payload_size))) { const size_t pl_mtu = getPresentationLayerMTU(ins); const int32_t maybe_can_id = makeCANID(transfer, ins->node_id, pl_mtu); @@ -589,7 +589,7 @@ int8_t canardTxPeek(const CanardInstance* const ins, CanardCANFrame* const out_f int8_t out = -CANARD_ERROR_INVALID_ARGUMENT; if ((ins != NULL) && (out_frame != NULL)) { - CanardInternalTxQueueItem* const tqi = ins->_tx_queue; + const CanardInternalTxQueueItem* const tqi = ins->_tx_queue; if (tqi != NULL) { out_frame->timestamp_usec = tqi->deadline_usec; @@ -638,7 +638,7 @@ uint16_t canardDSDLFloat16Serialize(const CanardIEEE754Binary32 value) const Float32Bits f32inf = {.bits = ((uint32_t) 255U) << 23U}; // NOLINT NOSONAR const Float32Bits f16inf = {.bits = ((uint32_t) 31U) << 23U}; // NOLINT NOSONAR const Float32Bits magic = {.bits = ((uint32_t) 15U) << 23U}; // NOLINT NOSONAR - Float32Bits in = {.real = value}; + Float32Bits in = {.real = value}; // NOSONAR const uint32_t sign = in.bits & (((uint32_t) 1U) << 31U); // NOLINT in.bits ^= sign; uint16_t out = 0; diff --git a/tests/test_self.cpp b/tests/test_self.cpp index f08e3013..3f4712df 100644 --- a/tests/test_self.cpp +++ b/tests/test_self.cpp @@ -32,13 +32,12 @@ TEST_CASE("TestAllocator") al.deallocate(a); REQUIRE(2 == al.getNumAllocatedFragments()); REQUIRE(477 == al.getTotalAllocatedAmount()); + REQUIRE_THROWS_AS(al.deallocate(a), std::logic_error); auto d = al.allocate(100); REQUIRE(3 == al.getNumAllocatedFragments()); REQUIRE(577 == al.getTotalAllocatedAmount()); - REQUIRE_THROWS_AS(al.deallocate(a), std::logic_error); - al.deallocate(c); REQUIRE(2 == al.getNumAllocatedFragments()); REQUIRE(556 == al.getTotalAllocatedAmount()); From 8b7551fd826538573c6ecf8758bf564c081bbe74 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 15 Feb 2020 00:28:33 +0200 Subject: [PATCH 031/100] DSDL primitive serialization routines moved into a separate pair of files to enhance modularity --- libcanard/canard.c | 68 +------------------------ libcanard/canard.h | 85 ------------------------------- libcanard/canard_dsdl.c | 83 ++++++++++++++++++++++++++++++ libcanard/canard_dsdl.h | 108 ++++++++++++++++++++++++++++++++++++++++ tests/CMakeLists.txt | 2 +- tests/test_float16.cpp | 31 ++++++------ 6 files changed, 210 insertions(+), 167 deletions(-) create mode 100644 libcanard/canard_dsdl.c create mode 100644 libcanard/canard_dsdl.h diff --git a/libcanard/canard.c b/libcanard/canard.c index b6c4c95b..cc4c56ac 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -10,8 +10,7 @@ /// By default, this macro resolves to the standard assert(). The user can redefine this if necessary. /// To disable assertion checks completely, make it expand into `(void)(0)`. #ifndef CANARD_ASSERT -// Intentional violation of MISRA: assertion macro cannot be replaced with a function definition. -# define CANARD_ASSERT(x) assert(x) // NOSONAR +# define CANARD_ASSERT assert #endif /// This macro is needed only for testing and for library development. Do not redefine this in production. @@ -25,7 +24,7 @@ # error "Unsupported language: ISO C99 or a newer version is required." #endif -#if __STDC_VERSION__ < 201112L +#ifndef static_assert // Intentional violation of MISRA: static assertion macro cannot be replaced with a function definition. # define static_assert(x, ...) typedef char _static_assert_gl(_static_assertion_, __LINE__)[(x) ? 1 : -1] // NOSONAR # define _static_assert_gl(a, b) _static_assert_gl_impl(a, b) // NOSONAR @@ -615,66 +614,3 @@ void canardTxPop(CanardInstance* const ins) ins->_tx_queue = next; } } - -// ---------------------------------------- FLOAT16 SERIALIZATION ---------------------------------------- - -#if CANARD_PLATFORM_IEEE754 - -// Intentional violation of MISRA: we need this union because the alternative is far more error prone. -// We have to rely on low-level data representation details to do the conversion; unions are helpful. -typedef union // NOSONAR -{ - uint32_t bits; - CanardIEEE754Binary32 real; -} Float32Bits; -static_assert(4 == sizeof(CanardIEEE754Binary32), "Native float format shall match IEEE 754 binary32"); -static_assert(4 == sizeof(Float32Bits), "Native float format shall match IEEE 754 binary32"); - -uint16_t canardDSDLFloat16Serialize(const CanardIEEE754Binary32 value) -{ - // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. - // The no-lint statements suppress the warning about the use of union. This is required for low-level bit access. - const uint32_t round_mask = ~(uint32_t) 0x0FFFU; // NOLINT - const Float32Bits f32inf = {.bits = ((uint32_t) 255U) << 23U}; // NOLINT NOSONAR - const Float32Bits f16inf = {.bits = ((uint32_t) 31U) << 23U}; // NOLINT NOSONAR - const Float32Bits magic = {.bits = ((uint32_t) 15U) << 23U}; // NOLINT NOSONAR - Float32Bits in = {.real = value}; // NOSONAR - const uint32_t sign = in.bits & (((uint32_t) 1U) << 31U); // NOLINT - in.bits ^= sign; - uint16_t out = 0; - if (in.bits >= f32inf.bits) - { - out = (in.bits > f32inf.bits) ? (uint16_t) 0x7FFFU : (uint16_t) 0x7C00U; // NOLINT - } - else - { - in.bits &= round_mask; - in.real *= magic.real; - in.bits -= round_mask; - if (in.bits > f16inf.bits) - { - in.bits = f16inf.bits; - } - out = (uint16_t)(in.bits >> 13U); // NOLINT - } - out |= (uint16_t)(sign >> 16U); // NOLINT - return out; -} - -CanardIEEE754Binary32 canardDSDLFloat16Deserialize(const uint16_t value) -{ - // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. - // The no-lint statements suppress the warning about the use of union. This is required for low-level bit access. - const Float32Bits magic = {.bits = ((uint32_t) 0xEFU) << 23U}; // NOLINT NOSONAR - const Float32Bits inf_nan = {.bits = ((uint32_t) 0x8FU) << 23U}; // NOLINT NOSONAR - Float32Bits out = {.bits = ((uint32_t)(value & 0x7FFFU)) << 13U}; // NOLINT NOSONAR - out.real *= magic.real; - if (out.real >= inf_nan.real) - { - out.bits |= ((uint32_t) 0xFFU) << 23U; // NOLINT - } - out.bits |= ((uint32_t)(value & 0x8000U)) << 16U; // NOLINT - return out.real; -} - -#endif // CANARD_PLATFORM_IEEE754 diff --git a/libcanard/canard.h b/libcanard/canard.h index 86cb89dc..15f41c57 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -6,7 +6,6 @@ #ifndef CANARD_H_INCLUDED #define CANARD_H_INCLUDED -#include #include #include #include @@ -51,13 +50,6 @@ extern "C" { /// If not specified, the transfer-ID timeout will take this value for all new input sessions. #define CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC 2000000UL -/// Auto-detected properties of the target platform. -#define CANARD_PLATFORM_TWOS_COMPLEMENT \ - ((INT8_MIN == -128) && (INT8_MAX == 127) && (INT16_MIN == -32768) && (INT16_MAX == 32767) && \ - (INT32_MIN == -0x80000000LL) && (INT32_MAX == 0x7FFFFFFFLL) && (INT64_MAX == 0x7FFFFFFFFFFFFFFFLL)) -#define CANARD_PLATFORM_IEEE754 \ - ((FLT_RADIX == 2) && (FLT_MANT_DIG == 24) && (FLT_MIN_EXP == -125) && (FLT_MAX_EXP == 128)) - // Forward declarations. typedef struct CanardInstance CanardInstance; @@ -318,83 +310,6 @@ int8_t canardRxAccept(CanardInstance* const ins, const uint8_t iface_index, CanardTransfer* const out_transfer); -#if CANARD_PLATFORM_TWOS_COMPLEMENT - -/// This function may be used to serialize values for later transmission in a UAVCAN transfer. -/// It serializes a primitive value -- boolean, integer, character, or floating point -- and puts it at the -/// specified bit offset in the specified contiguous buffer. -/// Simple objects can also be serialized manually instead of using this function. -/// -/// Caveat: This function works correctly only on platforms that use two's complement signed integer representation. -/// I am not aware of any modern microarchitecture that uses anything else than two's complement, so it should not -/// limit the portability. -/// -/// The type of the value pointed to by 'value' is defined as follows: -/// -/// | bit_length | value points to | -/// |------------|------------------------------------------| -/// | 1 | bool (may be incompatible with uint8_t!) | -/// | [2, 8] | uint8_t, int8_t, or char | -/// | [9, 16] | uint16_t, int16_t | -/// | [17, 32] | uint32_t, int32_t, or 32-bit float | -/// | [33, 64] | uint64_t, int64_t, or 64-bit float | -/// -/// @param destination Destination buffer where the result will be stored. -/// @param offset_bit Offset, in bits, from the beginning of the destination buffer. -/// @param length_bit Length of the value, in bits; see the table. -/// @param value Pointer to the value; see the table. -void canardDSDLPrimitiveSerialize(void* const destination, - const size_t offset_bit, - const uint8_t length_bit, - const void* const value); - -/// This function may be used to extract values from received UAVCAN transfers. It deserializes a scalar value -- -/// boolean, integer, character, or floating point -- from the specified bit position in the source buffer. -/// -/// Caveat: This function works correctly only on platforms that use two's complement signed integer representation. -/// I am not aware of any modern microarchitecture that uses anything else than two's complement, so it should not -/// limit the portability. -/// -/// The type of the value pointed to by 'out_value' is defined as follows: -/// -/// | bit_length | is_signed | out_value points to | -/// |------------|-------------|------------------------------------------| -/// | 1 | false | bool (may be incompatible with uint8_t!) | -/// | 1 | true | N/A | -/// | [2, 8] | false | uint8_t, or char | -/// | [2, 8] | true | int8_t, or char | -/// | [9, 16] | false | uint16_t | -/// | [9, 16] | true | int16_t | -/// | [17, 32] | false | uint32_t | -/// | [17, 32] | true | int32_t, or 32-bit float IEEE 754 | -/// | [33, 64] | false | uint64_t | -/// | [33, 64] | true | int64_t, or 64-bit float IEEE 754 | -/// -/// @param source The source buffer where the data will be read from. -/// @param offset_bit Offset, in bits, from the beginning of the source buffer. -/// @param length_bit Length of the value, in bits; see the table. -/// @param is_signed True if the value can be negative (i.e., sign bit extension is needed); see the table. -/// @param out_value Pointer to the output storage; see the table. -void canardDSDLPrimitiveDeserialize(const void* const source, - const size_t offset_bit, - const uint8_t length_bit, - const bool is_signed, - void* const out_value); - -#endif // CANARD_PLATFORM_TWOS_COMPLEMENT - -#if CANARD_PLATFORM_IEEE754 - -/// IEEE 754 binary16 marshaling helpers. -/// These functions convert between the native float and the standard IEEE 754 binary16 float (a.k.a. half precision). -/// It is assumed that the native float is IEEE 754 binary32, otherwise, the results may be unpredictable. -/// Majority of modern computers and microcontrollers use IEEE 754, so this limitation should not limit the portability. -typedef float CanardIEEE754Binary32; -uint16_t canardDSDLFloat16Serialize(const CanardIEEE754Binary32 value); -CanardIEEE754Binary32 canardDSDLFloat16Deserialize(const uint16_t value); - -#endif // CANARD_PLATFORM_IEEE754 - #ifdef __cplusplus } #endif diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c new file mode 100644 index 00000000..0b6bd3bb --- /dev/null +++ b/libcanard/canard_dsdl.c @@ -0,0 +1,83 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. + +#include "canard_dsdl.h" +#include + +#ifndef CANARD_ASSERT +# define CANARD_ASSERT assert +#endif + +#ifndef static_assert +// Intentional violation of MISRA: static assertion macro cannot be replaced with a function definition. +# define static_assert(x, ...) typedef char _static_assert_gl(_static_assertion_, __LINE__)[(x) ? 1 : -1] // NOSONAR +# define _static_assert_gl(a, b) _static_assert_gl_impl(a, b) // NOSONAR +// Intentional violation of MISRA: the paste operator ## cannot be avoided in this context. +# define _static_assert_gl_impl(a, b) a##b // NOSONAR +#endif + +#if CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT + +// TODO implement + +#endif // CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT + +#if CANARD_DSDL_PLATFORM_IEEE754 + +// Intentional violation of MISRA: we need this union because the alternative is far more error prone. +// We have to rely on low-level data representation details to do the conversion; unions are helpful. +typedef union // NOSONAR +{ + uint32_t bits; + CanardDSDLFloatNative real; +} Float32Bits; +static_assert(sizeof(Float32Bits) == 4, "Unsupported float model"); + +uint16_t canardDSDLFloat16Pack(const CanardDSDLFloatNative value) +{ + // The no-lint statements suppress the warnings about magic numbers. + // The no-lint statements suppress the warning about the use of union. This is required for low-level bit access. + const uint32_t round_mask = ~(uint32_t) 0x0FFFU; // NOLINT + const Float32Bits f32inf = {.bits = ((uint32_t) 255U) << 23U}; // NOLINT NOSONAR + const Float32Bits f16inf = {.bits = ((uint32_t) 31U) << 23U}; // NOLINT NOSONAR + const Float32Bits magic = {.bits = ((uint32_t) 15U) << 23U}; // NOLINT NOSONAR + Float32Bits in = {.real = value}; // NOSONAR + const uint32_t sign = in.bits & (((uint32_t) 1U) << 31U); // NOLINT + in.bits ^= sign; + uint16_t out = 0; + if (in.bits >= f32inf.bits) + { + out = (in.bits > f32inf.bits) ? (uint16_t) 0x7FFFU : (uint16_t) 0x7C00U; // NOLINT + } + else + { + in.bits &= round_mask; + in.real *= magic.real; + in.bits -= round_mask; + if (in.bits > f16inf.bits) + { + in.bits = f16inf.bits; + } + out = (uint16_t)(in.bits >> 13U); // NOLINT + } + out |= (uint16_t)(sign >> 16U); // NOLINT + return out; +} + +CanardDSDLFloatNative canardDSDLFloat16Unpack(const uint16_t value) +{ + // The no-lint statements suppress the warnings about magic numbers. + // The no-lint statements suppress the warning about the use of union. This is required for low-level bit access. + const Float32Bits magic = {.bits = ((uint32_t) 0xEFU) << 23U}; // NOLINT NOSONAR + const Float32Bits inf_nan = {.bits = ((uint32_t) 0x8FU) << 23U}; // NOLINT NOSONAR + Float32Bits out = {.bits = ((uint32_t)(value & 0x7FFFU)) << 13U}; // NOLINT NOSONAR + out.real *= magic.real; + if (out.real >= inf_nan.real) + { + out.bits |= ((uint32_t) 0xFFU) << 23U; // NOLINT + } + out.bits |= ((uint32_t)(value & 0x8000U)) << 16U; // NOLINT + return out.real; +} + +#endif // CANARD_DSDL_PLATFORM_IEEE754 diff --git a/libcanard/canard_dsdl.h b/libcanard/canard_dsdl.h new file mode 100644 index 00000000..21363c16 --- /dev/null +++ b/libcanard/canard_dsdl.h @@ -0,0 +1,108 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. +// +// This is a trivial optional extension library that contains basic DSDL primitive serialization routines. +// It is intended for use in simple applications where auto-generated DSDL serialization logic is not available. +// The functions are fully stateless (pure); read their documentation comments for usage information. +// This is an optional part of libcanard that can be omitted if this functionality is not required by the application. + +#ifndef CANARD_DSDL_H_INCLUDED +#define CANARD_DSDL_H_INCLUDED + +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT \ + ((INT8_MIN == -128) && (INT8_MAX == 127) && (INT16_MIN == -32768) && (INT16_MAX == 32767) && \ + (INT32_MIN == -0x80000000LL) && (INT32_MAX == 0x7FFFFFFFLL) && (INT64_MAX == 0x7FFFFFFFFFFFFFFFLL)) + +#define CANARD_DSDL_PLATFORM_IEEE754 \ + ((FLT_RADIX == 2) && (FLT_MANT_DIG == 24) && (FLT_MIN_EXP == -125) && (FLT_MAX_EXP == 128)) + +#if CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT + +/// This function may be used to serialize values for later transmission in a UAVCAN transfer. +/// It serializes a primitive value -- boolean, integer, character, or floating point -- and puts it at the +/// specified bit offset in the specified contiguous buffer. +/// Simple objects can also be serialized manually instead of using this function. +/// +/// The function is only available if the platform uses two's complement signed integer representation. +/// +/// The type of the value pointed to by 'value' is defined as follows: +/// +/// | bit_length | value points to | +/// |------------|------------------------------------------| +/// | 1 | bool (may be incompatible with uint8_t!) | +/// | [2, 8] | uint8_t, int8_t, or char | +/// | [9, 16] | uint16_t, int16_t | +/// | [17, 32] | uint32_t, int32_t, or 32-bit float | +/// | [33, 64] | uint64_t, int64_t, or 64-bit float | +/// +/// @param destination Destination buffer where the result will be stored. +/// @param offset_bit Offset, in bits, from the beginning of the destination buffer. +/// @param length_bit Length of the value, in bits; see the table. +/// @param value Pointer to the value; see the table. +void canardDSDLPrimitiveSerialize(void* const destination, + const size_t offset_bit, + const uint8_t length_bit, + const void* const value); + +/// This function may be used to extract values from received UAVCAN transfers. It deserializes a scalar value -- +/// boolean, integer, character, or floating point -- from the specified bit position in the source buffer. +/// +/// The function is only available if the platform uses two's complement signed integer representation. +/// +/// The type of the value pointed to by 'out_value' is defined as follows: +/// +/// | bit_length | is_signed | out_value points to | +/// |------------|-------------|------------------------------------------| +/// | 1 | false | bool (may be incompatible with uint8_t!) | +/// | 1 | true | N/A | +/// | [2, 8] | false | uint8_t, or char | +/// | [2, 8] | true | int8_t, or char | +/// | [9, 16] | false | uint16_t | +/// | [9, 16] | true | int16_t | +/// | [17, 32] | false | uint32_t | +/// | [17, 32] | true | int32_t, or 32-bit float IEEE 754 | +/// | [33, 64] | false | uint64_t | +/// | [33, 64] | true | int64_t, or 64-bit float IEEE 754 | +/// +/// @param source The source buffer where the data will be read from. +/// @param offset_bit Offset, in bits, from the beginning of the source buffer. +/// @param length_bit Length of the value, in bits; see the table. +/// @param is_signed True if the value can be negative (i.e., sign bit extension is needed); see the table. +/// @param out_value Pointer to the output storage; see the table. +void canardDSDLPrimitiveDeserialize(const void* const source, + const size_t offset_bit, + const uint8_t length_bit, + const bool is_signed, + void* const out_value); + +#endif // CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT + +#if CANARD_DSDL_PLATFORM_IEEE754 + +/// This alias for the native float is required to comply with MISRA. +typedef float CanardDSDLFloatNative; + +/// Convert a native float into the standard IEEE 754 binary16 format. The byte order is native. +/// Overflow collapses into infinity with the same sign. +/// This function is only available if the native float format is IEEE 754 binary32. +uint16_t canardDSDLFloat16Pack(const CanardDSDLFloatNative value); + +/// Convert a standard IEEE 754 binary16 value into the native float format. The byte order is native. +/// This function is only available if the native float format is IEEE 754 binary32. +CanardDSDLFloatNative canardDSDLFloat16Unpack(const uint16_t value); + +#endif // CANARD_DSDL_PLATFORM_IEEE754 + +#ifdef __cplusplus +} +#endif +#endif // CANARD_DSDL_H_INCLUDED diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 29d926c7..e6ab1de1 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -53,7 +53,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Woverloaded-virtual -Wsign-promo -Wold- include_directories(catch ${library_dir}) -set(common_sources catch/main.cpp ${library_dir}/canard.c) +set(common_sources catch/main.cpp ${library_dir}/canard.c ${library_dir}/canard_dsdl.c) function(gen_test name files compile_definitions compile_features compile_flags link_flags) add_executable(${name} ${common_sources} ${files}) diff --git a/tests/test_float16.cpp b/tests/test_float16.cpp index 3a674794..02a7c15c 100644 --- a/tests/test_float16.cpp +++ b/tests/test_float16.cpp @@ -1,30 +1,31 @@ // This software is distributed under the terms of the MIT License. // Copyright (c) 2016-2020 UAVCAN Development Team. +#include "canard_dsdl.h" #include "internals.hpp" #include -TEST_CASE("canardDSDLFloat16Serialize") +TEST_CASE("canardDSDLFloat16Pack") { // Reference values were generated manually with libuavcan and numpy.float16(). - REQUIRE(0b0000000000000000 == canardDSDLFloat16Serialize(0.0F)); - REQUIRE(0b0011110000000000 == canardDSDLFloat16Serialize(1.0F)); - REQUIRE(0b1100000000000000 == canardDSDLFloat16Serialize(-2.0F)); - REQUIRE(0b0111110000000000 == canardDSDLFloat16Serialize(999999.0F)); // +inf - REQUIRE(0b1111101111111111 == canardDSDLFloat16Serialize(-65519.0F)); // -max - REQUIRE(0b0111111111111111 == canardDSDLFloat16Serialize(std::nanf(""))); // nan + REQUIRE(0b0000000000000000 == canardDSDLFloat16Pack(0.0F)); + REQUIRE(0b0011110000000000 == canardDSDLFloat16Pack(1.0F)); + REQUIRE(0b1100000000000000 == canardDSDLFloat16Pack(-2.0F)); + REQUIRE(0b0111110000000000 == canardDSDLFloat16Pack(999999.0F)); // +inf + REQUIRE(0b1111101111111111 == canardDSDLFloat16Pack(-65519.0F)); // -max + REQUIRE(0b0111111111111111 == canardDSDLFloat16Pack(std::nanf(""))); // nan } -TEST_CASE("canardDSDLFloat16Deserialize") +TEST_CASE("canardDSDLFloat16Unpack") { // Reference values were generated manually with libuavcan and numpy.float16(). - REQUIRE(Approx(0.0F) == canardDSDLFloat16Deserialize(0b0000000000000000)); - REQUIRE(Approx(1.0F) == canardDSDLFloat16Deserialize(0b0011110000000000)); - REQUIRE(Approx(-2.0F) == canardDSDLFloat16Deserialize(0b1100000000000000)); - REQUIRE(Approx(-65504.0F) == canardDSDLFloat16Deserialize(0b1111101111111111)); - REQUIRE(std::isinf(canardDSDLFloat16Deserialize(0b0111110000000000))); + REQUIRE(Approx(0.0F) == canardDSDLFloat16Unpack(0b0000000000000000)); + REQUIRE(Approx(1.0F) == canardDSDLFloat16Unpack(0b0011110000000000)); + REQUIRE(Approx(-2.0F) == canardDSDLFloat16Unpack(0b1100000000000000)); + REQUIRE(Approx(-65504.0F) == canardDSDLFloat16Unpack(0b1111101111111111)); + REQUIRE(std::isinf(canardDSDLFloat16Unpack(0b0111110000000000))); - REQUIRE(bool(std::isnan(canardDSDLFloat16Deserialize(0b0111111111111111)))); + REQUIRE(bool(std::isnan(canardDSDLFloat16Unpack(0b0111111111111111)))); } TEST_CASE("canardDSDLFloat16") @@ -32,7 +33,7 @@ TEST_CASE("canardDSDLFloat16") float x = -1000.0F; while (x <= 1000.0F) { - REQUIRE(Approx(x) == canardDSDLFloat16Deserialize(canardDSDLFloat16Serialize(x))); + REQUIRE(Approx(x) == canardDSDLFloat16Unpack(canardDSDLFloat16Pack(x))); x += 0.5F; } } From fd3044855aa747b87696599eee299db25ca9aa87 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 15 Feb 2020 01:16:40 +0200 Subject: [PATCH 032/100] Update the heap memory model description --- libcanard/canard.c | 39 ++++++++++++++++++++++++++++----------- libcanard/canard.h | 16 +++++++++++++--- 2 files changed, 41 insertions(+), 14 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index cc4c56ac..29c5eceb 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -114,7 +114,11 @@ CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const uint16_t service_id, // ---------------------------------------- TRANSMISSION ---------------------------------------- -/// The fields are ordered to minimize padding on all platforms. +/// The memory requirement model provided in the documentation assumes that the maximum size of this structure never +/// exceeds 32 bytes on any conventional platform. The sizeof() of this structure, per the C standard, assumes that +/// the length of the flex array member is zero. +/// A user that needs a detailed analysis of the worst-case memory consumption may compute the size of this structure +/// for the particular platform at hand manually or by evaluating its sizeof(). typedef struct CanardInternalTxQueueItem { struct CanardInternalTxQueueItem* next; @@ -492,22 +496,22 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, // ---------------------------------------- RECEPTION ---------------------------------------- -/// The fields are ordered to minimize padding on all platforms. +/// The memory requirement model provided in the documentation assumes that the maximum size of this structure never +/// exceeds 64 bytes on any conventional platform. +/// A user that needs a detailed analysis of the worst-case memory consumption may compute the size of this structure +/// for the particular platform at hand manually or by evaluating its sizeof(). typedef struct CanardInternalRxSession { struct CanardInternalRxSession* next; - size_t payload_capacity; ///< Payload past this limit may be discarded by the library. - size_t payload_size; ///< How many bytes received so far. - uint8_t* payload; - + size_t payload_capacity; ///< Payload past this limit may be discarded by the library. + size_t payload_size; ///< How many bytes received so far. + uint8_t* payload; ///< Allocated once when the first frame of the transfer is recevied. uint64_t timestamp_usec; ///< Time of last update of this session. Used for removal on timeout. uint32_t transfer_id_timeout_usec; ///< When (current time - update timestamp) exceeds this, it's dead. - - const uint32_t session_specifier; ///< Differentiates this session from other sessions. - - uint16_t calculated_crc; ///< Updated with the received payload in real time. - uint8_t iface_index; + uint32_t session_specifier; ///< Differentiates this session from other sessions. + uint16_t calculated_crc; ///< Updated with the received payload in real time. + uint8_t iface_index; ///< Arbitrary value in [0, 255]. uint8_t transfer_id; bool next_toggle; } CanardInternalRxSession; @@ -614,3 +618,16 @@ void canardTxPop(CanardInstance* const ins) ins->_tx_queue = next; } } + +int8_t canardRxAccept(CanardInstance* const ins, + const CanardCANFrame* const frame, + const uint8_t iface_index, + CanardTransfer* const out_transfer) +{ + int8_t out = -CANARD_ERROR_INVALID_ARGUMENT; + if ((ins != NULL) && (frame != NULL) && (out_transfer != NULL)) + { + (void) iface_index; + } + return out; +} diff --git a/libcanard/canard.h b/libcanard/canard.h index 15f41c57..4e545fb4 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -256,9 +256,11 @@ CanardInstance canardInit(const CanardHeapAllocate heap_allocate, /// The time complexity is O(s+t), where s is the amount of payload in the transfer, and t is the number of /// frames already enqueued in the transmission queue. /// -/// The heap memory requirement is one allocation per transport frame. In other words, a single-frame transfer takes -/// one allocation; a multi-frame transfer of N frames takes N allocations. The maximum size of each allocation is -/// sizeof(CanardInternalTxQueueItem) plus MTU. +/// The heap memory requirement is one allocation per transport frame. +/// A single-frame transfer takes one allocation; a multi-frame transfer of N frames takes N allocations. +/// The maximum size of each allocation is sizeof(CanardInternalTxQueueItem) plus MTU, +/// where sizeof(CanardInternalTxQueueItem) is at most 32 bytes on any conventional platform (typically smaller). +/// For example, if the MTU is 64 bytes, the allocation size will never exceed 96 bytes on any conventional platform. int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfer); /// Access the top element of the prioritized transmission queue. The queue itself is not modified (i.e., the @@ -305,6 +307,14 @@ int8_t canardTxPeek(const CanardInstance* const ins, CanardCANFrame* const out_f /// The time complexity is constant. void canardTxPop(CanardInstance* const ins); +/// If any of the input pointers are NULL, does nothing and returns a negated invalid argument error immediately. +/// Any value of iface_index is accepted; that is, up to 256 redundant transports are supported. +/// The interface from which the transfer is accepted is always the same as iface_index. +/// A frame that initiates a new transfer may require up to two heap allocations: one of size +/// sizeof(CanardInternalRxSession) (which never exceeds 64 bytes on any conventional platform), +/// and the other of size CanardRxMetadata.payload_size_max, as returned by the application. +/// The first allocation will not take place if a transfer under this session was seen earlier (i.e., the state +/// already exists). int8_t canardRxAccept(CanardInstance* const ins, const CanardCANFrame* const frame, const uint8_t iface_index, From ec884d2150501027eca569cde604580eea49ec04 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 15 Feb 2020 05:53:58 +0200 Subject: [PATCH 033/100] Moving on with the RX pipeline --- libcanard/canard.c | 115 +++++++++++++++++++++++++++++++++++++++++---- libcanard/canard.h | 20 +++++--- tests/helpers.hpp | 4 +- tests/test_tx.cpp | 2 +- 4 files changed, 123 insertions(+), 18 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 29c5eceb..392f2ad9 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -86,6 +86,8 @@ CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const size_t size, const voi #define FLAG_SERVICE_NOT_MESSAGE (UINT32_C(1) << 25U) #define FLAG_ANONYMOUS_MESSAGE (UINT32_C(1) << 24U) #define FLAG_REQUEST_NOT_RESPONSE (UINT32_C(1) << 24U) +#define FLAG_RESERVED_23 (UINT32_C(1) << 23U) +#define FLAG_RESERVED_07 (UINT32_C(1) << 7U) CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const uint16_t subject_id, const uint8_t src_node_id); CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const uint16_t subject_id, const uint8_t src_node_id) @@ -508,7 +510,7 @@ typedef struct CanardInternalRxSession size_t payload_size; ///< How many bytes received so far. uint8_t* payload; ///< Allocated once when the first frame of the transfer is recevied. uint64_t timestamp_usec; ///< Time of last update of this session. Used for removal on timeout. - uint32_t transfer_id_timeout_usec; ///< When (current time - update timestamp) exceeds this, it's dead. + uint64_t transfer_id_timeout_usec; ///< When (current time - update timestamp) exceeds this, it's dead. uint32_t session_specifier; ///< Differentiates this session from other sessions. uint16_t calculated_crc; ///< Updated with the received payload in real time. uint8_t iface_index; ///< Arbitrary value in [0, 255]. @@ -516,6 +518,94 @@ typedef struct CanardInternalRxSession bool next_toggle; } CanardInternalRxSession; +/// High-level transport frame model. +typedef struct +{ + CanardPriority priority; + + CanardTransferKind transfer_kind; + uint16_t port_id; + uint8_t source_node_id; + uint8_t destination_node_id; + + uint8_t transfer_id; + bool start_of_transfer; + bool end_of_transfer; + bool toggle; + + size_t payload_size; + const uint8_t* payload; +} FrameModel; + +/// Returns truth if the frame is valid and parsed successfully. False if the frame is not a valid UAVCAN/CAN frame. +CANARD_INTERNAL bool tryParseFrame(const CanardFrame* const frame, FrameModel* const out_result); +CANARD_INTERNAL bool tryParseFrame(const CanardFrame* const frame, FrameModel* const out_result) +{ + CANARD_ASSERT(frame != NULL); + CANARD_ASSERT(frame->extended_can_id <= CAN_EXT_ID_MASK); + CANARD_ASSERT(out_result != NULL); + bool valid = false; + if (frame->payload_size > 0) + { + CANARD_ASSERT(frame->payload != NULL); + + // CAN ID parsing. + const uint32_t can_id = frame->extended_can_id; + out_result->priority = (CanardPriority)((can_id >> OFFSET_PRIORITY) & CANARD_PRIORITY_MAX); + out_result->source_node_id = (uint8_t)(can_id & CANARD_NODE_ID_MAX); + if (0 == (can_id & FLAG_SERVICE_NOT_MESSAGE)) + { + valid = (0 == (can_id & FLAG_RESERVED_23)) && (0 == (can_id & FLAG_RESERVED_07)); + out_result->transfer_kind = CanardTransferKindMessage; + out_result->port_id = (uint16_t)((can_id >> OFFSET_SUBJECT_ID) & CANARD_SUBJECT_ID_MAX); + if ((can_id & FLAG_ANONYMOUS_MESSAGE) != 0) + { + out_result->source_node_id = CANARD_NODE_ID_UNSET; + } + out_result->destination_node_id = CANARD_NODE_ID_UNSET; + } + else + { + valid = (0 == (can_id & FLAG_RESERVED_23)); + out_result->transfer_kind = + ((can_id & FLAG_REQUEST_NOT_RESPONSE) != 0) ? CanardTransferKindRequest : CanardTransferKindResponse; + out_result->port_id = (uint16_t)((can_id >> OFFSET_SERVICE_ID) & CANARD_SERVICE_ID_MAX); + out_result->destination_node_id = (uint8_t)((can_id >> OFFSET_DST_NODE_ID) & CANARD_NODE_ID_MAX); + } + + // Payload parsing. + out_result->payload_size = frame->payload_size - 1U; // Cut off the tail byte. + out_result->payload = (const uint8_t*) frame->payload; + + // Tail byte parsing. + // Intentional MISRA violation: indexing on a pointer. This is done to avoid pointer arithmetics. + const uint8_t tail = out_result->payload[out_result->payload_size]; + out_result->transfer_id = tail & CANARD_TRANSFER_ID_MAX; + out_result->start_of_transfer = ((tail & TAIL_START_OF_TRANSFER) != 0); + out_result->end_of_transfer = ((tail & TAIL_END_OF_TRANSFER) != 0); + out_result->toggle = ((tail & TAIL_TOGGLE) != 0); + valid = valid && (out_result->start_of_transfer ? out_result->toggle : true); + } + return valid; +} + +CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, + const FrameModel* const model, + const uint8_t iface_index, + CanardTransfer* const out_transfer) +{ + CANARD_ASSERT(ins != NULL); + CANARD_ASSERT(model != NULL); + CANARD_ASSERT(model->payload != NULL); + CANARD_ASSERT(out_transfer != NULL); + + int8_t out = 0; + + (void) iface_index; + + return out; +} + // ---------------------------------------- PUBLIC API ---------------------------------------- const uint8_t CanardCANDLCToLength[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64}; @@ -587,7 +677,7 @@ int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const tran return out; } -int8_t canardTxPeek(const CanardInstance* const ins, CanardCANFrame* const out_frame) +int8_t canardTxPeek(const CanardInstance* const ins, CanardFrame* const out_frame) { int8_t out = -CANARD_ERROR_INVALID_ARGUMENT; if ((ins != NULL) && (out_frame != NULL)) @@ -619,15 +709,24 @@ void canardTxPop(CanardInstance* const ins) } } -int8_t canardRxAccept(CanardInstance* const ins, - const CanardCANFrame* const frame, - const uint8_t iface_index, - CanardTransfer* const out_transfer) +int8_t canardRxAccept(CanardInstance* const ins, + const CanardFrame* const frame, + const uint8_t iface_index, + CanardTransfer* const out_transfer) { int8_t out = -CANARD_ERROR_INVALID_ARGUMENT; - if ((ins != NULL) && (frame != NULL) && (out_transfer != NULL)) + if ((ins != NULL) && (out_transfer != NULL) && (frame != NULL) && (frame->extended_can_id <= CAN_EXT_ID_MASK) && + ((frame->payload != NULL) || (frame->payload_size == 0))) { - (void) iface_index; + FrameModel model = {0}; + if (tryParseFrame(frame, &model)) + { + out = acceptFrame(ins, &model, iface_index, out_transfer); + } + else + { + out = 0; // Bad input frame is obviously not an error. + } } return out; } diff --git a/libcanard/canard.h b/libcanard/canard.h index 4e545fb4..f94d1f9a 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -88,7 +88,7 @@ typedef struct /// The useful data in the frame. The length value is not to be confused with DLC! size_t payload_size; const void* payload; -} CanardCANFrame; +} CanardFrame; /// Conversion look-up tables between CAN DLC and data length. extern const uint8_t CanardCANDLCToLength[16]; @@ -289,7 +289,7 @@ int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const tran /// actions are performed. /// /// The time complexity is constant. -int8_t canardTxPeek(const CanardInstance* const ins, CanardCANFrame* const out_frame); +int8_t canardTxPeek(const CanardInstance* const ins, CanardFrame* const out_frame); /// Remove and free the top element from the prioritized transmission queue. /// The application should invoke this function after the top frame obtained through canardTxPeek() has been @@ -307,7 +307,13 @@ int8_t canardTxPeek(const CanardInstance* const ins, CanardCANFrame* const out_f /// The time complexity is constant. void canardTxPop(CanardInstance* const ins); -/// If any of the input pointers are NULL, does nothing and returns a negated invalid argument error immediately. +/// The function does nothing and returns a negated invalid argument error immediately if any condition is true: +/// - Any of the input arguments that are pointers are NULL. +/// - The payload pointer of the input frame is NULL while its size is non-zero. +/// - The CAN ID of the input frame is not less than 2**29=0x20000000. +/// +/// The MTU of the accepted frame is not limited and is not dependent on the MTU setting of the local node; +/// that is, any MTU is accepted. /// Any value of iface_index is accepted; that is, up to 256 redundant transports are supported. /// The interface from which the transfer is accepted is always the same as iface_index. /// A frame that initiates a new transfer may require up to two heap allocations: one of size @@ -315,10 +321,10 @@ void canardTxPop(CanardInstance* const ins); /// and the other of size CanardRxMetadata.payload_size_max, as returned by the application. /// The first allocation will not take place if a transfer under this session was seen earlier (i.e., the state /// already exists). -int8_t canardRxAccept(CanardInstance* const ins, - const CanardCANFrame* const frame, - const uint8_t iface_index, - CanardTransfer* const out_transfer); +int8_t canardRxAccept(CanardInstance* const ins, + const CanardFrame* const frame, + const uint8_t iface_index, + CanardTransfer* const out_transfer); #ifdef __cplusplus } diff --git a/tests/helpers.hpp b/tests/helpers.hpp index e82d4567..79d964d7 100644 --- a/tests/helpers.hpp +++ b/tests/helpers.hpp @@ -173,11 +173,11 @@ class Instance [[nodiscard]] auto txPush(const CanardTransfer& transfer) { return canardTxPush(&canard_, &transfer); } - [[nodiscard]] auto txPeek(CanardCANFrame& out_frame) const { return canardTxPeek(&canard_, &out_frame); } + [[nodiscard]] auto txPeek(CanardFrame& out_frame) const { return canardTxPeek(&canard_, &out_frame); } void txPop() { canardTxPop(&canard_); } - [[nodiscard]] auto rxAccept(const CanardCANFrame& frame, const uint8_t iface_index, CanardTransfer& out_transfer) + [[nodiscard]] auto rxAccept(const CanardFrame& frame, const uint8_t iface_index, CanardTransfer& out_transfer) { return canardRxAccept(&canard_, &frame, iface_index, &out_transfer); } diff --git a/tests/test_tx.cpp b/tests/test_tx.cpp index 3e118378..2b508407 100644 --- a/tests/test_tx.cpp +++ b/tests/test_tx.cpp @@ -125,7 +125,7 @@ TEST_CASE("TxBasic0") // Pop the queue. // hex(pyuavcan.transport.commons.crc.CRC16CCITT.new(list(range(8))).value) constexpr std::uint16_t CRC8 = 0x178DU; - CanardCANFrame frame{}; + CanardFrame frame{}; REQUIRE(1 == ins.txPeek(frame)); REQUIRE(frame.payload_size == 12); REQUIRE(0 == std::memcmp(frame.payload, payload.data(), 8)); From eaf9537220e0cd3c05ae327420cc8f23dc6fc853 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 15 Feb 2020 14:11:57 +0200 Subject: [PATCH 034/100] Pending refactoring of rx_filter() --- libcanard/canard.c | 117 +++++++++++++++++++++++++++++++++++++++------ libcanard/canard.h | 15 ++++++ 2 files changed, 118 insertions(+), 14 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 392f2ad9..082f954d 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -506,14 +506,13 @@ typedef struct CanardInternalRxSession { struct CanardInternalRxSession* next; - size_t payload_capacity; ///< Payload past this limit may be discarded by the library. - size_t payload_size; ///< How many bytes received so far. - uint8_t* payload; ///< Allocated once when the first frame of the transfer is recevied. - uint64_t timestamp_usec; ///< Time of last update of this session. Used for removal on timeout. - uint64_t transfer_id_timeout_usec; ///< When (current time - update timestamp) exceeds this, it's dead. - uint32_t session_specifier; ///< Differentiates this session from other sessions. - uint16_t calculated_crc; ///< Updated with the received payload in real time. - uint8_t iface_index; ///< Arbitrary value in [0, 255]. + size_t payload_capacity; ///< Payload past this limit may be discarded by the library. + size_t payload_size; ///< How many bytes received so far. + uint8_t* payload; ///< Allocated once when the first frame of the transfer is received. + uint64_t timestamp_usec; ///< Time of last update of this session. Used for removal on timeout. + uint32_t session_specifier; ///< Differentiates this session from other sessions. + uint16_t calculated_crc; ///< Updated with the received payload in real time. + uint8_t iface_index; ///< Arbitrary value in [0, 255]. uint8_t transfer_id; bool next_toggle; } CanardInternalRxSession; @@ -521,6 +520,8 @@ typedef struct CanardInternalRxSession /// High-level transport frame model. typedef struct { + uint64_t timestamp_usec; + CanardPriority priority; CanardTransferKind transfer_kind; @@ -548,6 +549,7 @@ CANARD_INTERNAL bool tryParseFrame(const CanardFrame* const frame, FrameModel* c if (frame->payload_size > 0) { CANARD_ASSERT(frame->payload != NULL); + out_result->timestamp_usec = frame->timestamp_usec; // CAN ID parsing. const uint32_t can_id = frame->extended_can_id; @@ -584,21 +586,96 @@ CANARD_INTERNAL bool tryParseFrame(const CanardFrame* const frame, FrameModel* c out_result->start_of_transfer = ((tail & TAIL_START_OF_TRANSFER) != 0); out_result->end_of_transfer = ((tail & TAIL_END_OF_TRANSFER) != 0); out_result->toggle = ((tail & TAIL_TOGGLE) != 0); - valid = valid && (out_result->start_of_transfer ? out_result->toggle : true); + + // Final validation. + valid = valid && (out_result->start_of_transfer ? out_result->toggle : true); // Protocol version check. + valid = valid && ((out_result->source_node_id == CANARD_NODE_ID_UNSET) + ? (out_result->start_of_transfer && out_result->end_of_transfer) // Single-frame. + : true); } return valid; } -CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, - const FrameModel* const model, - const uint8_t iface_index, - CanardTransfer* const out_transfer) +CANARD_INTERNAL CanardRxMetadata callRxFilter(const CanardInstance* const ins, const FrameModel* const model); +CANARD_INTERNAL CanardRxMetadata callRxFilter(const CanardInstance* const ins, const FrameModel* const model) +{ + CANARD_ASSERT(ins->rx_filter != NULL); + return ins->rx_filter(ins, model->port_id, model->transfer_kind, model->source_node_id); +} + +CANARD_INTERNAL void initRxTransferFromFrame(const FrameModel* const model, CanardTransfer* const out_transfer); +CANARD_INTERNAL void initRxTransferFromFrame(const FrameModel* const model, CanardTransfer* const out_transfer) +{ + CANARD_ASSERT(model != NULL); + CANARD_ASSERT(model->payload != NULL); + CANARD_ASSERT(out_transfer != NULL); + out_transfer->timestamp_usec = model->timestamp_usec; + out_transfer->priority = model->priority; + out_transfer->transfer_kind = model->transfer_kind; + out_transfer->port_id = model->port_id; + out_transfer->remote_node_id = model->source_node_id; + out_transfer->transfer_id = model->transfer_id; + out_transfer->payload_size = model->payload_size; + out_transfer->payload = model->payload; +} + +CANARD_INTERNAL CanardInternalRxSession* findRxSession(CanardInstance* const ins, const uint32_t session_specifier); +CANARD_INTERNAL CanardInternalRxSession* findRxSession(CanardInstance* const ins, const uint32_t session_specifier) +{ + CANARD_ASSERT(ins != NULL); + // The linear search should be replaced with O(log n) at least, O(1) is preferred. Please help us here. + CanardInternalRxSession* p = ins->_rx_sessions; + while (p != NULL) + { + if (p->session_specifier == session_specifier) + { + break; + } + p = p->next; + } + return p; +} + +CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, + const FrameModel* const model, + const uint8_t iface_index, + const CanardRxMetadata* const rx_meta, + CanardTransfer* const out_transfer); +CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, + const FrameModel* const model, + const uint8_t iface_index, + const CanardRxMetadata* const rx_meta, + CanardTransfer* const out_transfer) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(model != NULL); CANARD_ASSERT(model->payload != NULL); CANARD_ASSERT(out_transfer != NULL); + uint32_t session_specifier = UINT32_MAX; + if (model->transfer_kind == CanardTransferKindMessage) + { + session_specifier = makeMessageSessionSpecifier(model->port_id, model->source_node_id); + } + else if ((model->transfer_kind == CanardTransferKindRequest) || + (model->transfer_kind == CanardTransferKindResponse)) + { + session_specifier = makeServiceSessionSpecifier(model->port_id, + model->transfer_kind == CanardTransferKindRequest, + model->source_node_id, + model->destination_node_id); + } + else + { + CANARD_ASSERT(false); // Internal protocol violation. Not expected to happen. + } + + (void) rx_meta; + CanardInternalRxSession* session = findRxSession(ins, session_specifier); + if (NULL == session) + { + } + int8_t out = 0; (void) iface_index; @@ -721,7 +798,19 @@ int8_t canardRxAccept(CanardInstance* const ins, FrameModel model = {0}; if (tryParseFrame(frame, &model)) { - out = acceptFrame(ins, &model, iface_index, out_transfer); + const CanardRxMetadata rx_meta = callRxFilter(ins, &model); + if (rx_meta.transfer_id_timeout_usec > 0U) + { + if (model.source_node_id == CANARD_NODE_ID_UNSET) + { + initRxTransferFromFrame(&model, out_transfer); + out = 1; + } + else + { + out = acceptFrame(ins, &model, iface_index, &rx_meta, out_transfer); + } + } } else { diff --git a/libcanard/canard.h b/libcanard/canard.h index f94d1f9a..3f55f244 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -312,10 +312,25 @@ void canardTxPop(CanardInstance* const ins); /// - The payload pointer of the input frame is NULL while its size is non-zero. /// - The CAN ID of the input frame is not less than 2**29=0x20000000. /// +/// RX filter callback behavior? +/// +/// The function returns zero if any of the following conditions are true; the general policy is that protocol errors +/// are not escalated because they do not construe a node-local error: +/// - The received frame is not a valid UAVCAN/CAN transport frame; in this case the frame is silently ignored. +/// - The received frame is a valid UAVCAN/CAN transport frame, but it belongs to a session that is not +/// relevant to the application (i.e., the application reported via the RX filter callback that the library +/// need not receive transfers from this session). +/// - The received frame is a valid UAVCAN/CAN transport frame, but it did not complete a transfer, or it belongs +/// to an invalid frame sequence. +/// /// The MTU of the accepted frame is not limited and is not dependent on the MTU setting of the local node; /// that is, any MTU is accepted. +/// /// Any value of iface_index is accepted; that is, up to 256 redundant transports are supported. /// The interface from which the transfer is accepted is always the same as iface_index. +/// +/// +/// /// A frame that initiates a new transfer may require up to two heap allocations: one of size /// sizeof(CanardInternalRxSession) (which never exceeds 64 bytes on any conventional platform), /// and the other of size CanardRxMetadata.payload_size_max, as returned by the application. From fd4a225f07ecc29244a05c2a112c36927594da7b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 15 Feb 2020 18:22:28 +0200 Subject: [PATCH 035/100] RX revamping WIP --- libcanard/canard.c | 378 +++++++++++++++++++++++--------------------- libcanard/canard.h | 121 ++++++++------ tests/internals.hpp | 2 +- 3 files changed, 273 insertions(+), 228 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 082f954d..8cc8b521 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -47,13 +47,15 @@ // ---------------------------------------- TRANSFER CRC ---------------------------------------- +typedef uint16_t TransferCRC; + #define CRC_INITIAL 0xFFFFU #define CRC_SIZE_BYTES 2U -CANARD_INTERNAL uint16_t crcAddByte(const uint16_t crc, const uint8_t byte); -CANARD_INTERNAL uint16_t crcAddByte(const uint16_t crc, const uint8_t byte) +CANARD_INTERNAL TransferCRC crcAddByte(const TransferCRC crc, const uint8_t byte); +CANARD_INTERNAL TransferCRC crcAddByte(const TransferCRC crc, const uint8_t byte) { - uint16_t out = crc ^ (uint16_t)((uint16_t)(byte) << BITS_PER_BYTE); + TransferCRC out = crc ^ (uint16_t)((uint16_t)(byte) << BITS_PER_BYTE); for (uint8_t i = 0; i < BITS_PER_BYTE; i++) // Should we use a table instead? Adds 512 bytes of ROM. { // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. @@ -62,11 +64,11 @@ CANARD_INTERNAL uint16_t crcAddByte(const uint16_t crc, const uint8_t byte) return out; } -CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const size_t size, const void* const data); -CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const size_t size, const void* const data) +CANARD_INTERNAL TransferCRC crcAdd(const TransferCRC crc, const size_t size, const void* const data); +CANARD_INTERNAL TransferCRC crcAdd(const TransferCRC crc, const size_t size, const void* const data) { CANARD_ASSERT((data != NULL) || (size == 0U)); - uint16_t out = crc; + TransferCRC out = crc; const uint8_t* p = (const uint8_t*) data; for (size_t i = 0; i < size; i++) { @@ -89,22 +91,22 @@ CANARD_INTERNAL uint16_t crcAdd(const uint16_t crc, const size_t size, const voi #define FLAG_RESERVED_23 (UINT32_C(1) << 23U) #define FLAG_RESERVED_07 (UINT32_C(1) << 7U) -CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const uint16_t subject_id, const uint8_t src_node_id); -CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const uint16_t subject_id, const uint8_t src_node_id) +CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const CanardPortID subject_id, const CanardNodeID src_node_id); +CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const CanardPortID subject_id, const CanardNodeID src_node_id) { CANARD_ASSERT(src_node_id <= CANARD_NODE_ID_MAX); CANARD_ASSERT(subject_id <= CANARD_SUBJECT_ID_MAX); return src_node_id | ((uint32_t) subject_id << OFFSET_SUBJECT_ID); } -CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const uint16_t service_id, - const bool request_not_response, - const uint8_t src_node_id, - const uint8_t dst_node_id); -CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const uint16_t service_id, - const bool request_not_response, - const uint8_t src_node_id, - const uint8_t dst_node_id) +CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const CanardPortID service_id, + const bool request_not_response, + const CanardNodeID src_node_id, + const CanardNodeID dst_node_id); +CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const CanardPortID service_id, + const bool request_not_response, + const CanardNodeID src_node_id, + const CanardNodeID dst_node_id) { CANARD_ASSERT(src_node_id <= CANARD_NODE_ID_MAX); CANARD_ASSERT(dst_node_id <= CANARD_NODE_ID_MAX); @@ -121,13 +123,14 @@ CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const uint16_t service_id, /// the length of the flex array member is zero. /// A user that needs a detailed analysis of the worst-case memory consumption may compute the size of this structure /// for the particular platform at hand manually or by evaluating its sizeof(). +/// The fields are ordered to minimize the amount of padding on all conventional platforms. typedef struct CanardInternalTxQueueItem { struct CanardInternalTxQueueItem* next; - uint32_t id; - uint64_t deadline_usec; - size_t payload_size; + CanardMicrosecond deadline_usec; + size_t payload_size; + uint32_t id; // Intentional violation of MISRA: this flex array is the lesser of three evils. The other two are: // - Use pointer, make it point to the remainder of the allocated memory following this structure. @@ -158,10 +161,10 @@ CANARD_INTERNAL size_t getPresentationLayerMTU(const CanardInstance* const ins) } CANARD_INTERNAL int32_t makeCANID(const CanardTransfer* const tr, - const uint8_t local_node_id, + const CanardNodeID local_node_id, const size_t presentation_layer_mtu); CANARD_INTERNAL int32_t makeCANID(const CanardTransfer* const tr, - const uint8_t local_node_id, + const CanardNodeID local_node_id, const size_t presentation_layer_mtu) { CANARD_ASSERT(tr != NULL); @@ -178,7 +181,8 @@ CANARD_INTERNAL int32_t makeCANID(const CanardTransfer* const tr, else if (tr->payload_size <= presentation_layer_mtu) { CANARD_ASSERT((tr->payload != NULL) || (tr->payload_size == 0U)); - const uint8_t c = (uint8_t)(crcAdd(CRC_INITIAL, tr->payload_size, tr->payload) & CANARD_NODE_ID_MAX); + const CanardNodeID c = + (CanardNodeID)(crcAdd(CRC_INITIAL, tr->payload_size, tr->payload) & CANARD_NODE_ID_MAX); const uint32_t spec = makeMessageSessionSpecifier(tr->port_id, c) | FLAG_ANONYMOUS_MESSAGE; CANARD_ASSERT(spec <= CAN_EXT_ID_MASK); out = (int32_t) spec; @@ -225,14 +229,14 @@ CANARD_INTERNAL int32_t makeCANID(const CanardTransfer* const tr, return out; } -CANARD_INTERNAL uint8_t makeTailByte(const bool start_of_transfer, - const bool end_of_transfer, - const bool toggle, - const uint8_t transfer_id); -CANARD_INTERNAL uint8_t makeTailByte(const bool start_of_transfer, - const bool end_of_transfer, - const bool toggle, - const uint8_t transfer_id) +CANARD_INTERNAL uint8_t makeTailByte(const bool start_of_transfer, + const bool end_of_transfer, + const bool toggle, + const CanardTransferID transfer_id); +CANARD_INTERNAL uint8_t makeTailByte(const bool start_of_transfer, + const bool end_of_transfer, + const bool toggle, + const CanardTransferID transfer_id) { CANARD_ASSERT(start_of_transfer ? toggle : true); return (uint8_t)((start_of_transfer ? TAIL_START_OF_TRANSFER : 0U) | (end_of_transfer ? TAIL_END_OF_TRANSFER : 0U) | @@ -250,14 +254,14 @@ CANARD_INTERNAL size_t roundFramePayloadSizeUp(const size_t x) return CanardCANDLCToLength[y]; } -CANARD_INTERNAL CanardInternalTxQueueItem* allocateTxQueueItem(CanardInstance* const ins, - const uint32_t id, - const uint64_t deadline_usec, - const size_t payload_size); -CANARD_INTERNAL CanardInternalTxQueueItem* allocateTxQueueItem(CanardInstance* const ins, - const uint32_t id, - const uint64_t deadline_usec, - const size_t payload_size) +CANARD_INTERNAL CanardInternalTxQueueItem* allocateTxQueueItem(CanardInstance* const ins, + const uint32_t id, + const CanardMicrosecond deadline_usec, + const size_t payload_size); +CANARD_INTERNAL CanardInternalTxQueueItem* allocateTxQueueItem(CanardInstance* const ins, + const uint32_t id, + const CanardMicrosecond deadline_usec, + const size_t payload_size) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(payload_size > 0U); // UAVCAN/CAN doesn't allow zero-payload frames. @@ -266,9 +270,9 @@ CANARD_INTERNAL CanardInternalTxQueueItem* allocateTxQueueItem(CanardInstance* c if (out != NULL) { out->next = NULL; - out->id = id; out->deadline_usec = deadline_usec; out->payload_size = payload_size; + out->id = id; } return out; } @@ -298,18 +302,18 @@ CANARD_INTERNAL CanardInternalTxQueueItem* findTxQueueSupremum(const CanardInsta } /// Returns the number of frames enqueued or error (i.e., =1 or <0). -CANARD_INTERNAL int32_t pushSingleFrameTransfer(CanardInstance* const ins, - const uint64_t deadline_usec, - const uint32_t can_id, - const uint8_t transfer_id, - const size_t payload_size, - const void* const payload); -CANARD_INTERNAL int32_t pushSingleFrameTransfer(CanardInstance* const ins, - const uint64_t deadline_usec, - const uint32_t can_id, - const uint8_t transfer_id, - const size_t payload_size, - const void* const payload) +CANARD_INTERNAL int32_t pushSingleFrameTransfer(CanardInstance* const ins, + const CanardMicrosecond deadline_usec, + const uint32_t can_id, + const CanardTransferID transfer_id, + const size_t payload_size, + const void* const payload); +CANARD_INTERNAL int32_t pushSingleFrameTransfer(CanardInstance* const ins, + const CanardMicrosecond deadline_usec, + const uint32_t can_id, + const CanardTransferID transfer_id, + const size_t payload_size, + const void* const payload) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT((payload != NULL) || (payload_size == 0)); @@ -358,20 +362,20 @@ CANARD_INTERNAL int32_t pushSingleFrameTransfer(CanardInstance* const ins, } /// Returns the number of frames enqueued or error. -CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, - const size_t presentation_layer_mtu, - const uint64_t deadline_usec, - const uint32_t can_id, - const uint8_t transfer_id, - const size_t payload_size, - const void* const payload); -CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, - const size_t presentation_layer_mtu, - const uint64_t deadline_usec, - const uint32_t can_id, - const uint8_t transfer_id, - const size_t payload_size, - const void* const payload) +CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, + const size_t presentation_layer_mtu, + const CanardMicrosecond deadline_usec, + const uint32_t can_id, + const CanardTransferID transfer_id, + const size_t payload_size, + const void* const payload); +CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, + const size_t presentation_layer_mtu, + const CanardMicrosecond deadline_usec, + const uint32_t can_id, + const CanardTransferID transfer_id, + const size_t payload_size, + const void* const payload) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(presentation_layer_mtu > 0U); @@ -385,7 +389,7 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, const size_t payload_size_with_crc = payload_size + CRC_SIZE_BYTES; size_t offset = 0U; - uint16_t crc = crcAdd(CRC_INITIAL, payload_size, payload); + TransferCRC crc = crcAdd(CRC_INITIAL, payload_size, payload); bool start_of_transfer = true; bool toggle = true; const uint8_t* payload_ptr = (const uint8_t*) payload; @@ -499,40 +503,59 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, // ---------------------------------------- RECEPTION ---------------------------------------- /// The memory requirement model provided in the documentation assumes that the maximum size of this structure never -/// exceeds 64 bytes on any conventional platform. +/// exceeds 24 bytes on any conventional platform. The sizeof() of this structure, per the C standard, assumes that +/// the length of the flex array member is zero. +/// A user that needs a detailed analysis of the worst-case memory consumption may compute the size of this structure +/// for the particular platform at hand manually or by evaluating its sizeof(). +/// The fields are ordered to minimize the amount of padding on all conventional platforms. +typedef struct +{ + CanardMicrosecond transfer_timestamp_usec; ///< Timestamp of the last received start-of-transfer. + size_t payload_size; ///< How many bytes received so far. + TransferCRC calculated_crc; ///< Updated with the received payload in real time. + CanardTransferID toggle_and_transfer_id; ///< Toggle and transfer-ID combined into one field to reduce footprint. + uint8_t iface_index; ///< Arbitrary value in [0, 255]. + + // Intentional violation of MISRA: this flex array is the lesser of three evils. The other two are: + // - Use pointer, make it point to the remainder of the allocated memory following this structure. + // The pointer is bad because it requires us to use pointer arithmetics and adds sizeof(void*) of waste per item. + // - Use a separate memory allocation for data. This is terribly wasteful. + uint8_t payload[]; // NOSONAR +} RxSession; + +#define SESSIONS_PER_SUBSCRIPTION (CANARD_NODE_ID_MAX + 1U) + +/// The memory requirement model provided in the documentation assumes that the maximum size of this structure never +/// exceeds (129*sizeof(void*) + 24 bytes) on any conventional platform. /// A user that needs a detailed analysis of the worst-case memory consumption may compute the size of this structure /// for the particular platform at hand manually or by evaluating its sizeof(). -typedef struct CanardInternalRxSession +/// The fields are ordered to minimize the amount of padding on all conventional platforms. +typedef struct CanardInternalRxSubscription { - struct CanardInternalRxSession* next; - - size_t payload_capacity; ///< Payload past this limit may be discarded by the library. - size_t payload_size; ///< How many bytes received so far. - uint8_t* payload; ///< Allocated once when the first frame of the transfer is received. - uint64_t timestamp_usec; ///< Time of last update of this session. Used for removal on timeout. - uint32_t session_specifier; ///< Differentiates this session from other sessions. - uint16_t calculated_crc; ///< Updated with the received payload in real time. - uint8_t iface_index; ///< Arbitrary value in [0, 255]. - uint8_t transfer_id; - bool next_toggle; -} CanardInternalRxSession; + struct CanardInternalRxSubscription* next; + + RxSession* sessions[SESSIONS_PER_SUBSCRIPTION]; + CanardMicrosecond transfer_id_timeout_usec; + size_t payload_size_bytes_max; ///< Payload past this limit may be discarded by the library. + CanardPortID port_id; +} CanardInternalRxSubscription; /// High-level transport frame model. typedef struct { - uint64_t timestamp_usec; + CanardMicrosecond timestamp_usec; CanardPriority priority; CanardTransferKind transfer_kind; - uint16_t port_id; - uint8_t source_node_id; - uint8_t destination_node_id; + CanardPortID port_id; + CanardNodeID source_node_id; + CanardNodeID destination_node_id; - uint8_t transfer_id; - bool start_of_transfer; - bool end_of_transfer; - bool toggle; + CanardTransferID transfer_id; + bool start_of_transfer; + bool end_of_transfer; + bool toggle; size_t payload_size; const uint8_t* payload; @@ -554,12 +577,12 @@ CANARD_INTERNAL bool tryParseFrame(const CanardFrame* const frame, FrameModel* c // CAN ID parsing. const uint32_t can_id = frame->extended_can_id; out_result->priority = (CanardPriority)((can_id >> OFFSET_PRIORITY) & CANARD_PRIORITY_MAX); - out_result->source_node_id = (uint8_t)(can_id & CANARD_NODE_ID_MAX); + out_result->source_node_id = (CanardNodeID)(can_id & CANARD_NODE_ID_MAX); if (0 == (can_id & FLAG_SERVICE_NOT_MESSAGE)) { valid = (0 == (can_id & FLAG_RESERVED_23)) && (0 == (can_id & FLAG_RESERVED_07)); out_result->transfer_kind = CanardTransferKindMessage; - out_result->port_id = (uint16_t)((can_id >> OFFSET_SUBJECT_ID) & CANARD_SUBJECT_ID_MAX); + out_result->port_id = (CanardPortID)((can_id >> OFFSET_SUBJECT_ID) & CANARD_SUBJECT_ID_MAX); if ((can_id & FLAG_ANONYMOUS_MESSAGE) != 0) { out_result->source_node_id = CANARD_NODE_ID_UNSET; @@ -571,8 +594,8 @@ CANARD_INTERNAL bool tryParseFrame(const CanardFrame* const frame, FrameModel* c valid = (0 == (can_id & FLAG_RESERVED_23)); out_result->transfer_kind = ((can_id & FLAG_REQUEST_NOT_RESPONSE) != 0) ? CanardTransferKindRequest : CanardTransferKindResponse; - out_result->port_id = (uint16_t)((can_id >> OFFSET_SERVICE_ID) & CANARD_SERVICE_ID_MAX); - out_result->destination_node_id = (uint8_t)((can_id >> OFFSET_DST_NODE_ID) & CANARD_NODE_ID_MAX); + out_result->port_id = (CanardPortID)((can_id >> OFFSET_SERVICE_ID) & CANARD_SERVICE_ID_MAX); + out_result->destination_node_id = (CanardNodeID)((can_id >> OFFSET_DST_NODE_ID) & CANARD_NODE_ID_MAX); } // Payload parsing. @@ -596,13 +619,6 @@ CANARD_INTERNAL bool tryParseFrame(const CanardFrame* const frame, FrameModel* c return valid; } -CANARD_INTERNAL CanardRxMetadata callRxFilter(const CanardInstance* const ins, const FrameModel* const model); -CANARD_INTERNAL CanardRxMetadata callRxFilter(const CanardInstance* const ins, const FrameModel* const model) -{ - CANARD_ASSERT(ins->rx_filter != NULL); - return ins->rx_filter(ins, model->port_id, model->transfer_kind, model->source_node_id); -} - CANARD_INTERNAL void initRxTransferFromFrame(const FrameModel* const model, CanardTransfer* const out_transfer); CANARD_INTERNAL void initRxTransferFromFrame(const FrameModel* const model, CanardTransfer* const out_transfer) { @@ -619,70 +635,6 @@ CANARD_INTERNAL void initRxTransferFromFrame(const FrameModel* const model, Cana out_transfer->payload = model->payload; } -CANARD_INTERNAL CanardInternalRxSession* findRxSession(CanardInstance* const ins, const uint32_t session_specifier); -CANARD_INTERNAL CanardInternalRxSession* findRxSession(CanardInstance* const ins, const uint32_t session_specifier) -{ - CANARD_ASSERT(ins != NULL); - // The linear search should be replaced with O(log n) at least, O(1) is preferred. Please help us here. - CanardInternalRxSession* p = ins->_rx_sessions; - while (p != NULL) - { - if (p->session_specifier == session_specifier) - { - break; - } - p = p->next; - } - return p; -} - -CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, - const FrameModel* const model, - const uint8_t iface_index, - const CanardRxMetadata* const rx_meta, - CanardTransfer* const out_transfer); -CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, - const FrameModel* const model, - const uint8_t iface_index, - const CanardRxMetadata* const rx_meta, - CanardTransfer* const out_transfer) -{ - CANARD_ASSERT(ins != NULL); - CANARD_ASSERT(model != NULL); - CANARD_ASSERT(model->payload != NULL); - CANARD_ASSERT(out_transfer != NULL); - - uint32_t session_specifier = UINT32_MAX; - if (model->transfer_kind == CanardTransferKindMessage) - { - session_specifier = makeMessageSessionSpecifier(model->port_id, model->source_node_id); - } - else if ((model->transfer_kind == CanardTransferKindRequest) || - (model->transfer_kind == CanardTransferKindResponse)) - { - session_specifier = makeServiceSessionSpecifier(model->port_id, - model->transfer_kind == CanardTransferKindRequest, - model->source_node_id, - model->destination_node_id); - } - else - { - CANARD_ASSERT(false); // Internal protocol violation. Not expected to happen. - } - - (void) rx_meta; - CanardInternalRxSession* session = findRxSession(ins, session_specifier); - if (NULL == session) - { - } - - int8_t out = 0; - - (void) iface_index; - - return out; -} - // ---------------------------------------- PUBLIC API ---------------------------------------- const uint8_t CanardCANDLCToLength[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64}; @@ -697,22 +649,18 @@ const uint8_t CanardCANLengthToDLC[65] = { 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, // 49-64 }; -CanardInstance canardInit(const CanardHeapAllocate heap_allocate, - const CanardHeapFree heap_free, - const CanardRxFilter rx_filter) +CanardInstance canardInit(const CanardHeapAllocate heap_allocate, const CanardHeapFree heap_free) { CANARD_ASSERT(heap_allocate != NULL); CANARD_ASSERT(heap_free != NULL); - CANARD_ASSERT(rx_filter != NULL); const CanardInstance out = { - .user_reference = NULL, - .mtu_bytes = CANARD_MTU_CAN_FD, - .node_id = CANARD_NODE_ID_UNSET, - .heap_allocate = heap_allocate, - .heap_free = heap_free, - .rx_filter = rx_filter, - ._rx_sessions = NULL, - ._tx_queue = NULL, + .user_reference = NULL, + .mtu_bytes = CANARD_MTU_CAN_FD, + .node_id = CANARD_NODE_ID_UNSET, + .heap_allocate = heap_allocate, + .heap_free = heap_free, + ._rx_subscriptions = {NULL}, + ._tx_queue = NULL, }; return out; } @@ -798,8 +746,7 @@ int8_t canardRxAccept(CanardInstance* const ins, FrameModel model = {0}; if (tryParseFrame(frame, &model)) { - const CanardRxMetadata rx_meta = callRxFilter(ins, &model); - if (rx_meta.transfer_id_timeout_usec > 0U) + if (true) // TODO stub { if (model.source_node_id == CANARD_NODE_ID_UNSET) { @@ -808,7 +755,8 @@ int8_t canardRxAccept(CanardInstance* const ins, } else { - out = acceptFrame(ins, &model, iface_index, &rx_meta, out_transfer); + (void) iface_index; + out = -1; // TODO stub } } } @@ -819,3 +767,77 @@ int8_t canardRxAccept(CanardInstance* const ins, } return out; } + +int8_t canardRxSubscribe(CanardInstance* const ins, + const CanardTransferKind transfer_kind, + const CanardPortID port_id, + const size_t payload_size_bytes_max, + const CanardMicrosecond transfer_id_timeout_usec) +{ + const uint8_t transfer_kind_index = (uint8_t) transfer_kind; + int8_t out = -CANARD_ERROR_INVALID_ARGUMENT; + if ((ins != NULL) && (transfer_kind_index < CANARD_NUM_TRANSFER_KINDS)) + { + canardRxUnsubscribe(ins, transfer_kind, port_id); // Reset to the initial state. + + CanardInternalRxSubscription* const subs = + (CanardInternalRxSubscription*) ins->heap_allocate(ins, sizeof(CanardInternalRxSubscription)); + if (subs != NULL) + { + for (size_t i = 0; i < SESSIONS_PER_SUBSCRIPTION; i++) + { + // The sessions will be created ad-hoc. Normally, for a low-jitter deterministic system, + // we could have pre-allocated sessions here, but that requires too much memory to be feasible. + subs->sessions[i] = NULL; + } + subs->transfer_id_timeout_usec = transfer_id_timeout_usec; + subs->payload_size_bytes_max = payload_size_bytes_max; + subs->port_id = port_id; + subs->next = ins->_rx_subscriptions[transfer_kind_index]; + + ins->_rx_subscriptions[transfer_kind_index] = subs; + + out = 0; + } + else + { + out = -CANARD_ERROR_OUT_OF_MEMORY; + } + } + return out; +} + +void canardRxUnsubscribe(CanardInstance* const ins, const CanardTransferKind transfer_kind, const CanardPortID port_id) +{ + const uint8_t transfer_kind_index = (uint8_t) transfer_kind; + if ((ins != NULL) && (transfer_kind_index < CANARD_NUM_TRANSFER_KINDS)) + { + CanardInternalRxSubscription* prev = NULL; + CanardInternalRxSubscription* subs = ins->_rx_subscriptions[transfer_kind_index]; + while ((subs != NULL) && (subs->port_id != port_id)) + { + prev = subs; + subs = subs->next; + } + + if (subs != NULL) + { + CANARD_ASSERT(subs->port_id == port_id); + if (prev != NULL) + { + prev->next = subs->next; + } + else + { + ins->_rx_subscriptions[transfer_kind_index] = subs->next; + } + + for (size_t i = 0; i < SESSIONS_PER_SUBSCRIPTION; i++) + { + ins->heap_free(ins, subs->sessions[i]); // The pointer may be NULL but it's acceptable. + subs->sessions[i] = NULL; + } + ins->heap_free(ins, subs); + } + } +} diff --git a/libcanard/canard.h b/libcanard/canard.h index 3f55f244..c42763be 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -16,10 +16,12 @@ extern "C" { /// Semantic version of this library (not the UAVCAN specification). /// API will be backward compatible within the same major version. -#define CANARD_VERSION_MAJOR 1 +#define CANARD_VERSION_MAJOR 0 +#define CANARD_VERSION_MINOR 1 /// The version number of the UAVCAN specification implemented by this library. #define CANARD_UAVCAN_SPECIFICATION_VERSION_MAJOR 1 +#define CANARD_UAVCAN_SPECIFICATION_VERSION_MINOR 0 /// These error codes may be returned from the library API calls whose return type is a signed integer /// in the negated form (i.e., code 2 returned as -2). @@ -53,6 +55,11 @@ extern "C" { // Forward declarations. typedef struct CanardInstance CanardInstance; +typedef uint64_t CanardMicrosecond; +typedef uint16_t CanardPortID; +typedef uint8_t CanardNodeID; +typedef uint8_t CanardTransferID; + /// Transfer priority level mnemonics per the recommendations given in the UAVCAN specification. typedef enum { @@ -73,6 +80,7 @@ typedef enum CanardTransferKindResponse = 1, ///< Point-to-point, from server to client. CanardTransferKindRequest = 2, ///< Point-to-point, from client to server. } CanardTransferKind; +#define CANARD_NUM_TRANSFER_KINDS 3 /// CAN data frame with an extended 29-bit ID. RTR/Error frames are not used and therefore not modeled here. typedef struct @@ -80,7 +88,7 @@ typedef struct /// For RX frames: reception timestamp. /// For TX frames: transmission deadline. /// The time system may be arbitrary as long as the clock is monotonic (steady). - uint64_t timestamp_usec; + CanardMicrosecond timestamp_usec; /// 29-bit extended ID. The bits above 29-th are zero/ignored. uint32_t extended_can_id; @@ -90,8 +98,10 @@ typedef struct const void* payload; } CanardFrame; -/// Conversion look-up tables between CAN DLC and data length. +/// Conversion look-up table from CAN DLC to data length. extern const uint8_t CanardCANDLCToLength[16]; + +/// Conversion look-up table from data length to CAN DLC; the length is rounded up. extern const uint8_t CanardCANLengthToDLC[65]; typedef struct @@ -99,53 +109,25 @@ typedef struct /// For RX transfers: reception timestamp. /// For TX transfers: transmission deadline. /// The time system may be arbitrary as long as the clock is monotonic (steady). - uint64_t timestamp_usec; + CanardMicrosecond timestamp_usec; CanardPriority priority; CanardTransferKind transfer_kind; /// Subject-ID for message publications; service-ID for service requests/responses. - uint16_t port_id; + CanardPortID port_id; /// For outgoing message transfers or for incoming anonymous transfers, the value is CANARD_NODE_ID_UNSET. - uint8_t remote_node_id; + CanardNodeID remote_node_id; - uint8_t transfer_id; + CanardTransferID transfer_id; /// The const pointer makes it incompatible with free(), but we have to tolerate that due to the limitations of C. size_t payload_size; const void* payload; } CanardTransfer; -/// The application supplies the library with this information when a new transfer should be received. -typedef struct -{ - /// The transfer-ID timeout for this session. - /// If no specific requirements are defined, the default CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC should be used. - /// Zero timeout indicates that this transfer should not be received (all its frames will be silently dropped). - uint64_t transfer_id_timeout_usec; - - /// The maximum payload size of the transfer (i.e., the maximum size of the serialized DSDL object), in bytes. - /// Payloads larger than this will be silently truncated per the Implicit Truncation Rule defined in Specification. - /// Per Specification, the transfer CRC of multi-frame transfers is always validated regardless of the - /// implicit truncation rule. - /// Zero is also a valid value indicating that the transfer shall be accepted but the payload need not be stored. - size_t payload_size_max; -} CanardRxMetadata; - -/// The application shall implement this function and supply a pointer to it to the library during initialization. -/// The library calls this function to determine whether a transfer should be received. -/// @param ins Library instance. -/// @param port_id Subject-ID or service-ID of the transfer. -/// @param transfer_kind Message or service transfer. -/// @param source_node_id Node-ID of the origin; CANARD_NODE_ID_UNSET if anonymous. -/// @returns CanardTransferReceptionParameters. -typedef CanardRxMetadata (*CanardRxFilter)(const CanardInstance* ins, - uint16_t port_id, - CanardTransferKind transfer_kind, - uint8_t source_node_id); - typedef void* (*CanardHeapAllocate)(CanardInstance* ins, size_t amount); /// Free as in freedom. @@ -174,25 +156,29 @@ struct CanardInstance /// The node-ID of the local node. The default value is CANARD_NODE_ID_UNSET. /// Per the UAVCAN Specification, the node-ID should not be assigned more than once. /// Invalid values are treated as CANARD_NODE_ID_UNSET. - uint8_t node_id; + CanardNodeID node_id; /// Callbacks invoked by the library. See their type documentation for details. /// They SHALL be valid function pointers at all times. /// /// The time complexity parameters given in the API documentation are made on the assumption that - /// the heap management functions (allocate and free) have constant complexity. + /// the heap management functions (allocate and free) have constant time complexity O(1). /// - /// There are only two API functions that may lead to allocation of heap memory: + /// There are only three API functions that may lead to allocation of heap memory: /// - canardTxPush() /// - canardRxAccept() + /// - canardRxSubscribe() /// Their exact memory requirement model is specified in their documentation. + /// + /// By design, the library does not require the application to engage in manual memory management. + /// All pointers to heap memory are managed entirely by the library itself, thus eliminating the risk of + /// memory leaks in the application. CanardHeapAllocate heap_allocate; CanardHeapFree heap_free; - CanardRxFilter rx_filter; /// These fields are for internal use only. Do not access from the application. - struct CanardInternalRxSession* _rx_sessions; - struct CanardInternalTxQueueItem* _tx_queue; + struct CanardInternalRxSubscription* _rx_subscriptions[CANARD_NUM_TRANSFER_KINDS]; + struct CanardInternalTxQueueItem* _tx_queue; }; /// Initialize a new library instance. @@ -200,9 +186,11 @@ struct CanardInstance /// The time complexity parameters given in the API documentation are made on the assumption that the heap management /// functions (allocate and free) have constant complexity. /// If any of the pointers are NULL, the behavior is undefined. -CanardInstance canardInit(const CanardHeapAllocate heap_allocate, - const CanardHeapFree heap_free, - const CanardRxFilter rx_filter); +/// +/// The instance does not hold any resources itself except the heap memory. If the instance should be de-initialized, +/// the application shall clear the TX queue by calling the pop function repeatedly, and remove all RX subscriptions. +/// Once that is done, the instance will be holding no memory resources, so it can be discarded freely. +CanardInstance canardInit(const CanardHeapAllocate heap_allocate, const CanardHeapFree heap_free); /// Serializes a transfer into a sequence of transport frames, and inserts them into the prioritized transmission /// queue at the appropriate position. Afterwards, the application is supposed to take the enqueued frames from @@ -312,24 +300,29 @@ void canardTxPop(CanardInstance* const ins); /// - The payload pointer of the input frame is NULL while its size is non-zero. /// - The CAN ID of the input frame is not less than 2**29=0x20000000. /// -/// RX filter callback behavior? -/// /// The function returns zero if any of the following conditions are true; the general policy is that protocol errors /// are not escalated because they do not construe a node-local error: /// - The received frame is not a valid UAVCAN/CAN transport frame; in this case the frame is silently ignored. /// - The received frame is a valid UAVCAN/CAN transport frame, but it belongs to a session that is not /// relevant to the application (i.e., the application reported via the RX filter callback that the library /// need not receive transfers from this session). -/// - The received frame is a valid UAVCAN/CAN transport frame, but it did not complete a transfer, or it belongs -/// to an invalid frame sequence. +/// - The received frame is a valid UAVCAN/CAN transport frame, but it did not complete a transfer, or it forms +/// an invalid frame sequence. /// /// The MTU of the accepted frame is not limited and is not dependent on the MTU setting of the local node; /// that is, any MTU is accepted. /// +/// Free the payload buffer? Keep it allocated forever, do not require the application to clean anything. +/// This will also relieve us from allocating new storage for single-frame transfers. +/// /// Any value of iface_index is accepted; that is, up to 256 redundant transports are supported. /// The interface from which the transfer is accepted is always the same as iface_index. /// -/// +/// The time complexity is O(n) where n is the number of subject-IDs or service-IDs subscribed to by the application, +/// depending on whether the frame is of the message kind of of the service kind. +/// Observe that the time complexity is invariant to the network configuration (such as the number of online nodes), +/// which is an important design guarantee for real-time applications. +/// Frames that are not valid UAVCAN/CAN frames are discarded in constant time. /// /// A frame that initiates a new transfer may require up to two heap allocations: one of size /// sizeof(CanardInternalRxSession) (which never exceeds 64 bytes on any conventional platform), @@ -341,6 +334,36 @@ int8_t canardRxAccept(CanardInstance* const ins, const uint8_t iface_index, CanardTransfer* const out_transfer); +/// The library allocates large look-up tables to ensure that the temporal properties of its algorithms are +/// invariant to the network configuration (i.e., a node that is validated on a network containing one other node +/// will provably perform identically on a network that contains X nodes). +/// See for context: https://github.com/UAVCAN/libuavcan/issues/185#issuecomment-440354858. +/// This is a conscious time-memory trade-off. It may have adverse effects on RAM-constrained applications, +/// but this is considered tolerable because it is expected that the types of applications leveraging Libcanard +/// will be either real-time function nodes where time determinism is critical, or bootloaders where time determinism +/// is usually not required but the amount of available memory is not an issue (the main constraint is ROM, not RAM). +/// +/// HEAP MEMORY REQUIREMENT MODEL. +/// RETURN VALUES. +/// +/// If such subscription already exists, it will be removed first as if canardRxUnsubscribe() was +/// invoked by the application, and then re-created anew with the new parameters. +/// +/// The time complexity is linear from the number of current subscriptions under the specified transfer kind. +int8_t canardRxSubscribe(CanardInstance* const ins, + const CanardTransferKind transfer_kind, + const CanardPortID port_id, + const size_t payload_size_bytes_max, + const CanardMicrosecond transfer_id_timeout_usec); + +/// Reverse the effect of canardRxSubscribe(). +/// If any of the arguments are invalid or if such subscription does not exist, the function has no effect. +/// If the subscription is found, all its heap memory is de-allocated; to determine the amount of memory freed, +/// please refer to the heap memory requirement models of canardRxSubscribe() and canardRxAccept(). +/// +/// The time complexity is linear from the number of current subscriptions under the specified transfer kind. +void canardRxUnsubscribe(CanardInstance* const ins, const CanardTransferKind transfer_kind, const CanardPortID port_id); + #ifdef __cplusplus } #endif diff --git a/tests/internals.hpp b/tests/internals.hpp index 818c7b8a..0924322f 100644 --- a/tests/internals.hpp +++ b/tests/internals.hpp @@ -16,9 +16,9 @@ struct TxQueueItem final { TxQueueItem* next = nullptr; - std::uint32_t id = 0; std::uint64_t deadline_usec = 0; std::size_t payload_size = 0; + std::uint32_t id = 0; std::array payload{}; // The real definition has a flex array here. From 9e109f8da2e533241638baf91575252fd053448d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 15 Feb 2020 22:54:13 +0200 Subject: [PATCH 036/100] RX pipeline refactoring continues; the new heap memory model requires careful rechecking in the morning --- libcanard/canard.c | 213 +++++++++++++++++++++++++++++---------- libcanard/canard.h | 35 +++++-- tests/helpers.hpp | 37 +------ tests/test_internals.cpp | 6 +- 4 files changed, 187 insertions(+), 104 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 8cc8b521..42ea62c1 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -10,7 +10,8 @@ /// By default, this macro resolves to the standard assert(). The user can redefine this if necessary. /// To disable assertion checks completely, make it expand into `(void)(0)`. #ifndef CANARD_ASSERT -# define CANARD_ASSERT assert +// Intentional violation of MISRA: assertion macro cannot be replaced with a function definition. +# define CANARD_ASSERT(x) assert(x) // NOSONAR #endif /// This macro is needed only for testing and for library development. Do not redefine this in production. @@ -619,20 +620,119 @@ CANARD_INTERNAL bool tryParseFrame(const CanardFrame* const frame, FrameModel* c return valid; } -CANARD_INTERNAL void initRxTransferFromFrame(const FrameModel* const model, CanardTransfer* const out_transfer); -CANARD_INTERNAL void initRxTransferFromFrame(const FrameModel* const model, CanardTransfer* const out_transfer) +CANARD_INTERNAL void initRxTransferFromFrame(const FrameModel* const frame, CanardTransfer* const out_transfer); +CANARD_INTERNAL void initRxTransferFromFrame(const FrameModel* const frame, CanardTransfer* const out_transfer) { - CANARD_ASSERT(model != NULL); - CANARD_ASSERT(model->payload != NULL); + CANARD_ASSERT(frame != NULL); + CANARD_ASSERT(frame->payload != NULL); CANARD_ASSERT(out_transfer != NULL); - out_transfer->timestamp_usec = model->timestamp_usec; - out_transfer->priority = model->priority; - out_transfer->transfer_kind = model->transfer_kind; - out_transfer->port_id = model->port_id; - out_transfer->remote_node_id = model->source_node_id; - out_transfer->transfer_id = model->transfer_id; - out_transfer->payload_size = model->payload_size; - out_transfer->payload = model->payload; + out_transfer->timestamp_usec = frame->timestamp_usec; + out_transfer->priority = frame->priority; + out_transfer->transfer_kind = frame->transfer_kind; + out_transfer->port_id = frame->port_id; + out_transfer->remote_node_id = frame->source_node_id; + out_transfer->transfer_id = frame->transfer_id; + out_transfer->payload_size = frame->payload_size; + out_transfer->payload = frame->payload; +} + +CANARD_INTERNAL int8_t updateRxSession(RxSession* const rxs, + const FrameModel* const frame, + const uint8_t iface_index, + CanardTransfer* const out_transfer, + const CanardMicrosecond transfer_id_timeout_usec, + const size_t payload_size_bytes_max); +CANARD_INTERNAL int8_t updateRxSession(RxSession* const rxs, + const FrameModel* const frame, + const uint8_t iface_index, + CanardTransfer* const out_transfer, + const CanardMicrosecond transfer_id_timeout_usec, + const size_t payload_size_bytes_max) +{ + CANARD_ASSERT(rxs != NULL); + CANARD_ASSERT(frame != NULL); + CANARD_ASSERT(frame->payload != NULL); + CANARD_ASSERT(out_transfer != NULL); + + int8_t out = 0; + + (void) iface_index; + (void) transfer_id_timeout_usec; + (void) payload_size_bytes_max; + + return out; +} + +CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, + const FrameModel* const frame, + const uint8_t iface_index, + CanardTransfer* const out_transfer); +CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, + const FrameModel* const frame, + const uint8_t iface_index, + CanardTransfer* const out_transfer) +{ + CANARD_ASSERT(ins != NULL); + CANARD_ASSERT(frame != NULL); + CANARD_ASSERT(frame->payload != NULL); + CANARD_ASSERT((CANARD_NODE_ID_UNSET == frame->destination_node_id) || (ins->node_id == frame->destination_node_id)); + CANARD_ASSERT(out_transfer != NULL); + + // Find subscription. This is the reason the function has a linear time complexity from the number of subscriptions. + CanardInternalRxSubscription* sub = ins->_rx_subscriptions[(size_t) frame->transfer_kind]; + while ((sub != NULL) && (sub->port_id != frame->port_id)) + { + sub = sub->next; + } + + // If the subscription is not found, that means that the application doesn't want this transfer. Ignore the frame. + int8_t out = 0; + if (sub != NULL) + { + CANARD_ASSERT(sub->port_id == frame->port_id); + if (frame->source_node_id <= CANARD_NODE_ID_MAX) + { + // If such session does not exist, create it. This only makes sense if this is the first frame of a + // transfer, otherwise, we won't be able to receive the transfer anyway so we don't bother. + if ((NULL == sub->sessions[frame->source_node_id]) && frame->start_of_transfer) + { + RxSession* const rxs = + (RxSession*) ins->heap_allocate(ins, sizeof(RxSession) + sub->payload_size_bytes_max); + sub->sessions[frame->source_node_id] = rxs; + if (rxs != NULL) + { + rxs->transfer_timestamp_usec = frame->timestamp_usec; + rxs->payload_size = 0U; + rxs->calculated_crc = CRC_INITIAL; + rxs->toggle_and_transfer_id = TAIL_TOGGLE; + } + else + { + out = -CANARD_ERROR_OUT_OF_MEMORY; + } + } + // There are two possible reasons why the session may not exist: 1. OOM; 2. SOT-miss. + if (sub->sessions[frame->source_node_id] != NULL) + { + CANARD_ASSERT(out == 0); + out = updateRxSession(sub->sessions[frame->source_node_id], + frame, + iface_index, + out_transfer, + sub->transfer_id_timeout_usec, + sub->payload_size_bytes_max); + } + } + else + { + CANARD_ASSERT(frame->source_node_id == CANARD_NODE_ID_UNSET); + // Anonymous transfers are stateless. No need to update the state machine, just blindly accept it. + initRxTransferFromFrame(frame, out_transfer); + out = 1; + } + } + CANARD_ASSERT(out <= 1); + return out; } // ---------------------------------------- PUBLIC API ---------------------------------------- @@ -746,23 +846,18 @@ int8_t canardRxAccept(CanardInstance* const ins, FrameModel model = {0}; if (tryParseFrame(frame, &model)) { - if (true) // TODO stub + if ((CANARD_NODE_ID_UNSET == model.destination_node_id) || (ins->node_id == model.destination_node_id)) { - if (model.source_node_id == CANARD_NODE_ID_UNSET) - { - initRxTransferFromFrame(&model, out_transfer); - out = 1; - } - else - { - (void) iface_index; - out = -1; // TODO stub - } + out = acceptFrame(ins, &model, iface_index, out_transfer); + } + else + { + out = 0; // Mis-addressed frame is obviously not an error. } } else { - out = 0; // Bad input frame is obviously not an error. + out = 0; // A non-UAVCAN/CAN input frame is obviously not an error. } } return out; @@ -774,46 +869,49 @@ int8_t canardRxSubscribe(CanardInstance* const ins, const size_t payload_size_bytes_max, const CanardMicrosecond transfer_id_timeout_usec) { - const uint8_t transfer_kind_index = (uint8_t) transfer_kind; - int8_t out = -CANARD_ERROR_INVALID_ARGUMENT; - if ((ins != NULL) && (transfer_kind_index < CANARD_NUM_TRANSFER_KINDS)) + int8_t out = -CANARD_ERROR_INVALID_ARGUMENT; + const size_t tk = (size_t) transfer_kind; + if ((ins != NULL) && (tk < CANARD_NUM_TRANSFER_KINDS)) { - canardRxUnsubscribe(ins, transfer_kind, port_id); // Reset to the initial state. - - CanardInternalRxSubscription* const subs = - (CanardInternalRxSubscription*) ins->heap_allocate(ins, sizeof(CanardInternalRxSubscription)); - if (subs != NULL) + out = canardRxUnsubscribe(ins, transfer_kind, port_id); // Reset to the initial state. + if (out >= 0) { - for (size_t i = 0; i < SESSIONS_PER_SUBSCRIPTION; i++) + CanardInternalRxSubscription* const subs = + (CanardInternalRxSubscription*) ins->heap_allocate(ins, sizeof(CanardInternalRxSubscription)); + if (subs != NULL) { - // The sessions will be created ad-hoc. Normally, for a low-jitter deterministic system, - // we could have pre-allocated sessions here, but that requires too much memory to be feasible. - subs->sessions[i] = NULL; + for (size_t i = 0; i < SESSIONS_PER_SUBSCRIPTION; i++) + { + // The sessions will be created ad-hoc. Normally, for a low-jitter deterministic system, + // we could have pre-allocated sessions here, but that requires too much memory to be feasible. + subs->sessions[i] = NULL; + } + subs->transfer_id_timeout_usec = transfer_id_timeout_usec; + subs->payload_size_bytes_max = payload_size_bytes_max; + subs->port_id = port_id; + subs->next = ins->_rx_subscriptions[tk]; + ins->_rx_subscriptions[tk] = subs; + out = (out > 0) ? 0 : 1; + } + else + { + out = -CANARD_ERROR_OUT_OF_MEMORY; } - subs->transfer_id_timeout_usec = transfer_id_timeout_usec; - subs->payload_size_bytes_max = payload_size_bytes_max; - subs->port_id = port_id; - subs->next = ins->_rx_subscriptions[transfer_kind_index]; - - ins->_rx_subscriptions[transfer_kind_index] = subs; - - out = 0; - } - else - { - out = -CANARD_ERROR_OUT_OF_MEMORY; } } return out; } -void canardRxUnsubscribe(CanardInstance* const ins, const CanardTransferKind transfer_kind, const CanardPortID port_id) +int8_t canardRxUnsubscribe(CanardInstance* const ins, + const CanardTransferKind transfer_kind, + const CanardPortID port_id) { - const uint8_t transfer_kind_index = (uint8_t) transfer_kind; - if ((ins != NULL) && (transfer_kind_index < CANARD_NUM_TRANSFER_KINDS)) + int8_t out = -CANARD_ERROR_INVALID_ARGUMENT; + const size_t tk = (size_t) transfer_kind; + if ((ins != NULL) && (tk < CANARD_NUM_TRANSFER_KINDS)) { CanardInternalRxSubscription* prev = NULL; - CanardInternalRxSubscription* subs = ins->_rx_subscriptions[transfer_kind_index]; + CanardInternalRxSubscription* subs = ins->_rx_subscriptions[tk]; while ((subs != NULL) && (subs->port_id != port_id)) { prev = subs; @@ -823,13 +921,15 @@ void canardRxUnsubscribe(CanardInstance* const ins, const CanardTransferKind tra if (subs != NULL) { CANARD_ASSERT(subs->port_id == port_id); + out = 1; + if (prev != NULL) { prev->next = subs->next; } else { - ins->_rx_subscriptions[transfer_kind_index] = subs->next; + ins->_rx_subscriptions[tk] = subs->next; } for (size_t i = 0; i < SESSIONS_PER_SUBSCRIPTION; i++) @@ -839,5 +939,10 @@ void canardRxUnsubscribe(CanardInstance* const ins, const CanardTransferKind tra } ins->heap_free(ins, subs); } + else + { + out = 0; + } } + return out; } diff --git a/libcanard/canard.h b/libcanard/canard.h index c42763be..33ff540e 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -322,13 +322,10 @@ void canardTxPop(CanardInstance* const ins); /// depending on whether the frame is of the message kind of of the service kind. /// Observe that the time complexity is invariant to the network configuration (such as the number of online nodes), /// which is an important design guarantee for real-time applications. +/// Unicast frames where the destination does not equal the local node-ID are discarded in constant time. /// Frames that are not valid UAVCAN/CAN frames are discarded in constant time. /// -/// A frame that initiates a new transfer may require up to two heap allocations: one of size -/// sizeof(CanardInternalRxSession) (which never exceeds 64 bytes on any conventional platform), -/// and the other of size CanardRxMetadata.payload_size_max, as returned by the application. -/// The first allocation will not take place if a transfer under this session was seen earlier (i.e., the state -/// already exists). +/// HEAP MEMORY REQUIREMENT MODEL. int8_t canardRxAccept(CanardInstance* const ins, const CanardFrame* const frame, const uint8_t iface_index, @@ -343,12 +340,24 @@ int8_t canardRxAccept(CanardInstance* const ins, /// will be either real-time function nodes where time determinism is critical, or bootloaders where time determinism /// is usually not required but the amount of available memory is not an issue (the main constraint is ROM, not RAM). /// -/// HEAP MEMORY REQUIREMENT MODEL. -/// RETURN VALUES. -/// /// If such subscription already exists, it will be removed first as if canardRxUnsubscribe() was /// invoked by the application, and then re-created anew with the new parameters. /// +/// Once a new RX session is allocated, it will never be removed as long as the subscription is active. +/// The rationale for this behavior is that real-time networks typically do not change their configuration at runtime; +/// hence, it is possible to reduce the worst-case computational complexity of the library routines by never +/// deallocating sessions once allocated. If this behavior is found to be undesirable, the application can force +/// deallocation of all unused states by re-creating the subscription anew. +/// +/// HEAP MEMORY REQUIREMENT MODEL. +/// +/// The return value is 1 if a new subscription has been created as requested. +/// The return value is 0 if such subscription existed at the time the function was invoked. In this case, +/// the existing subscription is terminated and then a new one is created in its place. Pending transfers may be lost. +/// The return value is negative in case of an error: a negated invalid argument error code if any of the arguments are +/// invalid, or the negated out-of-memory error if the new subscription could not be allocated due to the heap memory +/// being exhausted. +/// /// The time complexity is linear from the number of current subscriptions under the specified transfer kind. int8_t canardRxSubscribe(CanardInstance* const ins, const CanardTransferKind transfer_kind, @@ -357,12 +366,18 @@ int8_t canardRxSubscribe(CanardInstance* const ins, const CanardMicrosecond transfer_id_timeout_usec); /// Reverse the effect of canardRxSubscribe(). -/// If any of the arguments are invalid or if such subscription does not exist, the function has no effect. /// If the subscription is found, all its heap memory is de-allocated; to determine the amount of memory freed, /// please refer to the heap memory requirement models of canardRxSubscribe() and canardRxAccept(). +/// This function does not allocate new heap memory. +/// +/// The return value is 1 if such subscription existed (and, therefore, it was removed). +/// The return value is 0 if such subscription does not exist. In this case, the function has no effect. +/// The return value is a negated invalid argument error if any of the input arguments are invalid. /// /// The time complexity is linear from the number of current subscriptions under the specified transfer kind. -void canardRxUnsubscribe(CanardInstance* const ins, const CanardTransferKind transfer_kind, const CanardPortID port_id); +int8_t canardRxUnsubscribe(CanardInstance* const ins, + const CanardTransferKind transfer_kind, + const CanardPortID port_id); #ifdef __cplusplus } diff --git a/tests/helpers.hpp b/tests/helpers.hpp index 79d964d7..3fda8bd3 100644 --- a/tests/helpers.hpp +++ b/tests/helpers.hpp @@ -29,18 +29,6 @@ inline void free(CanardInstance* const ins, void* const pointer) } } // namespace dummy_allocator -inline auto rejectAllRxFilter(const CanardInstance* const ins, - const std::uint16_t port_id, - const CanardTransferKind transfer_kind, - const std::uint8_t source_node_id) -> CanardRxMetadata -{ - (void) ins; - (void) port_id; - (void) transfer_kind; - (void) source_node_id; - return CanardRxMetadata{}; -} - /// An allocator that sits on top of the standard malloc() providing additional testing capabilities. /// It allows the user to specify the maximum amount of memory that can be allocated; further requests will emulate OOM. class TestAllocator @@ -123,9 +111,7 @@ class TestAllocator /// An enhancing wrapper over the library to remove boilerplate from the tests. class Instance { - CanardInstance canard_ = canardInit(&Instance::trampolineAllocate, // - &Instance::trampolineDeallocate, - &Instance::trampolineApplyRxFilter); + CanardInstance canard_ = canardInit(&Instance::trampolineAllocate, &Instance::trampolineDeallocate); TestAllocator allocator_; static auto trampolineAllocate(CanardInstance* const ins, const std::size_t amount) -> void* @@ -140,27 +126,6 @@ class Instance p->allocator_.deallocate(pointer); } - static auto trampolineApplyRxFilter(const CanardInstance* const ins, - const std::uint16_t port_id, - const CanardTransferKind transfer_kind, - const std::uint8_t source_node_id) -> CanardRxMetadata - { - auto p = reinterpret_cast(ins->user_reference); - return p->applyRxFilter(port_id, transfer_kind, source_node_id); - } - -protected: - /// The default behavior is to discard everything. Override to change. - [[nodiscard]] virtual auto applyRxFilter(const std::uint16_t port_id, - const CanardTransferKind transfer_kind, - const std::uint8_t source_node_id) const -> CanardRxMetadata - { - (void) port_id; - (void) transfer_kind; - (void) source_node_id; - return {}; - } - public: Instance() { canard_.user_reference = this; } diff --git a/tests/test_internals.cpp b/tests/test_internals.cpp index 7647907d..824479a0 100644 --- a/tests/test_internals.cpp +++ b/tests/test_internals.cpp @@ -29,8 +29,7 @@ TEST_CASE("SessionSpecifier") TEST_CASE("getPresentationLayerMTU") { - auto ins = - canardInit(&helpers::dummy_allocator::allocate, &helpers::dummy_allocator::free, &helpers::rejectAllRxFilter); + auto ins = canardInit(&helpers::dummy_allocator::allocate, &helpers::dummy_allocator::free); REQUIRE(63 == internals::getPresentationLayerMTU(&ins)); // This is the default. ins.mtu_bytes = 0; REQUIRE(7 == internals::getPresentationLayerMTU(&ins)); @@ -189,8 +188,7 @@ TEST_CASE("findTxQueueSupremum") using internals::findTxQueueSupremum; using TxQueueItem = internals::TxQueueItem; - auto ins = - canardInit(&helpers::dummy_allocator::allocate, &helpers::dummy_allocator::free, &helpers::rejectAllRxFilter); + auto ins = canardInit(&helpers::dummy_allocator::allocate, &helpers::dummy_allocator::free); const auto find = [&](std::uint32_t x) -> TxQueueItem* { return findTxQueueSupremum(&ins, x); }; From a987ced05674336ff36f97aac25893c0631b7a2f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 15 Feb 2020 23:11:37 +0200 Subject: [PATCH 037/100] Minor clarifications in the comments & static analysis fixes --- libcanard/canard.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 42ea62c1..049bfc26 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -136,7 +136,7 @@ typedef struct CanardInternalTxQueueItem // Intentional violation of MISRA: this flex array is the lesser of three evils. The other two are: // - Use pointer, make it point to the remainder of the allocated memory following this structure. // The pointer is bad because it requires us to use pointer arithmetics and adds sizeof(void*) of waste per item. - // - Use a separate memory allocation for data. This is terribly wasteful. + // - Use a separate memory allocation for data. This is terribly wasteful (both time & memory). uint8_t payload[]; // NOSONAR } CanardInternalTxQueueItem; @@ -265,7 +265,7 @@ CANARD_INTERNAL CanardInternalTxQueueItem* allocateTxQueueItem(CanardInstance* c const size_t payload_size) { CANARD_ASSERT(ins != NULL); - CANARD_ASSERT(payload_size > 0U); // UAVCAN/CAN doesn't allow zero-payload frames. + CANARD_ASSERT(payload_size > 0U); CanardInternalTxQueueItem* const out = (CanardInternalTxQueueItem*) ins->heap_allocate(ins, sizeof(CanardInternalTxQueueItem) + payload_size); if (out != NULL) @@ -292,7 +292,7 @@ CANARD_INTERNAL CanardInternalTxQueueItem* findTxQueueSupremum(const CanardInsta } else { - // The linear search should be replaced with O(log n) at least. Please help us here. + // TODO The linear search should be replaced with O(log n) at least. Please help us here. while ((out != NULL) && (out->next != NULL) && (out->next->id <= can_id)) { out = out->next; @@ -520,7 +520,7 @@ typedef struct // Intentional violation of MISRA: this flex array is the lesser of three evils. The other two are: // - Use pointer, make it point to the remainder of the allocated memory following this structure. // The pointer is bad because it requires us to use pointer arithmetics and adds sizeof(void*) of waste per item. - // - Use a separate memory allocation for data. This is terribly wasteful. + // - Use a separate memory allocation for data. This is terribly wasteful (both time & memory). uint8_t payload[]; // NOSONAR } RxSession; @@ -613,7 +613,7 @@ CANARD_INTERNAL bool tryParseFrame(const CanardFrame* const frame, FrameModel* c // Final validation. valid = valid && (out_result->start_of_transfer ? out_result->toggle : true); // Protocol version check. - valid = valid && ((out_result->source_node_id == CANARD_NODE_ID_UNSET) + valid = valid && ((CANARD_NODE_ID_UNSET == out_result->source_node_id) ? (out_result->start_of_transfer && out_result->end_of_transfer) // Single-frame. : true); } @@ -841,7 +841,7 @@ int8_t canardRxAccept(CanardInstance* const ins, { int8_t out = -CANARD_ERROR_INVALID_ARGUMENT; if ((ins != NULL) && (out_transfer != NULL) && (frame != NULL) && (frame->extended_can_id <= CAN_EXT_ID_MASK) && - ((frame->payload != NULL) || (frame->payload_size == 0))) + ((frame->payload != NULL) || (0 == frame->payload_size))) { FrameModel model = {0}; if (tryParseFrame(frame, &model)) From f83025c9b2bb6bf7b55d6c2311f331a5fef4d5f2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 16 Feb 2020 20:19:15 +0200 Subject: [PATCH 038/100] Zero-copy reception --- libcanard/canard.c | 165 +++++++++++++++++----------------------- libcanard/canard.h | 109 +++++++++++++++----------- libcanard/canard_dsdl.c | 2 +- 3 files changed, 135 insertions(+), 141 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 049bfc26..290d27a6 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -267,7 +267,7 @@ CANARD_INTERNAL CanardInternalTxQueueItem* allocateTxQueueItem(CanardInstance* c CANARD_ASSERT(ins != NULL); CANARD_ASSERT(payload_size > 0U); CanardInternalTxQueueItem* const out = - (CanardInternalTxQueueItem*) ins->heap_allocate(ins, sizeof(CanardInternalTxQueueItem) + payload_size); + (CanardInternalTxQueueItem*) ins->memory_allocate(ins, sizeof(CanardInternalTxQueueItem) + payload_size); if (out != NULL) { out->next = NULL; @@ -492,7 +492,7 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, while (head != NULL) { CanardInternalTxQueueItem* const next = head->next; - ins->heap_free(ins, head); + ins->memory_free(ins, head); head = next; } } @@ -504,43 +504,22 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, // ---------------------------------------- RECEPTION ---------------------------------------- /// The memory requirement model provided in the documentation assumes that the maximum size of this structure never -/// exceeds 24 bytes on any conventional platform. The sizeof() of this structure, per the C standard, assumes that -/// the length of the flex array member is zero. +/// exceeds 32 bytes on any conventional platform. /// A user that needs a detailed analysis of the worst-case memory consumption may compute the size of this structure /// for the particular platform at hand manually or by evaluating its sizeof(). /// The fields are ordered to minimize the amount of padding on all conventional platforms. -typedef struct +typedef struct CanardInternalRxSession { CanardMicrosecond transfer_timestamp_usec; ///< Timestamp of the last received start-of-transfer. size_t payload_size; ///< How many bytes received so far. + uint8_t* payload; ///< Dynamically allocated and handed off to the application when done. TransferCRC calculated_crc; ///< Updated with the received payload in real time. CanardTransferID toggle_and_transfer_id; ///< Toggle and transfer-ID combined into one field to reduce footprint. uint8_t iface_index; ///< Arbitrary value in [0, 255]. - - // Intentional violation of MISRA: this flex array is the lesser of three evils. The other two are: - // - Use pointer, make it point to the remainder of the allocated memory following this structure. - // The pointer is bad because it requires us to use pointer arithmetics and adds sizeof(void*) of waste per item. - // - Use a separate memory allocation for data. This is terribly wasteful (both time & memory). - uint8_t payload[]; // NOSONAR -} RxSession; +} CanardInternalRxSession; #define SESSIONS_PER_SUBSCRIPTION (CANARD_NODE_ID_MAX + 1U) -/// The memory requirement model provided in the documentation assumes that the maximum size of this structure never -/// exceeds (129*sizeof(void*) + 24 bytes) on any conventional platform. -/// A user that needs a detailed analysis of the worst-case memory consumption may compute the size of this structure -/// for the particular platform at hand manually or by evaluating its sizeof(). -/// The fields are ordered to minimize the amount of padding on all conventional platforms. -typedef struct CanardInternalRxSubscription -{ - struct CanardInternalRxSubscription* next; - - RxSession* sessions[SESSIONS_PER_SUBSCRIPTION]; - CanardMicrosecond transfer_id_timeout_usec; - size_t payload_size_bytes_max; ///< Payload past this limit may be discarded by the library. - CanardPortID port_id; -} CanardInternalRxSubscription; - /// High-level transport frame model. typedef struct { @@ -636,18 +615,18 @@ CANARD_INTERNAL void initRxTransferFromFrame(const FrameModel* const frame, Cana out_transfer->payload = frame->payload; } -CANARD_INTERNAL int8_t updateRxSession(RxSession* const rxs, - const FrameModel* const frame, - const uint8_t iface_index, - CanardTransfer* const out_transfer, - const CanardMicrosecond transfer_id_timeout_usec, - const size_t payload_size_bytes_max); -CANARD_INTERNAL int8_t updateRxSession(RxSession* const rxs, - const FrameModel* const frame, - const uint8_t iface_index, - CanardTransfer* const out_transfer, - const CanardMicrosecond transfer_id_timeout_usec, - const size_t payload_size_bytes_max) +CANARD_INTERNAL int8_t updateRxSession(CanardInternalRxSession* const rxs, + const FrameModel* const frame, + const uint8_t iface_index, + CanardTransfer* const out_transfer, + const CanardMicrosecond transfer_id_timeout_usec, + const size_t payload_size_bytes_max); +CANARD_INTERNAL int8_t updateRxSession(CanardInternalRxSession* const rxs, + const FrameModel* const frame, + const uint8_t iface_index, + CanardTransfer* const out_transfer, + const CanardMicrosecond transfer_id_timeout_usec, + const size_t payload_size_bytes_max) { CANARD_ASSERT(rxs != NULL); CANARD_ASSERT(frame != NULL); @@ -679,32 +658,36 @@ CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, CANARD_ASSERT(out_transfer != NULL); // Find subscription. This is the reason the function has a linear time complexity from the number of subscriptions. - CanardInternalRxSubscription* sub = ins->_rx_subscriptions[(size_t) frame->transfer_kind]; - while ((sub != NULL) && (sub->port_id != frame->port_id)) + CanardRxSubscription* sub = ins->_rx_subscriptions[(size_t) frame->transfer_kind]; + while ((sub != NULL) && (sub->_port_id != frame->port_id)) { - sub = sub->next; + sub = sub->_next; } // If the subscription is not found, that means that the application doesn't want this transfer. Ignore the frame. int8_t out = 0; if (sub != NULL) { - CANARD_ASSERT(sub->port_id == frame->port_id); + CANARD_ASSERT(sub->_port_id == frame->port_id); if (frame->source_node_id <= CANARD_NODE_ID_MAX) { // If such session does not exist, create it. This only makes sense if this is the first frame of a // transfer, otherwise, we won't be able to receive the transfer anyway so we don't bother. - if ((NULL == sub->sessions[frame->source_node_id]) && frame->start_of_transfer) + if ((NULL == sub->_sessions[frame->source_node_id]) && frame->start_of_transfer) { - RxSession* const rxs = - (RxSession*) ins->heap_allocate(ins, sizeof(RxSession) + sub->payload_size_bytes_max); - sub->sessions[frame->source_node_id] = rxs; + CanardInternalRxSession* const rxs = + (CanardInternalRxSession*) ins->memory_allocate(ins, + sizeof(CanardInternalRxSession) + + sub->_payload_size_bytes_max); + sub->_sessions[frame->source_node_id] = rxs; if (rxs != NULL) { rxs->transfer_timestamp_usec = frame->timestamp_usec; rxs->payload_size = 0U; + rxs->payload = NULL; rxs->calculated_crc = CRC_INITIAL; rxs->toggle_and_transfer_id = TAIL_TOGGLE; + rxs->iface_index = 0U; } else { @@ -712,15 +695,15 @@ CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, } } // There are two possible reasons why the session may not exist: 1. OOM; 2. SOT-miss. - if (sub->sessions[frame->source_node_id] != NULL) + if (sub->_sessions[frame->source_node_id] != NULL) { CANARD_ASSERT(out == 0); - out = updateRxSession(sub->sessions[frame->source_node_id], + out = updateRxSession(sub->_sessions[frame->source_node_id], frame, iface_index, out_transfer, - sub->transfer_id_timeout_usec, - sub->payload_size_bytes_max); + sub->_transfer_id_timeout_usec, + sub->_payload_size_bytes_max); } } else @@ -749,16 +732,16 @@ const uint8_t CanardCANLengthToDLC[65] = { 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, // 49-64 }; -CanardInstance canardInit(const CanardHeapAllocate heap_allocate, const CanardHeapFree heap_free) +CanardInstance canardInit(const CanardMemoryAllocate memory_allocate, const CanardMemoryFree memory_free) { - CANARD_ASSERT(heap_allocate != NULL); - CANARD_ASSERT(heap_free != NULL); + CANARD_ASSERT(memory_allocate != NULL); + CANARD_ASSERT(memory_free != NULL); const CanardInstance out = { .user_reference = NULL, .mtu_bytes = CANARD_MTU_CAN_FD, .node_id = CANARD_NODE_ID_UNSET, - .heap_allocate = heap_allocate, - .heap_free = heap_free, + .memory_allocate = memory_allocate, + .memory_free = memory_free, ._rx_subscriptions = {NULL}, ._tx_queue = NULL, }; @@ -829,7 +812,7 @@ void canardTxPop(CanardInstance* const ins) if ((ins != NULL) && (ins->_tx_queue != NULL)) { CanardInternalTxQueueItem* const next = ins->_tx_queue->next; - ins->heap_free(ins, ins->_tx_queue); + ins->memory_free(ins, ins->_tx_queue); ins->_tx_queue = next; } } @@ -863,40 +846,32 @@ int8_t canardRxAccept(CanardInstance* const ins, return out; } -int8_t canardRxSubscribe(CanardInstance* const ins, - const CanardTransferKind transfer_kind, - const CanardPortID port_id, - const size_t payload_size_bytes_max, - const CanardMicrosecond transfer_id_timeout_usec) +int8_t canardRxSubscribe(CanardInstance* const ins, + const CanardTransferKind transfer_kind, + const CanardPortID port_id, + const size_t payload_size_bytes_max, + const CanardMicrosecond transfer_id_timeout_usec, + CanardRxSubscription* const out_subscription) { int8_t out = -CANARD_ERROR_INVALID_ARGUMENT; const size_t tk = (size_t) transfer_kind; - if ((ins != NULL) && (tk < CANARD_NUM_TRANSFER_KINDS)) + if ((ins != NULL) && (out_subscription != NULL) && (tk < CANARD_NUM_TRANSFER_KINDS)) { out = canardRxUnsubscribe(ins, transfer_kind, port_id); // Reset to the initial state. if (out >= 0) { - CanardInternalRxSubscription* const subs = - (CanardInternalRxSubscription*) ins->heap_allocate(ins, sizeof(CanardInternalRxSubscription)); - if (subs != NULL) - { - for (size_t i = 0; i < SESSIONS_PER_SUBSCRIPTION; i++) - { - // The sessions will be created ad-hoc. Normally, for a low-jitter deterministic system, - // we could have pre-allocated sessions here, but that requires too much memory to be feasible. - subs->sessions[i] = NULL; - } - subs->transfer_id_timeout_usec = transfer_id_timeout_usec; - subs->payload_size_bytes_max = payload_size_bytes_max; - subs->port_id = port_id; - subs->next = ins->_rx_subscriptions[tk]; - ins->_rx_subscriptions[tk] = subs; - out = (out > 0) ? 0 : 1; - } - else + for (size_t i = 0; i < SESSIONS_PER_SUBSCRIPTION; i++) { - out = -CANARD_ERROR_OUT_OF_MEMORY; + // The sessions will be created ad-hoc. Normally, for a low-jitter deterministic system, + // we could have pre-allocated sessions here, but that requires too much memory to be feasible. + out_subscription->_sessions[i] = NULL; } + out_subscription->_transfer_id_timeout_usec = transfer_id_timeout_usec; + out_subscription->_payload_size_bytes_max = payload_size_bytes_max; + out_subscription->_port_id = port_id; + out_subscription->_next = ins->_rx_subscriptions[tk]; + ins->_rx_subscriptions[tk] = out_subscription; + out = (out > 0) ? 0 : 1; } } return out; @@ -910,34 +885,34 @@ int8_t canardRxUnsubscribe(CanardInstance* const ins, const size_t tk = (size_t) transfer_kind; if ((ins != NULL) && (tk < CANARD_NUM_TRANSFER_KINDS)) { - CanardInternalRxSubscription* prev = NULL; - CanardInternalRxSubscription* subs = ins->_rx_subscriptions[tk]; - while ((subs != NULL) && (subs->port_id != port_id)) + CanardRxSubscription* prv = NULL; + CanardRxSubscription* sub = ins->_rx_subscriptions[tk]; + while ((sub != NULL) && (sub->_port_id != port_id)) { - prev = subs; - subs = subs->next; + prv = sub; + sub = sub->_next; } - if (subs != NULL) + if (sub != NULL) { - CANARD_ASSERT(subs->port_id == port_id); + CANARD_ASSERT(sub->_port_id == port_id); out = 1; - if (prev != NULL) + if (prv != NULL) { - prev->next = subs->next; + prv->_next = sub->_next; } else { - ins->_rx_subscriptions[tk] = subs->next; + ins->_rx_subscriptions[tk] = sub->_next; } for (size_t i = 0; i < SESSIONS_PER_SUBSCRIPTION; i++) { - ins->heap_free(ins, subs->sessions[i]); // The pointer may be NULL but it's acceptable. - subs->sessions[i] = NULL; + ins->memory_free(ins, (sub->_sessions[i] != NULL) ? sub->_sessions[i]->payload : NULL); + ins->memory_free(ins, sub->_sessions[i]); + sub->_sessions[i] = NULL; } - ins->heap_free(ins, subs); } else { diff --git a/libcanard/canard.h b/libcanard/canard.h index 33ff540e..6d2a2dfe 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -27,7 +27,7 @@ extern "C" { /// in the negated form (i.e., code 2 returned as -2). /// API calls whose return type is not a signer integer cannot fail by contract. /// No other error states may occur in the library. -/// By contract, a deterministic application with a properly sized heap will never encounter any of the listed errors. +/// By contract, a deterministic application with a properly sized memory pool will never encounter errors. /// The error code 1 is not used because -1 is often used as a generic error code in 3rd-party code. #define CANARD_ERROR_INVALID_ARGUMENT 2 #define CANARD_ERROR_OUT_OF_MEMORY 3 @@ -128,10 +128,29 @@ typedef struct const void* payload; } CanardTransfer; -typedef void* (*CanardHeapAllocate)(CanardInstance* ins, size_t amount); +/// Transfer subscription state. The application can register its interest in a particular kind of data exchanged +/// over the bus by creating such subscription objects. Frames that carry data for which there is no active +/// subscription will be dropped by the library. +/// +/// WARNING: SUBSCRIPTION INSTANCES SHALL NOT BE COPIED OR MUTATED BY THE APPLICATION. +/// Every field is named starting with an underscore to emphasize that the application shall not mess with it. +/// Unfortunately, C, being such a limited language, does not allow us to construct a better API. +/// +/// The memory footprint of a subscription is large. On a 32-bit platform it slightly exceeds half a KiB. +/// This is what we call a time-memory trade-off: we use a large look-up table to ensure deterministic runtime behavior. +typedef struct CanardRxSubscription +{ + struct CanardRxSubscription* _next; + struct CanardInternalRxSession* _sessions[CANARD_NODE_ID_MAX + 1U]; + CanardMicrosecond _transfer_id_timeout_usec; + size_t _payload_size_bytes_max; + CanardPortID _port_id; +} CanardRxSubscription; + +typedef void* (*CanardMemoryAllocate)(CanardInstance* ins, size_t amount); /// Free as in freedom. -typedef void (*CanardHeapFree)(CanardInstance* ins, void* pointer); +typedef void (*CanardMemoryFree)(CanardInstance* ins, void* pointer); /// This is the core structure that keeps all of the states and allocated resources of the library instance. /// Fields whose names begin with an underscore SHALL NOT be accessed by the application, @@ -162,35 +181,31 @@ struct CanardInstance /// They SHALL be valid function pointers at all times. /// /// The time complexity parameters given in the API documentation are made on the assumption that - /// the heap management functions (allocate and free) have constant time complexity O(1). + /// the memory management functions (allocate and free) have constant time complexity O(1). /// - /// There are only three API functions that may lead to allocation of heap memory: + /// There are only two API functions that may lead to allocation of memory: /// - canardTxPush() /// - canardRxAccept() - /// - canardRxSubscribe() /// Their exact memory requirement model is specified in their documentation. - /// - /// By design, the library does not require the application to engage in manual memory management. - /// All pointers to heap memory are managed entirely by the library itself, thus eliminating the risk of - /// memory leaks in the application. - CanardHeapAllocate heap_allocate; - CanardHeapFree heap_free; + CanardMemoryAllocate memory_allocate; + CanardMemoryFree memory_free; /// These fields are for internal use only. Do not access from the application. - struct CanardInternalRxSubscription* _rx_subscriptions[CANARD_NUM_TRANSFER_KINDS]; - struct CanardInternalTxQueueItem* _tx_queue; + CanardRxSubscription* _rx_subscriptions[CANARD_NUM_TRANSFER_KINDS]; + struct CanardInternalTxQueueItem* _tx_queue; }; /// Initialize a new library instance. /// The default values will be assigned as specified in the structure field documentation. -/// The time complexity parameters given in the API documentation are made on the assumption that the heap management +/// The time complexity parameters given in the API documentation are made on the assumption that the memory management /// functions (allocate and free) have constant complexity. /// If any of the pointers are NULL, the behavior is undefined. /// -/// The instance does not hold any resources itself except the heap memory. If the instance should be de-initialized, -/// the application shall clear the TX queue by calling the pop function repeatedly, and remove all RX subscriptions. -/// Once that is done, the instance will be holding no memory resources, so it can be discarded freely. -CanardInstance canardInit(const CanardHeapAllocate heap_allocate, const CanardHeapFree heap_free); +/// The instance does not hold any resources itself except the allocated memory. +/// If the instance should be de-initialized, the application shall clear the TX queue by calling the pop function +/// repeatedly, and remove all RX subscriptions. Once that is done, the instance will be holding no memory resources, +/// so it can be discarded freely. +CanardInstance canardInit(const CanardMemoryAllocate memory_allocate, const CanardMemoryFree memory_free); /// Serializes a transfer into a sequence of transport frames, and inserts them into the prioritized transmission /// queue at the appropriate position. Afterwards, the application is supposed to take the enqueued frames from @@ -219,7 +234,7 @@ CanardInstance canardInit(const CanardHeapAllocate heap_allocate, const CanardHe /// the counter is incremented by one. /// The recommended approach to storing the transfer-ID counters is to use static or member variables. /// Sophisticated applications may find this approach unsuitable, in which case O(1) static look-up tables -/// or heap-allocated data structures should be considered. +/// or other deterministic data structures should be considered. /// /// Returns the number of frames enqueued into the prioritized TX queue (which is always a positive number) /// in case of success. Returns a negated error code in case of failure. Zero cannot be returned. @@ -237,14 +252,14 @@ CanardInstance canardInit(const CanardHeapAllocate heap_allocate, const CanardHe /// - If the transfer-ID is above the maximum, the excessive bits are silently masked away /// (i.e., the modulo is computed automatically, so the caller doesn't have to bother). /// -/// An out-of-memory error is returned if a TX frame could not be allocated due to the heap being exhausted. +/// An out-of-memory error is returned if a TX frame could not be allocated due to the memory being exhausted. /// In that case, all previously allocated frames will be purged automatically. /// In other words, either all frames of the transfer are enqueued successfully, or none are. /// /// The time complexity is O(s+t), where s is the amount of payload in the transfer, and t is the number of /// frames already enqueued in the transmission queue. /// -/// The heap memory requirement is one allocation per transport frame. +/// The memory allocation requirement is one allocation per transport frame. /// A single-frame transfer takes one allocation; a multi-frame transfer of N frames takes N allocations. /// The maximum size of each allocation is sizeof(CanardInternalTxQueueItem) plus MTU, /// where sizeof(CanardInternalTxQueueItem) is at most 32 bytes on any conventional platform (typically smaller). @@ -309,29 +324,34 @@ void canardTxPop(CanardInstance* const ins); /// - The received frame is a valid UAVCAN/CAN transport frame, but it did not complete a transfer, or it forms /// an invalid frame sequence. /// +/// The function returns 1 (one) if the new frame completed a transfer. In this case, the details of the transfer +/// are stored into out_transfer, and the payload ownership is passed into that object. This means that the application +/// is responsible for deallocating the payload buffer when the processing is done. This design is implemented to +/// facilitate zero-copy data exchange across the protocol stack: once a buffer is allocated, the data is never copied +/// around but only passed by reference. +/// /// The MTU of the accepted frame is not limited and is not dependent on the MTU setting of the local node; /// that is, any MTU is accepted. /// -/// Free the payload buffer? Keep it allocated forever, do not require the application to clean anything. -/// This will also relieve us from allocating new storage for single-frame transfers. -/// /// Any value of iface_index is accepted; that is, up to 256 redundant transports are supported. /// The interface from which the transfer is accepted is always the same as iface_index. /// /// The time complexity is O(n) where n is the number of subject-IDs or service-IDs subscribed to by the application, -/// depending on whether the frame is of the message kind of of the service kind. -/// Observe that the time complexity is invariant to the network configuration (such as the number of online nodes), -/// which is an important design guarantee for real-time applications. +/// depending on whether the frame is of the message kind of of the service kind. Observe that the time complexity +/// is invariant to the network configuration (such as the number of online nodes), which is an important design +/// guarantee for real-time applications. The time complexity is only dependent on the number of active subscriptions +/// for a given transfer kind, which is well-controlled by the local application. +/// /// Unicast frames where the destination does not equal the local node-ID are discarded in constant time. /// Frames that are not valid UAVCAN/CAN frames are discarded in constant time. /// -/// HEAP MEMORY REQUIREMENT MODEL. +/// MEMORY ALLOCATION REQUIREMENT MODEL. int8_t canardRxAccept(CanardInstance* const ins, const CanardFrame* const frame, const uint8_t iface_index, CanardTransfer* const out_transfer); -/// The library allocates large look-up tables to ensure that the temporal properties of its algorithms are +/// Subscription instances have large look-up tables to ensure that the temporal properties of the algorithms are /// invariant to the network configuration (i.e., a node that is validated on a network containing one other node /// will provably perform identically on a network that contains X nodes). /// See for context: https://github.com/UAVCAN/libuavcan/issues/185#issuecomment-440354858. @@ -346,35 +366,34 @@ int8_t canardRxAccept(CanardInstance* const ins, /// Once a new RX session is allocated, it will never be removed as long as the subscription is active. /// The rationale for this behavior is that real-time networks typically do not change their configuration at runtime; /// hence, it is possible to reduce the worst-case computational complexity of the library routines by never -/// deallocating sessions once allocated. If this behavior is found to be undesirable, the application can force -/// deallocation of all unused states by re-creating the subscription anew. -/// -/// HEAP MEMORY REQUIREMENT MODEL. +/// deallocating sessions once allocated. The size of an RX state is at most 32 bytes on any conventional platform. +/// If this behavior is found to be undesirable, the application can force deallocation of all unused states by +/// re-creating the subscription anew. /// /// The return value is 1 if a new subscription has been created as requested. /// The return value is 0 if such subscription existed at the time the function was invoked. In this case, /// the existing subscription is terminated and then a new one is created in its place. Pending transfers may be lost. -/// The return value is negative in case of an error: a negated invalid argument error code if any of the arguments are -/// invalid, or the negated out-of-memory error if the new subscription could not be allocated due to the heap memory -/// being exhausted. +/// The return value is a negated invalid argument error if any of the input arguments are invalid. /// /// The time complexity is linear from the number of current subscriptions under the specified transfer kind. -int8_t canardRxSubscribe(CanardInstance* const ins, - const CanardTransferKind transfer_kind, - const CanardPortID port_id, - const size_t payload_size_bytes_max, - const CanardMicrosecond transfer_id_timeout_usec); +/// This function does not allocate new memory. The function may deallocate memory if such subscription already existed. +int8_t canardRxSubscribe(CanardInstance* const ins, + const CanardTransferKind transfer_kind, + const CanardPortID port_id, + const size_t payload_size_bytes_max, + const CanardMicrosecond transfer_id_timeout_usec, + CanardRxSubscription* const out_subscription); /// Reverse the effect of canardRxSubscribe(). -/// If the subscription is found, all its heap memory is de-allocated; to determine the amount of memory freed, -/// please refer to the heap memory requirement models of canardRxSubscribe() and canardRxAccept(). -/// This function does not allocate new heap memory. +/// If the subscription is found, all its memory is de-allocated; to determine the amount of memory freed, +/// please refer to the memory allocation requirement model of canardRxAccept(). /// /// The return value is 1 if such subscription existed (and, therefore, it was removed). /// The return value is 0 if such subscription does not exist. In this case, the function has no effect. /// The return value is a negated invalid argument error if any of the input arguments are invalid. /// /// The time complexity is linear from the number of current subscriptions under the specified transfer kind. +/// This function does not allocate new memory. int8_t canardRxUnsubscribe(CanardInstance* const ins, const CanardTransferKind transfer_kind, const CanardPortID port_id); diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index 0b6bd3bb..596f3e1a 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -31,7 +31,7 @@ typedef union // NOSONAR uint32_t bits; CanardDSDLFloatNative real; } Float32Bits; -static_assert(sizeof(Float32Bits) == 4, "Unsupported float model"); +static_assert(4 == sizeof(Float32Bits), "Unsupported float model"); uint16_t canardDSDLFloat16Pack(const CanardDSDLFloatNative value) { From 951902b5baaf936857d17ceec48825a594d5db0a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 16 Feb 2020 22:11:48 +0200 Subject: [PATCH 039/100] Update the docs --- libcanard/canard.c | 32 ++++++++++++++++++++------------ libcanard/canard.h | 24 ++++++++++++++++++++---- 2 files changed, 40 insertions(+), 16 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 290d27a6..9335b83f 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -503,6 +503,8 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, // ---------------------------------------- RECEPTION ---------------------------------------- +#define SESSIONS_PER_SUBSCRIPTION (CANARD_NODE_ID_MAX + 1U) + /// The memory requirement model provided in the documentation assumes that the maximum size of this structure never /// exceeds 32 bytes on any conventional platform. /// A user that needs a detailed analysis of the worst-case memory consumption may compute the size of this structure @@ -518,8 +520,6 @@ typedef struct CanardInternalRxSession uint8_t iface_index; ///< Arbitrary value in [0, 255]. } CanardInternalRxSession; -#define SESSIONS_PER_SUBSCRIPTION (CANARD_NODE_ID_MAX + 1U) - /// High-level transport frame model. typedef struct { @@ -615,19 +615,22 @@ CANARD_INTERNAL void initRxTransferFromFrame(const FrameModel* const frame, Cana out_transfer->payload = frame->payload; } -CANARD_INTERNAL int8_t updateRxSession(CanardInternalRxSession* const rxs, +CANARD_INTERNAL int8_t updateRxSession(CanardInstance* const ins, + CanardInternalRxSession* const rxs, const FrameModel* const frame, const uint8_t iface_index, - CanardTransfer* const out_transfer, const CanardMicrosecond transfer_id_timeout_usec, - const size_t payload_size_bytes_max); -CANARD_INTERNAL int8_t updateRxSession(CanardInternalRxSession* const rxs, + const size_t payload_size_bytes_max, + CanardTransfer* const out_transfer); +CANARD_INTERNAL int8_t updateRxSession(CanardInstance* const ins, + CanardInternalRxSession* const rxs, const FrameModel* const frame, const uint8_t iface_index, - CanardTransfer* const out_transfer, const CanardMicrosecond transfer_id_timeout_usec, - const size_t payload_size_bytes_max) + const size_t payload_size_bytes_max, + CanardTransfer* const out_transfer) { + CANARD_ASSERT(ins != NULL); CANARD_ASSERT(rxs != NULL); CANARD_ASSERT(frame != NULL); CANARD_ASSERT(frame->payload != NULL); @@ -698,12 +701,13 @@ CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, if (sub->_sessions[frame->source_node_id] != NULL) { CANARD_ASSERT(out == 0); - out = updateRxSession(sub->_sessions[frame->source_node_id], + out = updateRxSession(ins, + sub->_sessions[frame->source_node_id], frame, iface_index, - out_transfer, sub->_transfer_id_timeout_usec, - sub->_payload_size_bytes_max); + sub->_payload_size_bytes_max, + out_transfer); } } else @@ -857,13 +861,17 @@ int8_t canardRxSubscribe(CanardInstance* const ins, const size_t tk = (size_t) transfer_kind; if ((ins != NULL) && (out_subscription != NULL) && (tk < CANARD_NUM_TRANSFER_KINDS)) { - out = canardRxUnsubscribe(ins, transfer_kind, port_id); // Reset to the initial state. + // Reset to the initial state. This is absolutely critical because the new payload size limit may be larger + // than the old value; if there are any payload buffers allocated, we may overrun them because they are shorter + // than the new payload limit. So we clear the subscription and thus ensure that no overrun may occur. + out = canardRxUnsubscribe(ins, transfer_kind, port_id); if (out >= 0) { for (size_t i = 0; i < SESSIONS_PER_SUBSCRIPTION; i++) { // The sessions will be created ad-hoc. Normally, for a low-jitter deterministic system, // we could have pre-allocated sessions here, but that requires too much memory to be feasible. + // We could accept an extra argument that would instruct us to pre-allocate sessions here? out_subscription->_sessions[i] = NULL; } out_subscription->_transfer_id_timeout_usec = transfer_id_timeout_usec; diff --git a/libcanard/canard.h b/libcanard/canard.h index 6d2a2dfe..adda3643 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -140,11 +140,27 @@ typedef struct /// This is what we call a time-memory trade-off: we use a large look-up table to ensure deterministic runtime behavior. typedef struct CanardRxSubscription { - struct CanardRxSubscription* _next; + struct CanardRxSubscription* _next; + + /// The current architecture is a sort of an acceptable middle ground between worst-case execution time and memory + /// consumption. Instead of statically pre-allocating a dedicated RX session for each remote node-ID here in + /// this table, we only keep pointers, which are NULL by default, populating a new RX session dynamically + /// on an ad-hoc basis when we first receive a transfer from that node. This is still deterministic because our + /// memory allocation routines are assumed to be deterministic and we make at most one allocation per remote node, + /// but the disadvantage is that these additional operations increase the upper bound on the execution time. + /// Further, the pointers here add an extra indirection, which is bad for systems that leverage cached memory, + /// plus a pointer itself takes about 2-8 bytes of memory, too. + /// + /// A far more predictable and a much simpler approach is to pre-allocate states here statically instead of keeping + /// just pointers, but it would push the size of this instance from about 0.5 KiB to ~3 KiB for a typical 32-bit + /// system. Since this is a general-purpose library, we have to pick a middle ground so we use the more complex + /// but more memory-efficient approach. Implementations that are more optimized for low-jitter real-time + /// applications may prefer the other approach. struct CanardInternalRxSession* _sessions[CANARD_NODE_ID_MAX + 1U]; - CanardMicrosecond _transfer_id_timeout_usec; - size_t _payload_size_bytes_max; - CanardPortID _port_id; + + CanardMicrosecond _transfer_id_timeout_usec; + size_t _payload_size_bytes_max; + CanardPortID _port_id; } CanardRxSubscription; typedef void* (*CanardMemoryAllocate)(CanardInstance* ins, size_t amount); From 24ab7ecebd2ff9614dd95eef7b10ab9f39236953 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 17 Feb 2020 02:16:26 +0200 Subject: [PATCH 040/100] RX pipeline seems to be finished, but requires review & refactoring --- libcanard/canard.c | 175 ++++++++++++++++++++++++++++++++++++++++----- libcanard/canard.h | 33 +++++---- 2 files changed, 177 insertions(+), 31 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 9335b83f..f86acb38 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -51,6 +51,7 @@ typedef uint16_t TransferCRC; #define CRC_INITIAL 0xFFFFU +#define CRC_RESIDUE 0x0000U #define CRC_SIZE_BYTES 2U CANARD_INTERNAL TransferCRC crcAddByte(const TransferCRC crc, const uint8_t byte); @@ -517,7 +518,7 @@ typedef struct CanardInternalRxSession uint8_t* payload; ///< Dynamically allocated and handed off to the application when done. TransferCRC calculated_crc; ///< Updated with the received payload in real time. CanardTransferID toggle_and_transfer_id; ///< Toggle and transfer-ID combined into one field to reduce footprint. - uint8_t iface_index; ///< Arbitrary value in [0, 255]. + uint8_t redundant_transport_index; ///< Arbitrary value in [0, 255]. } CanardInternalRxSession; /// High-level transport frame model. @@ -583,7 +584,7 @@ CANARD_INTERNAL bool tryParseFrame(const CanardFrame* const frame, FrameModel* c out_result->payload = (const uint8_t*) frame->payload; // Tail byte parsing. - // Intentional MISRA violation: indexing on a pointer. This is done to avoid pointer arithmetics. + // Intentional violation of MISRA: indexing on a pointer. This is done to avoid pointer arithmetics. const uint8_t tail = out_result->payload[out_result->payload_size]; out_result->transfer_id = tail & CANARD_TRANSFER_ID_MAX; out_result->start_of_transfer = ((tail & TAIL_START_OF_TRANSFER) != 0); @@ -615,17 +616,94 @@ CANARD_INTERNAL void initRxTransferFromFrame(const FrameModel* const frame, Cana out_transfer->payload = frame->payload; } +/// The implementation is borrowed from the Specification. +CANARD_INTERNAL uint8_t computeTransferIDDifference(const uint8_t a, const uint8_t b); +CANARD_INTERNAL uint8_t computeTransferIDDifference(const uint8_t a, const uint8_t b) +{ + int16_t d = (int16_t) a - (int16_t) b; + if (d < 0) + { + d = (int16_t)(d + (int16_t)(1U << CANARD_TRANSFER_ID_BIT_LENGTH)); + } + return (uint8_t) d; +} + +CANARD_INTERNAL int8_t processRxSessionPayload(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const size_t payload_size_bytes_max, + const size_t payload_size, + const void* const payload); +CANARD_INTERNAL int8_t processRxSessionPayload(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const size_t payload_size_bytes_max, + const size_t payload_size, + const void* const payload) +{ + // Allocate the payload lazily, as late as possible. It may already be allocated from an earlier write. + if (rxs->payload == NULL) + { + CANARD_ASSERT(rxs->payload_size == 0); + rxs->payload = ins->memory_allocate(ins, payload_size_bytes_max); + } + + int8_t out = 0; + if (rxs->payload != NULL) + { + // Update the CRC. Observe that the implicit truncation rule may apply here: the payload may be truncated, + // but its CRC is validated always anyway. + rxs->calculated_crc = crcAdd(rxs->calculated_crc, payload_size, payload); + + // Copy the payload into the contiguous buffer. Apply the implicit truncation rule if necessary. + size_t bytes_to_copy = payload_size; + if ((rxs->payload_size + bytes_to_copy) > payload_size_bytes_max) + { + bytes_to_copy = payload_size_bytes_max - rxs->payload_size; + CANARD_ASSERT((rxs->payload_size + bytes_to_copy) == payload_size_bytes_max); + CANARD_ASSERT(bytes_to_copy < payload_size); + } + // Intentional violation of MISRA: indexing on a pointer. This is done to avoid pointer arithmetics. + (void) memcpy(&rxs->payload[rxs->payload_size], payload, bytes_to_copy); // NOLINT NOSONAR + rxs->payload_size += bytes_to_copy; + } + else + { + out = -CANARD_ERROR_OUT_OF_MEMORY; + } + CANARD_ASSERT(out <= 0); + return out; +} + +CANARD_INTERNAL void prepareRxSessionForNextTransfer(CanardInstance* const ins, CanardInternalRxSession* const rxs); +CANARD_INTERNAL void prepareRxSessionForNextTransfer(CanardInstance* const ins, CanardInternalRxSession* const rxs) +{ + CANARD_ASSERT(ins != NULL); + CANARD_ASSERT(rxs != NULL); + ins->memory_free(ins, rxs->payload); // May be NULL, which is OK. + rxs->payload = NULL; + rxs->payload_size = 0U; + rxs->calculated_crc = CRC_INITIAL; + rxs->toggle_and_transfer_id = + (CanardTransferID)(TAIL_TOGGLE | ((rxs->toggle_and_transfer_id + 1U) & CANARD_TRANSFER_ID_MAX)); +} + +/// RX session state machine update is the most intricate part of any UAVCAN transport implementation. +/// The state model used here is derived from the reference pseudocode given in the original UAVCAN v0 specification. +/// The UAVCAN/CAN v1 specification, which this library is an implementation of, does not provide any reference +/// pseudocode. Instead, it takes a higher-level, more abstract approach, where only the high-level requirements +/// are given and the particular algorithms are left to be implementation-defined. Such abstract approach is much +/// advantageous because it allows implementers to choose whatever solution works best for the specific application +/// at hand, while wire compatibility is still guaranteed by the high-level requirements given in the specification. CANARD_INTERNAL int8_t updateRxSession(CanardInstance* const ins, CanardInternalRxSession* const rxs, const FrameModel* const frame, - const uint8_t iface_index, + const uint8_t redundant_transport_index, const CanardMicrosecond transfer_id_timeout_usec, const size_t payload_size_bytes_max, CanardTransfer* const out_transfer); CANARD_INTERNAL int8_t updateRxSession(CanardInstance* const ins, CanardInternalRxSession* const rxs, const FrameModel* const frame, - const uint8_t iface_index, + const uint8_t redundant_transport_index, const CanardMicrosecond transfer_id_timeout_usec, const size_t payload_size_bytes_max, CanardTransfer* const out_transfer) @@ -634,24 +712,85 @@ CANARD_INTERNAL int8_t updateRxSession(CanardInstance* const ins, CANARD_ASSERT(rxs != NULL); CANARD_ASSERT(frame != NULL); CANARD_ASSERT(frame->payload != NULL); + CANARD_ASSERT(frame->transfer_id <= CANARD_TRANSFER_ID_MAX); CANARD_ASSERT(out_transfer != NULL); + // Resolve the state flags. + const bool not_initialized = (0 == rxs->transfer_timestamp_usec); + + const bool tid_timed_out = (frame->timestamp_usec > rxs->transfer_timestamp_usec) && + ((frame->timestamp_usec - rxs->transfer_timestamp_usec) > transfer_id_timeout_usec); + + const bool not_previous_tid = + computeTransferIDDifference(rxs->toggle_and_transfer_id & CANARD_TRANSFER_ID_MAX, frame->transfer_id) > 1; + + const bool need_restart = not_initialized || tid_timed_out || (frame->start_of_transfer && not_previous_tid); + + if (need_restart) + { + rxs->transfer_timestamp_usec = frame->timestamp_usec; + rxs->redundant_transport_index = redundant_transport_index; + rxs->toggle_and_transfer_id = TAIL_TOGGLE | frame->transfer_id; + } + int8_t out = 0; - (void) iface_index; - (void) transfer_id_timeout_usec; - (void) payload_size_bytes_max; + if (need_restart && !frame->start_of_transfer) + { + prepareRxSessionForNextTransfer(ins, rxs); // SOT-miss, no point going further. + } + else + { + const bool correct_transport = (redundant_transport_index == rxs->redundant_transport_index); + const bool correct_toggle = (frame->toggle == ((rxs->toggle_and_transfer_id & TAIL_TOGGLE) != 0)); + const bool correct_tid = (frame->transfer_id == (rxs->toggle_and_transfer_id & CANARD_TRANSFER_ID_MAX)); + if (correct_transport && correct_toggle && correct_tid) + { + // The transfer timestamp is the timestamp of its first frame. + if (frame->start_of_transfer) + { + rxs->transfer_timestamp_usec = frame->timestamp_usec; + } + + out = processRxSessionPayload(ins, rxs, payload_size_bytes_max, frame->payload_size, frame->payload); + if (out < 0) + { + CANARD_ASSERT(out == -CANARD_ERROR_OUT_OF_MEMORY); + prepareRxSessionForNextTransfer(ins, rxs); // Out-of-memory. + } + else if (frame->end_of_transfer) + { + if (CRC_RESIDUE == rxs->calculated_crc) + { + initRxTransferFromFrame(frame, out_transfer); + out_transfer->timestamp_usec = rxs->transfer_timestamp_usec; + out_transfer->payload_size = rxs->payload_size; + out_transfer->payload = rxs->payload; + + rxs->payload = NULL; // Ownership passed over to the application, nullify to prevent freeing. + + out = 1; // One transfer received, notify the application. + } + prepareRxSessionForNextTransfer(ins, rxs); // Successful completion. + } + else + { + const CanardTransferID tog = ((rxs->toggle_and_transfer_id & TAIL_TOGGLE) == 0) ? TAIL_TOGGLE : 0U; + rxs->toggle_and_transfer_id = tog | (rxs->toggle_and_transfer_id & CANARD_TRANSFER_ID_MAX); + } + } + } return out; } CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, const FrameModel* const frame, - const uint8_t iface_index, + const uint8_t redundant_transport_index, CanardTransfer* const out_transfer); CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, const FrameModel* const frame, - const uint8_t iface_index, + const uint8_t redundant_transport_index, CanardTransfer* const out_transfer) { CANARD_ASSERT(ins != NULL); @@ -685,12 +824,12 @@ CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, sub->_sessions[frame->source_node_id] = rxs; if (rxs != NULL) { - rxs->transfer_timestamp_usec = frame->timestamp_usec; - rxs->payload_size = 0U; - rxs->payload = NULL; - rxs->calculated_crc = CRC_INITIAL; - rxs->toggle_and_transfer_id = TAIL_TOGGLE; - rxs->iface_index = 0U; + rxs->transfer_timestamp_usec = 0U; + rxs->payload_size = 0U; + rxs->payload = NULL; + rxs->calculated_crc = CRC_INITIAL; + rxs->toggle_and_transfer_id = TAIL_TOGGLE; + rxs->redundant_transport_index = 0U; } else { @@ -704,7 +843,7 @@ CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, out = updateRxSession(ins, sub->_sessions[frame->source_node_id], frame, - iface_index, + redundant_transport_index, sub->_transfer_id_timeout_usec, sub->_payload_size_bytes_max, out_transfer); @@ -823,7 +962,7 @@ void canardTxPop(CanardInstance* const ins) int8_t canardRxAccept(CanardInstance* const ins, const CanardFrame* const frame, - const uint8_t iface_index, + const uint8_t redundant_transport_index, CanardTransfer* const out_transfer) { int8_t out = -CANARD_ERROR_INVALID_ARGUMENT; @@ -835,7 +974,7 @@ int8_t canardRxAccept(CanardInstance* const ins, { if ((CANARD_NODE_ID_UNSET == model.destination_node_id) || (ins->node_id == model.destination_node_id)) { - out = acceptFrame(ins, &model, iface_index, out_transfer); + out = acceptFrame(ins, &model, redundant_transport_index, out_transfer); } else { diff --git a/libcanard/canard.h b/libcanard/canard.h index adda3643..05cd0720 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -87,7 +87,7 @@ typedef struct { /// For RX frames: reception timestamp. /// For TX frames: transmission deadline. - /// The time system may be arbitrary as long as the clock is monotonic (steady). + /// The time system may be arbitrary as long as the clock is monotonic (steady). Zero is not a valid timestamp. CanardMicrosecond timestamp_usec; /// 29-bit extended ID. The bits above 29-th are zero/ignored. @@ -108,9 +108,11 @@ typedef struct { /// For RX transfers: reception timestamp. /// For TX transfers: transmission deadline. - /// The time system may be arbitrary as long as the clock is monotonic (steady). + /// The time system may be arbitrary as long as the clock is monotonic (steady). Zero is not a valid timestamp. CanardMicrosecond timestamp_usec; + /// Per the Specification, all frames belonging to a given transfer shall share the same priority level. + /// If this is not the case, then this field contains the priority level of the last frame to arrive. CanardPriority priority; CanardTransferKind transfer_kind; @@ -342,21 +344,24 @@ void canardTxPop(CanardInstance* const ins); /// /// The function returns 1 (one) if the new frame completed a transfer. In this case, the details of the transfer /// are stored into out_transfer, and the payload ownership is passed into that object. This means that the application -/// is responsible for deallocating the payload buffer when the processing is done. This design is implemented to -/// facilitate zero-copy data exchange across the protocol stack: once a buffer is allocated, the data is never copied -/// around but only passed by reference. +/// is responsible for deallocating the payload buffer when the processing is done by invoking memory_free. +/// This design is chosen to facilitate zero-copy data exchange across the protocol stack: once a buffer is allocated, +/// its data is never copied around but only passed by reference. This design allows us to reduce the worst-case +/// execution time and reduce jitter caused by the linear time complexity of memcpy(). /// /// The MTU of the accepted frame is not limited and is not dependent on the MTU setting of the local node; /// that is, any MTU is accepted. /// -/// Any value of iface_index is accepted; that is, up to 256 redundant transports are supported. -/// The interface from which the transfer is accepted is always the same as iface_index. +/// Any value of redundant_transport_index is accepted; that is, up to 256 redundant transports are supported. +/// The index of the transport from which the transfer is accepted is always the same as redundant_transport_index. /// -/// The time complexity is O(n) where n is the number of subject-IDs or service-IDs subscribed to by the application, -/// depending on whether the frame is of the message kind of of the service kind. Observe that the time complexity -/// is invariant to the network configuration (such as the number of online nodes), which is an important design -/// guarantee for real-time applications. The time complexity is only dependent on the number of active subscriptions -/// for a given transfer kind, which is well-controlled by the local application. +/// The time complexity is O(n+s) where n is the number of subject-IDs or service-IDs subscribed to by the application, +/// depending on whether the frame is of the message kind of of the service kind, and s is the amount of payload in the +/// received transport frame (because it will be copied into an internal contiguous buffer). +/// Observe that the time complexity is invariant to the network configuration (such as the number of online nodes), +/// which is an important design guarantee for real-time applications. +/// The time complexity is only dependent on the number of active subscriptions for a given transfer kind, +/// and the MTU, both of which are easy to predict and account for. /// /// Unicast frames where the destination does not equal the local node-ID are discarded in constant time. /// Frames that are not valid UAVCAN/CAN frames are discarded in constant time. @@ -364,7 +369,7 @@ void canardTxPop(CanardInstance* const ins); /// MEMORY ALLOCATION REQUIREMENT MODEL. int8_t canardRxAccept(CanardInstance* const ins, const CanardFrame* const frame, - const uint8_t iface_index, + const uint8_t redundant_transport_index, CanardTransfer* const out_transfer); /// Subscription instances have large look-up tables to ensure that the temporal properties of the algorithms are @@ -386,6 +391,8 @@ int8_t canardRxAccept(CanardInstance* const ins, /// If this behavior is found to be undesirable, the application can force deallocation of all unused states by /// re-creating the subscription anew. /// +/// The transport fail-over timeout (if redundant transports are used) is the same as the transfer-ID timeout. +/// /// The return value is 1 if a new subscription has been created as requested. /// The return value is 0 if such subscription existed at the time the function was invoked. In this case, /// the existing subscription is terminated and then a new one is created in its place. Pending transfers may be lost. From 468965fbf7644317288bc72a338cd473db2886f1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 17 Feb 2020 04:13:17 +0200 Subject: [PATCH 041/100] RX pipeline refactoring --- libcanard/canard.c | 321 +++++++++++++++++++++++++-------------------- libcanard/canard.h | 11 +- 2 files changed, 186 insertions(+), 146 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index f86acb38..ab3ee96a 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -620,61 +620,64 @@ CANARD_INTERNAL void initRxTransferFromFrame(const FrameModel* const frame, Cana CANARD_INTERNAL uint8_t computeTransferIDDifference(const uint8_t a, const uint8_t b); CANARD_INTERNAL uint8_t computeTransferIDDifference(const uint8_t a, const uint8_t b) { - int16_t d = (int16_t) a - (int16_t) b; - if (d < 0) + int16_t diff = (int16_t)(((int16_t) a) - ((int16_t) b)); + if (diff < 0) { - d = (int16_t)(d + (int16_t)(1U << CANARD_TRANSFER_ID_BIT_LENGTH)); + const uint8_t modulo = 1U << CANARD_TRANSFER_ID_BIT_LENGTH; + diff = (int16_t)(diff + (int16_t) modulo); } - return (uint8_t) d; + return (uint8_t) diff; } -CANARD_INTERNAL int8_t processRxSessionPayload(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const size_t payload_size_bytes_max, - const size_t payload_size, - const void* const payload); -CANARD_INTERNAL int8_t processRxSessionPayload(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const size_t payload_size_bytes_max, - const size_t payload_size, - const void* const payload) +CANARD_INTERNAL int8_t rxWritePayload(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const size_t payload_size_max, + const size_t payload_size, + const void* const payload); +CANARD_INTERNAL int8_t rxWritePayload(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const size_t payload_size_max, + const size_t payload_size, + const void* const payload) { - // Allocate the payload lazily, as late as possible. It may already be allocated from an earlier write. - if (rxs->payload == NULL) + // Allocate the payload lazily, as late as possible. + if ((NULL == rxs->payload) && (payload_size_max > 0U)) { CANARD_ASSERT(rxs->payload_size == 0); - rxs->payload = ins->memory_allocate(ins, payload_size_bytes_max); + rxs->payload = ins->memory_allocate(ins, payload_size_max); } int8_t out = 0; if (rxs->payload != NULL) { - // Update the CRC. Observe that the implicit truncation rule may apply here: the payload may be truncated, - // but its CRC is validated always anyway. - rxs->calculated_crc = crcAdd(rxs->calculated_crc, payload_size, payload); - // Copy the payload into the contiguous buffer. Apply the implicit truncation rule if necessary. size_t bytes_to_copy = payload_size; - if ((rxs->payload_size + bytes_to_copy) > payload_size_bytes_max) + if ((rxs->payload_size + bytes_to_copy) > payload_size_max) { - bytes_to_copy = payload_size_bytes_max - rxs->payload_size; - CANARD_ASSERT((rxs->payload_size + bytes_to_copy) == payload_size_bytes_max); + CANARD_ASSERT(rxs->payload_size <= payload_size_max); + bytes_to_copy = payload_size_max - rxs->payload_size; + CANARD_ASSERT((rxs->payload_size + bytes_to_copy) == payload_size_max); CANARD_ASSERT(bytes_to_copy < payload_size); } + // This memcpy() call here is one of the two variable-complexity operations in the RX pipeline; + // the other one is the search of the matching subscription state. + // Excepting these two cases, the entire RX pipeline contains neither loops nor recursion. // Intentional violation of MISRA: indexing on a pointer. This is done to avoid pointer arithmetics. (void) memcpy(&rxs->payload[rxs->payload_size], payload, bytes_to_copy); // NOLINT NOSONAR rxs->payload_size += bytes_to_copy; + CANARD_ASSERT(rxs->payload_size <= payload_size_max); } else { - out = -CANARD_ERROR_OUT_OF_MEMORY; + CANARD_ASSERT(rxs->payload_size == 0); + out = (payload_size_max > 0U) ? -CANARD_ERROR_OUT_OF_MEMORY : 0; } CANARD_ASSERT(out <= 0); return out; } -CANARD_INTERNAL void prepareRxSessionForNextTransfer(CanardInstance* const ins, CanardInternalRxSession* const rxs); -CANARD_INTERNAL void prepareRxSessionForNextTransfer(CanardInstance* const ins, CanardInternalRxSession* const rxs) +CANARD_INTERNAL void rxRestart(CanardInstance* const ins, CanardInternalRxSession* const rxs); +CANARD_INTERNAL void rxRestart(CanardInstance* const ins, CanardInternalRxSession* const rxs) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(rxs != NULL); @@ -686,6 +689,68 @@ CANARD_INTERNAL void prepareRxSessionForNextTransfer(CanardInstance* const ins, (CanardTransferID)(TAIL_TOGGLE | ((rxs->toggle_and_transfer_id + 1U) & CANARD_TRANSFER_ID_MAX)); } +CANARD_INTERNAL int8_t rxAcceptFrame(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const FrameModel* const frame, + const size_t payload_size_max, + CanardTransfer* const out_transfer) +{ + CANARD_ASSERT(ins != NULL); + CANARD_ASSERT(rxs != NULL); + CANARD_ASSERT(frame != NULL); + CANARD_ASSERT(frame->payload != NULL); + CANARD_ASSERT(frame->transfer_id <= CANARD_TRANSFER_ID_MAX); + CANARD_ASSERT(out_transfer != NULL); + + if (frame->start_of_transfer) // The transfer timestamp is the timestamp of its first frame. + { + rxs->transfer_timestamp_usec = frame->timestamp_usec; + } + + size_t payload_size = frame->payload_size; + const bool single_frame = frame->start_of_transfer && frame->end_of_transfer; + if (!single_frame) + { + // Update the CRC. Observe that the implicit truncation rule may apply here: the payload may be + // truncated, but its CRC is validated always anyway. + rxs->calculated_crc = crcAdd(rxs->calculated_crc, frame->payload_size, frame->payload); + // Drop the last two bytes of the payload because we don't want to expose the transfer-CRC to the user. + if (frame->end_of_transfer) + { + payload_size = (payload_size > CRC_SIZE_BYTES) ? (payload_size - CRC_SIZE_BYTES) : 0U; + } + } + + int8_t out = rxWritePayload(ins, rxs, payload_size_max, payload_size, frame->payload); + if (out < 0) + { + CANARD_ASSERT(-CANARD_ERROR_OUT_OF_MEMORY == out); + rxRestart(ins, rxs); // Out-of-memory. + } + else if (frame->end_of_transfer) + { + CANARD_ASSERT(0 == out); + if (single_frame || (CRC_RESIDUE == rxs->calculated_crc)) + { + initRxTransferFromFrame(frame, out_transfer); + out_transfer->timestamp_usec = rxs->transfer_timestamp_usec; + out_transfer->payload_size = rxs->payload_size; + out_transfer->payload = rxs->payload; + + rxs->payload = NULL; // Ownership passed over to the application, nullify to prevent freeing. + + out = 1; // One transfer received, notify the application. + } + rxRestart(ins, rxs); // Successful completion. + } + else + { + const CanardTransferID tog = (0 == (rxs->toggle_and_transfer_id & TAIL_TOGGLE)) ? TAIL_TOGGLE : 0U; + rxs->toggle_and_transfer_id = (CanardTransferID)(tog | (rxs->toggle_and_transfer_id & CANARD_TRANSFER_ID_MAX)); + } + return out; +} + /// RX session state machine update is the most intricate part of any UAVCAN transport implementation. /// The state model used here is derived from the reference pseudocode given in the original UAVCAN v0 specification. /// The UAVCAN/CAN v1 specification, which this library is an implementation of, does not provide any reference @@ -693,29 +758,26 @@ CANARD_INTERNAL void prepareRxSessionForNextTransfer(CanardInstance* const ins, /// are given and the particular algorithms are left to be implementation-defined. Such abstract approach is much /// advantageous because it allows implementers to choose whatever solution works best for the specific application /// at hand, while wire compatibility is still guaranteed by the high-level requirements given in the specification. -CANARD_INTERNAL int8_t updateRxSession(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const FrameModel* const frame, - const uint8_t redundant_transport_index, - const CanardMicrosecond transfer_id_timeout_usec, - const size_t payload_size_bytes_max, - CanardTransfer* const out_transfer); -CANARD_INTERNAL int8_t updateRxSession(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const FrameModel* const frame, - const uint8_t redundant_transport_index, - const CanardMicrosecond transfer_id_timeout_usec, - const size_t payload_size_bytes_max, - CanardTransfer* const out_transfer) +CANARD_INTERNAL int8_t rxUpdate(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const FrameModel* const frame, + const uint8_t redundant_transport_index, + const CanardMicrosecond transfer_id_timeout_usec, + const size_t payload_size_max, + CanardTransfer* const out_transfer); +CANARD_INTERNAL int8_t rxUpdate(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const FrameModel* const frame, + const uint8_t redundant_transport_index, + const CanardMicrosecond transfer_id_timeout_usec, + const size_t payload_size_max, + CanardTransfer* const out_transfer) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(rxs != NULL); CANARD_ASSERT(frame != NULL); - CANARD_ASSERT(frame->payload != NULL); - CANARD_ASSERT(frame->transfer_id <= CANARD_TRANSFER_ID_MAX); CANARD_ASSERT(out_transfer != NULL); - // Resolve the state flags. const bool not_initialized = (0 == rxs->transfer_timestamp_usec); const bool tid_timed_out = (frame->timestamp_usec > rxs->transfer_timestamp_usec) && @@ -734,10 +796,9 @@ CANARD_INTERNAL int8_t updateRxSession(CanardInstance* const ins, } int8_t out = 0; - - if (need_restart && !frame->start_of_transfer) + if (need_restart && (!frame->start_of_transfer)) { - prepareRxSessionForNextTransfer(ins, rxs); // SOT-miss, no point going further. + rxRestart(ins, rxs); // SOT-miss, no point going further. } else { @@ -746,118 +807,75 @@ CANARD_INTERNAL int8_t updateRxSession(CanardInstance* const ins, const bool correct_tid = (frame->transfer_id == (rxs->toggle_and_transfer_id & CANARD_TRANSFER_ID_MAX)); if (correct_transport && correct_toggle && correct_tid) { - // The transfer timestamp is the timestamp of its first frame. - if (frame->start_of_transfer) - { - rxs->transfer_timestamp_usec = frame->timestamp_usec; - } - - out = processRxSessionPayload(ins, rxs, payload_size_bytes_max, frame->payload_size, frame->payload); - if (out < 0) - { - CANARD_ASSERT(out == -CANARD_ERROR_OUT_OF_MEMORY); - prepareRxSessionForNextTransfer(ins, rxs); // Out-of-memory. - } - else if (frame->end_of_transfer) - { - if (CRC_RESIDUE == rxs->calculated_crc) - { - initRxTransferFromFrame(frame, out_transfer); - out_transfer->timestamp_usec = rxs->transfer_timestamp_usec; - out_transfer->payload_size = rxs->payload_size; - out_transfer->payload = rxs->payload; - - rxs->payload = NULL; // Ownership passed over to the application, nullify to prevent freeing. - - out = 1; // One transfer received, notify the application. - } - prepareRxSessionForNextTransfer(ins, rxs); // Successful completion. - } - else - { - const CanardTransferID tog = ((rxs->toggle_and_transfer_id & TAIL_TOGGLE) == 0) ? TAIL_TOGGLE : 0U; - rxs->toggle_and_transfer_id = tog | (rxs->toggle_and_transfer_id & CANARD_TRANSFER_ID_MAX); - } + out = rxAcceptFrame(ins, rxs, frame, payload_size_max, out_transfer); } } - return out; } -CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, - const FrameModel* const frame, - const uint8_t redundant_transport_index, - CanardTransfer* const out_transfer); -CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, - const FrameModel* const frame, - const uint8_t redundant_transport_index, - CanardTransfer* const out_transfer) +CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, + CanardRxSubscription* const subscription, + const FrameModel* const frame, + const uint8_t redundant_transport_index, + CanardTransfer* const out_transfer); +CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, + CanardRxSubscription* const subscription, + const FrameModel* const frame, + const uint8_t redundant_transport_index, + CanardTransfer* const out_transfer) { CANARD_ASSERT(ins != NULL); + CANARD_ASSERT(subscription != NULL); + CANARD_ASSERT(subscription->_port_id == frame->port_id); CANARD_ASSERT(frame != NULL); CANARD_ASSERT(frame->payload != NULL); CANARD_ASSERT((CANARD_NODE_ID_UNSET == frame->destination_node_id) || (ins->node_id == frame->destination_node_id)); CANARD_ASSERT(out_transfer != NULL); - // Find subscription. This is the reason the function has a linear time complexity from the number of subscriptions. - CanardRxSubscription* sub = ins->_rx_subscriptions[(size_t) frame->transfer_kind]; - while ((sub != NULL) && (sub->_port_id != frame->port_id)) - { - sub = sub->_next; - } - - // If the subscription is not found, that means that the application doesn't want this transfer. Ignore the frame. int8_t out = 0; - if (sub != NULL) + if (frame->source_node_id <= CANARD_NODE_ID_MAX) { - CANARD_ASSERT(sub->_port_id == frame->port_id); - if (frame->source_node_id <= CANARD_NODE_ID_MAX) + // If such session does not exist, create it. This only makes sense if this is the first frame of a + // transfer, otherwise, we won't be able to receive the transfer anyway so we don't bother. + if ((NULL == subscription->_sessions[frame->source_node_id]) && frame->start_of_transfer) { - // If such session does not exist, create it. This only makes sense if this is the first frame of a - // transfer, otherwise, we won't be able to receive the transfer anyway so we don't bother. - if ((NULL == sub->_sessions[frame->source_node_id]) && frame->start_of_transfer) + CanardInternalRxSession* const rxs = + (CanardInternalRxSession*) ins->memory_allocate(ins, sizeof(CanardInternalRxSession)); + subscription->_sessions[frame->source_node_id] = rxs; + if (rxs != NULL) { - CanardInternalRxSession* const rxs = - (CanardInternalRxSession*) ins->memory_allocate(ins, - sizeof(CanardInternalRxSession) + - sub->_payload_size_bytes_max); - sub->_sessions[frame->source_node_id] = rxs; - if (rxs != NULL) - { - rxs->transfer_timestamp_usec = 0U; - rxs->payload_size = 0U; - rxs->payload = NULL; - rxs->calculated_crc = CRC_INITIAL; - rxs->toggle_and_transfer_id = TAIL_TOGGLE; - rxs->redundant_transport_index = 0U; - } - else - { - out = -CANARD_ERROR_OUT_OF_MEMORY; - } + rxs->transfer_timestamp_usec = 0U; + rxs->payload_size = 0U; + rxs->payload = NULL; + rxs->calculated_crc = CRC_INITIAL; + rxs->toggle_and_transfer_id = TAIL_TOGGLE; + rxs->redundant_transport_index = 0U; } - // There are two possible reasons why the session may not exist: 1. OOM; 2. SOT-miss. - if (sub->_sessions[frame->source_node_id] != NULL) + else { - CANARD_ASSERT(out == 0); - out = updateRxSession(ins, - sub->_sessions[frame->source_node_id], - frame, - redundant_transport_index, - sub->_transfer_id_timeout_usec, - sub->_payload_size_bytes_max, - out_transfer); + out = -CANARD_ERROR_OUT_OF_MEMORY; } } - else + // There are two possible reasons why the session may not exist: 1. OOM; 2. SOT-miss. + if (subscription->_sessions[frame->source_node_id] != NULL) { - CANARD_ASSERT(frame->source_node_id == CANARD_NODE_ID_UNSET); - // Anonymous transfers are stateless. No need to update the state machine, just blindly accept it. - initRxTransferFromFrame(frame, out_transfer); - out = 1; + CANARD_ASSERT(out == 0); + out = rxUpdate(ins, + subscription->_sessions[frame->source_node_id], + frame, + redundant_transport_index, + subscription->_transfer_id_timeout_usec, + subscription->_payload_size_max, + out_transfer); } } - CANARD_ASSERT(out <= 1); + else + { + CANARD_ASSERT(frame->source_node_id == CANARD_NODE_ID_UNSET); + // Anonymous transfers are stateless. No need to update the state machine, just blindly accept it. + initRxTransferFromFrame(frame, out_transfer); + out = 1; + } return out; } @@ -885,7 +903,7 @@ CanardInstance canardInit(const CanardMemoryAllocate memory_allocate, const Cana .node_id = CANARD_NODE_ID_UNSET, .memory_allocate = memory_allocate, .memory_free = memory_free, - ._rx_subscriptions = {NULL}, + ._rx_subscriptions = {NULL, NULL, NULL}, ._tx_queue = NULL, }; return out; @@ -974,25 +992,44 @@ int8_t canardRxAccept(CanardInstance* const ins, { if ((CANARD_NODE_ID_UNSET == model.destination_node_id) || (ins->node_id == model.destination_node_id)) { - out = acceptFrame(ins, &model, redundant_transport_index, out_transfer); + // Find subscription. This is the reason the function has a linear time complexity from the number of + // subscriptions. Note also that this one of the two variable-complexity operations in the RX pipeline; + // the other one is memcpy(). Excepting these two cases, the entire RX pipeline logic contains neither + // loops nor recursion. + CanardRxSubscription* sub = ins->_rx_subscriptions[(size_t) model.transfer_kind]; + while ((sub != NULL) && (sub->_port_id != model.port_id)) + { + sub = sub->_next; + } + + if (sub != NULL) + { + CANARD_ASSERT(sub->_port_id == model.port_id); + out = acceptFrame(ins, sub, &model, redundant_transport_index, out_transfer); + } + else + { + out = 0; // No matching subscription. + } } else { - out = 0; // Mis-addressed frame is obviously not an error. + out = 0; // Mis-addressed frame (normally it should be filtered out by the hardware). } } else { - out = 0; // A non-UAVCAN/CAN input frame is obviously not an error. + out = 0; // A non-UAVCAN/CAN input frame. } } + CANARD_ASSERT(out <= 1); return out; } int8_t canardRxSubscribe(CanardInstance* const ins, const CanardTransferKind transfer_kind, const CanardPortID port_id, - const size_t payload_size_bytes_max, + const size_t payload_size_max, const CanardMicrosecond transfer_id_timeout_usec, CanardRxSubscription* const out_subscription) { @@ -1014,7 +1051,7 @@ int8_t canardRxSubscribe(CanardInstance* const ins, out_subscription->_sessions[i] = NULL; } out_subscription->_transfer_id_timeout_usec = transfer_id_timeout_usec; - out_subscription->_payload_size_bytes_max = payload_size_bytes_max; + out_subscription->_payload_size_max = payload_size_max; out_subscription->_port_id = port_id; out_subscription->_next = ins->_rx_subscriptions[tk]; ins->_rx_subscriptions[tk] = out_subscription; diff --git a/libcanard/canard.h b/libcanard/canard.h index 05cd0720..4ec582d6 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -161,7 +161,7 @@ typedef struct CanardRxSubscription struct CanardInternalRxSession* _sessions[CANARD_NODE_ID_MAX + 1U]; CanardMicrosecond _transfer_id_timeout_usec; - size_t _payload_size_bytes_max; + size_t _payload_size_max; CanardPortID _port_id; } CanardRxSubscription; @@ -348,6 +348,8 @@ void canardTxPop(CanardInstance* const ins); /// This design is chosen to facilitate zero-copy data exchange across the protocol stack: once a buffer is allocated, /// its data is never copied around but only passed by reference. This design allows us to reduce the worst-case /// execution time and reduce jitter caused by the linear time complexity of memcpy(). +/// There is a special case, however: if the payload_size_max is zero, the payload pointer will be NULL, since there +/// is no data to store and so a buffer is not needed. /// /// The MTU of the accepted frame is not limited and is not dependent on the MTU setting of the local node; /// that is, any MTU is accepted. @@ -360,8 +362,9 @@ void canardTxPop(CanardInstance* const ins); /// received transport frame (because it will be copied into an internal contiguous buffer). /// Observe that the time complexity is invariant to the network configuration (such as the number of online nodes), /// which is an important design guarantee for real-time applications. -/// The time complexity is only dependent on the number of active subscriptions for a given transfer kind, -/// and the MTU, both of which are easy to predict and account for. +/// The execution time is only dependent on the number of active subscriptions for a given transfer kind, +/// and the MTU, both of which are easy to predict and account for. Excepting the subscription search and the +/// payload data copying, the entire RX pipeline contains neither loops nor recursion. /// /// Unicast frames where the destination does not equal the local node-ID are discarded in constant time. /// Frames that are not valid UAVCAN/CAN frames are discarded in constant time. @@ -403,7 +406,7 @@ int8_t canardRxAccept(CanardInstance* const ins, int8_t canardRxSubscribe(CanardInstance* const ins, const CanardTransferKind transfer_kind, const CanardPortID port_id, - const size_t payload_size_bytes_max, + const size_t payload_size_max, const CanardMicrosecond transfer_id_timeout_usec, CanardRxSubscription* const out_subscription); From a61b50572b54084b6253eaffac756bf8d237d55e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 17 Feb 2020 04:29:14 +0200 Subject: [PATCH 042/100] Missing declaration --- libcanard/canard.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libcanard/canard.c b/libcanard/canard.c index ab3ee96a..83cb6e57 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -689,6 +689,11 @@ CANARD_INTERNAL void rxRestart(CanardInstance* const ins, CanardInternalRxSessio (CanardTransferID)(TAIL_TOGGLE | ((rxs->toggle_and_transfer_id + 1U) & CANARD_TRANSFER_ID_MAX)); } +CANARD_INTERNAL int8_t rxAcceptFrame(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const FrameModel* const frame, + const size_t payload_size_max, + CanardTransfer* const out_transfer); CANARD_INTERNAL int8_t rxAcceptFrame(CanardInstance* const ins, CanardInternalRxSession* const rxs, const FrameModel* const frame, From 23a47eb17ff73ac87b97597833d0a7c582245fc3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 17 Feb 2020 04:46:47 +0200 Subject: [PATCH 043/100] Internal simplification --- libcanard/canard.c | 22 ++++++++++------------ 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 83cb6e57..f26f0418 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -44,7 +44,7 @@ #define BITS_PER_BYTE 8U #define BYTE_MAX 0xFFU -#define PADDING_BYTE 0U +#define PADDING_BYTE_VALUE 0U // ---------------------------------------- TRANSFER CRC ---------------------------------------- @@ -339,7 +339,7 @@ CANARD_INTERNAL int32_t pushSingleFrameTransfer(CanardInstance* const ins, // Clang-Tidy raises an error recommending the use of memset_s() instead. // We ignore this recommendation because it is not available in C99. - (void) memset(&tqi->payload[payload_size], PADDING_BYTE, padding_size); // NOLINT + (void) memset(&tqi->payload[payload_size], PADDING_BYTE_VALUE, padding_size); // NOLINT tqi->payload[frame_payload_size - 1U] = makeTailByte(true, true, true, transfer_id); CanardInternalTxQueueItem* const sup = findTxQueueSupremum(ins, can_id); @@ -443,9 +443,9 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, // Insert padding -- only in the last frame. Don't forget to include padding into the CRC. while ((frame_offset + CRC_SIZE_BYTES) < frame_payload_size) { - tail->payload[frame_offset] = PADDING_BYTE; + tail->payload[frame_offset] = PADDING_BYTE_VALUE; ++frame_offset; - crc = crcAddByte(crc, PADDING_BYTE); + crc = crcAddByte(crc, PADDING_BYTE_VALUE); } // Insert the CRC. @@ -783,21 +783,18 @@ CANARD_INTERNAL int8_t rxUpdate(CanardInstance* const ins, CANARD_ASSERT(frame != NULL); CANARD_ASSERT(out_transfer != NULL); - const bool not_initialized = (0 == rxs->transfer_timestamp_usec); - const bool tid_timed_out = (frame->timestamp_usec > rxs->transfer_timestamp_usec) && ((frame->timestamp_usec - rxs->transfer_timestamp_usec) > transfer_id_timeout_usec); const bool not_previous_tid = computeTransferIDDifference(rxs->toggle_and_transfer_id & CANARD_TRANSFER_ID_MAX, frame->transfer_id) > 1; - const bool need_restart = not_initialized || tid_timed_out || (frame->start_of_transfer && not_previous_tid); + const bool need_restart = tid_timed_out || (frame->start_of_transfer && not_previous_tid); if (need_restart) { - rxs->transfer_timestamp_usec = frame->timestamp_usec; rxs->redundant_transport_index = redundant_transport_index; - rxs->toggle_and_transfer_id = TAIL_TOGGLE | frame->transfer_id; + rxs->toggle_and_transfer_id = (CanardTransferID)(TAIL_TOGGLE | frame->transfer_id); } int8_t out = 0; @@ -834,6 +831,7 @@ CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, CANARD_ASSERT(subscription->_port_id == frame->port_id); CANARD_ASSERT(frame != NULL); CANARD_ASSERT(frame->payload != NULL); + CANARD_ASSERT(frame->transfer_id <= CANARD_TRANSFER_ID_MAX); CANARD_ASSERT((CANARD_NODE_ID_UNSET == frame->destination_node_id) || (ins->node_id == frame->destination_node_id)); CANARD_ASSERT(out_transfer != NULL); @@ -849,12 +847,12 @@ CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, subscription->_sessions[frame->source_node_id] = rxs; if (rxs != NULL) { - rxs->transfer_timestamp_usec = 0U; + rxs->transfer_timestamp_usec = frame->timestamp_usec; rxs->payload_size = 0U; rxs->payload = NULL; rxs->calculated_crc = CRC_INITIAL; - rxs->toggle_and_transfer_id = TAIL_TOGGLE; - rxs->redundant_transport_index = 0U; + rxs->toggle_and_transfer_id = (CanardTransferID)(TAIL_TOGGLE | frame->transfer_id); + rxs->redundant_transport_index = redundant_transport_index; } else { From 0bc8cf41d710dc5dc685f21a2a3295510d1620d3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 17 Feb 2020 05:20:44 +0200 Subject: [PATCH 044/100] Global renamings, no structural changes --- libcanard/canard.c | 366 +++++++++++++++++++-------------------- libcanard/canard.h | 4 +- tests/internals.hpp | 30 ++-- tests/test_internals.cpp | 200 ++++++++++----------- 4 files changed, 300 insertions(+), 300 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index f26f0418..f82330be 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -5,7 +5,7 @@ #include #include -// ---------------------------------------- BUILD CONFIGURATION ---------------------------------------- +// --------------------------------------------- BUILD CONFIGURATION --------------------------------------------- /// By default, this macro resolves to the standard assert(). The user can redefine this if necessary. /// To disable assertion checks completely, make it expand into `(void)(0)`. @@ -33,7 +33,7 @@ # define _static_assert_gl_impl(a, b) a##b // NOSONAR #endif -// ---------------------------------------- COMMON CONSTANTS ---------------------------------------- +// --------------------------------------------- COMMON CONSTANTS --------------------------------------------- #define TAIL_START_OF_TRANSFER 128U #define TAIL_END_OF_TRANSFER 64U @@ -46,7 +46,18 @@ #define PADDING_BYTE_VALUE 0U -// ---------------------------------------- TRANSFER CRC ---------------------------------------- +#define OFFSET_PRIORITY 26U +#define OFFSET_SUBJECT_ID 8U +#define OFFSET_SERVICE_ID 14U +#define OFFSET_DST_NODE_ID 7U + +#define FLAG_SERVICE_NOT_MESSAGE (UINT32_C(1) << 25U) +#define FLAG_ANONYMOUS_MESSAGE (UINT32_C(1) << 24U) +#define FLAG_REQUEST_NOT_RESPONSE (UINT32_C(1) << 24U) +#define FLAG_RESERVED_23 (UINT32_C(1) << 23U) +#define FLAG_RESERVED_07 (UINT32_C(1) << 7U) + +// --------------------------------------------- TRANSFER CRC --------------------------------------------- typedef uint16_t TransferCRC; @@ -57,11 +68,12 @@ typedef uint16_t TransferCRC; CANARD_INTERNAL TransferCRC crcAddByte(const TransferCRC crc, const uint8_t byte); CANARD_INTERNAL TransferCRC crcAddByte(const TransferCRC crc, const uint8_t byte) { - TransferCRC out = crc ^ (uint16_t)((uint16_t)(byte) << BITS_PER_BYTE); + static const TransferCRC TopBit = 0x8000U; + static const TransferCRC Poly = 0x1021U; + TransferCRC out = crc ^ (uint16_t)((uint16_t)(byte) << BITS_PER_BYTE); for (uint8_t i = 0; i < BITS_PER_BYTE; i++) // Should we use a table instead? Adds 512 bytes of ROM. { - // The no-lint statements suppress the warnings about magic numbers. These numbers are not magic. - out = ((out & 0x8000U) != 0U) ? ((uint16_t)(out << 1U) ^ 0x1021U) : (uint16_t)(out << 1U); // NOLINT + out = ((out & TopBit) != 0U) ? ((uint16_t)(out << 1U) ^ Poly) : (uint16_t)(out << 1U); } return out; } @@ -80,35 +92,24 @@ CANARD_INTERNAL TransferCRC crcAdd(const TransferCRC crc, const size_t size, con return out; } -// ---------------------------------------- SESSION SPECIFIER ---------------------------------------- +// --------------------------------------------- TRANSMISSION --------------------------------------------- -#define OFFSET_PRIORITY 26U -#define OFFSET_SUBJECT_ID 8U -#define OFFSET_SERVICE_ID 14U -#define OFFSET_DST_NODE_ID 7U - -#define FLAG_SERVICE_NOT_MESSAGE (UINT32_C(1) << 25U) -#define FLAG_ANONYMOUS_MESSAGE (UINT32_C(1) << 24U) -#define FLAG_REQUEST_NOT_RESPONSE (UINT32_C(1) << 24U) -#define FLAG_RESERVED_23 (UINT32_C(1) << 23U) -#define FLAG_RESERVED_07 (UINT32_C(1) << 7U) - -CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const CanardPortID subject_id, const CanardNodeID src_node_id); -CANARD_INTERNAL uint32_t makeMessageSessionSpecifier(const CanardPortID subject_id, const CanardNodeID src_node_id) +CANARD_INTERNAL uint32_t txMakeMessageSessionSpecifier(const CanardPortID subject_id, const CanardNodeID src_node_id); +CANARD_INTERNAL uint32_t txMakeMessageSessionSpecifier(const CanardPortID subject_id, const CanardNodeID src_node_id) { CANARD_ASSERT(src_node_id <= CANARD_NODE_ID_MAX); CANARD_ASSERT(subject_id <= CANARD_SUBJECT_ID_MAX); return src_node_id | ((uint32_t) subject_id << OFFSET_SUBJECT_ID); } -CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const CanardPortID service_id, - const bool request_not_response, - const CanardNodeID src_node_id, - const CanardNodeID dst_node_id); -CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const CanardPortID service_id, - const bool request_not_response, - const CanardNodeID src_node_id, - const CanardNodeID dst_node_id) +CANARD_INTERNAL uint32_t txMakeServiceSessionSpecifier(const CanardPortID service_id, + const bool request_not_response, + const CanardNodeID src_node_id, + const CanardNodeID dst_node_id); +CANARD_INTERNAL uint32_t txMakeServiceSessionSpecifier(const CanardPortID service_id, + const bool request_not_response, + const CanardNodeID src_node_id, + const CanardNodeID dst_node_id) { CANARD_ASSERT(src_node_id <= CANARD_NODE_ID_MAX); CANARD_ASSERT(dst_node_id <= CANARD_NODE_ID_MAX); @@ -118,8 +119,6 @@ CANARD_INTERNAL uint32_t makeServiceSessionSpecifier(const CanardPortID service_ (request_not_response ? FLAG_REQUEST_NOT_RESPONSE : 0U) | FLAG_SERVICE_NOT_MESSAGE; } -// ---------------------------------------- TRANSMISSION ---------------------------------------- - /// The memory requirement model provided in the documentation assumes that the maximum size of this structure never /// exceeds 32 bytes on any conventional platform. The sizeof() of this structure, per the C standard, assumes that /// the length of the flex array member is zero. @@ -142,8 +141,8 @@ typedef struct CanardInternalTxQueueItem } CanardInternalTxQueueItem; /// This is the transport MTU rounded up to next full DLC minus the tail byte. -CANARD_INTERNAL size_t getPresentationLayerMTU(const CanardInstance* const ins); -CANARD_INTERNAL size_t getPresentationLayerMTU(const CanardInstance* const ins) +CANARD_INTERNAL size_t txGetPresentationLayerMTU(const CanardInstance* const ins); +CANARD_INTERNAL size_t txGetPresentationLayerMTU(const CanardInstance* const ins) { const size_t max_index = (sizeof(CanardCANLengthToDLC) / sizeof(CanardCANLengthToDLC[0])) - 1U; size_t mtu = 0U; @@ -162,12 +161,12 @@ CANARD_INTERNAL size_t getPresentationLayerMTU(const CanardInstance* const ins) return mtu - 1U; } -CANARD_INTERNAL int32_t makeCANID(const CanardTransfer* const tr, - const CanardNodeID local_node_id, - const size_t presentation_layer_mtu); -CANARD_INTERNAL int32_t makeCANID(const CanardTransfer* const tr, - const CanardNodeID local_node_id, - const size_t presentation_layer_mtu) +CANARD_INTERNAL int32_t txMakeCANID(const CanardTransfer* const tr, + const CanardNodeID local_node_id, + const size_t presentation_layer_mtu); +CANARD_INTERNAL int32_t txMakeCANID(const CanardTransfer* const tr, + const CanardNodeID local_node_id, + const size_t presentation_layer_mtu) { CANARD_ASSERT(tr != NULL); CANARD_ASSERT(presentation_layer_mtu > 0); @@ -177,7 +176,7 @@ CANARD_INTERNAL int32_t makeCANID(const CanardTransfer* const tr, { if (local_node_id <= CANARD_NODE_ID_MAX) { - out = (int32_t) makeMessageSessionSpecifier(tr->port_id, local_node_id); + out = (int32_t) txMakeMessageSessionSpecifier(tr->port_id, local_node_id); CANARD_ASSERT(out >= 0); } else if (tr->payload_size <= presentation_layer_mtu) @@ -185,7 +184,7 @@ CANARD_INTERNAL int32_t makeCANID(const CanardTransfer* const tr, CANARD_ASSERT((tr->payload != NULL) || (tr->payload_size == 0U)); const CanardNodeID c = (CanardNodeID)(crcAdd(CRC_INITIAL, tr->payload_size, tr->payload) & CANARD_NODE_ID_MAX); - const uint32_t spec = makeMessageSessionSpecifier(tr->port_id, c) | FLAG_ANONYMOUS_MESSAGE; + const uint32_t spec = txMakeMessageSessionSpecifier(tr->port_id, c) | FLAG_ANONYMOUS_MESSAGE; CANARD_ASSERT(spec <= CAN_EXT_ID_MASK); out = (int32_t) spec; } @@ -199,10 +198,10 @@ CANARD_INTERNAL int32_t makeCANID(const CanardTransfer* const tr, { if (local_node_id <= CANARD_NODE_ID_MAX) { - out = (int32_t) makeServiceSessionSpecifier(tr->port_id, - tr->transfer_kind == CanardTransferKindRequest, - local_node_id, - tr->remote_node_id); + out = (int32_t) txMakeServiceSessionSpecifier(tr->port_id, + tr->transfer_kind == CanardTransferKindRequest, + local_node_id, + tr->remote_node_id); CANARD_ASSERT(out >= 0); } else @@ -231,14 +230,14 @@ CANARD_INTERNAL int32_t makeCANID(const CanardTransfer* const tr, return out; } -CANARD_INTERNAL uint8_t makeTailByte(const bool start_of_transfer, - const bool end_of_transfer, - const bool toggle, - const CanardTransferID transfer_id); -CANARD_INTERNAL uint8_t makeTailByte(const bool start_of_transfer, - const bool end_of_transfer, - const bool toggle, - const CanardTransferID transfer_id) +CANARD_INTERNAL uint8_t txMakeTailByte(const bool start_of_transfer, + const bool end_of_transfer, + const bool toggle, + const CanardTransferID transfer_id); +CANARD_INTERNAL uint8_t txMakeTailByte(const bool start_of_transfer, + const bool end_of_transfer, + const bool toggle, + const CanardTransferID transfer_id) { CANARD_ASSERT(start_of_transfer ? toggle : true); return (uint8_t)((start_of_transfer ? TAIL_START_OF_TRANSFER : 0U) | (end_of_transfer ? TAIL_END_OF_TRANSFER : 0U) | @@ -246,8 +245,8 @@ CANARD_INTERNAL uint8_t makeTailByte(const bool start_of_transfer, } /// Takes a frame payload size, returns a new size that is >=x and is rounded up to the nearest valid DLC. -CANARD_INTERNAL size_t roundFramePayloadSizeUp(const size_t x); -CANARD_INTERNAL size_t roundFramePayloadSizeUp(const size_t x) +CANARD_INTERNAL size_t txRoundFramePayloadSizeUp(const size_t x); +CANARD_INTERNAL size_t txRoundFramePayloadSizeUp(const size_t x) { CANARD_ASSERT(x < (sizeof(CanardCANLengthToDLC) / sizeof(CanardCANLengthToDLC[0]))); // Suppressing a false-positive out-of-bounds access error from Sonar. Its control flow analyser is misbehaving. @@ -256,11 +255,11 @@ CANARD_INTERNAL size_t roundFramePayloadSizeUp(const size_t x) return CanardCANDLCToLength[y]; } -CANARD_INTERNAL CanardInternalTxQueueItem* allocateTxQueueItem(CanardInstance* const ins, +CANARD_INTERNAL CanardInternalTxQueueItem* txAllocateQueueItem(CanardInstance* const ins, const uint32_t id, const CanardMicrosecond deadline_usec, const size_t payload_size); -CANARD_INTERNAL CanardInternalTxQueueItem* allocateTxQueueItem(CanardInstance* const ins, +CANARD_INTERNAL CanardInternalTxQueueItem* txAllocateQueueItem(CanardInstance* const ins, const uint32_t id, const CanardMicrosecond deadline_usec, const size_t payload_size) @@ -281,8 +280,8 @@ CANARD_INTERNAL CanardInternalTxQueueItem* allocateTxQueueItem(CanardInstance* c /// Returns the element after which new elements with the specified CAN ID should be inserted. /// Returns NULL if the element shall be inserted in the beginning of the list (i.e., no prior elements). -CANARD_INTERNAL CanardInternalTxQueueItem* findTxQueueSupremum(const CanardInstance* const ins, const uint32_t can_id); -CANARD_INTERNAL CanardInternalTxQueueItem* findTxQueueSupremum(const CanardInstance* const ins, const uint32_t can_id) +CANARD_INTERNAL CanardInternalTxQueueItem* txFindQueueSupremum(const CanardInstance* const ins, const uint32_t can_id); +CANARD_INTERNAL CanardInternalTxQueueItem* txFindQueueSupremum(const CanardInstance* const ins, const uint32_t can_id) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(can_id <= CAN_EXT_ID_MASK); @@ -304,29 +303,29 @@ CANARD_INTERNAL CanardInternalTxQueueItem* findTxQueueSupremum(const CanardInsta } /// Returns the number of frames enqueued or error (i.e., =1 or <0). -CANARD_INTERNAL int32_t pushSingleFrameTransfer(CanardInstance* const ins, - const CanardMicrosecond deadline_usec, - const uint32_t can_id, - const CanardTransferID transfer_id, - const size_t payload_size, - const void* const payload); -CANARD_INTERNAL int32_t pushSingleFrameTransfer(CanardInstance* const ins, - const CanardMicrosecond deadline_usec, - const uint32_t can_id, - const CanardTransferID transfer_id, - const size_t payload_size, - const void* const payload) +CANARD_INTERNAL int32_t txPushSingleFrame(CanardInstance* const ins, + const CanardMicrosecond deadline_usec, + const uint32_t can_id, + const CanardTransferID transfer_id, + const size_t payload_size, + const void* const payload); +CANARD_INTERNAL int32_t txPushSingleFrame(CanardInstance* const ins, + const CanardMicrosecond deadline_usec, + const uint32_t can_id, + const CanardTransferID transfer_id, + const size_t payload_size, + const void* const payload) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT((payload != NULL) || (payload_size == 0)); - const size_t frame_payload_size = roundFramePayloadSizeUp(payload_size + 1U); + const size_t frame_payload_size = txRoundFramePayloadSizeUp(payload_size + 1U); CANARD_ASSERT(frame_payload_size > payload_size); const size_t padding_size = frame_payload_size - payload_size - 1U; CANARD_ASSERT((padding_size + payload_size + 1U) == frame_payload_size); int32_t out = 0; - CanardInternalTxQueueItem* const tqi = allocateTxQueueItem(ins, can_id, deadline_usec, frame_payload_size); + CanardInternalTxQueueItem* const tqi = txAllocateQueueItem(ins, can_id, deadline_usec, frame_payload_size); if (tqi != NULL) { if (payload_size > 0U) // The check is needed to avoid calling memcpy() with a NULL pointer, it's an UB. @@ -341,8 +340,8 @@ CANARD_INTERNAL int32_t pushSingleFrameTransfer(CanardInstance* const ins, // We ignore this recommendation because it is not available in C99. (void) memset(&tqi->payload[payload_size], PADDING_BYTE_VALUE, padding_size); // NOLINT - tqi->payload[frame_payload_size - 1U] = makeTailByte(true, true, true, transfer_id); - CanardInternalTxQueueItem* const sup = findTxQueueSupremum(ins, can_id); + tqi->payload[frame_payload_size - 1U] = txMakeTailByte(true, true, true, transfer_id); + CanardInternalTxQueueItem* const sup = txFindQueueSupremum(ins, can_id); if (sup != NULL) { tqi->next = sup->next; @@ -364,20 +363,20 @@ CANARD_INTERNAL int32_t pushSingleFrameTransfer(CanardInstance* const ins, } /// Returns the number of frames enqueued or error. -CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, - const size_t presentation_layer_mtu, - const CanardMicrosecond deadline_usec, - const uint32_t can_id, - const CanardTransferID transfer_id, - const size_t payload_size, - const void* const payload); -CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, - const size_t presentation_layer_mtu, - const CanardMicrosecond deadline_usec, - const uint32_t can_id, - const CanardTransferID transfer_id, - const size_t payload_size, - const void* const payload) +CANARD_INTERNAL int32_t txPushMultiFrame(CanardInstance* const ins, + const size_t presentation_layer_mtu, + const CanardMicrosecond deadline_usec, + const uint32_t can_id, + const CanardTransferID transfer_id, + const size_t payload_size, + const void* const payload); +CANARD_INTERNAL int32_t txPushMultiFrame(CanardInstance* const ins, + const size_t presentation_layer_mtu, + const CanardMicrosecond deadline_usec, + const uint32_t can_id, + const CanardTransferID transfer_id, + const size_t payload_size, + const void* const payload) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(presentation_layer_mtu > 0U); @@ -401,10 +400,11 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, ++out; const size_t frame_payload_size_with_tail = ((payload_size_with_crc - offset) < presentation_layer_mtu) - ? roundFramePayloadSizeUp((payload_size_with_crc - offset) + 1U) // Add padding only in the last frame. + ? txRoundFramePayloadSizeUp((payload_size_with_crc - offset) + + 1U) // Add padding only in the last frame. : (presentation_layer_mtu + 1U); CanardInternalTxQueueItem* const tqi = - allocateTxQueueItem(ins, can_id, deadline_usec, frame_payload_size_with_tail); + txAllocateQueueItem(ins, can_id, deadline_usec, frame_payload_size_with_tail); if (NULL == head) { head = tqi; @@ -466,7 +466,7 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, // Finalize the frame. CANARD_ASSERT((frame_offset + 1U) == tail->payload_size); tail->payload[frame_offset] = - makeTailByte(start_of_transfer, offset >= payload_size_with_crc, toggle, transfer_id); + txMakeTailByte(start_of_transfer, offset >= payload_size_with_crc, toggle, transfer_id); start_of_transfer = false; toggle = !toggle; } @@ -475,7 +475,7 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, { CANARD_ASSERT(head->next != NULL); // This is not a single-frame transfer so at least two frames shall exist. CANARD_ASSERT(tail->next == NULL); // The list shall be properly terminated. - CanardInternalTxQueueItem* const sup = findTxQueueSupremum(ins, can_id); + CanardInternalTxQueueItem* const sup = txFindQueueSupremum(ins, can_id); if (NULL == sup) // Once the insertion point is located, we insert the entire frame sequence in constant time. { tail->next = ins->_tx_queue; @@ -502,9 +502,9 @@ CANARD_INTERNAL int32_t pushMultiFrameTransfer(CanardInstance* const ins, return out; } -// ---------------------------------------- RECEPTION ---------------------------------------- +// --------------------------------------------- RECEPTION --------------------------------------------- -#define SESSIONS_PER_SUBSCRIPTION (CANARD_NODE_ID_MAX + 1U) +#define RX_SESSIONS_PER_SUBSCRIPTION (CANARD_NODE_ID_MAX + 1U) /// The memory requirement model provided in the documentation assumes that the maximum size of this structure never /// exceeds 32 bytes on any conventional platform. @@ -540,11 +540,11 @@ typedef struct size_t payload_size; const uint8_t* payload; -} FrameModel; +} RxFrameModel; /// Returns truth if the frame is valid and parsed successfully. False if the frame is not a valid UAVCAN/CAN frame. -CANARD_INTERNAL bool tryParseFrame(const CanardFrame* const frame, FrameModel* const out_result); -CANARD_INTERNAL bool tryParseFrame(const CanardFrame* const frame, FrameModel* const out_result) +CANARD_INTERNAL bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel* const out_result); +CANARD_INTERNAL bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel* const out_result) { CANARD_ASSERT(frame != NULL); CANARD_ASSERT(frame->extended_can_id <= CAN_EXT_ID_MASK); @@ -600,8 +600,8 @@ CANARD_INTERNAL bool tryParseFrame(const CanardFrame* const frame, FrameModel* c return valid; } -CANARD_INTERNAL void initRxTransferFromFrame(const FrameModel* const frame, CanardTransfer* const out_transfer); -CANARD_INTERNAL void initRxTransferFromFrame(const FrameModel* const frame, CanardTransfer* const out_transfer) +CANARD_INTERNAL void rxInitTransferFromFrame(const RxFrameModel* const frame, CanardTransfer* const out_transfer); +CANARD_INTERNAL void rxInitTransferFromFrame(const RxFrameModel* const frame, CanardTransfer* const out_transfer) { CANARD_ASSERT(frame != NULL); CANARD_ASSERT(frame->payload != NULL); @@ -617,8 +617,8 @@ CANARD_INTERNAL void initRxTransferFromFrame(const FrameModel* const frame, Cana } /// The implementation is borrowed from the Specification. -CANARD_INTERNAL uint8_t computeTransferIDDifference(const uint8_t a, const uint8_t b); -CANARD_INTERNAL uint8_t computeTransferIDDifference(const uint8_t a, const uint8_t b) +CANARD_INTERNAL uint8_t rxComputeTransferIDDifference(const uint8_t a, const uint8_t b); +CANARD_INTERNAL uint8_t rxComputeTransferIDDifference(const uint8_t a, const uint8_t b) { int16_t diff = (int16_t)(((int16_t) a) - ((int16_t) b)); if (diff < 0) @@ -629,16 +629,16 @@ CANARD_INTERNAL uint8_t computeTransferIDDifference(const uint8_t a, const uint8 return (uint8_t) diff; } -CANARD_INTERNAL int8_t rxWritePayload(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const size_t payload_size_max, - const size_t payload_size, - const void* const payload); -CANARD_INTERNAL int8_t rxWritePayload(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const size_t payload_size_max, - const size_t payload_size, - const void* const payload) +CANARD_INTERNAL int8_t rxSessionWritePayload(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const size_t payload_size_max, + const size_t payload_size, + const void* const payload); +CANARD_INTERNAL int8_t rxSessionWritePayload(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const size_t payload_size_max, + const size_t payload_size, + const void* const payload) { // Allocate the payload lazily, as late as possible. if ((NULL == rxs->payload) && (payload_size_max > 0U)) @@ -676,8 +676,8 @@ CANARD_INTERNAL int8_t rxWritePayload(CanardInstance* const ins, return out; } -CANARD_INTERNAL void rxRestart(CanardInstance* const ins, CanardInternalRxSession* const rxs); -CANARD_INTERNAL void rxRestart(CanardInstance* const ins, CanardInternalRxSession* const rxs) +CANARD_INTERNAL void rxSessionRestart(CanardInstance* const ins, CanardInternalRxSession* const rxs); +CANARD_INTERNAL void rxSessionRestart(CanardInstance* const ins, CanardInternalRxSession* const rxs) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(rxs != NULL); @@ -689,16 +689,16 @@ CANARD_INTERNAL void rxRestart(CanardInstance* const ins, CanardInternalRxSessio (CanardTransferID)(TAIL_TOGGLE | ((rxs->toggle_and_transfer_id + 1U) & CANARD_TRANSFER_ID_MAX)); } -CANARD_INTERNAL int8_t rxAcceptFrame(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const FrameModel* const frame, - const size_t payload_size_max, - CanardTransfer* const out_transfer); -CANARD_INTERNAL int8_t rxAcceptFrame(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const FrameModel* const frame, - const size_t payload_size_max, - CanardTransfer* const out_transfer) +CANARD_INTERNAL int8_t rxSessionAcceptFrame(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const RxFrameModel* const frame, + const size_t payload_size_max, + CanardTransfer* const out_transfer); +CANARD_INTERNAL int8_t rxSessionAcceptFrame(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const RxFrameModel* const frame, + const size_t payload_size_max, + CanardTransfer* const out_transfer) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(rxs != NULL); @@ -726,18 +726,18 @@ CANARD_INTERNAL int8_t rxAcceptFrame(CanardInstance* const ins, } } - int8_t out = rxWritePayload(ins, rxs, payload_size_max, payload_size, frame->payload); + int8_t out = rxSessionWritePayload(ins, rxs, payload_size_max, payload_size, frame->payload); if (out < 0) { CANARD_ASSERT(-CANARD_ERROR_OUT_OF_MEMORY == out); - rxRestart(ins, rxs); // Out-of-memory. + rxSessionRestart(ins, rxs); // Out-of-memory. } else if (frame->end_of_transfer) { CANARD_ASSERT(0 == out); if (single_frame || (CRC_RESIDUE == rxs->calculated_crc)) { - initRxTransferFromFrame(frame, out_transfer); + rxInitTransferFromFrame(frame, out_transfer); out_transfer->timestamp_usec = rxs->transfer_timestamp_usec; out_transfer->payload_size = rxs->payload_size; out_transfer->payload = rxs->payload; @@ -746,7 +746,7 @@ CANARD_INTERNAL int8_t rxAcceptFrame(CanardInstance* const ins, out = 1; // One transfer received, notify the application. } - rxRestart(ins, rxs); // Successful completion. + rxSessionRestart(ins, rxs); // Successful completion. } else { @@ -763,20 +763,20 @@ CANARD_INTERNAL int8_t rxAcceptFrame(CanardInstance* const ins, /// are given and the particular algorithms are left to be implementation-defined. Such abstract approach is much /// advantageous because it allows implementers to choose whatever solution works best for the specific application /// at hand, while wire compatibility is still guaranteed by the high-level requirements given in the specification. -CANARD_INTERNAL int8_t rxUpdate(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const FrameModel* const frame, - const uint8_t redundant_transport_index, - const CanardMicrosecond transfer_id_timeout_usec, - const size_t payload_size_max, - CanardTransfer* const out_transfer); -CANARD_INTERNAL int8_t rxUpdate(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const FrameModel* const frame, - const uint8_t redundant_transport_index, - const CanardMicrosecond transfer_id_timeout_usec, - const size_t payload_size_max, - CanardTransfer* const out_transfer) +CANARD_INTERNAL int8_t rxSessionUpdate(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const RxFrameModel* const frame, + const uint8_t redundant_transport_index, + const CanardMicrosecond transfer_id_timeout_usec, + const size_t payload_size_max, + CanardTransfer* const out_transfer); +CANARD_INTERNAL int8_t rxSessionUpdate(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const RxFrameModel* const frame, + const uint8_t redundant_transport_index, + const CanardMicrosecond transfer_id_timeout_usec, + const size_t payload_size_max, + CanardTransfer* const out_transfer) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(rxs != NULL); @@ -787,7 +787,7 @@ CANARD_INTERNAL int8_t rxUpdate(CanardInstance* const ins, ((frame->timestamp_usec - rxs->transfer_timestamp_usec) > transfer_id_timeout_usec); const bool not_previous_tid = - computeTransferIDDifference(rxs->toggle_and_transfer_id & CANARD_TRANSFER_ID_MAX, frame->transfer_id) > 1; + rxComputeTransferIDDifference(rxs->toggle_and_transfer_id & CANARD_TRANSFER_ID_MAX, frame->transfer_id) > 1; const bool need_restart = tid_timed_out || (frame->start_of_transfer && not_previous_tid); @@ -800,7 +800,7 @@ CANARD_INTERNAL int8_t rxUpdate(CanardInstance* const ins, int8_t out = 0; if (need_restart && (!frame->start_of_transfer)) { - rxRestart(ins, rxs); // SOT-miss, no point going further. + rxSessionRestart(ins, rxs); // SOT-miss, no point going further. } else { @@ -809,22 +809,22 @@ CANARD_INTERNAL int8_t rxUpdate(CanardInstance* const ins, const bool correct_tid = (frame->transfer_id == (rxs->toggle_and_transfer_id & CANARD_TRANSFER_ID_MAX)); if (correct_transport && correct_toggle && correct_tid) { - out = rxAcceptFrame(ins, rxs, frame, payload_size_max, out_transfer); + out = rxSessionAcceptFrame(ins, rxs, frame, payload_size_max, out_transfer); } } return out; } -CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, - CanardRxSubscription* const subscription, - const FrameModel* const frame, - const uint8_t redundant_transport_index, - CanardTransfer* const out_transfer); -CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, - CanardRxSubscription* const subscription, - const FrameModel* const frame, - const uint8_t redundant_transport_index, - CanardTransfer* const out_transfer) +CANARD_INTERNAL int8_t rxAcceptFrame(CanardInstance* const ins, + CanardRxSubscription* const subscription, + const RxFrameModel* const frame, + const uint8_t redundant_transport_index, + CanardTransfer* const out_transfer); +CANARD_INTERNAL int8_t rxAcceptFrame(CanardInstance* const ins, + CanardRxSubscription* const subscription, + const RxFrameModel* const frame, + const uint8_t redundant_transport_index, + CanardTransfer* const out_transfer) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(subscription != NULL); @@ -863,26 +863,26 @@ CANARD_INTERNAL int8_t acceptFrame(CanardInstance* const ins, if (subscription->_sessions[frame->source_node_id] != NULL) { CANARD_ASSERT(out == 0); - out = rxUpdate(ins, - subscription->_sessions[frame->source_node_id], - frame, - redundant_transport_index, - subscription->_transfer_id_timeout_usec, - subscription->_payload_size_max, - out_transfer); + out = rxSessionUpdate(ins, + subscription->_sessions[frame->source_node_id], + frame, + redundant_transport_index, + subscription->_transfer_id_timeout_usec, + subscription->_payload_size_max, + out_transfer); } } else { CANARD_ASSERT(frame->source_node_id == CANARD_NODE_ID_UNSET); // Anonymous transfers are stateless. No need to update the state machine, just blindly accept it. - initRxTransferFromFrame(frame, out_transfer); + rxInitTransferFromFrame(frame, out_transfer); out = 1; } return out; } -// ---------------------------------------- PUBLIC API ---------------------------------------- +// --------------------------------------------- PUBLIC API --------------------------------------------- const uint8_t CanardCANDLCToLength[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64}; const uint8_t CanardCANLengthToDLC[65] = { @@ -917,28 +917,28 @@ int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const tran int32_t out = -CANARD_ERROR_INVALID_ARGUMENT; if ((ins != NULL) && (transfer != NULL) && ((transfer->payload != NULL) || (0U == transfer->payload_size))) { - const size_t pl_mtu = getPresentationLayerMTU(ins); - const int32_t maybe_can_id = makeCANID(transfer, ins->node_id, pl_mtu); + const size_t pl_mtu = txGetPresentationLayerMTU(ins); + const int32_t maybe_can_id = txMakeCANID(transfer, ins->node_id, pl_mtu); if (maybe_can_id >= 0) { if (transfer->payload_size <= pl_mtu) { - out = pushSingleFrameTransfer(ins, - transfer->timestamp_usec, - (uint32_t) maybe_can_id, - transfer->transfer_id, - transfer->payload_size, - transfer->payload); + out = txPushSingleFrame(ins, + transfer->timestamp_usec, + (uint32_t) maybe_can_id, + transfer->transfer_id, + transfer->payload_size, + transfer->payload); } else { - out = pushMultiFrameTransfer(ins, - pl_mtu, - transfer->timestamp_usec, - (uint32_t) maybe_can_id, - transfer->transfer_id, - transfer->payload_size, - transfer->payload); + out = txPushMultiFrame(ins, + pl_mtu, + transfer->timestamp_usec, + (uint32_t) maybe_can_id, + transfer->transfer_id, + transfer->payload_size, + transfer->payload); } } else @@ -990,8 +990,8 @@ int8_t canardRxAccept(CanardInstance* const ins, if ((ins != NULL) && (out_transfer != NULL) && (frame != NULL) && (frame->extended_can_id <= CAN_EXT_ID_MASK) && ((frame->payload != NULL) || (0 == frame->payload_size))) { - FrameModel model = {0}; - if (tryParseFrame(frame, &model)) + RxFrameModel model = {0}; + if (rxTryParseFrame(frame, &model)) { if ((CANARD_NODE_ID_UNSET == model.destination_node_id) || (ins->node_id == model.destination_node_id)) { @@ -1008,7 +1008,7 @@ int8_t canardRxAccept(CanardInstance* const ins, if (sub != NULL) { CANARD_ASSERT(sub->_port_id == model.port_id); - out = acceptFrame(ins, sub, &model, redundant_transport_index, out_transfer); + out = rxAcceptFrame(ins, sub, &model, redundant_transport_index, out_transfer); } else { @@ -1046,7 +1046,7 @@ int8_t canardRxSubscribe(CanardInstance* const ins, out = canardRxUnsubscribe(ins, transfer_kind, port_id); if (out >= 0) { - for (size_t i = 0; i < SESSIONS_PER_SUBSCRIPTION; i++) + for (size_t i = 0; i < RX_SESSIONS_PER_SUBSCRIPTION; i++) { // The sessions will be created ad-hoc. Normally, for a low-jitter deterministic system, // we could have pre-allocated sessions here, but that requires too much memory to be feasible. @@ -1094,7 +1094,7 @@ int8_t canardRxUnsubscribe(CanardInstance* const ins, ins->_rx_subscriptions[tk] = sub->_next; } - for (size_t i = 0; i < SESSIONS_PER_SUBSCRIPTION; i++) + for (size_t i = 0; i < RX_SESSIONS_PER_SUBSCRIPTION; i++) { ins->memory_free(ins, (sub->_sessions[i] != NULL) ? sub->_sessions[i]->payload : NULL); ins->memory_free(ins, sub->_sessions[i]); diff --git a/libcanard/canard.h b/libcanard/canard.h index 4ec582d6..b59fb3b1 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -87,7 +87,7 @@ typedef struct { /// For RX frames: reception timestamp. /// For TX frames: transmission deadline. - /// The time system may be arbitrary as long as the clock is monotonic (steady). Zero is not a valid timestamp. + /// The time system may be arbitrary as long as the clock is monotonic (steady). CanardMicrosecond timestamp_usec; /// 29-bit extended ID. The bits above 29-th are zero/ignored. @@ -108,7 +108,7 @@ typedef struct { /// For RX transfers: reception timestamp. /// For TX transfers: transmission deadline. - /// The time system may be arbitrary as long as the clock is monotonic (steady). Zero is not a valid timestamp. + /// The time system may be arbitrary as long as the clock is monotonic (steady). CanardMicrosecond timestamp_usec; /// Per the Specification, all frames belonging to a given transfer shall share the same priority level. diff --git a/tests/internals.hpp b/tests/internals.hpp index 0924322f..8c15efd3 100644 --- a/tests/internals.hpp +++ b/tests/internals.hpp @@ -49,25 +49,25 @@ extern "C" { auto crcAdd(const std::uint16_t crc, const std::size_t size, const void* const bytes) -> std::uint16_t; -auto makeMessageSessionSpecifier(const std::uint16_t subject_id, const std::uint8_t src_node_id) -> std::uint32_t; -auto makeServiceSessionSpecifier(const std::uint16_t service_id, - const bool request_not_response, - const std::uint8_t src_node_id, - const std::uint8_t dst_node_id) -> std::uint32_t; +auto txMakeMessageSessionSpecifier(const std::uint16_t subject_id, const std::uint8_t src_node_id) -> std::uint32_t; +auto txMakeServiceSessionSpecifier(const std::uint16_t service_id, + const bool request_not_response, + const std::uint8_t src_node_id, + const std::uint8_t dst_node_id) -> std::uint32_t; -auto getPresentationLayerMTU(const CanardInstance* const ins) -> std::size_t; +auto txGetPresentationLayerMTU(const CanardInstance* const ins) -> std::size_t; -auto makeCANID(const CanardTransfer* const transfer, - const std::uint8_t local_node_id, - const std::size_t presentation_layer_mtu) -> std::int32_t; +auto txMakeCANID(const CanardTransfer* const transfer, + const std::uint8_t local_node_id, + const std::size_t presentation_layer_mtu) -> std::int32_t; -auto makeTailByte(const bool start_of_transfer, - const bool end_of_transfer, - const bool toggle, - const std::uint8_t transfer_id) -> std::uint8_t; +auto txMakeTailByte(const bool start_of_transfer, + const bool end_of_transfer, + const bool toggle, + const std::uint8_t transfer_id) -> std::uint8_t; -auto roundFramePayloadSizeUp(const std::size_t x) -> std::size_t; +auto txRoundFramePayloadSizeUp(const std::size_t x) -> std::size_t; -auto findTxQueueSupremum(const CanardInstance* const ins, const std::uint32_t can_id) -> TxQueueItem*; +auto txFindQueueSupremum(const CanardInstance* const ins, const std::uint32_t can_id) -> TxQueueItem*; } } // namespace internals diff --git a/tests/test_internals.cpp b/tests/test_internals.cpp index 824479a0..440ffd7e 100644 --- a/tests/test_internals.cpp +++ b/tests/test_internals.cpp @@ -20,30 +20,30 @@ TEST_CASE("TransferCRC") TEST_CASE("SessionSpecifier") { REQUIRE(0b000'00'0011001100110011'0'1010101 == - internals::makeMessageSessionSpecifier(0b0011001100110011, 0b1010101)); + internals::txMakeMessageSessionSpecifier(0b0011001100110011, 0b1010101)); REQUIRE(0b000'11'0100110011'0101010'1010101 == - internals::makeServiceSessionSpecifier(0b0100110011, true, 0b1010101, 0b0101010)); + internals::txMakeServiceSessionSpecifier(0b0100110011, true, 0b1010101, 0b0101010)); REQUIRE(0b000'10'0100110011'1010101'0101010 == - internals::makeServiceSessionSpecifier(0b0100110011, false, 0b0101010, 0b1010101)); + internals::txMakeServiceSessionSpecifier(0b0100110011, false, 0b0101010, 0b1010101)); } -TEST_CASE("getPresentationLayerMTU") +TEST_CASE("txGetPresentationLayerMTU") { auto ins = canardInit(&helpers::dummy_allocator::allocate, &helpers::dummy_allocator::free); - REQUIRE(63 == internals::getPresentationLayerMTU(&ins)); // This is the default. + REQUIRE(63 == internals::txGetPresentationLayerMTU(&ins)); // This is the default. ins.mtu_bytes = 0; - REQUIRE(7 == internals::getPresentationLayerMTU(&ins)); + REQUIRE(7 == internals::txGetPresentationLayerMTU(&ins)); ins.mtu_bytes = 255; - REQUIRE(63 == internals::getPresentationLayerMTU(&ins)); + REQUIRE(63 == internals::txGetPresentationLayerMTU(&ins)); ins.mtu_bytes = 32; - REQUIRE(31 == internals::getPresentationLayerMTU(&ins)); + REQUIRE(31 == internals::txGetPresentationLayerMTU(&ins)); ins.mtu_bytes = 30; // Round up. - REQUIRE(31 == internals::getPresentationLayerMTU(&ins)); + REQUIRE(31 == internals::txGetPresentationLayerMTU(&ins)); } -TEST_CASE("makeCANID") +TEST_CASE("txMakeCANID") { - using internals::makeCANID; + using internals::txMakeCANID; CanardTransfer transfer{}; std::vector transfer_payload; @@ -73,124 +73,124 @@ TEST_CASE("makeCANID") // MESSAGE TRANSFERS REQUIRE(0b000'00'0011001100110011'0'1010101 == // Regular message. - makeCANID(mk_transfer(CanardPriorityExceptional, - CanardTransferKindMessage, - 0b0011001100110011, - CANARD_NODE_ID_UNSET), - 0b1010101, - 7U)); + txMakeCANID(mk_transfer(CanardPriorityExceptional, + CanardTransferKindMessage, + 0b0011001100110011, + CANARD_NODE_ID_UNSET), + 0b1010101, + 7U)); REQUIRE(0b111'00'0011001100110011'0'1010101 == // Regular message. - makeCANID(mk_transfer(CanardPriorityOptional, - CanardTransferKindMessage, - 0b0011001100110011, - CANARD_NODE_ID_UNSET), - 0b1010101, - 7U)); + txMakeCANID(mk_transfer(CanardPriorityOptional, + CanardTransferKindMessage, + 0b0011001100110011, + CANARD_NODE_ID_UNSET), + 0b1010101, + 7U)); REQUIRE((0b010'01'0011001100110011'0'0000000U | (crc123 & CANARD_NODE_ID_MAX)) == // Anonymous message. - makeCANID(mk_transfer(CanardPriorityFast, - CanardTransferKindMessage, - 0b0011001100110011, - CANARD_NODE_ID_UNSET, - {1, 2, 3}), - 128U, // Invalid local node-ID. - 7U)); + txMakeCANID(mk_transfer(CanardPriorityFast, + CanardTransferKindMessage, + 0b0011001100110011, + CANARD_NODE_ID_UNSET, + {1, 2, 3}), + 128U, // Invalid local node-ID. + 7U)); REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Multi-frame anonymous messages are not allowed. - makeCANID(mk_transfer(CanardPriorityImmediate, - CanardTransferKindMessage, - 0b0011001100110011, - CANARD_NODE_ID_UNSET, - {1, 2, 3, 4, 5, 6, 7, 8}), - 128U, // Invalid local node-ID is treated as anonymous/unset. - 7U)); + txMakeCANID(mk_transfer(CanardPriorityImmediate, + CanardTransferKindMessage, + 0b0011001100110011, + CANARD_NODE_ID_UNSET, + {1, 2, 3, 4, 5, 6, 7, 8}), + 128U, // Invalid local node-ID is treated as anonymous/unset. + 7U)); REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Bad remote node-ID -- unicast messages not supported. - makeCANID(mk_transfer(CanardPriorityHigh, CanardTransferKindMessage, 0b0011001100110011, 123U), 0U, 7U)); + txMakeCANID(mk_transfer(CanardPriorityHigh, CanardTransferKindMessage, 0b0011001100110011, 123U), 0U, 7U)); REQUIRE( -CANARD_ERROR_INVALID_ARGUMENT == // Bad subject-ID. - makeCANID(mk_transfer(CanardPriorityLow, CanardTransferKindMessage, 0xFFFFU, CANARD_NODE_ID_UNSET), 0U, 7U)); + txMakeCANID(mk_transfer(CanardPriorityLow, CanardTransferKindMessage, 0xFFFFU, CANARD_NODE_ID_UNSET), 0U, 7U)); REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Bad priority. - makeCANID(mk_transfer(PriorityAlias{123}.prio, - CanardTransferKindMessage, - 0b0011001100110011, - CANARD_NODE_ID_UNSET), - 0b1010101, - 7U)); + txMakeCANID(mk_transfer(PriorityAlias{123}.prio, + CanardTransferKindMessage, + 0b0011001100110011, + CANARD_NODE_ID_UNSET), + 0b1010101, + 7U)); // SERVICE TRANSFERS REQUIRE(0b000'11'0100110011'0101010'1010101 == // Request. - makeCANID(mk_transfer(CanardPriorityExceptional, CanardTransferKindRequest, 0b0100110011, 0b0101010), - 0b1010101, - 7U)); + txMakeCANID(mk_transfer(CanardPriorityExceptional, CanardTransferKindRequest, 0b0100110011, 0b0101010), + 0b1010101, + 7U)); REQUIRE(0b111'10'0100110011'0101010'1010101 == // Response. - makeCANID(mk_transfer(CanardPriorityOptional, CanardTransferKindResponse, 0b0100110011, 0b0101010), - 0b1010101, - 7U)); + txMakeCANID(mk_transfer(CanardPriorityOptional, CanardTransferKindResponse, 0b0100110011, 0b0101010), + 0b1010101, + 7U)); REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Anonymous service transfers not permitted. - makeCANID(mk_transfer(CanardPriorityExceptional, CanardTransferKindRequest, 0b0100110011, 0b0101010), - CANARD_NODE_ID_UNSET, - 7U)); + txMakeCANID(mk_transfer(CanardPriorityExceptional, CanardTransferKindRequest, 0b0100110011, 0b0101010), + CANARD_NODE_ID_UNSET, + 7U)); REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Broadcast service transfers not permitted. - makeCANID(mk_transfer(CanardPrioritySlow, CanardTransferKindResponse, 0b0100110011, CANARD_NODE_ID_UNSET), - 0b1010101, - 7U)); + txMakeCANID(mk_transfer(CanardPrioritySlow, CanardTransferKindResponse, 0b0100110011, CANARD_NODE_ID_UNSET), + 0b1010101, + 7U)); REQUIRE( -CANARD_ERROR_INVALID_ARGUMENT == // Bad service-ID. - makeCANID(mk_transfer(CanardPriorityNominal, CanardTransferKindResponse, 0xFFFFU, 0b0101010), 0b1010101, 7U)); + txMakeCANID(mk_transfer(CanardPriorityNominal, CanardTransferKindResponse, 0xFFFFU, 0b0101010), 0b1010101, 7U)); REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Bad priority. - makeCANID(mk_transfer(PriorityAlias{123}.prio, CanardTransferKindResponse, 0b0100110011, 0b0101010), - 0b1010101, - 7U)); + txMakeCANID(mk_transfer(PriorityAlias{123}.prio, CanardTransferKindResponse, 0b0100110011, 0b0101010), + 0b1010101, + 7U)); } -TEST_CASE("makeTailByte") +TEST_CASE("txMakeTailByte") { - using internals::makeTailByte; - REQUIRE(0b111'00000 == makeTailByte(true, true, true, 0U)); - REQUIRE(0b111'00000 == makeTailByte(true, true, true, 32U)); - REQUIRE(0b111'11111 == makeTailByte(true, true, true, 31U)); - REQUIRE(0b011'11111 == makeTailByte(false, true, true, 31U)); - REQUIRE(0b101'11110 == makeTailByte(true, false, true, 30U)); - REQUIRE(0b001'11101 == makeTailByte(false, false, true, 29U)); - REQUIRE(0b010'00001 == makeTailByte(false, true, false, 1U)); + using internals::txMakeTailByte; + REQUIRE(0b111'00000 == txMakeTailByte(true, true, true, 0U)); + REQUIRE(0b111'00000 == txMakeTailByte(true, true, true, 32U)); + REQUIRE(0b111'11111 == txMakeTailByte(true, true, true, 31U)); + REQUIRE(0b011'11111 == txMakeTailByte(false, true, true, 31U)); + REQUIRE(0b101'11110 == txMakeTailByte(true, false, true, 30U)); + REQUIRE(0b001'11101 == txMakeTailByte(false, false, true, 29U)); + REQUIRE(0b010'00001 == txMakeTailByte(false, true, false, 1U)); } -TEST_CASE("roundFramePayloadSizeUp") +TEST_CASE("txRoundFramePayloadSizeUp") { - using internals::roundFramePayloadSizeUp; - REQUIRE(0 == roundFramePayloadSizeUp(0)); - REQUIRE(1 == roundFramePayloadSizeUp(1)); - REQUIRE(2 == roundFramePayloadSizeUp(2)); - REQUIRE(3 == roundFramePayloadSizeUp(3)); - REQUIRE(4 == roundFramePayloadSizeUp(4)); - REQUIRE(5 == roundFramePayloadSizeUp(5)); - REQUIRE(6 == roundFramePayloadSizeUp(6)); - REQUIRE(7 == roundFramePayloadSizeUp(7)); - REQUIRE(8 == roundFramePayloadSizeUp(8)); - REQUIRE(12 == roundFramePayloadSizeUp(9)); - REQUIRE(12 == roundFramePayloadSizeUp(10)); - REQUIRE(12 == roundFramePayloadSizeUp(11)); - REQUIRE(12 == roundFramePayloadSizeUp(12)); - REQUIRE(16 == roundFramePayloadSizeUp(13)); - REQUIRE(16 == roundFramePayloadSizeUp(14)); - REQUIRE(16 == roundFramePayloadSizeUp(15)); - REQUIRE(16 == roundFramePayloadSizeUp(16)); - REQUIRE(20 == roundFramePayloadSizeUp(17)); - REQUIRE(20 == roundFramePayloadSizeUp(20)); - REQUIRE(32 == roundFramePayloadSizeUp(30)); - REQUIRE(32 == roundFramePayloadSizeUp(32)); - REQUIRE(48 == roundFramePayloadSizeUp(40)); - REQUIRE(48 == roundFramePayloadSizeUp(48)); - REQUIRE(64 == roundFramePayloadSizeUp(50)); - REQUIRE(64 == roundFramePayloadSizeUp(64)); + using internals::txRoundFramePayloadSizeUp; + REQUIRE(0 == txRoundFramePayloadSizeUp(0)); + REQUIRE(1 == txRoundFramePayloadSizeUp(1)); + REQUIRE(2 == txRoundFramePayloadSizeUp(2)); + REQUIRE(3 == txRoundFramePayloadSizeUp(3)); + REQUIRE(4 == txRoundFramePayloadSizeUp(4)); + REQUIRE(5 == txRoundFramePayloadSizeUp(5)); + REQUIRE(6 == txRoundFramePayloadSizeUp(6)); + REQUIRE(7 == txRoundFramePayloadSizeUp(7)); + REQUIRE(8 == txRoundFramePayloadSizeUp(8)); + REQUIRE(12 == txRoundFramePayloadSizeUp(9)); + REQUIRE(12 == txRoundFramePayloadSizeUp(10)); + REQUIRE(12 == txRoundFramePayloadSizeUp(11)); + REQUIRE(12 == txRoundFramePayloadSizeUp(12)); + REQUIRE(16 == txRoundFramePayloadSizeUp(13)); + REQUIRE(16 == txRoundFramePayloadSizeUp(14)); + REQUIRE(16 == txRoundFramePayloadSizeUp(15)); + REQUIRE(16 == txRoundFramePayloadSizeUp(16)); + REQUIRE(20 == txRoundFramePayloadSizeUp(17)); + REQUIRE(20 == txRoundFramePayloadSizeUp(20)); + REQUIRE(32 == txRoundFramePayloadSizeUp(30)); + REQUIRE(32 == txRoundFramePayloadSizeUp(32)); + REQUIRE(48 == txRoundFramePayloadSizeUp(40)); + REQUIRE(48 == txRoundFramePayloadSizeUp(48)); + REQUIRE(64 == txRoundFramePayloadSizeUp(50)); + REQUIRE(64 == txRoundFramePayloadSizeUp(64)); } -TEST_CASE("findTxQueueSupremum") +TEST_CASE("txFindQueueSupremum") { - using internals::findTxQueueSupremum; + using internals::txFindQueueSupremum; using TxQueueItem = internals::TxQueueItem; auto ins = canardInit(&helpers::dummy_allocator::allocate, &helpers::dummy_allocator::free); - const auto find = [&](std::uint32_t x) -> TxQueueItem* { return findTxQueueSupremum(&ins, x); }; + const auto find = [&](std::uint32_t x) -> TxQueueItem* { return txFindQueueSupremum(&ins, x); }; REQUIRE(nullptr == find(0)); REQUIRE(nullptr == find((1UL << 29U) - 1U)); From 249594da9fc519ade85f4274b83661140cf5135a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 17 Feb 2020 05:32:37 +0200 Subject: [PATCH 045/100] More structured organization of the test suite; no logical changes --- libcanard/canard.c | 276 +++++++++--------- tests/CMakeLists.txt | 4 +- tests/{internals.hpp => exposed.hpp} | 4 +- tests/helpers.hpp | 4 +- tests/test_private_crc.cpp | 17 ++ ...test_internals.cpp => test_private_tx.cpp} | 43 +-- ...st_float16.cpp => test_public_float16.cpp} | 2 +- tests/{test_tx.cpp => test_public_tx.cpp} | 4 +- tests/test_self.cpp | 2 +- 9 files changed, 180 insertions(+), 176 deletions(-) rename tests/{internals.hpp => exposed.hpp} (98%) create mode 100644 tests/test_private_crc.cpp rename tests/{test_internals.cpp => test_private_tx.cpp} (87%) rename tests/{test_float16.cpp => test_public_float16.cpp} (98%) rename tests/{test_tx.cpp => test_public_tx.cpp} (99%) diff --git a/libcanard/canard.c b/libcanard/canard.c index f82330be..1f36cb1c 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -15,10 +15,10 @@ #endif /// This macro is needed only for testing and for library development. Do not redefine this in production. -#if defined(CANARD_EXPOSE_INTERNALS) && CANARD_EXPOSE_INTERNALS -# define CANARD_INTERNAL +#if defined(CANARD_EXPOSE_PRIVATE) && CANARD_EXPOSE_PRIVATE +# define CANARD_PRIVATE #else -# define CANARD_INTERNAL static inline +# define CANARD_PRIVATE static inline #endif #if !defined(__STDC_VERSION__) || (__STDC_VERSION__ < 199901L) @@ -65,8 +65,8 @@ typedef uint16_t TransferCRC; #define CRC_RESIDUE 0x0000U #define CRC_SIZE_BYTES 2U -CANARD_INTERNAL TransferCRC crcAddByte(const TransferCRC crc, const uint8_t byte); -CANARD_INTERNAL TransferCRC crcAddByte(const TransferCRC crc, const uint8_t byte) +CANARD_PRIVATE TransferCRC crcAddByte(const TransferCRC crc, const uint8_t byte); +CANARD_PRIVATE TransferCRC crcAddByte(const TransferCRC crc, const uint8_t byte) { static const TransferCRC TopBit = 0x8000U; static const TransferCRC Poly = 0x1021U; @@ -78,8 +78,8 @@ CANARD_INTERNAL TransferCRC crcAddByte(const TransferCRC crc, const uint8_t byte return out; } -CANARD_INTERNAL TransferCRC crcAdd(const TransferCRC crc, const size_t size, const void* const data); -CANARD_INTERNAL TransferCRC crcAdd(const TransferCRC crc, const size_t size, const void* const data) +CANARD_PRIVATE TransferCRC crcAdd(const TransferCRC crc, const size_t size, const void* const data); +CANARD_PRIVATE TransferCRC crcAdd(const TransferCRC crc, const size_t size, const void* const data) { CANARD_ASSERT((data != NULL) || (size == 0U)); TransferCRC out = crc; @@ -94,31 +94,6 @@ CANARD_INTERNAL TransferCRC crcAdd(const TransferCRC crc, const size_t size, con // --------------------------------------------- TRANSMISSION --------------------------------------------- -CANARD_INTERNAL uint32_t txMakeMessageSessionSpecifier(const CanardPortID subject_id, const CanardNodeID src_node_id); -CANARD_INTERNAL uint32_t txMakeMessageSessionSpecifier(const CanardPortID subject_id, const CanardNodeID src_node_id) -{ - CANARD_ASSERT(src_node_id <= CANARD_NODE_ID_MAX); - CANARD_ASSERT(subject_id <= CANARD_SUBJECT_ID_MAX); - return src_node_id | ((uint32_t) subject_id << OFFSET_SUBJECT_ID); -} - -CANARD_INTERNAL uint32_t txMakeServiceSessionSpecifier(const CanardPortID service_id, - const bool request_not_response, - const CanardNodeID src_node_id, - const CanardNodeID dst_node_id); -CANARD_INTERNAL uint32_t txMakeServiceSessionSpecifier(const CanardPortID service_id, - const bool request_not_response, - const CanardNodeID src_node_id, - const CanardNodeID dst_node_id) -{ - CANARD_ASSERT(src_node_id <= CANARD_NODE_ID_MAX); - CANARD_ASSERT(dst_node_id <= CANARD_NODE_ID_MAX); - CANARD_ASSERT(service_id <= CANARD_SERVICE_ID_MAX); - return src_node_id | (((uint32_t) dst_node_id) << OFFSET_DST_NODE_ID) | // - (((uint32_t) service_id) << OFFSET_SERVICE_ID) | // - (request_not_response ? FLAG_REQUEST_NOT_RESPONSE : 0U) | FLAG_SERVICE_NOT_MESSAGE; -} - /// The memory requirement model provided in the documentation assumes that the maximum size of this structure never /// exceeds 32 bytes on any conventional platform. The sizeof() of this structure, per the C standard, assumes that /// the length of the flex array member is zero. @@ -140,9 +115,34 @@ typedef struct CanardInternalTxQueueItem uint8_t payload[]; // NOSONAR } CanardInternalTxQueueItem; +CANARD_PRIVATE uint32_t txMakeMessageSessionSpecifier(const CanardPortID subject_id, const CanardNodeID src_node_id); +CANARD_PRIVATE uint32_t txMakeMessageSessionSpecifier(const CanardPortID subject_id, const CanardNodeID src_node_id) +{ + CANARD_ASSERT(src_node_id <= CANARD_NODE_ID_MAX); + CANARD_ASSERT(subject_id <= CANARD_SUBJECT_ID_MAX); + return src_node_id | ((uint32_t) subject_id << OFFSET_SUBJECT_ID); +} + +CANARD_PRIVATE uint32_t txMakeServiceSessionSpecifier(const CanardPortID service_id, + const bool request_not_response, + const CanardNodeID src_node_id, + const CanardNodeID dst_node_id); +CANARD_PRIVATE uint32_t txMakeServiceSessionSpecifier(const CanardPortID service_id, + const bool request_not_response, + const CanardNodeID src_node_id, + const CanardNodeID dst_node_id) +{ + CANARD_ASSERT(src_node_id <= CANARD_NODE_ID_MAX); + CANARD_ASSERT(dst_node_id <= CANARD_NODE_ID_MAX); + CANARD_ASSERT(service_id <= CANARD_SERVICE_ID_MAX); + return src_node_id | (((uint32_t) dst_node_id) << OFFSET_DST_NODE_ID) | // + (((uint32_t) service_id) << OFFSET_SERVICE_ID) | // + (request_not_response ? FLAG_REQUEST_NOT_RESPONSE : 0U) | FLAG_SERVICE_NOT_MESSAGE; +} + /// This is the transport MTU rounded up to next full DLC minus the tail byte. -CANARD_INTERNAL size_t txGetPresentationLayerMTU(const CanardInstance* const ins); -CANARD_INTERNAL size_t txGetPresentationLayerMTU(const CanardInstance* const ins) +CANARD_PRIVATE size_t txGetPresentationLayerMTU(const CanardInstance* const ins); +CANARD_PRIVATE size_t txGetPresentationLayerMTU(const CanardInstance* const ins) { const size_t max_index = (sizeof(CanardCANLengthToDLC) / sizeof(CanardCANLengthToDLC[0])) - 1U; size_t mtu = 0U; @@ -161,12 +161,12 @@ CANARD_INTERNAL size_t txGetPresentationLayerMTU(const CanardInstance* const ins return mtu - 1U; } -CANARD_INTERNAL int32_t txMakeCANID(const CanardTransfer* const tr, - const CanardNodeID local_node_id, - const size_t presentation_layer_mtu); -CANARD_INTERNAL int32_t txMakeCANID(const CanardTransfer* const tr, - const CanardNodeID local_node_id, - const size_t presentation_layer_mtu) +CANARD_PRIVATE int32_t txMakeCANID(const CanardTransfer* const tr, + const CanardNodeID local_node_id, + const size_t presentation_layer_mtu); +CANARD_PRIVATE int32_t txMakeCANID(const CanardTransfer* const tr, + const CanardNodeID local_node_id, + const size_t presentation_layer_mtu) { CANARD_ASSERT(tr != NULL); CANARD_ASSERT(presentation_layer_mtu > 0); @@ -230,14 +230,14 @@ CANARD_INTERNAL int32_t txMakeCANID(const CanardTransfer* const tr, return out; } -CANARD_INTERNAL uint8_t txMakeTailByte(const bool start_of_transfer, - const bool end_of_transfer, - const bool toggle, - const CanardTransferID transfer_id); -CANARD_INTERNAL uint8_t txMakeTailByte(const bool start_of_transfer, - const bool end_of_transfer, - const bool toggle, - const CanardTransferID transfer_id) +CANARD_PRIVATE uint8_t txMakeTailByte(const bool start_of_transfer, + const bool end_of_transfer, + const bool toggle, + const CanardTransferID transfer_id); +CANARD_PRIVATE uint8_t txMakeTailByte(const bool start_of_transfer, + const bool end_of_transfer, + const bool toggle, + const CanardTransferID transfer_id) { CANARD_ASSERT(start_of_transfer ? toggle : true); return (uint8_t)((start_of_transfer ? TAIL_START_OF_TRANSFER : 0U) | (end_of_transfer ? TAIL_END_OF_TRANSFER : 0U) | @@ -245,8 +245,8 @@ CANARD_INTERNAL uint8_t txMakeTailByte(const bool start_of_transfer, } /// Takes a frame payload size, returns a new size that is >=x and is rounded up to the nearest valid DLC. -CANARD_INTERNAL size_t txRoundFramePayloadSizeUp(const size_t x); -CANARD_INTERNAL size_t txRoundFramePayloadSizeUp(const size_t x) +CANARD_PRIVATE size_t txRoundFramePayloadSizeUp(const size_t x); +CANARD_PRIVATE size_t txRoundFramePayloadSizeUp(const size_t x) { CANARD_ASSERT(x < (sizeof(CanardCANLengthToDLC) / sizeof(CanardCANLengthToDLC[0]))); // Suppressing a false-positive out-of-bounds access error from Sonar. Its control flow analyser is misbehaving. @@ -255,14 +255,14 @@ CANARD_INTERNAL size_t txRoundFramePayloadSizeUp(const size_t x) return CanardCANDLCToLength[y]; } -CANARD_INTERNAL CanardInternalTxQueueItem* txAllocateQueueItem(CanardInstance* const ins, - const uint32_t id, - const CanardMicrosecond deadline_usec, - const size_t payload_size); -CANARD_INTERNAL CanardInternalTxQueueItem* txAllocateQueueItem(CanardInstance* const ins, - const uint32_t id, - const CanardMicrosecond deadline_usec, - const size_t payload_size) +CANARD_PRIVATE CanardInternalTxQueueItem* txAllocateQueueItem(CanardInstance* const ins, + const uint32_t id, + const CanardMicrosecond deadline_usec, + const size_t payload_size); +CANARD_PRIVATE CanardInternalTxQueueItem* txAllocateQueueItem(CanardInstance* const ins, + const uint32_t id, + const CanardMicrosecond deadline_usec, + const size_t payload_size) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(payload_size > 0U); @@ -280,8 +280,8 @@ CANARD_INTERNAL CanardInternalTxQueueItem* txAllocateQueueItem(CanardInstance* c /// Returns the element after which new elements with the specified CAN ID should be inserted. /// Returns NULL if the element shall be inserted in the beginning of the list (i.e., no prior elements). -CANARD_INTERNAL CanardInternalTxQueueItem* txFindQueueSupremum(const CanardInstance* const ins, const uint32_t can_id); -CANARD_INTERNAL CanardInternalTxQueueItem* txFindQueueSupremum(const CanardInstance* const ins, const uint32_t can_id) +CANARD_PRIVATE CanardInternalTxQueueItem* txFindQueueSupremum(const CanardInstance* const ins, const uint32_t can_id); +CANARD_PRIVATE CanardInternalTxQueueItem* txFindQueueSupremum(const CanardInstance* const ins, const uint32_t can_id) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(can_id <= CAN_EXT_ID_MASK); @@ -303,18 +303,18 @@ CANARD_INTERNAL CanardInternalTxQueueItem* txFindQueueSupremum(const CanardInsta } /// Returns the number of frames enqueued or error (i.e., =1 or <0). -CANARD_INTERNAL int32_t txPushSingleFrame(CanardInstance* const ins, - const CanardMicrosecond deadline_usec, - const uint32_t can_id, - const CanardTransferID transfer_id, - const size_t payload_size, - const void* const payload); -CANARD_INTERNAL int32_t txPushSingleFrame(CanardInstance* const ins, - const CanardMicrosecond deadline_usec, - const uint32_t can_id, - const CanardTransferID transfer_id, - const size_t payload_size, - const void* const payload) +CANARD_PRIVATE int32_t txPushSingleFrame(CanardInstance* const ins, + const CanardMicrosecond deadline_usec, + const uint32_t can_id, + const CanardTransferID transfer_id, + const size_t payload_size, + const void* const payload); +CANARD_PRIVATE int32_t txPushSingleFrame(CanardInstance* const ins, + const CanardMicrosecond deadline_usec, + const uint32_t can_id, + const CanardTransferID transfer_id, + const size_t payload_size, + const void* const payload) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT((payload != NULL) || (payload_size == 0)); @@ -363,20 +363,20 @@ CANARD_INTERNAL int32_t txPushSingleFrame(CanardInstance* const ins, } /// Returns the number of frames enqueued or error. -CANARD_INTERNAL int32_t txPushMultiFrame(CanardInstance* const ins, - const size_t presentation_layer_mtu, - const CanardMicrosecond deadline_usec, - const uint32_t can_id, - const CanardTransferID transfer_id, - const size_t payload_size, - const void* const payload); -CANARD_INTERNAL int32_t txPushMultiFrame(CanardInstance* const ins, - const size_t presentation_layer_mtu, - const CanardMicrosecond deadline_usec, - const uint32_t can_id, - const CanardTransferID transfer_id, - const size_t payload_size, - const void* const payload) +CANARD_PRIVATE int32_t txPushMultiFrame(CanardInstance* const ins, + const size_t presentation_layer_mtu, + const CanardMicrosecond deadline_usec, + const uint32_t can_id, + const CanardTransferID transfer_id, + const size_t payload_size, + const void* const payload); +CANARD_PRIVATE int32_t txPushMultiFrame(CanardInstance* const ins, + const size_t presentation_layer_mtu, + const CanardMicrosecond deadline_usec, + const uint32_t can_id, + const CanardTransferID transfer_id, + const size_t payload_size, + const void* const payload) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(presentation_layer_mtu > 0U); @@ -543,8 +543,8 @@ typedef struct } RxFrameModel; /// Returns truth if the frame is valid and parsed successfully. False if the frame is not a valid UAVCAN/CAN frame. -CANARD_INTERNAL bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel* const out_result); -CANARD_INTERNAL bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel* const out_result) +CANARD_PRIVATE bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel* const out_result); +CANARD_PRIVATE bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel* const out_result) { CANARD_ASSERT(frame != NULL); CANARD_ASSERT(frame->extended_can_id <= CAN_EXT_ID_MASK); @@ -600,8 +600,8 @@ CANARD_INTERNAL bool rxTryParseFrame(const CanardFrame* const frame, RxFrameMode return valid; } -CANARD_INTERNAL void rxInitTransferFromFrame(const RxFrameModel* const frame, CanardTransfer* const out_transfer); -CANARD_INTERNAL void rxInitTransferFromFrame(const RxFrameModel* const frame, CanardTransfer* const out_transfer) +CANARD_PRIVATE void rxInitTransferFromFrame(const RxFrameModel* const frame, CanardTransfer* const out_transfer); +CANARD_PRIVATE void rxInitTransferFromFrame(const RxFrameModel* const frame, CanardTransfer* const out_transfer) { CANARD_ASSERT(frame != NULL); CANARD_ASSERT(frame->payload != NULL); @@ -617,8 +617,8 @@ CANARD_INTERNAL void rxInitTransferFromFrame(const RxFrameModel* const frame, Ca } /// The implementation is borrowed from the Specification. -CANARD_INTERNAL uint8_t rxComputeTransferIDDifference(const uint8_t a, const uint8_t b); -CANARD_INTERNAL uint8_t rxComputeTransferIDDifference(const uint8_t a, const uint8_t b) +CANARD_PRIVATE uint8_t rxComputeTransferIDDifference(const uint8_t a, const uint8_t b); +CANARD_PRIVATE uint8_t rxComputeTransferIDDifference(const uint8_t a, const uint8_t b) { int16_t diff = (int16_t)(((int16_t) a) - ((int16_t) b)); if (diff < 0) @@ -629,16 +629,16 @@ CANARD_INTERNAL uint8_t rxComputeTransferIDDifference(const uint8_t a, const uin return (uint8_t) diff; } -CANARD_INTERNAL int8_t rxSessionWritePayload(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const size_t payload_size_max, - const size_t payload_size, - const void* const payload); -CANARD_INTERNAL int8_t rxSessionWritePayload(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const size_t payload_size_max, - const size_t payload_size, - const void* const payload) +CANARD_PRIVATE int8_t rxSessionWritePayload(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const size_t payload_size_max, + const size_t payload_size, + const void* const payload); +CANARD_PRIVATE int8_t rxSessionWritePayload(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const size_t payload_size_max, + const size_t payload_size, + const void* const payload) { // Allocate the payload lazily, as late as possible. if ((NULL == rxs->payload) && (payload_size_max > 0U)) @@ -676,8 +676,8 @@ CANARD_INTERNAL int8_t rxSessionWritePayload(CanardInstance* const ins, return out; } -CANARD_INTERNAL void rxSessionRestart(CanardInstance* const ins, CanardInternalRxSession* const rxs); -CANARD_INTERNAL void rxSessionRestart(CanardInstance* const ins, CanardInternalRxSession* const rxs) +CANARD_PRIVATE void rxSessionRestart(CanardInstance* const ins, CanardInternalRxSession* const rxs); +CANARD_PRIVATE void rxSessionRestart(CanardInstance* const ins, CanardInternalRxSession* const rxs) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(rxs != NULL); @@ -689,16 +689,16 @@ CANARD_INTERNAL void rxSessionRestart(CanardInstance* const ins, CanardInternalR (CanardTransferID)(TAIL_TOGGLE | ((rxs->toggle_and_transfer_id + 1U) & CANARD_TRANSFER_ID_MAX)); } -CANARD_INTERNAL int8_t rxSessionAcceptFrame(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const RxFrameModel* const frame, - const size_t payload_size_max, - CanardTransfer* const out_transfer); -CANARD_INTERNAL int8_t rxSessionAcceptFrame(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const RxFrameModel* const frame, - const size_t payload_size_max, - CanardTransfer* const out_transfer) +CANARD_PRIVATE int8_t rxSessionAcceptFrame(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const RxFrameModel* const frame, + const size_t payload_size_max, + CanardTransfer* const out_transfer); +CANARD_PRIVATE int8_t rxSessionAcceptFrame(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const RxFrameModel* const frame, + const size_t payload_size_max, + CanardTransfer* const out_transfer) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(rxs != NULL); @@ -763,20 +763,20 @@ CANARD_INTERNAL int8_t rxSessionAcceptFrame(CanardInstance* const ins, /// are given and the particular algorithms are left to be implementation-defined. Such abstract approach is much /// advantageous because it allows implementers to choose whatever solution works best for the specific application /// at hand, while wire compatibility is still guaranteed by the high-level requirements given in the specification. -CANARD_INTERNAL int8_t rxSessionUpdate(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const RxFrameModel* const frame, - const uint8_t redundant_transport_index, - const CanardMicrosecond transfer_id_timeout_usec, - const size_t payload_size_max, - CanardTransfer* const out_transfer); -CANARD_INTERNAL int8_t rxSessionUpdate(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const RxFrameModel* const frame, - const uint8_t redundant_transport_index, - const CanardMicrosecond transfer_id_timeout_usec, - const size_t payload_size_max, - CanardTransfer* const out_transfer) +CANARD_PRIVATE int8_t rxSessionUpdate(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const RxFrameModel* const frame, + const uint8_t redundant_transport_index, + const CanardMicrosecond transfer_id_timeout_usec, + const size_t payload_size_max, + CanardTransfer* const out_transfer); +CANARD_PRIVATE int8_t rxSessionUpdate(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const RxFrameModel* const frame, + const uint8_t redundant_transport_index, + const CanardMicrosecond transfer_id_timeout_usec, + const size_t payload_size_max, + CanardTransfer* const out_transfer) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(rxs != NULL); @@ -815,16 +815,16 @@ CANARD_INTERNAL int8_t rxSessionUpdate(CanardInstance* const ins, return out; } -CANARD_INTERNAL int8_t rxAcceptFrame(CanardInstance* const ins, - CanardRxSubscription* const subscription, - const RxFrameModel* const frame, - const uint8_t redundant_transport_index, - CanardTransfer* const out_transfer); -CANARD_INTERNAL int8_t rxAcceptFrame(CanardInstance* const ins, - CanardRxSubscription* const subscription, - const RxFrameModel* const frame, - const uint8_t redundant_transport_index, - CanardTransfer* const out_transfer) +CANARD_PRIVATE int8_t rxAcceptFrame(CanardInstance* const ins, + CanardRxSubscription* const subscription, + const RxFrameModel* const frame, + const uint8_t redundant_transport_index, + CanardTransfer* const out_transfer); +CANARD_PRIVATE int8_t rxAcceptFrame(CanardInstance* const ins, + CanardRxSubscription* const subscription, + const RxFrameModel* const frame, + const uint8_t redundant_transport_index, + CanardTransfer* const out_transfer) { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(subscription != NULL); diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index e6ab1de1..d5556adb 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -74,6 +74,6 @@ function(gen_test_matrix name files compile_definitions) endif () endfunction() -gen_test_matrix(test_internals test_internals.cpp CANARD_EXPOSE_INTERNALS=1) +gen_test_matrix(test_private "test_private_crc.cpp;test_private_tx.cpp" CANARD_EXPOSE_PRIVATE=1) -gen_test_matrix(test_general "test_float16.cpp;test_tx.cpp;test_self.cpp" "") +gen_test_matrix(test_public "test_public_float16.cpp;test_public_tx.cpp;test_self.cpp" "") diff --git a/tests/internals.hpp b/tests/exposed.hpp similarity index 98% rename from tests/internals.hpp rename to tests/exposed.hpp index 8c15efd3..c8654907 100644 --- a/tests/internals.hpp +++ b/tests/exposed.hpp @@ -10,7 +10,7 @@ /// Definitions that are not exposed by the library but that are needed for testing. /// Please keep them in sync with the library by manually updating as necessary. -namespace internals +namespace exposed { struct TxQueueItem final { @@ -70,4 +70,4 @@ auto txRoundFramePayloadSizeUp(const std::size_t x) -> std::size_t; auto txFindQueueSupremum(const CanardInstance* const ins, const std::uint32_t can_id) -> TxQueueItem*; } -} // namespace internals +} // namespace exposed diff --git a/tests/helpers.hpp b/tests/helpers.hpp index 3fda8bd3..b2363c62 100644 --- a/tests/helpers.hpp +++ b/tests/helpers.hpp @@ -4,7 +4,7 @@ #pragma once #include "canard.h" -#include "internals.hpp" +#include "exposed.hpp" #include #include #include @@ -155,7 +155,7 @@ class Instance [[nodiscard]] auto getTxQueueRoot() const { - return reinterpret_cast(canard_._tx_queue); + return reinterpret_cast(canard_._tx_queue); } [[nodiscard]] auto getTxQueueLength() const diff --git a/tests/test_private_crc.cpp b/tests/test_private_crc.cpp new file mode 100644 index 00000000..b8bc8df8 --- /dev/null +++ b/tests/test_private_crc.cpp @@ -0,0 +1,17 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. + +#include "exposed.hpp" + +TEST_CASE("TransferCRC") +{ + using exposed::crcAdd; + std::uint16_t crc = 0xFFFFU; + + crc = crcAdd(crc, 1, "1"); + crc = crcAdd(crc, 1, "2"); + crc = crcAdd(crc, 1, "3"); + REQUIRE(0x5BCEU == crc); // Using Libuavcan as reference + crc = crcAdd(crc, 6, "456789"); + REQUIRE(0x29B1U == crc); +} diff --git a/tests/test_internals.cpp b/tests/test_private_tx.cpp similarity index 87% rename from tests/test_internals.cpp rename to tests/test_private_tx.cpp index 440ffd7e..08df4872 100644 --- a/tests/test_internals.cpp +++ b/tests/test_private_tx.cpp @@ -1,49 +1,36 @@ // This software is distributed under the terms of the MIT License. // Copyright (c) 2016-2020 UAVCAN Development Team. -#include "internals.hpp" +#include "exposed.hpp" #include "helpers.hpp" -TEST_CASE("TransferCRC") -{ - using internals::crcAdd; - std::uint16_t crc = 0xFFFFU; - - crc = crcAdd(crc, 1, "1"); - crc = crcAdd(crc, 1, "2"); - crc = crcAdd(crc, 1, "3"); - REQUIRE(0x5BCEU == crc); // Using Libuavcan as reference - crc = crcAdd(crc, 6, "456789"); - REQUIRE(0x29B1U == crc); -} - TEST_CASE("SessionSpecifier") { REQUIRE(0b000'00'0011001100110011'0'1010101 == - internals::txMakeMessageSessionSpecifier(0b0011001100110011, 0b1010101)); + exposed::txMakeMessageSessionSpecifier(0b0011001100110011, 0b1010101)); REQUIRE(0b000'11'0100110011'0101010'1010101 == - internals::txMakeServiceSessionSpecifier(0b0100110011, true, 0b1010101, 0b0101010)); + exposed::txMakeServiceSessionSpecifier(0b0100110011, true, 0b1010101, 0b0101010)); REQUIRE(0b000'10'0100110011'1010101'0101010 == - internals::txMakeServiceSessionSpecifier(0b0100110011, false, 0b0101010, 0b1010101)); + exposed::txMakeServiceSessionSpecifier(0b0100110011, false, 0b0101010, 0b1010101)); } TEST_CASE("txGetPresentationLayerMTU") { auto ins = canardInit(&helpers::dummy_allocator::allocate, &helpers::dummy_allocator::free); - REQUIRE(63 == internals::txGetPresentationLayerMTU(&ins)); // This is the default. + REQUIRE(63 == exposed::txGetPresentationLayerMTU(&ins)); // This is the default. ins.mtu_bytes = 0; - REQUIRE(7 == internals::txGetPresentationLayerMTU(&ins)); + REQUIRE(7 == exposed::txGetPresentationLayerMTU(&ins)); ins.mtu_bytes = 255; - REQUIRE(63 == internals::txGetPresentationLayerMTU(&ins)); + REQUIRE(63 == exposed::txGetPresentationLayerMTU(&ins)); ins.mtu_bytes = 32; - REQUIRE(31 == internals::txGetPresentationLayerMTU(&ins)); + REQUIRE(31 == exposed::txGetPresentationLayerMTU(&ins)); ins.mtu_bytes = 30; // Round up. - REQUIRE(31 == internals::txGetPresentationLayerMTU(&ins)); + REQUIRE(31 == exposed::txGetPresentationLayerMTU(&ins)); } TEST_CASE("txMakeCANID") { - using internals::txMakeCANID; + using exposed::txMakeCANID; CanardTransfer transfer{}; std::vector transfer_payload; @@ -63,7 +50,7 @@ TEST_CASE("txMakeCANID") return &transfer; }; - const auto crc123 = internals::crcAdd(0xFFFFU, 3, "\x01\x02\x03"); + const auto crc123 = exposed::crcAdd(0xFFFFU, 3, "\x01\x02\x03"); union PriorityAlias { @@ -143,7 +130,7 @@ TEST_CASE("txMakeCANID") TEST_CASE("txMakeTailByte") { - using internals::txMakeTailByte; + using exposed::txMakeTailByte; REQUIRE(0b111'00000 == txMakeTailByte(true, true, true, 0U)); REQUIRE(0b111'00000 == txMakeTailByte(true, true, true, 32U)); REQUIRE(0b111'11111 == txMakeTailByte(true, true, true, 31U)); @@ -155,7 +142,7 @@ TEST_CASE("txMakeTailByte") TEST_CASE("txRoundFramePayloadSizeUp") { - using internals::txRoundFramePayloadSizeUp; + using exposed::txRoundFramePayloadSizeUp; REQUIRE(0 == txRoundFramePayloadSizeUp(0)); REQUIRE(1 == txRoundFramePayloadSizeUp(1)); REQUIRE(2 == txRoundFramePayloadSizeUp(2)); @@ -185,8 +172,8 @@ TEST_CASE("txRoundFramePayloadSizeUp") TEST_CASE("txFindQueueSupremum") { - using internals::txFindQueueSupremum; - using TxQueueItem = internals::TxQueueItem; + using exposed::txFindQueueSupremum; + using TxQueueItem = exposed::TxQueueItem; auto ins = canardInit(&helpers::dummy_allocator::allocate, &helpers::dummy_allocator::free); diff --git a/tests/test_float16.cpp b/tests/test_public_float16.cpp similarity index 98% rename from tests/test_float16.cpp rename to tests/test_public_float16.cpp index 02a7c15c..fdf94064 100644 --- a/tests/test_float16.cpp +++ b/tests/test_public_float16.cpp @@ -2,7 +2,7 @@ // Copyright (c) 2016-2020 UAVCAN Development Team. #include "canard_dsdl.h" -#include "internals.hpp" +#include "exposed.hpp" #include TEST_CASE("canardDSDLFloat16Pack") diff --git a/tests/test_tx.cpp b/tests/test_public_tx.cpp similarity index 99% rename from tests/test_tx.cpp rename to tests/test_public_tx.cpp index 2b508407..7a56833c 100644 --- a/tests/test_tx.cpp +++ b/tests/test_public_tx.cpp @@ -1,13 +1,13 @@ // This software is distributed under the terms of the MIT License. // Copyright (c) 2016-2020 UAVCAN Development Team. -#include "internals.hpp" +#include "exposed.hpp" #include "helpers.hpp" #include TEST_CASE("TxBasic0") { - using internals::TxQueueItem; + using exposed::TxQueueItem; helpers::Instance ins; diff --git a/tests/test_self.cpp b/tests/test_self.cpp index 3f4712df..9237999b 100644 --- a/tests/test_self.cpp +++ b/tests/test_self.cpp @@ -1,7 +1,7 @@ // This software is distributed under the terms of the MIT License. // Copyright (c) 2016-2020 UAVCAN Development Team. -#include "internals.hpp" +#include "exposed.hpp" #include "helpers.hpp" TEST_CASE("TestAllocator") From 4ed9f7735f83384bebc0473b04364edcb8d85046 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 17 Feb 2020 06:30:42 +0200 Subject: [PATCH 046/100] Fix coverage --- .travis.yml | 2 +- libcanard/canard.c | 12 ++++++------ tests/CMakeLists.txt | 2 +- tests/test_private_rx.cpp | 7 +++++++ 4 files changed, 15 insertions(+), 8 deletions(-) create mode 100644 tests/test_private_rx.cpp diff --git a/.travis.yml b/.travis.yml index df941027..ff7d6b89 100644 --- a/.travis.yml +++ b/.travis.yml @@ -25,7 +25,7 @@ matrix: - build-wrapper-linux-x86-64 --out-dir sonar-dump make all # The build wrapper comes from Sonar Cloud. - make test # TODO: COMBINE COVERAGE FROM ALL EXECUTABLES! - - find CMakeFiles/test_general_cov.dir/ -name 'canard.c.*' -type f -print -exec mv -f {} . \; + - find CMakeFiles/test_public_cov.dir/ -name 'canard.c.*' -type f -print -exec mv -f {} . \; - gcov-9 libcanard/canard.c --object-file canard*gcda --object-file canard*gcno # RELEASE diff --git a/libcanard/canard.c b/libcanard/canard.c index 1f36cb1c..9df1c54e 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -35,15 +35,11 @@ // --------------------------------------------- COMMON CONSTANTS --------------------------------------------- -#define TAIL_START_OF_TRANSFER 128U -#define TAIL_END_OF_TRANSFER 64U -#define TAIL_TOGGLE 32U - -#define CAN_EXT_ID_MASK ((UINT32_C(1) << 29U) - 1U) - #define BITS_PER_BYTE 8U #define BYTE_MAX 0xFFU +#define CAN_EXT_ID_MASK ((UINT32_C(1) << 29U) - 1U) + #define PADDING_BYTE_VALUE 0U #define OFFSET_PRIORITY 26U @@ -57,6 +53,10 @@ #define FLAG_RESERVED_23 (UINT32_C(1) << 23U) #define FLAG_RESERVED_07 (UINT32_C(1) << 7U) +#define TAIL_START_OF_TRANSFER 128U +#define TAIL_END_OF_TRANSFER 64U +#define TAIL_TOGGLE 32U + // --------------------------------------------- TRANSFER CRC --------------------------------------------- typedef uint16_t TransferCRC; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index d5556adb..b0afe31b 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -74,6 +74,6 @@ function(gen_test_matrix name files compile_definitions) endif () endfunction() -gen_test_matrix(test_private "test_private_crc.cpp;test_private_tx.cpp" CANARD_EXPOSE_PRIVATE=1) +gen_test_matrix(test_private "test_private_crc.cpp;test_private_rx.cpp;test_private_tx.cpp" CANARD_EXPOSE_PRIVATE=1) gen_test_matrix(test_public "test_public_float16.cpp;test_public_tx.cpp;test_self.cpp" "") diff --git a/tests/test_private_rx.cpp b/tests/test_private_rx.cpp new file mode 100644 index 00000000..b194e4ee --- /dev/null +++ b/tests/test_private_rx.cpp @@ -0,0 +1,7 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. + +#include "exposed.hpp" +#include "helpers.hpp" + +TEST_CASE("rxTryParseFrame") {} From ec68f80fb4078c8d22c34d4fd956ca7bd8e9394f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 17 Feb 2020 10:43:14 +0200 Subject: [PATCH 047/100] AVR GCC compilation test --- .travis.yml | 12 ++++++++++++ libcanard/canard_dsdl.c | 14 +++++++------- 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/.travis.yml b/.travis.yml index ff7d6b89..82cb86db 100644 --- a/.travis.yml +++ b/.travis.yml @@ -75,5 +75,17 @@ matrix: - 'modified="$(git status --porcelain --untracked-files=no)"' - 'if [ -n "$modified" ]; then echo "Run make format to reformat the code properly."; exit 1; fi' + # -------------------- AVR GCC -------------------- + - language: c + addons: + apt: + packages: + - gcc-avr + - avr-libc + script: + # This is a trivial test where we only check whether it compiles at all with all warnings enabled. + # TODO: Write unit tests and run them on an emulator. + - avr-gcc libcanard/*.c -c -std=c99 -mmcu=at90can64 -Wall -Wextra -Werror -pedantic -Wno-unused-parameter + git: depth: false # Disable shallow clone because it is incompatible with SonarCloud diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index 596f3e1a..9ee70b97 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -37,17 +37,17 @@ uint16_t canardDSDLFloat16Pack(const CanardDSDLFloatNative value) { // The no-lint statements suppress the warnings about magic numbers. // The no-lint statements suppress the warning about the use of union. This is required for low-level bit access. - const uint32_t round_mask = ~(uint32_t) 0x0FFFU; // NOLINT + const uint32_t round_mask = ~(uint32_t) 0x0FFFU; // NOLINT NOSONAR const Float32Bits f32inf = {.bits = ((uint32_t) 255U) << 23U}; // NOLINT NOSONAR const Float32Bits f16inf = {.bits = ((uint32_t) 31U) << 23U}; // NOLINT NOSONAR const Float32Bits magic = {.bits = ((uint32_t) 15U) << 23U}; // NOLINT NOSONAR Float32Bits in = {.real = value}; // NOSONAR - const uint32_t sign = in.bits & (((uint32_t) 1U) << 31U); // NOLINT + const uint32_t sign = in.bits & (((uint32_t) 1U) << 31U); // NOLINT NOSONAR in.bits ^= sign; uint16_t out = 0; if (in.bits >= f32inf.bits) { - out = (in.bits > f32inf.bits) ? (uint16_t) 0x7FFFU : (uint16_t) 0x7C00U; // NOLINT + out = (in.bits > f32inf.bits) ? (uint16_t) 0x7FFFU : (uint16_t) 0x7C00U; // NOLINT NOSONAR } else { @@ -58,9 +58,9 @@ uint16_t canardDSDLFloat16Pack(const CanardDSDLFloatNative value) { in.bits = f16inf.bits; } - out = (uint16_t)(in.bits >> 13U); // NOLINT + out = (uint16_t)(in.bits >> 13U); // NOLINT NOSONAR } - out |= (uint16_t)(sign >> 16U); // NOLINT + out |= (uint16_t)(sign >> 16U); // NOLINT NOSONAR return out; } @@ -74,9 +74,9 @@ CanardDSDLFloatNative canardDSDLFloat16Unpack(const uint16_t value) out.real *= magic.real; if (out.real >= inf_nan.real) { - out.bits |= ((uint32_t) 0xFFU) << 23U; // NOLINT + out.bits |= ((uint32_t) 0xFFU) << 23U; // NOLINT NOSONAR } - out.bits |= ((uint32_t)(value & 0x8000U)) << 16U; // NOLINT + out.bits |= ((uint32_t)(value & 0x8000U)) << 16U; // NOLINT NOSONAR return out.real; } From 573da6ee7646277a83b27b59e052bc9c685159f9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 17 Feb 2020 11:29:39 +0200 Subject: [PATCH 048/100] Improve coverage --- .travis.yml | 8 ++++---- sonar-project.properties | 1 + tests/test_private_rx.cpp | 5 ++++- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/.travis.yml b/.travis.yml index 82cb86db..c548a5d1 100644 --- a/.travis.yml +++ b/.travis.yml @@ -24,9 +24,9 @@ matrix: - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Debug -DNO_STATIC_ANALYSIS=1 - build-wrapper-linux-x86-64 --out-dir sonar-dump make all # The build wrapper comes from Sonar Cloud. - make test - # TODO: COMBINE COVERAGE FROM ALL EXECUTABLES! - - find CMakeFiles/test_public_cov.dir/ -name 'canard.c.*' -type f -print -exec mv -f {} . \; - - gcov-9 libcanard/canard.c --object-file canard*gcda --object-file canard*gcno + - find CMakeFiles/test_public_cov.dir/ -name 'canard*.c.*' -type f -print -exec mv -f {} . \; + - gcov-9 libcanard/canard.c --object-file canard.c.gcda --object-file canard.c.gcno + - gcov-9 libcanard/canard_dsdl.c --object-file canard_dsdl.c.gcda --object-file canard_dsdl.c.gcno # RELEASE - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Release -DNO_STATIC_ANALYSIS=1 @@ -85,7 +85,7 @@ matrix: script: # This is a trivial test where we only check whether it compiles at all with all warnings enabled. # TODO: Write unit tests and run them on an emulator. - - avr-gcc libcanard/*.c -c -std=c99 -mmcu=at90can64 -Wall -Wextra -Werror -pedantic -Wno-unused-parameter + - avr-gcc libcanard/*.c -c -std=c99 -mmcu=at90can64 -Wall -Wextra -Werror -pedantic -Wconversion -Wtype-limits git: depth: false # Disable shallow clone because it is incompatible with SonarCloud diff --git a/sonar-project.properties b/sonar-project.properties index fc1342bf..071f28ba 100644 --- a/sonar-project.properties +++ b/sonar-project.properties @@ -4,5 +4,6 @@ sonar.organization=uavcan sonar.sources=libcanard sonar.sourceEncoding=UTF-8 +sonar.cfamily.gcov.reportsPath=. sonar.cfamily.build-wrapper-output=sonar-dump sonar.cfamily.cache.enabled=false diff --git a/tests/test_private_rx.cpp b/tests/test_private_rx.cpp index b194e4ee..948a82ca 100644 --- a/tests/test_private_rx.cpp +++ b/tests/test_private_rx.cpp @@ -4,4 +4,7 @@ #include "exposed.hpp" #include "helpers.hpp" -TEST_CASE("rxTryParseFrame") {} +TEST_CASE("rxTryParseFrame") +{ + // +} From 80875648a4b97fd6e4f9b01eab076abf08266319 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 17 Feb 2020 15:52:36 +0200 Subject: [PATCH 049/100] RX frame parsing test --- libcanard/canard.c | 72 +++++++++++------------ libcanard/canard.h | 6 +- tests/exposed.hpp | 36 ++++++++++++ tests/test_private_rx.cpp | 120 +++++++++++++++++++++++++++++++++++++- 4 files changed, 192 insertions(+), 42 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 9df1c54e..b65074ba 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -524,77 +524,73 @@ typedef struct CanardInternalRxSession /// High-level transport frame model. typedef struct { - CanardMicrosecond timestamp_usec; - - CanardPriority priority; - + CanardMicrosecond timestamp_usec; + CanardPriority priority; CanardTransferKind transfer_kind; CanardPortID port_id; CanardNodeID source_node_id; CanardNodeID destination_node_id; - - CanardTransferID transfer_id; - bool start_of_transfer; - bool end_of_transfer; - bool toggle; - - size_t payload_size; - const uint8_t* payload; + CanardTransferID transfer_id; + bool start_of_transfer; + bool end_of_transfer; + bool toggle; + size_t payload_size; + const void* payload; } RxFrameModel; /// Returns truth if the frame is valid and parsed successfully. False if the frame is not a valid UAVCAN/CAN frame. -CANARD_PRIVATE bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel* const out_result); -CANARD_PRIVATE bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel* const out_result) +CANARD_PRIVATE bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel* const out); +CANARD_PRIVATE bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel* const out) { CANARD_ASSERT(frame != NULL); CANARD_ASSERT(frame->extended_can_id <= CAN_EXT_ID_MASK); - CANARD_ASSERT(out_result != NULL); + CANARD_ASSERT(out != NULL); bool valid = false; if (frame->payload_size > 0) { CANARD_ASSERT(frame->payload != NULL); - out_result->timestamp_usec = frame->timestamp_usec; + out->timestamp_usec = frame->timestamp_usec; // CAN ID parsing. - const uint32_t can_id = frame->extended_can_id; - out_result->priority = (CanardPriority)((can_id >> OFFSET_PRIORITY) & CANARD_PRIORITY_MAX); - out_result->source_node_id = (CanardNodeID)(can_id & CANARD_NODE_ID_MAX); + const uint32_t can_id = frame->extended_can_id; + out->priority = (CanardPriority)((can_id >> OFFSET_PRIORITY) & CANARD_PRIORITY_MAX); + out->source_node_id = (CanardNodeID)(can_id & CANARD_NODE_ID_MAX); if (0 == (can_id & FLAG_SERVICE_NOT_MESSAGE)) { - valid = (0 == (can_id & FLAG_RESERVED_23)) && (0 == (can_id & FLAG_RESERVED_07)); - out_result->transfer_kind = CanardTransferKindMessage; - out_result->port_id = (CanardPortID)((can_id >> OFFSET_SUBJECT_ID) & CANARD_SUBJECT_ID_MAX); + valid = (0 == (can_id & FLAG_RESERVED_23)) && (0 == (can_id & FLAG_RESERVED_07)); + out->transfer_kind = CanardTransferKindMessage; + out->port_id = (CanardPortID)((can_id >> OFFSET_SUBJECT_ID) & CANARD_SUBJECT_ID_MAX); if ((can_id & FLAG_ANONYMOUS_MESSAGE) != 0) { - out_result->source_node_id = CANARD_NODE_ID_UNSET; + out->source_node_id = CANARD_NODE_ID_UNSET; } - out_result->destination_node_id = CANARD_NODE_ID_UNSET; + out->destination_node_id = CANARD_NODE_ID_UNSET; } else { valid = (0 == (can_id & FLAG_RESERVED_23)); - out_result->transfer_kind = + out->transfer_kind = ((can_id & FLAG_REQUEST_NOT_RESPONSE) != 0) ? CanardTransferKindRequest : CanardTransferKindResponse; - out_result->port_id = (CanardPortID)((can_id >> OFFSET_SERVICE_ID) & CANARD_SERVICE_ID_MAX); - out_result->destination_node_id = (CanardNodeID)((can_id >> OFFSET_DST_NODE_ID) & CANARD_NODE_ID_MAX); + out->port_id = (CanardPortID)((can_id >> OFFSET_SERVICE_ID) & CANARD_SERVICE_ID_MAX); + out->destination_node_id = (CanardNodeID)((can_id >> OFFSET_DST_NODE_ID) & CANARD_NODE_ID_MAX); } // Payload parsing. - out_result->payload_size = frame->payload_size - 1U; // Cut off the tail byte. - out_result->payload = (const uint8_t*) frame->payload; + out->payload_size = frame->payload_size - 1U; // Cut off the tail byte. + out->payload = frame->payload; // Tail byte parsing. - // Intentional violation of MISRA: indexing on a pointer. This is done to avoid pointer arithmetics. - const uint8_t tail = out_result->payload[out_result->payload_size]; - out_result->transfer_id = tail & CANARD_TRANSFER_ID_MAX; - out_result->start_of_transfer = ((tail & TAIL_START_OF_TRANSFER) != 0); - out_result->end_of_transfer = ((tail & TAIL_END_OF_TRANSFER) != 0); - out_result->toggle = ((tail & TAIL_TOGGLE) != 0); + // Intentional violation of MISRA: pointer arithmetics is required to locate the tail byte. Unavoidable. + const uint8_t tail = *(((const uint8_t*) out->payload) + out->payload_size); // NOSONAR + out->transfer_id = tail & CANARD_TRANSFER_ID_MAX; + out->start_of_transfer = ((tail & TAIL_START_OF_TRANSFER) != 0); + out->end_of_transfer = ((tail & TAIL_END_OF_TRANSFER) != 0); + out->toggle = ((tail & TAIL_TOGGLE) != 0); // Final validation. - valid = valid && (out_result->start_of_transfer ? out_result->toggle : true); // Protocol version check. - valid = valid && ((CANARD_NODE_ID_UNSET == out_result->source_node_id) - ? (out_result->start_of_transfer && out_result->end_of_transfer) // Single-frame. + valid = valid && (out->start_of_transfer ? out->toggle : true); // Protocol version check. + valid = valid && ((CANARD_NODE_ID_UNSET == out->source_node_id) + ? (out->start_of_transfer && out->end_of_transfer) // Single-frame. : true); } return valid; diff --git a/libcanard/canard.h b/libcanard/canard.h index b59fb3b1..bfcbf45b 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -17,14 +17,14 @@ extern "C" { /// Semantic version of this library (not the UAVCAN specification). /// API will be backward compatible within the same major version. #define CANARD_VERSION_MAJOR 0 -#define CANARD_VERSION_MINOR 1 +#define CANARD_VERSION_MINOR 10 /// The version number of the UAVCAN specification implemented by this library. #define CANARD_UAVCAN_SPECIFICATION_VERSION_MAJOR 1 #define CANARD_UAVCAN_SPECIFICATION_VERSION_MINOR 0 /// These error codes may be returned from the library API calls whose return type is a signed integer -/// in the negated form (i.e., code 2 returned as -2). +/// in the negated form (e.g., code 2 returned as -2). /// API calls whose return type is not a signer integer cannot fail by contract. /// No other error states may occur in the library. /// By contract, a deterministic application with a properly sized memory pool will never encounter errors. @@ -352,7 +352,7 @@ void canardTxPop(CanardInstance* const ins); /// is no data to store and so a buffer is not needed. /// /// The MTU of the accepted frame is not limited and is not dependent on the MTU setting of the local node; -/// that is, any MTU is accepted. +/// that is, any MTU is accepted. The DLC compliance is not checked -- payload of any length (unlimited) is accepted. /// /// Any value of redundant_transport_index is accepted; that is, up to 256 redundant transports are supported. /// The index of the transport from which the transfer is accepted is always the same as redundant_transport_index. diff --git a/tests/exposed.hpp b/tests/exposed.hpp index c8654907..9a475cff 100644 --- a/tests/exposed.hpp +++ b/tests/exposed.hpp @@ -12,6 +12,8 @@ /// Please keep them in sync with the library by manually updating as necessary. namespace exposed { +using TransferCRC = std::uint16_t; + struct TxQueueItem final { TxQueueItem* next = nullptr; @@ -44,6 +46,32 @@ struct TxQueueItem final auto operator=(const TxQueueItem &&) -> TxQueueItem& = delete; }; +struct CanardInternalRxSession +{ + CanardMicrosecond transfer_timestamp_usec = std::numeric_limits::max(); + std::size_t payload_size = 0U; + std::uint8_t* payload = nullptr; + TransferCRC calculated_crc = 0U; + CanardTransferID toggle_and_transfer_id = std::numeric_limits::max(); + std::uint8_t redundant_transport_index = std::numeric_limits::max(); +}; + +struct RxFrameModel +{ + CanardMicrosecond timestamp_usec = std::numeric_limits::max(); + CanardPriority priority = CanardPriorityOptional; + CanardTransferKind transfer_kind = CanardTransferKindMessage; + CanardPortID port_id = std::numeric_limits::max(); + CanardNodeID source_node_id = CANARD_NODE_ID_UNSET; + CanardNodeID destination_node_id = CANARD_NODE_ID_UNSET; + CanardTransferID transfer_id = std::numeric_limits::max(); + bool start_of_transfer = false; + bool end_of_transfer = false; + bool toggle = false; + std::size_t payload_size = 0U; + const std::uint8_t* payload = nullptr; +}; + // Extern C effectively discards the outer namespaces. extern "C" { @@ -69,5 +97,13 @@ auto txMakeTailByte(const bool start_of_transfer, auto txRoundFramePayloadSizeUp(const std::size_t x) -> std::size_t; auto txFindQueueSupremum(const CanardInstance* const ins, const std::uint32_t can_id) -> TxQueueItem*; + +auto rxTryParseFrame(const CanardFrame* const frame, RxFrameModel* const out_result) -> bool; + +auto rxSessionWritePayload(CanardInstance* const ins, + CanardInternalRxSession* const rxs, + const std::size_t payload_size_max, + const std::size_t payload_size, + const void* const payload) -> std::int8_t; } } // namespace exposed diff --git a/tests/test_private_rx.cpp b/tests/test_private_rx.cpp index 948a82ca..77ae1b51 100644 --- a/tests/test_private_rx.cpp +++ b/tests/test_private_rx.cpp @@ -3,8 +3,126 @@ #include "exposed.hpp" #include "helpers.hpp" +#include TEST_CASE("rxTryParseFrame") { - // + using exposed::RxFrameModel; + using exposed::rxTryParseFrame; + + RxFrameModel model{}; + + const auto parse = [&](const CanardMicrosecond timestamp_usec, + const std::uint32_t extended_can_id, + const std::vector& payload) { + static std::vector payload_storage; + payload_storage = payload; + CanardFrame frame{}; + frame.timestamp_usec = timestamp_usec; + frame.extended_can_id = extended_can_id; + frame.payload_size = std::size(payload); + frame.payload = payload_storage.data(); + model = RxFrameModel{}; + return rxTryParseFrame(&frame, &model); + }; + + // MESSAGE + REQUIRE(parse(543210U, 0U, {0, 1, 2, 3})); + REQUIRE(model.timestamp_usec == 543210U); + REQUIRE(model.priority == CanardPriorityExceptional); + REQUIRE(model.transfer_kind == CanardTransferKindMessage); + REQUIRE(model.port_id == 0U); + REQUIRE(model.source_node_id == 0U); + REQUIRE(model.destination_node_id == CANARD_NODE_ID_UNSET); + REQUIRE(model.transfer_id == 3U); + REQUIRE(!model.start_of_transfer); + REQUIRE(!model.end_of_transfer); + REQUIRE(!model.toggle); + REQUIRE(model.payload_size == 3); + REQUIRE(model.payload[0] == 0); + REQUIRE(model.payload[1] == 1); + REQUIRE(model.payload[2] == 2); + + // MESSAGE + REQUIRE(parse(123456U, 0b001'00'0'110110011001100'0'0100111U, {0b101'00000U | 23U})); + REQUIRE(model.timestamp_usec == 123456U); + REQUIRE(model.priority == CanardPriorityImmediate); + REQUIRE(model.transfer_kind == CanardTransferKindMessage); + REQUIRE(model.port_id == 0b110110011001100U); + REQUIRE(model.source_node_id == 0b0100111U); + REQUIRE(model.destination_node_id == CANARD_NODE_ID_UNSET); + REQUIRE(model.transfer_id == 23U); + REQUIRE(model.start_of_transfer); + REQUIRE(!model.end_of_transfer); + REQUIRE(model.toggle); + REQUIRE(model.payload_size == 0); + // SIMILAR BUT INVALID + REQUIRE(!parse(123456U, 0b001'00'0'110110011001100'0'0100111U, {})); // NO TAIL BYTE + REQUIRE(!parse(123456U, 0b001'00'0'110110011001100'0'0100111U, {0b100'00000U | 23U})); // BAD TOGGLE + REQUIRE(!parse(123456U, 0b001'00'0'110110011001100'1'0100111U, {0b101'00000U | 23U})); // BAD RESERVED 07 + REQUIRE(!parse(123456U, 0b001'00'1'110110011001100'0'0100111U, {0b101'00000U | 23U})); // BAD RESERVED 23 + REQUIRE(!parse(123456U, 0b001'00'1'110110011001100'1'0100111U, {0b101'00000U | 23U})); // BAD RESERVED 07 23 + REQUIRE(!parse(123456U, 0b001'01'0'110110011001100'0'0100111U, {0b101'00000U | 23U})); // ANON NOT SINGLE FRAME + + // ANONYMOUS MESSAGE + REQUIRE(parse(12345U, 0b010'01'0'000110011001101'0'0100111U, {0b111'00000U | 0U})); + REQUIRE(model.timestamp_usec == 12345U); + REQUIRE(model.priority == CanardPriorityFast); + REQUIRE(model.transfer_kind == CanardTransferKindMessage); + REQUIRE(model.port_id == 0b000110011001101U); + REQUIRE(model.source_node_id == CANARD_NODE_ID_UNSET); + REQUIRE(model.destination_node_id == CANARD_NODE_ID_UNSET); + REQUIRE(model.transfer_id == 0U); + REQUIRE(model.start_of_transfer); + REQUIRE(model.end_of_transfer); + REQUIRE(model.toggle); + REQUIRE(model.payload_size == 0); + // SIMILAR BUT INVALID + REQUIRE(!parse(12345U, 0b010'01'0'110110011001100'0'0100111U, {})); // NO TAIL BYTE + REQUIRE(!parse(12345U, 0b010'01'0'110110011001100'0'0100111U, {0b110'00000U | 0U})); // BAD TOGGLE + REQUIRE(!parse(12345U, 0b010'01'0'110110011001100'1'0100111U, {0b111'00000U | 0U})); // BAD RESERVED 07 + REQUIRE(!parse(12345U, 0b010'01'1'110110011001100'0'0100111U, {0b111'00000U | 0U})); // BAD RESERVED 23 + REQUIRE(!parse(12345U, 0b010'01'1'110110011001100'1'0100111U, {0b111'00000U | 0U})); // BAD RESERVED 07 23 + REQUIRE(!parse(12345U, 0b010'01'0'110110011001100'0'0100111U, {0b101'00000U | 0U})); // ANON NOT SINGLE FRAME + + // REQUEST + REQUIRE(parse(999'999U, 0b011'11'0000110011'0011010'0100111U, {0, 1, 2, 3, 0b011'00000U | 31U})); + REQUIRE(model.timestamp_usec == 999'999U); + REQUIRE(model.priority == CanardPriorityHigh); + REQUIRE(model.transfer_kind == CanardTransferKindRequest); + REQUIRE(model.port_id == 0b0000110011U); + REQUIRE(model.source_node_id == 0b0100111U); + REQUIRE(model.destination_node_id == 0b0011010U); + REQUIRE(model.transfer_id == 31U); + REQUIRE(!model.start_of_transfer); + REQUIRE(model.end_of_transfer); + REQUIRE(model.toggle); + REQUIRE(model.payload_size == 4); + REQUIRE(model.payload[0] == 0); + REQUIRE(model.payload[1] == 1); + REQUIRE(model.payload[2] == 2); + REQUIRE(model.payload[3] == 3); + // SIMILAR BUT INVALID + REQUIRE(!parse(999'999U, 0b011'11'0000110011'0011010'0100111U, {})); // NO TAIL BYTE + REQUIRE(!parse(999'999U, 0b011'11'0000110011'0011010'0100111U, {0, 1, 2, 3, 0b110'00000U | 31U})); // BAD TOGGLE + REQUIRE(!parse(999'999U, 0b011'11'1000110011'0011010'0100111U, {0, 1, 2, 3, 0b011'00000U | 31U})); // BAD RESERVED + + // RESPONSE + REQUIRE(parse(888'888U, 0b100'10'0000110011'0100111'0011010U, {255, 0b010'00000U | 1U})); + REQUIRE(model.timestamp_usec == 888'888U); + REQUIRE(model.priority == CanardPriorityNominal); + REQUIRE(model.transfer_kind == CanardTransferKindResponse); + REQUIRE(model.port_id == 0b0000110011U); + REQUIRE(model.source_node_id == 0b0011010U); + REQUIRE(model.destination_node_id == 0b0100111U); + REQUIRE(model.transfer_id == 1U); + REQUIRE(!model.start_of_transfer); + REQUIRE(model.end_of_transfer); + REQUIRE(!model.toggle); + REQUIRE(model.payload_size == 1); + REQUIRE(model.payload[0] == 255); + // SIMILAR BUT INVALID + REQUIRE(!parse(888'888U, 0b100'10'0000110011'0100111'0011010U, {})); // NO TAIL BYTE + REQUIRE(!parse(888'888U, 0b100'10'0000110011'0100111'0011010U, {255, 0b100'00000U | 1U})); // BAD TOGGLE + REQUIRE(!parse(888'888U, 0b100'10'1000110011'0100111'0011010U, {255, 0b010'00000U | 1U})); // BAD RESERVED } From 568ac1fc46d97527c78885dd9b3620adf52a8c14 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 17 Feb 2020 17:00:26 +0200 Subject: [PATCH 050/100] Test rxSessionWritePayload --- libcanard/canard.c | 5 ++ tests/exposed.hpp | 14 +++-- tests/helpers.hpp | 21 ++++--- tests/test_private_rx.cpp | 123 ++++++++++++++++++++++++++++++++++++++ 4 files changed, 148 insertions(+), 15 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index b65074ba..eaee90a1 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -636,6 +636,11 @@ CANARD_PRIVATE int8_t rxSessionWritePayload(CanardInstance* const ins, const size_t payload_size, const void* const payload) { + CANARD_ASSERT(ins != NULL); + CANARD_ASSERT(rxs != NULL); + CANARD_ASSERT((payload != NULL) || (payload_size == 0U)); + CANARD_ASSERT(rxs->payload_size <= payload_size_max); // This invariant is enforced by the subscription logic. + // Allocate the payload lazily, as late as possible. if ((NULL == rxs->payload) && (payload_size_max > 0U)) { diff --git a/tests/exposed.hpp b/tests/exposed.hpp index 9a475cff..421fb95d 100644 --- a/tests/exposed.hpp +++ b/tests/exposed.hpp @@ -46,7 +46,7 @@ struct TxQueueItem final auto operator=(const TxQueueItem &&) -> TxQueueItem& = delete; }; -struct CanardInternalRxSession +struct RxSession { CanardMicrosecond transfer_timestamp_usec = std::numeric_limits::max(); std::size_t payload_size = 0U; @@ -100,10 +100,12 @@ auto txFindQueueSupremum(const CanardInstance* const ins, const std::uint32_t ca auto rxTryParseFrame(const CanardFrame* const frame, RxFrameModel* const out_result) -> bool; -auto rxSessionWritePayload(CanardInstance* const ins, - CanardInternalRxSession* const rxs, - const std::size_t payload_size_max, - const std::size_t payload_size, - const void* const payload) -> std::int8_t; +auto rxSessionWritePayload(CanardInstance* const ins, + RxSession* const rxs, + const std::size_t payload_size_max, + const std::size_t payload_size, + const void* const payload) -> std::int8_t; + +void rxSessionRestart(CanardInstance* const ins, RxSession* const rxs); } } // namespace exposed diff --git a/tests/helpers.hpp b/tests/helpers.hpp index b2363c62..3509e6da 100644 --- a/tests/helpers.hpp +++ b/tests/helpers.hpp @@ -63,7 +63,7 @@ class TestAllocator [[nodiscard]] auto allocate(const std::size_t amount) { void* p = nullptr; - if ((getTotalAllocatedAmount() + amount) <= ceiling_) + if ((amount > 0U) && ((getTotalAllocatedAmount() + amount) <= ceiling_)) { // Clang-tidy complains about manual memory management. Suppressed because we need it for testing purposes. p = std::malloc(amount); // NOLINT @@ -80,16 +80,19 @@ class TestAllocator void deallocate(void* const pointer) { - const auto it = allocated_.find(pointer); - if (it == std::end(allocated_)) + if (pointer != nullptr) { - throw std::logic_error("Heap corruption: an attempt to deallocate memory that is not allocated"); + const auto it = allocated_.find(pointer); + if (it == std::end(allocated_)) + { + throw std::logic_error("Heap corruption: an attempt to deallocate memory that is not allocated"); + } + // Damage the memory to make sure it's not used after deallocation. + std::generate_n(reinterpret_cast(pointer), it->second, &TestAllocator::getRandomByte); + // Clang-tidy complains about manual memory management. Suppressed because we need it for testing purposes. + std::free(it->first); // NOLINT + allocated_.erase(it); } - // Damage the memory to make sure it's not used after deallocation. - std::generate_n(reinterpret_cast(pointer), it->second, &TestAllocator::getRandomByte); - // Clang-tidy complains about manual memory management. Suppressed because we need it for testing purposes. - std::free(it->first); // NOLINT - allocated_.erase(it); } [[nodiscard]] auto getNumAllocatedFragments() const { return std::size(allocated_); } diff --git a/tests/test_private_rx.cpp b/tests/test_private_rx.cpp index 77ae1b51..f21dfdd5 100644 --- a/tests/test_private_rx.cpp +++ b/tests/test_private_rx.cpp @@ -126,3 +126,126 @@ TEST_CASE("rxTryParseFrame") REQUIRE(!parse(888'888U, 0b100'10'0000110011'0100111'0011010U, {255, 0b100'00000U | 1U})); // BAD TOGGLE REQUIRE(!parse(888'888U, 0b100'10'1000110011'0100111'0011010U, {255, 0b010'00000U | 1U})); // BAD RESERVED } + +TEST_CASE("rxSessionWritePayload") +{ + using helpers::Instance; + using exposed::RxSession; + using exposed::rxSessionWritePayload; + using exposed::rxSessionRestart; + + Instance ins; + RxSession rxs; + rxs.toggle_and_transfer_id = 0U; + + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 0); + + // Regular write, the RX state is uninitialized so a new allocation will take place. + REQUIRE(0 == rxSessionWritePayload(&ins.getInstance(), &rxs, 10, 5, "\x00\x01\x02\x03\x04")); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + REQUIRE(rxs.payload_size == 5); + REQUIRE(rxs.payload != nullptr); + REQUIRE(rxs.payload[0] == 0); + REQUIRE(rxs.payload[1] == 1); + REQUIRE(rxs.payload[2] == 2); + REQUIRE(rxs.payload[3] == 3); + REQUIRE(rxs.payload[4] == 4); + + // Appending the pre-allocated storage. + REQUIRE(0 == rxSessionWritePayload(&ins.getInstance(), &rxs, 10, 4, "\x05\x06\x07\x08")); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + REQUIRE(rxs.payload_size == 9); + REQUIRE(rxs.payload != nullptr); + REQUIRE(rxs.payload[0] == 0); + REQUIRE(rxs.payload[1] == 1); + REQUIRE(rxs.payload[2] == 2); + REQUIRE(rxs.payload[3] == 3); + REQUIRE(rxs.payload[4] == 4); + REQUIRE(rxs.payload[5] == 5); + REQUIRE(rxs.payload[6] == 6); + REQUIRE(rxs.payload[7] == 7); + REQUIRE(rxs.payload[8] == 8); + + // Implicit truncation -- too much payload, excess ignored. + REQUIRE(0 == rxSessionWritePayload(&ins.getInstance(), &rxs, 10, 3, "\x09\x0A\x0B")); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + REQUIRE(rxs.payload_size == 10); + REQUIRE(rxs.payload != nullptr); + REQUIRE(rxs.payload[0] == 0); + REQUIRE(rxs.payload[1] == 1); + REQUIRE(rxs.payload[2] == 2); + REQUIRE(rxs.payload[3] == 3); + REQUIRE(rxs.payload[4] == 4); + REQUIRE(rxs.payload[5] == 5); + REQUIRE(rxs.payload[6] == 6); + REQUIRE(rxs.payload[7] == 7); + REQUIRE(rxs.payload[8] == 8); + REQUIRE(rxs.payload[9] == 9); + + // Storage is already full, write ignored. + REQUIRE(0 == rxSessionWritePayload(&ins.getInstance(), &rxs, 10, 3, "\x0C\x0D\x0E")); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + REQUIRE(rxs.payload_size == 10); + REQUIRE(rxs.payload != nullptr); + REQUIRE(rxs.payload[0] == 0); + REQUIRE(rxs.payload[1] == 1); + REQUIRE(rxs.payload[2] == 2); + REQUIRE(rxs.payload[3] == 3); + REQUIRE(rxs.payload[4] == 4); + REQUIRE(rxs.payload[5] == 5); + REQUIRE(rxs.payload[6] == 6); + REQUIRE(rxs.payload[7] == 7); + REQUIRE(rxs.payload[8] == 8); + REQUIRE(rxs.payload[9] == 9); + + // Restart frees the buffer. The transfer-ID will be incremented, too. + rxSessionRestart(&ins.getInstance(), &rxs); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 0); + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); + REQUIRE(rxs.calculated_crc == 0xFFFFU); + REQUIRE(rxs.toggle_and_transfer_id == 33); + + // Double restart has no effect on memory. + rxs.calculated_crc = 0x1234U; + rxs.toggle_and_transfer_id = 23; + rxSessionRestart(&ins.getInstance(), &rxs); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 0); + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); + REQUIRE(rxs.calculated_crc == 0xFFFFU); + REQUIRE(rxs.toggle_and_transfer_id == (32U | 24U)); + + // Restart with a transfer-ID overflow. + rxs.calculated_crc = 0x1234U; + rxs.toggle_and_transfer_id = 31; + rxSessionRestart(&ins.getInstance(), &rxs); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 0); + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); + REQUIRE(rxs.calculated_crc == 0xFFFFU); + REQUIRE(rxs.toggle_and_transfer_id == 32U); + + // Write into a zero-capacity storage. NULL at the output. + REQUIRE(0 == rxSessionWritePayload(&ins.getInstance(), &rxs, 0, 3, "\x00\x01\x02")); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 0); + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); + + // Write with OOM. + ins.getAllocator().setAllocationCeiling(5); + REQUIRE(-CANARD_ERROR_OUT_OF_MEMORY == rxSessionWritePayload(&ins.getInstance(), &rxs, 10, 3, "\x00\x01\x02")); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 0); + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); +} From a6d38c662124d9f074727ef940ee47cf0e728368 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 17 Feb 2020 17:48:47 +0200 Subject: [PATCH 051/100] Formatting --- tests/test_private_rx.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tests/test_private_rx.cpp b/tests/test_private_rx.cpp index f21dfdd5..48350bb6 100644 --- a/tests/test_private_rx.cpp +++ b/tests/test_private_rx.cpp @@ -134,7 +134,7 @@ TEST_CASE("rxSessionWritePayload") using exposed::rxSessionWritePayload; using exposed::rxSessionRestart; - Instance ins; + Instance ins; RxSession rxs; rxs.toggle_and_transfer_id = 0U; @@ -213,7 +213,7 @@ TEST_CASE("rxSessionWritePayload") REQUIRE(rxs.toggle_and_transfer_id == 33); // Double restart has no effect on memory. - rxs.calculated_crc = 0x1234U; + rxs.calculated_crc = 0x1234U; rxs.toggle_and_transfer_id = 23; rxSessionRestart(&ins.getInstance(), &rxs); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); @@ -224,7 +224,7 @@ TEST_CASE("rxSessionWritePayload") REQUIRE(rxs.toggle_and_transfer_id == (32U | 24U)); // Restart with a transfer-ID overflow. - rxs.calculated_crc = 0x1234U; + rxs.calculated_crc = 0x1234U; rxs.toggle_and_transfer_id = 31; rxSessionRestart(&ins.getInstance(), &rxs); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); From 0e5e959aef71b3de327aeb9c23555ee4671731b2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 19 Feb 2020 05:48:15 +0200 Subject: [PATCH 052/100] Better (ugh) coverage tracking (hack, fix later) --- .travis.yml | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index c548a5d1..cdcae5f4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -24,9 +24,14 @@ matrix: - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Debug -DNO_STATIC_ANALYSIS=1 - build-wrapper-linux-x86-64 --out-dir sonar-dump make all # The build wrapper comes from Sonar Cloud. - make test + + # COVERAGE (this is a quick hack, please fix) - find CMakeFiles/test_public_cov.dir/ -name 'canard*.c.*' -type f -print -exec mv -f {} . \; - - gcov-9 libcanard/canard.c --object-file canard.c.gcda --object-file canard.c.gcno - - gcov-9 libcanard/canard_dsdl.c --object-file canard_dsdl.c.gcda --object-file canard_dsdl.c.gcno + - gcov-9 libcanard/canard.c -o canard.c.gcda -o canard.c.gcno -t > public_canard.gcov + - gcov-9 libcanard/canard_dsdl.c -o canard_dsdl.c.gcda -o canard_dsdl.c.gcno -t > public_canard_dsdl.gcov + - find CMakeFiles/test_private_cov.dir/ -name 'canard*.c.*' -type f -print -exec mv -f {} . \; + - gcov-9 libcanard/canard.c -o canard.c.gcda -o canard.c.gcno -t > private_canard.gcov + - gcov-9 libcanard/canard_dsdl.c -o canard_dsdl.c.gcda -o canard_dsdl.c.gcno -t > private_canard_dsdl.gcov # RELEASE - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Release -DNO_STATIC_ANALYSIS=1 From 46529d63a96847db074eb57e0f9af20d09887e4b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 19 Feb 2020 20:30:11 +0200 Subject: [PATCH 053/100] RX test WIP; the CI build is temporarily broken sry --- ...ct.properties => .sonar-project.properties | 0 libcanard/canard.c | 37 ++- libcanard/canard.h | 21 +- tests/CMakeLists.txt | 1 + tests/exposed.hpp | 8 + tests/helpers.hpp | 8 + tests/test_private_rx.cpp | 311 ++++++++++++++++++ 7 files changed, 371 insertions(+), 15 deletions(-) rename sonar-project.properties => .sonar-project.properties (100%) diff --git a/sonar-project.properties b/.sonar-project.properties similarity index 100% rename from sonar-project.properties rename to .sonar-project.properties diff --git a/libcanard/canard.c b/libcanard/canard.c index eaee90a1..84fe7f5a 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -588,10 +588,15 @@ CANARD_PRIVATE bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel out->toggle = ((tail & TAIL_TOGGLE) != 0); // Final validation. - valid = valid && (out->start_of_transfer ? out->toggle : true); // Protocol version check. - valid = valid && ((CANARD_NODE_ID_UNSET == out->source_node_id) - ? (out->start_of_transfer && out->end_of_transfer) // Single-frame. - : true); + // Protocol version check: if SOT is set, then the toggle shall also be set. + valid = valid && ((!out->start_of_transfer) || out->toggle); + // Anonymous transfers can be only single-frame transfers. + valid = valid && + ((out->start_of_transfer && out->end_of_transfer) || (CANARD_NODE_ID_UNSET != out->source_node_id)); + // Non-last frames of a multi-frame transfer shall contain at least some payload besides CRC. + // The specification requires that the MTU utilization shall be maximal but we don't want to rely on MTU here. + // This extension is spec-compatible. + valid = valid && ((out->payload_size > CRC_SIZE_BYTES) || out->end_of_transfer); } return valid; } @@ -723,7 +728,16 @@ CANARD_PRIVATE int8_t rxSessionAcceptFrame(CanardInstance* const ins, // Drop the last two bytes of the payload because we don't want to expose the transfer-CRC to the user. if (frame->end_of_transfer) { - payload_size = (payload_size > CRC_SIZE_BYTES) ? (payload_size - CRC_SIZE_BYTES) : 0U; + if (payload_size >= CRC_SIZE_BYTES) + { + payload_size -= CRC_SIZE_BYTES; + } + else + { + // Okay, some bytes of the payload were actually CRC. Backtrack to drop them from the output. + payload_size = 0U; + rxs->payload_size -= CRC_SIZE_BYTES - payload_size; + } } } @@ -762,8 +776,8 @@ CANARD_PRIVATE int8_t rxSessionAcceptFrame(CanardInstance* const ins, /// The UAVCAN/CAN v1 specification, which this library is an implementation of, does not provide any reference /// pseudocode. Instead, it takes a higher-level, more abstract approach, where only the high-level requirements /// are given and the particular algorithms are left to be implementation-defined. Such abstract approach is much -/// advantageous because it allows implementers to choose whatever solution works best for the specific application -/// at hand, while wire compatibility is still guaranteed by the high-level requirements given in the specification. +/// advantageous because it allows implementers to choose whatever solution works best for the specific application at +/// hand, while the wire compatibility is still guaranteed by the high-level requirements given in the specification. CANARD_PRIVATE int8_t rxSessionUpdate(CanardInstance* const ins, CanardInternalRxSession* const rxs, const RxFrameModel* const frame, @@ -790,12 +804,15 @@ CANARD_PRIVATE int8_t rxSessionUpdate(CanardInstance* const ins, const bool not_previous_tid = rxComputeTransferIDDifference(rxs->toggle_and_transfer_id & CANARD_TRANSFER_ID_MAX, frame->transfer_id) > 1; - const bool need_restart = tid_timed_out || (frame->start_of_transfer && not_previous_tid); + const bool need_restart = tid_timed_out || ((rxs->redundant_transport_index == redundant_transport_index) && + frame->start_of_transfer && not_previous_tid); if (need_restart) { - rxs->redundant_transport_index = redundant_transport_index; + rxs->payload_size = 0U; + rxs->calculated_crc = CRC_INITIAL; rxs->toggle_and_transfer_id = (CanardTransferID)(TAIL_TOGGLE | frame->transfer_id); + rxs->redundant_transport_index = redundant_transport_index; } int8_t out = 0; @@ -805,7 +822,7 @@ CANARD_PRIVATE int8_t rxSessionUpdate(CanardInstance* const ins, } else { - const bool correct_transport = (redundant_transport_index == rxs->redundant_transport_index); + const bool correct_transport = (rxs->redundant_transport_index == redundant_transport_index); const bool correct_toggle = (frame->toggle == ((rxs->toggle_and_transfer_id & TAIL_TOGGLE) != 0)); const bool correct_tid = (frame->transfer_id == (rxs->toggle_and_transfer_id & CANARD_TRANSFER_ID_MAX)); if (correct_transport && correct_toggle && correct_tid) diff --git a/libcanard/canard.h b/libcanard/canard.h index bfcbf45b..8d090412 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -17,7 +17,7 @@ extern "C" { /// Semantic version of this library (not the UAVCAN specification). /// API will be backward compatible within the same major version. #define CANARD_VERSION_MAJOR 0 -#define CANARD_VERSION_MINOR 10 +#define CANARD_VERSION_MINOR 100 /// The version number of the UAVCAN specification implemented by this library. #define CANARD_UAVCAN_SPECIFICATION_VERSION_MAJOR 1 @@ -255,7 +255,8 @@ CanardInstance canardInit(const CanardMemoryAllocate memory_allocate, const Cana /// or other deterministic data structures should be considered. /// /// Returns the number of frames enqueued into the prioritized TX queue (which is always a positive number) -/// in case of success. Returns a negated error code in case of failure. Zero cannot be returned. +/// in case of success (so that the application can track the number of items in the TX queue if necessary). +/// Returns a negated error code in case of failure. Zero is never returned. /// /// An invalid argument error may be returned in the following cases: /// - Any of the input arguments are NULL. @@ -345,12 +346,22 @@ void canardTxPop(CanardInstance* const ins); /// The function returns 1 (one) if the new frame completed a transfer. In this case, the details of the transfer /// are stored into out_transfer, and the payload ownership is passed into that object. This means that the application /// is responsible for deallocating the payload buffer when the processing is done by invoking memory_free. -/// This design is chosen to facilitate zero-copy data exchange across the protocol stack: once a buffer is allocated, -/// its data is never copied around but only passed by reference. This design allows us to reduce the worst-case -/// execution time and reduce jitter caused by the linear time complexity of memcpy(). +/// This design is chosen to facilitate almost zero-copy data exchange across the protocol stack: once a buffer is +/// allocated, its data is never copied around but only passed by reference. This design allows us to reduce the +/// worst-case execution time and reduce jitter caused by the linear time complexity of memcpy(). /// There is a special case, however: if the payload_size_max is zero, the payload pointer will be NULL, since there /// is no data to store and so a buffer is not needed. /// +/// One data copy still has to take place, though: it's the copy from the frame payload into the contiguous buffer. +/// In CAN, the MTU is small (at most 64 bytes for CAN FD), so the extra copy does not cost us much here, +/// but it allows us to completely decouple the lifetime of the input frame buffer from the lifetime of the final +/// transfer object, regardless of whether it's a single-frame or a multi-frame transfer. +/// If we were building, say, an UAVCAN/UDP library, then we would likely resort to a different design, where the +/// frame buffer is allocated once from the heap (which may be done from the interrupt handler if the heap is +/// sufficiently deterministic; an example of a suitable real-time heap is https://github.com/pavel-kirienko/o1heap), +/// and in the case of single-frame transfer it is then carried over to the application without copying. +/// This design somewhat complicates the driver layer though. +/// /// The MTU of the accepted frame is not limited and is not dependent on the MTU setting of the local node; /// that is, any MTU is accepted. The DLC compliance is not checked -- payload of any length (unlimited) is accepted. /// diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index b0afe31b..15c6d8b7 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -52,6 +52,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wtype-limits -Wzero-as-null-pointer-con set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Woverloaded-virtual -Wsign-promo -Wold-style-cast") include_directories(catch ${library_dir}) +add_definitions(-DCATCH_CONFIG_FAST_COMPILE=1) set(common_sources catch/main.cpp ${library_dir}/canard.c ${library_dir}/canard_dsdl.c) diff --git a/tests/exposed.hpp b/tests/exposed.hpp index 421fb95d..94ce299b 100644 --- a/tests/exposed.hpp +++ b/tests/exposed.hpp @@ -107,5 +107,13 @@ auto rxSessionWritePayload(CanardInstance* const ins, const void* const payload) -> std::int8_t; void rxSessionRestart(CanardInstance* const ins, RxSession* const rxs); + +auto rxSessionUpdate(CanardInstance* const ins, + RxSession* const rxs, + const RxFrameModel* const frame, + const std::uint8_t redundant_transport_index, + const CanardMicrosecond transfer_id_timeout_usec, + const std::size_t payload_size_max, + CanardTransfer* const out_transfer) -> std::int8_t; } } // namespace exposed diff --git a/tests/helpers.hpp b/tests/helpers.hpp index 3509e6da..061e0fa4 100644 --- a/tests/helpers.hpp +++ b/tests/helpers.hpp @@ -78,6 +78,14 @@ class TestAllocator return p; } + /// This overload is needed to avoid unnecessary const_cast<> in tests. + /// The casts are needed because allocated memory is pointed to by const-qualified pointers. + /// This is due to certain fundamental limitations of C; see the API docs for info. + void deallocate(const void* const pointer) + { + deallocate(const_cast(pointer)); // NOLINT + } + void deallocate(void* const pointer) { if (pointer != nullptr) diff --git a/tests/test_private_rx.cpp b/tests/test_private_rx.cpp index 48350bb6..2c734cf7 100644 --- a/tests/test_private_rx.cpp +++ b/tests/test_private_rx.cpp @@ -249,3 +249,314 @@ TEST_CASE("rxSessionWritePayload") REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); } + +TEST_CASE("rxSessionUpdate") +{ + using helpers::Instance; + using exposed::RxSession; + using exposed::RxFrameModel; + using exposed::rxSessionUpdate; + using exposed::crcAdd; + + Instance ins; + ins.getAllocator().setAllocationCeiling(10); + + RxFrameModel frame; + frame.timestamp_usec = 10'000'000; + frame.priority = CanardPrioritySlow; + frame.transfer_kind = CanardTransferKindMessage; + frame.port_id = 22'222; + frame.source_node_id = 55; + frame.destination_node_id = CANARD_NODE_ID_UNSET; + frame.transfer_id = 11; + frame.start_of_transfer = true; + frame.end_of_transfer = true; + frame.toggle = true; + frame.payload_size = 3; + frame.payload = reinterpret_cast("\x01\x01\x01"); + + RxSession rxs; + rxs.redundant_transport_index = 1; + + CanardTransfer transfer{}; + + const auto update = [&](const std::uint8_t redundant_transport_index, + const std::uint64_t tid_timeout_usec, + const std::size_t payload_size_max) { + return rxSessionUpdate(&ins.getInstance(), + &rxs, + &frame, + redundant_transport_index, + tid_timeout_usec, + payload_size_max, + &transfer); + }; + + const auto crc = [](const char* const string) { return crcAdd(0xFFFF, std::strlen(string), string); }; + + // Accept one transfer. + REQUIRE(1 == update(1, 1'000'000, 10)); + REQUIRE(rxs.transfer_timestamp_usec == 10'000'000); + REQUIRE(rxs.payload_size == 0); // Handed over to the output transfer. + REQUIRE(rxs.payload == nullptr); // Handed over to the output transfer. + REQUIRE(rxs.calculated_crc == 0xFFFF); + REQUIRE(rxs.toggle_and_transfer_id == (32U | 12U)); // Incremented. + REQUIRE(rxs.redundant_transport_index == 1); + REQUIRE(transfer.timestamp_usec == 10'000'000); + REQUIRE(transfer.priority == CanardPrioritySlow); + REQUIRE(transfer.transfer_kind == CanardTransferKindMessage); + REQUIRE(transfer.port_id == 22'222); + REQUIRE(transfer.remote_node_id == 55); + REQUIRE(transfer.transfer_id == 11); + REQUIRE(transfer.payload_size == 3); + REQUIRE(0 == std::memcmp(transfer.payload, "\x01\x01\x01", 3)); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + ins.getAllocator().deallocate(transfer.payload); + + // Valid next transfer, wrong transport. + frame.timestamp_usec = 10'000'100; + frame.transfer_id = 12; + frame.payload = reinterpret_cast("\x02\x02\x02"); + REQUIRE(0 == update(2, 1'000'000, 10)); + REQUIRE(rxs.transfer_timestamp_usec == 10'000'000); + REQUIRE(rxs.payload_size == 0); // Handed over to the output transfer. + REQUIRE(rxs.payload == nullptr); // Handed over to the output transfer. + REQUIRE(rxs.calculated_crc == 0xFFFF); + REQUIRE(rxs.toggle_and_transfer_id == (32U | 12U)); // Incremented. + REQUIRE(rxs.redundant_transport_index == 1); + + // Correct transport. + frame.timestamp_usec = 10'000'050; + frame.payload = reinterpret_cast("\x03\x03\x03"); + REQUIRE(1 == update(1, 1'000'000, 10)); + REQUIRE(rxs.transfer_timestamp_usec == 10'000'050); + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); + REQUIRE(rxs.calculated_crc == 0xFFFF); + REQUIRE(rxs.toggle_and_transfer_id == (32U | 13U)); + REQUIRE(rxs.redundant_transport_index == 1); + REQUIRE(transfer.timestamp_usec == 10'000'050); + REQUIRE(transfer.priority == CanardPrioritySlow); + REQUIRE(transfer.transfer_kind == CanardTransferKindMessage); + REQUIRE(transfer.port_id == 22'222); + REQUIRE(transfer.remote_node_id == 55); + REQUIRE(transfer.transfer_id == 12); + REQUIRE(transfer.payload_size == 3); + REQUIRE(0 == std::memcmp(transfer.payload, "\x03\x03\x03", 3)); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + ins.getAllocator().deallocate(transfer.payload); + + // Same TID. + frame.timestamp_usec = 10'000'200; + frame.transfer_id = 12; + frame.payload = reinterpret_cast("\x04\x04\x04"); + REQUIRE(0 == update(1, 1'000'200, 10)); + REQUIRE(rxs.transfer_timestamp_usec == 10'000'050); + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); + REQUIRE(rxs.calculated_crc == 0xFFFF); + REQUIRE(rxs.toggle_and_transfer_id == (32U | 13U)); + REQUIRE(rxs.redundant_transport_index == 1); + + // Restart due to TID timeout, switch iface. + frame.timestamp_usec = 20'000'000; + frame.transfer_id = 12; + frame.payload = reinterpret_cast("\x05\x05\x05"); + REQUIRE(1 == update(0, 1'000'000, 10)); + REQUIRE(rxs.transfer_timestamp_usec == 20'000'000); + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); + REQUIRE(rxs.calculated_crc == 0xFFFF); + REQUIRE(rxs.toggle_and_transfer_id == (32U | 13U)); + REQUIRE(rxs.redundant_transport_index == 0); + REQUIRE(transfer.timestamp_usec == 20'000'000); + REQUIRE(transfer.priority == CanardPrioritySlow); + REQUIRE(transfer.transfer_kind == CanardTransferKindMessage); + REQUIRE(transfer.port_id == 22'222); + REQUIRE(transfer.remote_node_id == 55); + REQUIRE(transfer.transfer_id == 12); + REQUIRE(transfer.payload_size == 3); + REQUIRE(0 == std::memcmp(transfer.payload, "\x05\x05\x05", 3)); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + ins.getAllocator().deallocate(transfer.payload); + + // Multi-frame, first. + frame.timestamp_usec = 20'000'100; + frame.transfer_id = 13; + frame.end_of_transfer = false; + frame.payload_size = 4; + frame.payload = reinterpret_cast("\x06\x06\x06\x06"); + REQUIRE(0 == update(0, 1'000'000, 10)); + REQUIRE(rxs.transfer_timestamp_usec == 20'000'100); + REQUIRE(rxs.payload_size == 4); + REQUIRE(0 == std::memcmp(rxs.payload, "\x06\x06\x06\x06", 4)); + REQUIRE(rxs.calculated_crc == crc("\x06\x06\x06\x06")); + REQUIRE(rxs.toggle_and_transfer_id == (0U | 13U)); + REQUIRE(rxs.redundant_transport_index == 0); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + + // Multi-frame, middle. + frame.timestamp_usec = 20'000'200; + frame.start_of_transfer = false; + frame.end_of_transfer = false; + frame.toggle = false; + frame.payload = reinterpret_cast("\x07\x07\x07\x07"); + REQUIRE(0 == update(0, 1'000'000, 10)); + REQUIRE(rxs.transfer_timestamp_usec == 20'000'100); + REQUIRE(rxs.payload_size == 8); + REQUIRE(0 == std::memcmp(rxs.payload, "\x06\x06\x06\x06\x07\x07\x07\x07", 8)); + REQUIRE(rxs.calculated_crc == crc("\x06\x06\x06\x06\x07\x07\x07\x07")); + REQUIRE(rxs.toggle_and_transfer_id == (32U | 13U)); + REQUIRE(rxs.redundant_transport_index == 0); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + + // Multi-frame, last, bad toggle. + frame.timestamp_usec = 20'000'300; + frame.start_of_transfer = false; + frame.end_of_transfer = true; + frame.toggle = false; + frame.payload = reinterpret_cast("\x08\x08\x08\x08"); + REQUIRE(0 == update(0, 1'000'000, 10)); + REQUIRE(rxs.transfer_timestamp_usec == 20'000'100); + REQUIRE(rxs.payload_size == 8); + REQUIRE(0 == std::memcmp(rxs.payload, "\x06\x06\x06\x06\x07\x07\x07\x07", 8)); + REQUIRE(rxs.calculated_crc == crc("\x06\x06\x06\x06\x07\x07\x07\x07")); + REQUIRE(rxs.toggle_and_transfer_id == (32U | 13U)); + REQUIRE(rxs.redundant_transport_index == 0); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + + // Multi-frame, last. + frame.timestamp_usec = 20'000'400; + frame.start_of_transfer = false; + frame.end_of_transfer = true; + frame.toggle = true; + frame.payload_size = 6; // The payload is IMPLICITLY TRUNCATED, and the CRC IS STILL VALIDATED. + frame.payload = reinterpret_cast("\x09\x09\x09\x09`\x9a"); + REQUIRE(1 == update(0, 1'000'000, 10)); + REQUIRE(rxs.transfer_timestamp_usec == 20'000'100); // First frame. + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); + REQUIRE(rxs.calculated_crc == 0xFFFF); + REQUIRE(rxs.toggle_and_transfer_id == (32U | 14U)); + REQUIRE(rxs.redundant_transport_index == 0); + REQUIRE(transfer.timestamp_usec == 20'000'100); + REQUIRE(transfer.priority == CanardPrioritySlow); + REQUIRE(transfer.transfer_kind == CanardTransferKindMessage); + REQUIRE(transfer.port_id == 22'222); + REQUIRE(transfer.remote_node_id == 55); + REQUIRE(transfer.transfer_id == 13); + REQUIRE(transfer.payload_size == 10); + REQUIRE(0 == std::memcmp(transfer.payload, "\x06\x06\x06\x06\x07\x07\x07\x07\x09\x09", 10)); // Truncated. + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + ins.getAllocator().deallocate(transfer.payload); + + // Restart by TID timeout, not the first frame. + frame.timestamp_usec = 30'000'000; + frame.transfer_id = 12; // Goes back. + frame.start_of_transfer = false; + frame.end_of_transfer = false; + frame.toggle = true; + frame.payload_size = 3; + frame.payload = reinterpret_cast("\x0A\x0A\x0A"); + REQUIRE(0 == update(2, 1'000'000, 10)); + REQUIRE(rxs.transfer_timestamp_usec == 20'000'100); // No change. + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); + REQUIRE(rxs.calculated_crc == 0xFFFF); + REQUIRE(rxs.toggle_and_transfer_id == (32U | 13U)); + REQUIRE(rxs.redundant_transport_index == 2); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 0); + + // Restart by TID mismatch. + frame.timestamp_usec = 20'000'200; // Goes back. + frame.transfer_id = 11; // Goes back. + frame.start_of_transfer = true; + frame.end_of_transfer = false; + frame.toggle = true; + frame.payload_size = 3; + frame.payload = reinterpret_cast("\x0B\x0B\x0B"); + REQUIRE(0 == update(2, 1'000'000, 10)); + REQUIRE(rxs.transfer_timestamp_usec == 20'000'200); + REQUIRE(rxs.payload_size == 3); + REQUIRE(0 == std::memcmp(rxs.payload, "\x0B\x0B\x0B", 3)); + REQUIRE(rxs.calculated_crc == crc("\x0B\x0B\x0B")); + REQUIRE(rxs.toggle_and_transfer_id == (0U | 11U)); + REQUIRE(rxs.redundant_transport_index == 2); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + + // Duplicate start rejected (toggle mismatch). + frame.timestamp_usec = 20'000'300; + frame.transfer_id = 11; + frame.start_of_transfer = true; + frame.end_of_transfer = true; + frame.toggle = true; + frame.payload_size = 3; + frame.payload = reinterpret_cast("\x0C\x0C\x0C"); + REQUIRE(0 == update(2, 1'000'000, 10)); + REQUIRE(rxs.transfer_timestamp_usec == 20'000'200); + REQUIRE(rxs.payload_size == 3); + REQUIRE(0 == std::memcmp(rxs.payload, "\x0B\x0B\x0B", 3)); + REQUIRE(rxs.calculated_crc == crc("\x0B\x0B\x0B")); + REQUIRE(rxs.toggle_and_transfer_id == (0U | 11U)); + REQUIRE(rxs.redundant_transport_index == 2); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + + // Continue & finalize. + frame.timestamp_usec = 20'000'400; + frame.transfer_id = 11; + frame.start_of_transfer = false; + frame.end_of_transfer = true; + frame.toggle = false; + frame.payload_size = 5; + frame.payload = reinterpret_cast("\x0D\x0D\x0D\xDA\xF2"); // CRC at the end. + REQUIRE(1 == update(2, 1'000'000, 10)); + REQUIRE(rxs.transfer_timestamp_usec == 20'000'200); + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); + REQUIRE(rxs.calculated_crc == 0xFFFF); + REQUIRE(rxs.toggle_and_transfer_id == (32U | 12U)); + REQUIRE(rxs.redundant_transport_index == 2); + REQUIRE(transfer.timestamp_usec == 20'000'200); + REQUIRE(transfer.priority == CanardPrioritySlow); + REQUIRE(transfer.transfer_kind == CanardTransferKindMessage); + REQUIRE(transfer.port_id == 22'222); + REQUIRE(transfer.remote_node_id == 55); + REQUIRE(transfer.transfer_id == 11); + REQUIRE(transfer.payload_size == 6); + REQUIRE(0 == std::memcmp(transfer.payload, "\x0B\x0B\x0B\x0D\x0D\x0D", 6)); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + ins.getAllocator().deallocate(transfer.payload); + + // CRC SPLIT -- first frame + frame.timestamp_usec = 30'000'000; + frame.transfer_id = 0; + frame.start_of_transfer = true; + frame.end_of_transfer = false; + frame.toggle = true; + frame.payload_size = 4; + frame.payload = reinterpret_cast("\x0E\x0E\x0E\x15"); + REQUIRE(0 == update(0, 1'000'000, 10)); + REQUIRE(rxs.transfer_timestamp_usec == 30'000'000); + REQUIRE(rxs.payload_size == 4); + REQUIRE(0 == std::memcmp(rxs.payload, "\x0E\x0E\x0E\x15", 4)); + REQUIRE(rxs.calculated_crc == crc("\x0E\x0E\x0E\x15")); + REQUIRE(rxs.toggle_and_transfer_id == 0); + REQUIRE(rxs.redundant_transport_index == 0); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + + // BAD CRC + + // OOM +} From 36edc2d0e8eca0f9f3d7335d7859e4389b21630e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 21 Feb 2020 00:25:50 +0200 Subject: [PATCH 054/100] rxSessionUpdate() test done --- libcanard/canard.c | 21 ++-- tests/test_private_rx.cpp | 249 ++++++++++++++++++++++++++------------ 2 files changed, 187 insertions(+), 83 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 84fe7f5a..ac694a21 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -40,6 +40,8 @@ #define CAN_EXT_ID_MASK ((UINT32_C(1) << 29U) - 1U) +#define MFT_NON_LAST_FRAME_PAYLOAD_MIN 7U + #define PADDING_BYTE_VALUE 0U #define OFFSET_PRIORITY 26U @@ -593,10 +595,10 @@ CANARD_PRIVATE bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel // Anonymous transfers can be only single-frame transfers. valid = valid && ((out->start_of_transfer && out->end_of_transfer) || (CANARD_NODE_ID_UNSET != out->source_node_id)); - // Non-last frames of a multi-frame transfer shall contain at least some payload besides CRC. - // The specification requires that the MTU utilization shall be maximal but we don't want to rely on MTU here. - // This extension is spec-compatible. - valid = valid && ((out->payload_size > CRC_SIZE_BYTES) || out->end_of_transfer); + // Non-last frames of a multi-frame transfer shall utilize the MTU fully. + valid = valid && ((out->payload_size >= MFT_NON_LAST_FRAME_PAYLOAD_MIN) || out->end_of_transfer); + // A frame that is a part of a multi-frame transfer cannot be empty (tail byte not included). + valid = valid && ((out->payload_size > 0) || (out->start_of_transfer && out->end_of_transfer)); } return valid; } @@ -728,15 +730,20 @@ CANARD_PRIVATE int8_t rxSessionAcceptFrame(CanardInstance* const ins, // Drop the last two bytes of the payload because we don't want to expose the transfer-CRC to the user. if (frame->end_of_transfer) { + CANARD_ASSERT(rxs->payload_size >= MFT_NON_LAST_FRAME_PAYLOAD_MIN); // Enforced in rxTryParseFrame(). if (payload_size >= CRC_SIZE_BYTES) { payload_size -= CRC_SIZE_BYTES; } else { - // Okay, some bytes of the payload were actually CRC. Backtrack to drop them from the output. - payload_size = 0U; - rxs->payload_size -= CRC_SIZE_BYTES - payload_size; + // Edge case: this frame contains only a part of the transfer CRC and no payload. + // This means that the last byte of the accepted payload is actually a part of the transfer CRC. + // Backtrack to drop it from the output. + const size_t backtrack = CRC_SIZE_BYTES - payload_size; + payload_size = 0U; + CANARD_ASSERT(rxs->payload_size >= backtrack); + rxs->payload_size -= backtrack; } } } diff --git a/tests/test_private_rx.cpp b/tests/test_private_rx.cpp index 2c734cf7..29b4af57 100644 --- a/tests/test_private_rx.cpp +++ b/tests/test_private_rx.cpp @@ -27,24 +27,32 @@ TEST_CASE("rxTryParseFrame") }; // MESSAGE - REQUIRE(parse(543210U, 0U, {0, 1, 2, 3})); + REQUIRE(parse(543210U, 0U, {0, 1, 2, 3, 4, 5, 6, 7})); REQUIRE(model.timestamp_usec == 543210U); REQUIRE(model.priority == CanardPriorityExceptional); REQUIRE(model.transfer_kind == CanardTransferKindMessage); REQUIRE(model.port_id == 0U); REQUIRE(model.source_node_id == 0U); REQUIRE(model.destination_node_id == CANARD_NODE_ID_UNSET); - REQUIRE(model.transfer_id == 3U); + REQUIRE(model.transfer_id == 7U); REQUIRE(!model.start_of_transfer); REQUIRE(!model.end_of_transfer); REQUIRE(!model.toggle); - REQUIRE(model.payload_size == 3); + REQUIRE(model.payload_size == 7); REQUIRE(model.payload[0] == 0); REQUIRE(model.payload[1] == 1); REQUIRE(model.payload[2] == 2); + REQUIRE(model.payload[3] == 3); + REQUIRE(model.payload[4] == 4); + REQUIRE(model.payload[5] == 5); + REQUIRE(model.payload[6] == 6); + // SIMILAR BUT INVALID + REQUIRE(!parse(543210U, 0U, {})); // NO TAIL BYTE + REQUIRE(!parse(543210U, 0U, {0})); // MFT FRAMES REQUIRE PAYLOAD + REQUIRE(!parse(543210U, 0U, {0, 1, 2, 3, 4, 5, 6})); // MFT NON-LAST FRAME PAYLOAD CAN'T BE SHORTER THAN 7 // MESSAGE - REQUIRE(parse(123456U, 0b001'00'0'110110011001100'0'0100111U, {0b101'00000U | 23U})); + REQUIRE(parse(123456U, 0b001'00'0'110110011001100'0'0100111U, {0, 1, 2, 3, 4, 5, 6, 0b101'00000U | 23U})); REQUIRE(model.timestamp_usec == 123456U); REQUIRE(model.priority == CanardPriorityImmediate); REQUIRE(model.transfer_kind == CanardTransferKindMessage); @@ -55,14 +63,27 @@ TEST_CASE("rxTryParseFrame") REQUIRE(model.start_of_transfer); REQUIRE(!model.end_of_transfer); REQUIRE(model.toggle); - REQUIRE(model.payload_size == 0); + REQUIRE(model.payload_size == 7); + REQUIRE(model.payload[0] == 0); + REQUIRE(model.payload[1] == 1); + REQUIRE(model.payload[2] == 2); + REQUIRE(model.payload[3] == 3); + REQUIRE(model.payload[4] == 4); + REQUIRE(model.payload[5] == 5); + REQUIRE(model.payload[6] == 6); // SIMILAR BUT INVALID - REQUIRE(!parse(123456U, 0b001'00'0'110110011001100'0'0100111U, {})); // NO TAIL BYTE - REQUIRE(!parse(123456U, 0b001'00'0'110110011001100'0'0100111U, {0b100'00000U | 23U})); // BAD TOGGLE - REQUIRE(!parse(123456U, 0b001'00'0'110110011001100'1'0100111U, {0b101'00000U | 23U})); // BAD RESERVED 07 - REQUIRE(!parse(123456U, 0b001'00'1'110110011001100'0'0100111U, {0b101'00000U | 23U})); // BAD RESERVED 23 - REQUIRE(!parse(123456U, 0b001'00'1'110110011001100'1'0100111U, {0b101'00000U | 23U})); // BAD RESERVED 07 23 - REQUIRE(!parse(123456U, 0b001'01'0'110110011001100'0'0100111U, {0b101'00000U | 23U})); // ANON NOT SINGLE FRAME + // NO TAIL BYTE + REQUIRE(!parse(123456U, 0b001'00'0'110110011001100'0'0100111U, {})); + // BAD TOGGLE + REQUIRE(!parse(123456U, 0b001'00'0'110110011001100'0'0100111U, {0, 1, 2, 3, 4, 5, 6, 0b100'00000U | 23U})); + // BAD RESERVED R07 + REQUIRE(!parse(123456U, 0b001'00'0'110110011001100'1'0100111U, {0, 1, 2, 3, 4, 5, 6, 0b101'00000U | 23U})); + // BAD RESERVED R23 + REQUIRE(!parse(123456U, 0b001'00'1'110110011001100'0'0100111U, {0, 1, 2, 3, 4, 5, 6, 0b101'00000U | 23U})); + // BAD RESERVED R07 R23 + REQUIRE(!parse(123456U, 0b001'00'1'110110011001100'1'0100111U, {0, 1, 2, 3, 4, 5, 6, 0b101'00000U | 23U})); + // ANON NOT SINGLE FRAME + REQUIRE(!parse(123456U, 0b001'01'0'110110011001100'0'0100111U, {0, 1, 2, 3, 4, 5, 6, 0b101'00000U | 23U})); // ANONYMOUS MESSAGE REQUIRE(parse(12345U, 0b010'01'0'000110011001101'0'0100111U, {0b111'00000U | 0U})); @@ -83,7 +104,6 @@ TEST_CASE("rxTryParseFrame") REQUIRE(!parse(12345U, 0b010'01'0'110110011001100'1'0100111U, {0b111'00000U | 0U})); // BAD RESERVED 07 REQUIRE(!parse(12345U, 0b010'01'1'110110011001100'0'0100111U, {0b111'00000U | 0U})); // BAD RESERVED 23 REQUIRE(!parse(12345U, 0b010'01'1'110110011001100'1'0100111U, {0b111'00000U | 0U})); // BAD RESERVED 07 23 - REQUIRE(!parse(12345U, 0b010'01'0'110110011001100'0'0100111U, {0b101'00000U | 0U})); // ANON NOT SINGLE FRAME // REQUEST REQUIRE(parse(999'999U, 0b011'11'0000110011'0011010'0100111U, {0, 1, 2, 3, 0b011'00000U | 31U})); @@ -259,7 +279,7 @@ TEST_CASE("rxSessionUpdate") using exposed::crcAdd; Instance ins; - ins.getAllocator().setAllocationCeiling(10); + ins.getAllocator().setAllocationCeiling(16); RxFrameModel frame; frame.timestamp_usec = 10'000'000; @@ -295,7 +315,7 @@ TEST_CASE("rxSessionUpdate") const auto crc = [](const char* const string) { return crcAdd(0xFFFF, std::strlen(string), string); }; // Accept one transfer. - REQUIRE(1 == update(1, 1'000'000, 10)); + REQUIRE(1 == update(1, 1'000'000, 16)); REQUIRE(rxs.transfer_timestamp_usec == 10'000'000); REQUIRE(rxs.payload_size == 0); // Handed over to the output transfer. REQUIRE(rxs.payload == nullptr); // Handed over to the output transfer. @@ -311,14 +331,14 @@ TEST_CASE("rxSessionUpdate") REQUIRE(transfer.payload_size == 3); REQUIRE(0 == std::memcmp(transfer.payload, "\x01\x01\x01", 3)); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); - REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); ins.getAllocator().deallocate(transfer.payload); // Valid next transfer, wrong transport. frame.timestamp_usec = 10'000'100; frame.transfer_id = 12; frame.payload = reinterpret_cast("\x02\x02\x02"); - REQUIRE(0 == update(2, 1'000'000, 10)); + REQUIRE(0 == update(2, 1'000'000, 16)); REQUIRE(rxs.transfer_timestamp_usec == 10'000'000); REQUIRE(rxs.payload_size == 0); // Handed over to the output transfer. REQUIRE(rxs.payload == nullptr); // Handed over to the output transfer. @@ -329,7 +349,7 @@ TEST_CASE("rxSessionUpdate") // Correct transport. frame.timestamp_usec = 10'000'050; frame.payload = reinterpret_cast("\x03\x03\x03"); - REQUIRE(1 == update(1, 1'000'000, 10)); + REQUIRE(1 == update(1, 1'000'000, 16)); REQUIRE(rxs.transfer_timestamp_usec == 10'000'050); REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); @@ -345,14 +365,14 @@ TEST_CASE("rxSessionUpdate") REQUIRE(transfer.payload_size == 3); REQUIRE(0 == std::memcmp(transfer.payload, "\x03\x03\x03", 3)); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); - REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); ins.getAllocator().deallocate(transfer.payload); // Same TID. frame.timestamp_usec = 10'000'200; frame.transfer_id = 12; frame.payload = reinterpret_cast("\x04\x04\x04"); - REQUIRE(0 == update(1, 1'000'200, 10)); + REQUIRE(0 == update(1, 1'000'200, 16)); REQUIRE(rxs.transfer_timestamp_usec == 10'000'050); REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); @@ -364,7 +384,7 @@ TEST_CASE("rxSessionUpdate") frame.timestamp_usec = 20'000'000; frame.transfer_id = 12; frame.payload = reinterpret_cast("\x05\x05\x05"); - REQUIRE(1 == update(0, 1'000'000, 10)); + REQUIRE(1 == update(0, 1'000'000, 16)); REQUIRE(rxs.transfer_timestamp_usec == 20'000'000); REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); @@ -380,40 +400,40 @@ TEST_CASE("rxSessionUpdate") REQUIRE(transfer.payload_size == 3); REQUIRE(0 == std::memcmp(transfer.payload, "\x05\x05\x05", 3)); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); - REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); ins.getAllocator().deallocate(transfer.payload); // Multi-frame, first. frame.timestamp_usec = 20'000'100; frame.transfer_id = 13; frame.end_of_transfer = false; - frame.payload_size = 4; - frame.payload = reinterpret_cast("\x06\x06\x06\x06"); - REQUIRE(0 == update(0, 1'000'000, 10)); + frame.payload_size = 7; + frame.payload = reinterpret_cast("\x06\x06\x06\x06\x06\x06\x06"); + REQUIRE(0 == update(0, 1'000'000, 16)); REQUIRE(rxs.transfer_timestamp_usec == 20'000'100); - REQUIRE(rxs.payload_size == 4); - REQUIRE(0 == std::memcmp(rxs.payload, "\x06\x06\x06\x06", 4)); - REQUIRE(rxs.calculated_crc == crc("\x06\x06\x06\x06")); + REQUIRE(rxs.payload_size == 7); + REQUIRE(0 == std::memcmp(rxs.payload, "\x06\x06\x06\x06\x06\x06\x06", 7)); + REQUIRE(rxs.calculated_crc == crc("\x06\x06\x06\x06\x06\x06\x06")); REQUIRE(rxs.toggle_and_transfer_id == (0U | 13U)); REQUIRE(rxs.redundant_transport_index == 0); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); - REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); // Multi-frame, middle. frame.timestamp_usec = 20'000'200; frame.start_of_transfer = false; frame.end_of_transfer = false; frame.toggle = false; - frame.payload = reinterpret_cast("\x07\x07\x07\x07"); - REQUIRE(0 == update(0, 1'000'000, 10)); + frame.payload = reinterpret_cast("\x07\x07\x07\x07\x07\x07\x07"); + REQUIRE(0 == update(0, 1'000'000, 16)); REQUIRE(rxs.transfer_timestamp_usec == 20'000'100); - REQUIRE(rxs.payload_size == 8); - REQUIRE(0 == std::memcmp(rxs.payload, "\x06\x06\x06\x06\x07\x07\x07\x07", 8)); - REQUIRE(rxs.calculated_crc == crc("\x06\x06\x06\x06\x07\x07\x07\x07")); + REQUIRE(rxs.payload_size == 14); + REQUIRE(0 == std::memcmp(rxs.payload, "\x06\x06\x06\x06\x06\x06\x06\x07\x07\x07\x07\x07\x07\x07", 14)); + REQUIRE(rxs.calculated_crc == crc("\x06\x06\x06\x06\x06\x06\x06\x07\x07\x07\x07\x07\x07\x07")); REQUIRE(rxs.toggle_and_transfer_id == (32U | 13U)); REQUIRE(rxs.redundant_transport_index == 0); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); - REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); // Multi-frame, last, bad toggle. frame.timestamp_usec = 20'000'300; @@ -421,15 +441,15 @@ TEST_CASE("rxSessionUpdate") frame.end_of_transfer = true; frame.toggle = false; frame.payload = reinterpret_cast("\x08\x08\x08\x08"); - REQUIRE(0 == update(0, 1'000'000, 10)); + REQUIRE(0 == update(0, 1'000'000, 16)); REQUIRE(rxs.transfer_timestamp_usec == 20'000'100); - REQUIRE(rxs.payload_size == 8); - REQUIRE(0 == std::memcmp(rxs.payload, "\x06\x06\x06\x06\x07\x07\x07\x07", 8)); - REQUIRE(rxs.calculated_crc == crc("\x06\x06\x06\x06\x07\x07\x07\x07")); + REQUIRE(rxs.payload_size == 14); + REQUIRE(0 == std::memcmp(rxs.payload, "\x06\x06\x06\x06\x06\x06\x06\x07\x07\x07\x07\x07\x07\x07", 14)); + REQUIRE(rxs.calculated_crc == crc("\x06\x06\x06\x06\x06\x06\x06\x07\x07\x07\x07\x07\x07\x07")); REQUIRE(rxs.toggle_and_transfer_id == (32U | 13U)); REQUIRE(rxs.redundant_transport_index == 0); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); - REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); // Multi-frame, last. frame.timestamp_usec = 20'000'400; @@ -437,8 +457,8 @@ TEST_CASE("rxSessionUpdate") frame.end_of_transfer = true; frame.toggle = true; frame.payload_size = 6; // The payload is IMPLICITLY TRUNCATED, and the CRC IS STILL VALIDATED. - frame.payload = reinterpret_cast("\x09\x09\x09\x09`\x9a"); - REQUIRE(1 == update(0, 1'000'000, 10)); + frame.payload = reinterpret_cast("\x09\x09\x09\x09\r\x93"); + REQUIRE(1 == update(0, 1'000'000, 16)); REQUIRE(rxs.transfer_timestamp_usec == 20'000'100); // First frame. REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); @@ -451,10 +471,10 @@ TEST_CASE("rxSessionUpdate") REQUIRE(transfer.port_id == 22'222); REQUIRE(transfer.remote_node_id == 55); REQUIRE(transfer.transfer_id == 13); - REQUIRE(transfer.payload_size == 10); - REQUIRE(0 == std::memcmp(transfer.payload, "\x06\x06\x06\x06\x07\x07\x07\x07\x09\x09", 10)); // Truncated. + REQUIRE(transfer.payload_size == 16); + REQUIRE(0 == std::memcmp(transfer.payload, "\x06\x06\x06\x06\x06\x06\x06\x07\x07\x07\x07\x07\x07\x07\x09\x09", 16)); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); - REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); ins.getAllocator().deallocate(transfer.payload); // Restart by TID timeout, not the first frame. @@ -463,9 +483,9 @@ TEST_CASE("rxSessionUpdate") frame.start_of_transfer = false; frame.end_of_transfer = false; frame.toggle = true; - frame.payload_size = 3; - frame.payload = reinterpret_cast("\x0A\x0A\x0A"); - REQUIRE(0 == update(2, 1'000'000, 10)); + frame.payload_size = 7; + frame.payload = reinterpret_cast("\x0A\x0A\x0A\x0A\x0A\x0A\x0A"); + REQUIRE(0 == update(2, 1'000'000, 16)); REQUIRE(rxs.transfer_timestamp_usec == 20'000'100); // No change. REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); @@ -481,17 +501,17 @@ TEST_CASE("rxSessionUpdate") frame.start_of_transfer = true; frame.end_of_transfer = false; frame.toggle = true; - frame.payload_size = 3; - frame.payload = reinterpret_cast("\x0B\x0B\x0B"); - REQUIRE(0 == update(2, 1'000'000, 10)); + frame.payload_size = 7; + frame.payload = reinterpret_cast("\x0B\x0B\x0B\x0B\x0B\x0B\x0B"); + REQUIRE(0 == update(2, 1'000'000, 16)); REQUIRE(rxs.transfer_timestamp_usec == 20'000'200); - REQUIRE(rxs.payload_size == 3); - REQUIRE(0 == std::memcmp(rxs.payload, "\x0B\x0B\x0B", 3)); - REQUIRE(rxs.calculated_crc == crc("\x0B\x0B\x0B")); + REQUIRE(rxs.payload_size == 7); + REQUIRE(0 == std::memcmp(rxs.payload, "\x0B\x0B\x0B\x0B\x0B\x0B\x0B", 7)); + REQUIRE(rxs.calculated_crc == crc("\x0B\x0B\x0B\x0B\x0B\x0B\x0B")); REQUIRE(rxs.toggle_and_transfer_id == (0U | 11U)); REQUIRE(rxs.redundant_transport_index == 2); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); - REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); // Duplicate start rejected (toggle mismatch). frame.timestamp_usec = 20'000'300; @@ -499,17 +519,17 @@ TEST_CASE("rxSessionUpdate") frame.start_of_transfer = true; frame.end_of_transfer = true; frame.toggle = true; - frame.payload_size = 3; - frame.payload = reinterpret_cast("\x0C\x0C\x0C"); - REQUIRE(0 == update(2, 1'000'000, 10)); + frame.payload_size = 7; + frame.payload = reinterpret_cast("\x0C\x0C\x0C\x0C\x0C\x0C\x0C"); + REQUIRE(0 == update(2, 1'000'000, 16)); REQUIRE(rxs.transfer_timestamp_usec == 20'000'200); - REQUIRE(rxs.payload_size == 3); - REQUIRE(0 == std::memcmp(rxs.payload, "\x0B\x0B\x0B", 3)); - REQUIRE(rxs.calculated_crc == crc("\x0B\x0B\x0B")); + REQUIRE(rxs.payload_size == 7); + REQUIRE(0 == std::memcmp(rxs.payload, "\x0B\x0B\x0B\x0B\x0B\x0B\x0B", 7)); + REQUIRE(rxs.calculated_crc == crc("\x0B\x0B\x0B\x0B\x0B\x0B\x0B")); REQUIRE(rxs.toggle_and_transfer_id == (0U | 11U)); REQUIRE(rxs.redundant_transport_index == 2); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); - REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); // Continue & finalize. frame.timestamp_usec = 20'000'400; @@ -518,8 +538,8 @@ TEST_CASE("rxSessionUpdate") frame.end_of_transfer = true; frame.toggle = false; frame.payload_size = 5; - frame.payload = reinterpret_cast("\x0D\x0D\x0D\xDA\xF2"); // CRC at the end. - REQUIRE(1 == update(2, 1'000'000, 10)); + frame.payload = reinterpret_cast("\x0D\x0D\x0DWd"); // CRC at the end. + REQUIRE(1 == update(2, 1'000'000, 16)); REQUIRE(rxs.transfer_timestamp_usec == 20'000'200); REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); @@ -532,31 +552,108 @@ TEST_CASE("rxSessionUpdate") REQUIRE(transfer.port_id == 22'222); REQUIRE(transfer.remote_node_id == 55); REQUIRE(transfer.transfer_id == 11); - REQUIRE(transfer.payload_size == 6); - REQUIRE(0 == std::memcmp(transfer.payload, "\x0B\x0B\x0B\x0D\x0D\x0D", 6)); + REQUIRE(transfer.payload_size == 10); + REQUIRE(0 == std::memcmp(transfer.payload, "\x0B\x0B\x0B\x0B\x0B\x0B\x0B\x0D\x0D\x0D", 10)); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); - REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); ins.getAllocator().deallocate(transfer.payload); - // CRC SPLIT -- first frame + // CRC SPLIT -- first frame. frame.timestamp_usec = 30'000'000; frame.transfer_id = 0; frame.start_of_transfer = true; frame.end_of_transfer = false; frame.toggle = true; - frame.payload_size = 4; - frame.payload = reinterpret_cast("\x0E\x0E\x0E\x15"); - REQUIRE(0 == update(0, 1'000'000, 10)); + frame.payload_size = 8; + frame.payload = reinterpret_cast("\x0E\x0E\x0E\x0E\x0E\x0E\x0E\xF7"); + REQUIRE(0 == update(0, 1'000'000, 16)); REQUIRE(rxs.transfer_timestamp_usec == 30'000'000); - REQUIRE(rxs.payload_size == 4); - REQUIRE(0 == std::memcmp(rxs.payload, "\x0E\x0E\x0E\x15", 4)); - REQUIRE(rxs.calculated_crc == crc("\x0E\x0E\x0E\x15")); + REQUIRE(rxs.payload_size == 8); + REQUIRE(0 == std::memcmp(rxs.payload, "\x0E\x0E\x0E\x0E\x0E\x0E\x0E\xF7", 8)); + REQUIRE(rxs.calculated_crc == crc("\x0E\x0E\x0E\x0E\x0E\x0E\x0E\xF7")); REQUIRE(rxs.toggle_and_transfer_id == 0); REQUIRE(rxs.redundant_transport_index == 0); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); - REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 10); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); - // BAD CRC + // CRC SPLIT -- second (last) frame. + frame.timestamp_usec = 30'000'100; + frame.transfer_id = 0; + frame.start_of_transfer = false; + frame.end_of_transfer = true; + frame.toggle = false; + frame.payload_size = 1; + frame.payload = reinterpret_cast("\xD7"); + REQUIRE(1 == update(0, 1'000'000, 16)); + REQUIRE(rxs.transfer_timestamp_usec == 30'000'000); + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); + REQUIRE(rxs.calculated_crc == 0xFFFF); + REQUIRE(rxs.toggle_and_transfer_id == (32U | 1U)); + REQUIRE(rxs.redundant_transport_index == 0); + REQUIRE(transfer.timestamp_usec == 30'000'000); + REQUIRE(transfer.priority == CanardPrioritySlow); + REQUIRE(transfer.transfer_kind == CanardTransferKindMessage); + REQUIRE(transfer.port_id == 22'222); + REQUIRE(transfer.remote_node_id == 55); + REQUIRE(transfer.transfer_id == 0); + REQUIRE(transfer.payload_size == 7); // ONE CRC BYTE BACKTRACKED! + REQUIRE(0 == std::memcmp(transfer.payload, "\x0E\x0E\x0E\x0E\x0E\x0E\x0E", 7)); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); + ins.getAllocator().deallocate(transfer.payload); + + // BAD CRC -- first frame. + frame.timestamp_usec = 30'001'000; + frame.transfer_id = 31; + frame.start_of_transfer = true; + frame.end_of_transfer = false; + frame.toggle = true; + frame.payload_size = 8; + frame.payload = reinterpret_cast("\x0E\x0E\x0E\x0E\x0E\x0E\x0E\xF7"); + REQUIRE(0 == update(0, 1'000'000, 16)); + REQUIRE(rxs.transfer_timestamp_usec == 30'001'000); + REQUIRE(rxs.payload_size == 8); + REQUIRE(0 == std::memcmp(rxs.payload, "\x0E\x0E\x0E\x0E\x0E\x0E\x0E\xF7", 8)); + REQUIRE(rxs.calculated_crc == crc("\x0E\x0E\x0E\x0E\x0E\x0E\x0E\xF7")); + REQUIRE(rxs.toggle_and_transfer_id == 31); + REQUIRE(rxs.redundant_transport_index == 0); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); - // OOM + // BAD CRC -- second (last) frame. + frame.timestamp_usec = 30'001'100; + frame.transfer_id = 31; + frame.start_of_transfer = false; + frame.end_of_transfer = true; + frame.toggle = false; + frame.payload_size = 1; + frame.payload = reinterpret_cast("\xD8"); + REQUIRE(0 == update(0, 1'000'000, 16)); + REQUIRE(rxs.transfer_timestamp_usec == 30'001'000); + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); + REQUIRE(rxs.calculated_crc == 0xFFFF); + REQUIRE(rxs.toggle_and_transfer_id == (32U | 0U)); + REQUIRE(rxs.redundant_transport_index == 0); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); // Deallocated on failure. + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 0); + + // OOM -- reset on error. + frame.timestamp_usec = 40'000'000; + frame.transfer_id = 30; + frame.start_of_transfer = true; + frame.end_of_transfer = false; + frame.toggle = true; + frame.payload_size = 8; + frame.payload = reinterpret_cast("\x0E\x0E\x0E\x0E\x0E\x0E\x0E\xF7"); + REQUIRE((-CANARD_ERROR_OUT_OF_MEMORY) == update(2, 1'000'000, 17)); // Exceeds the heap quota. + REQUIRE(rxs.transfer_timestamp_usec == 40'000'000); + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); + REQUIRE(rxs.calculated_crc == 0xFFFF); + REQUIRE(rxs.toggle_and_transfer_id == (32U | 31U)); // Reset. + REQUIRE(rxs.redundant_transport_index == 2); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 0); } From 41d2f80dcb68d846fa76ad29b9358a2e3706c0a4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 21 Feb 2020 00:42:57 +0200 Subject: [PATCH 055/100] Fix sonarqube and move configs into .travis.yml --- .sonar-project.properties | 9 --------- .travis.yml | 7 ++++++- 2 files changed, 6 insertions(+), 10 deletions(-) delete mode 100644 .sonar-project.properties diff --git a/.sonar-project.properties b/.sonar-project.properties deleted file mode 100644 index 071f28ba..00000000 --- a/.sonar-project.properties +++ /dev/null @@ -1,9 +0,0 @@ -sonar.projectKey=libcanard -sonar.organization=uavcan - -sonar.sources=libcanard -sonar.sourceEncoding=UTF-8 - -sonar.cfamily.gcov.reportsPath=. -sonar.cfamily.build-wrapper-output=sonar-dump -sonar.cfamily.cache.enabled=false diff --git a/.travis.yml b/.travis.yml index cdcae5f4..68986ae7 100644 --- a/.travis.yml +++ b/.travis.yml @@ -38,7 +38,12 @@ matrix: - make all VERBOSE=1 && make test # ANALYSIS - - sonar-scanner + - sonar-scanner -Dsonar.projectKey=libcanard \ + -Dsonar.organization=uavcan \ + -Dsonar.sources=libcanard \ + -Dsonar.cfamily.gcov.reportsPath=. \ + -Dsonar.cfamily.build-wrapper-output=sonar-dump \ + -Dsonar.cfamily.cache.enabled=false \ # -------------------- Clang 9 -------------------- - language: cpp From 5e9f7424f96987f57036e4f0ee155f043f99362d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 21 Feb 2020 01:11:09 +0200 Subject: [PATCH 056/100] I love YAML but sometimes it's like wtf --- .travis.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.travis.yml b/.travis.yml index 68986ae7..2cd66844 100644 --- a/.travis.yml +++ b/.travis.yml @@ -38,12 +38,12 @@ matrix: - make all VERBOSE=1 && make test # ANALYSIS - - sonar-scanner -Dsonar.projectKey=libcanard \ - -Dsonar.organization=uavcan \ - -Dsonar.sources=libcanard \ - -Dsonar.cfamily.gcov.reportsPath=. \ - -Dsonar.cfamily.build-wrapper-output=sonar-dump \ - -Dsonar.cfamily.cache.enabled=false \ + - 'sonar-scanner -Dsonar.projectKey=libcanard + -Dsonar.organization=uavcan + -Dsonar.sources=libcanard + -Dsonar.cfamily.gcov.reportsPath=. + -Dsonar.cfamily.build-wrapper-output=sonar-dump + -Dsonar.cfamily.cache.enabled=false' # -------------------- Clang 9 -------------------- - language: cpp From 97484cc5fa49b5416912cac1d04b255a3d7f94c1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 21 Feb 2020 04:46:06 +0200 Subject: [PATCH 057/100] rxAccept() testing WIP --- tests/CMakeLists.txt | 2 +- tests/helpers.hpp | 25 +++++++- tests/test_public_rx.cpp | 120 +++++++++++++++++++++++++++++++++++++++ 3 files changed, 144 insertions(+), 3 deletions(-) create mode 100644 tests/test_public_rx.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 15c6d8b7..98768cde 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -77,4 +77,4 @@ endfunction() gen_test_matrix(test_private "test_private_crc.cpp;test_private_rx.cpp;test_private_tx.cpp" CANARD_EXPOSE_PRIVATE=1) -gen_test_matrix(test_public "test_public_float16.cpp;test_public_tx.cpp;test_self.cpp" "") +gen_test_matrix(test_public "test_public_float16.cpp;test_public_tx.cpp;test_public_rx.cpp;test_self.cpp" "") diff --git a/tests/helpers.hpp b/tests/helpers.hpp index 061e0fa4..585dd648 100644 --- a/tests/helpers.hpp +++ b/tests/helpers.hpp @@ -153,9 +153,30 @@ class Instance void txPop() { canardTxPop(&canard_); } - [[nodiscard]] auto rxAccept(const CanardFrame& frame, const uint8_t iface_index, CanardTransfer& out_transfer) + [[nodiscard]] auto rxAccept(const CanardFrame& frame, + const uint8_t redundant_transport_index, + CanardTransfer& out_transfer) { - return canardRxAccept(&canard_, &frame, iface_index, &out_transfer); + return canardRxAccept(&canard_, &frame, redundant_transport_index, &out_transfer); + } + + [[nodiscard]] auto rxSubscribe(const CanardTransferKind transfer_kind, + const CanardPortID port_id, + const std::size_t payload_size_max, + const CanardMicrosecond transfer_id_timeout_usec, + CanardRxSubscription& out_subscription) + { + return canardRxSubscribe(&canard_, + transfer_kind, + port_id, + payload_size_max, + transfer_id_timeout_usec, + &out_subscription); + } + + [[nodiscard]] auto rxUnsubscribe(const CanardTransferKind transfer_kind, const CanardPortID port_id) + { + return canardRxUnsubscribe(&canard_, transfer_kind, port_id); } [[nodiscard]] auto getNodeID() const { return canard_.node_id; } diff --git a/tests/test_public_rx.cpp b/tests/test_public_rx.cpp new file mode 100644 index 00000000..2dfe4f47 --- /dev/null +++ b/tests/test_public_rx.cpp @@ -0,0 +1,120 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. + +#include "exposed.hpp" +#include "helpers.hpp" +#include + +TEST_CASE("RxBasic0") +{ + using helpers::Instance; + using exposed::RxSession; + + Instance ins; + CanardTransfer transfer{}; + + const auto accept = [&](const std::uint8_t redundant_transport_index, + const CanardMicrosecond timestamp_usec, + const std::uint32_t extended_can_id, + const std::vector& payload) { + static std::vector payload_storage; + payload_storage = payload; + CanardFrame frame{}; + frame.timestamp_usec = timestamp_usec; + frame.extended_can_id = extended_can_id; + frame.payload_size = std::size(payload); + frame.payload = payload_storage.data(); + return ins.rxAccept(frame, redundant_transport_index, transfer); + }; + + ins.getAllocator().setAllocationCeiling(sizeof(RxSession) + 16); // A session and a 16-byte payload buffer. + + // No subscriptions by default. + REQUIRE(ins.getInstance()._rx_subscriptions[0] == nullptr); + REQUIRE(ins.getInstance()._rx_subscriptions[1] == nullptr); + REQUIRE(ins.getInstance()._rx_subscriptions[2] == nullptr); + + // A valid single-frame transfer for which there is no subscription. + REQUIRE(0 == accept(0, 100'000'000, 0b001'00'0'110110011001100'0'0100111, {0b111'00000})); + + // Create a message subscription. + CanardRxSubscription sub_msg{}; + REQUIRE(1 == ins.rxSubscribe(CanardTransferKindMessage, 0b110110011001100, 32, 2'000'000, sub_msg)); // New. + REQUIRE(0 == ins.rxSubscribe(CanardTransferKindMessage, 0b110110011001100, 16, 1'000'000, sub_msg)); // Replaced. + REQUIRE(ins.getInstance()._rx_subscriptions[0] == &sub_msg); + REQUIRE(ins.getInstance()._rx_subscriptions[0]->_next == nullptr); + REQUIRE(ins.getInstance()._rx_subscriptions[0]->_port_id == 0b110110011001100); + REQUIRE(ins.getInstance()._rx_subscriptions[0]->_payload_size_max == 16); + REQUIRE(ins.getInstance()._rx_subscriptions[0]->_transfer_id_timeout_usec == 1'000'000); + for (auto _session : ins.getInstance()._rx_subscriptions[0]->_sessions) + { + REQUIRE(_session == nullptr); + } + REQUIRE(ins.getInstance()._rx_subscriptions[1] == nullptr); + REQUIRE(ins.getInstance()._rx_subscriptions[2] == nullptr); + + // Create a request subscription. + CanardRxSubscription sub_req{}; + REQUIRE(1 == ins.rxSubscribe(CanardTransferKindRequest, 0b0000110011, 20, 3'000'000, sub_req)); + REQUIRE(ins.getInstance()._rx_subscriptions[0] == &sub_msg); + REQUIRE(ins.getInstance()._rx_subscriptions[1] == nullptr); + REQUIRE(ins.getInstance()._rx_subscriptions[2] == &sub_req); + REQUIRE(ins.getInstance()._rx_subscriptions[2]->_next == nullptr); + REQUIRE(ins.getInstance()._rx_subscriptions[2]->_port_id == 0b0000110011); + REQUIRE(ins.getInstance()._rx_subscriptions[2]->_payload_size_max == 20); + REQUIRE(ins.getInstance()._rx_subscriptions[2]->_transfer_id_timeout_usec == 3'000'000); + for (auto _session : ins.getInstance()._rx_subscriptions[2]->_sessions) + { + REQUIRE(_session == nullptr); + } + + // Create a response subscription. + CanardRxSubscription sub_res{}; + REQUIRE(1 == ins.rxSubscribe(CanardTransferKindResponse, 0b0000111100, 10, 100'000, sub_res)); + REQUIRE(ins.getInstance()._rx_subscriptions[0] == &sub_msg); + REQUIRE(ins.getInstance()._rx_subscriptions[1] == &sub_res); + REQUIRE(ins.getInstance()._rx_subscriptions[1]->_next == nullptr); + REQUIRE(ins.getInstance()._rx_subscriptions[1]->_port_id == 0b0000111100); + REQUIRE(ins.getInstance()._rx_subscriptions[1]->_payload_size_max == 10); + REQUIRE(ins.getInstance()._rx_subscriptions[1]->_transfer_id_timeout_usec == 100'000); + for (auto _session : ins.getInstance()._rx_subscriptions[1]->_sessions) + { + REQUIRE(_session == nullptr); + } + REQUIRE(ins.getInstance()._rx_subscriptions[2] == &sub_req); + + // Accepted message. + REQUIRE(1 == accept(0, 100'000'001, 0b001'00'0'110110011001100'0'0100111, {0b111'00000})); + REQUIRE(transfer.timestamp_usec == 100'000'001); + REQUIRE(transfer.priority == CanardPriorityImmediate); + REQUIRE(transfer.transfer_kind == CanardTransferKindMessage); + REQUIRE(transfer.port_id == 0b110110011001100); + REQUIRE(transfer.remote_node_id == 0b0100111); + REQUIRE(transfer.transfer_id == 0); + REQUIRE(transfer.payload_size == 0); + REQUIRE(0 == std::memcmp(transfer.payload, "", 0)); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 2); // The SESSION and the PAYLOAD BUFFER. + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == (sizeof(RxSession) + 16)); + REQUIRE(ins.getInstance()._rx_subscriptions[0]->_sessions[0b0100111] != nullptr); +} + +TEST_CASE("RxSubscriptionErrors") +{ + using helpers::Instance; + Instance ins; + CanardRxSubscription sub{}; + + const union + { + std::uint64_t bits; + CanardTransferKind value; + } kind{std::numeric_limits::max()}; + + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardRxSubscribe(nullptr, CanardTransferKindMessage, 0, 0, 0, &sub)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardRxSubscribe(&ins.getInstance(), kind.value, 0, 0, 0, &sub)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == + canardRxSubscribe(&ins.getInstance(), CanardTransferKindMessage, 0, 0, 0, nullptr)); + + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardRxUnsubscribe(nullptr, CanardTransferKindMessage, 0)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardRxUnsubscribe(&ins.getInstance(), kind.value, 0)); +} From 58a4dfd616d229fcf734076e169b5831c3a70697 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 21 Feb 2020 06:29:19 +0200 Subject: [PATCH 058/100] RX pipeline test done --- libcanard/canard.c | 7 ++-- tests/test_private_rx.cpp | 2 ++ tests/test_public_rx.cpp | 69 +++++++++++++++++++++++++++++++++++++++ 3 files changed, 76 insertions(+), 2 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index ac694a21..7fbab3be 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -559,7 +559,6 @@ CANARD_PRIVATE bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel out->source_node_id = (CanardNodeID)(can_id & CANARD_NODE_ID_MAX); if (0 == (can_id & FLAG_SERVICE_NOT_MESSAGE)) { - valid = (0 == (can_id & FLAG_RESERVED_23)) && (0 == (can_id & FLAG_RESERVED_07)); out->transfer_kind = CanardTransferKindMessage; out->port_id = (CanardPortID)((can_id >> OFFSET_SUBJECT_ID) & CANARD_SUBJECT_ID_MAX); if ((can_id & FLAG_ANONYMOUS_MESSAGE) != 0) @@ -567,14 +566,18 @@ CANARD_PRIVATE bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel out->source_node_id = CANARD_NODE_ID_UNSET; } out->destination_node_id = CANARD_NODE_ID_UNSET; + // Reserved bits may be unreserved in the future. + valid = (0 == (can_id & FLAG_RESERVED_23)) && (0 == (can_id & FLAG_RESERVED_07)); } else { - valid = (0 == (can_id & FLAG_RESERVED_23)); out->transfer_kind = ((can_id & FLAG_REQUEST_NOT_RESPONSE) != 0) ? CanardTransferKindRequest : CanardTransferKindResponse; out->port_id = (CanardPortID)((can_id >> OFFSET_SERVICE_ID) & CANARD_SERVICE_ID_MAX); out->destination_node_id = (CanardNodeID)((can_id >> OFFSET_DST_NODE_ID) & CANARD_NODE_ID_MAX); + // The reserved bit may be unreserved in the future. It may be used to extend the service-ID to 10 bits. + // Per Specification, source cannot be the same as the destination. + valid = (0 == (can_id & FLAG_RESERVED_23)) && (out->source_node_id != out->destination_node_id); } // Payload parsing. diff --git a/tests/test_private_rx.cpp b/tests/test_private_rx.cpp index 29b4af57..3e4be0ac 100644 --- a/tests/test_private_rx.cpp +++ b/tests/test_private_rx.cpp @@ -126,6 +126,7 @@ TEST_CASE("rxTryParseFrame") REQUIRE(!parse(999'999U, 0b011'11'0000110011'0011010'0100111U, {})); // NO TAIL BYTE REQUIRE(!parse(999'999U, 0b011'11'0000110011'0011010'0100111U, {0, 1, 2, 3, 0b110'00000U | 31U})); // BAD TOGGLE REQUIRE(!parse(999'999U, 0b011'11'1000110011'0011010'0100111U, {0, 1, 2, 3, 0b011'00000U | 31U})); // BAD RESERVED + REQUIRE(!parse(999'999U, 0b011'11'0000110011'0100111'0100111U, {0, 1, 2, 3, 0b011'00000U | 31U})); // SRC == DST // RESPONSE REQUIRE(parse(888'888U, 0b100'10'0000110011'0100111'0011010U, {255, 0b010'00000U | 1U})); @@ -145,6 +146,7 @@ TEST_CASE("rxTryParseFrame") REQUIRE(!parse(888'888U, 0b100'10'0000110011'0100111'0011010U, {})); // NO TAIL BYTE REQUIRE(!parse(888'888U, 0b100'10'0000110011'0100111'0011010U, {255, 0b100'00000U | 1U})); // BAD TOGGLE REQUIRE(!parse(888'888U, 0b100'10'1000110011'0100111'0011010U, {255, 0b010'00000U | 1U})); // BAD RESERVED + REQUIRE(!parse(888'888U, 0b100'10'0000110011'0011010'0011010U, {255, 0b010'00000U | 1U})); // SRC == DST } TEST_CASE("rxSessionWritePayload") diff --git a/tests/test_public_rx.cpp b/tests/test_public_rx.cpp index 2dfe4f47..9cdf0aec 100644 --- a/tests/test_public_rx.cpp +++ b/tests/test_public_rx.cpp @@ -96,6 +96,75 @@ TEST_CASE("RxBasic0") REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 2); // The SESSION and the PAYLOAD BUFFER. REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == (sizeof(RxSession) + 16)); REQUIRE(ins.getInstance()._rx_subscriptions[0]->_sessions[0b0100111] != nullptr); + auto msg_payload = transfer.payload; // Will need it later. + + // Provide the space for an extra session and its payload. + ins.getAllocator().setAllocationCeiling(sizeof(RxSession) * 2 + 16 + 20); + + // Dropped request because the local node does not have a node-ID. + REQUIRE(0 == accept(0, 100'000'002, 0b011'11'0000110011'0011010'0100111, {0b111'00010})); + + // Dropped request because the local node has a different node-ID. + ins.setNodeID(0b0011010); + REQUIRE(0 == accept(0, 100'000'002, 0b011'11'0000110011'0011011'0100111, {0b111'00011})); + + // Same request accepted now. + REQUIRE(1 == accept(0, 100'000'002, 0b011'11'0000110011'0011010'0100101, {1, 2, 3, 0b111'00100})); + REQUIRE(transfer.timestamp_usec == 100'000'002); + REQUIRE(transfer.priority == CanardPriorityHigh); + REQUIRE(transfer.transfer_kind == CanardTransferKindRequest); + REQUIRE(transfer.port_id == 0b0000110011); + REQUIRE(transfer.remote_node_id == 0b0100101); + REQUIRE(transfer.transfer_id == 4); + REQUIRE(transfer.payload_size == 3); + REQUIRE(0 == std::memcmp(transfer.payload, "\x01\x02\x03", 3)); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 4); // Two SESSIONS and two PAYLOAD BUFFERS. + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == (2 * sizeof(RxSession) + 16 + 20)); + REQUIRE(ins.getInstance()._rx_subscriptions[2]->_sessions[0b0100101] != nullptr); + + // Response transfer not accepted because the local node has a different node-ID. + // There is no dynamic memory available, but it doesn't matter because a rejection does not require allocation. + REQUIRE(0 == accept(0, 100'000'002, 0b100'10'0000110011'0100111'0011011, {10, 20, 30, 0b111'00000})); + + // Response transfer not accepted due to OOM -- can't allocate RX session. + REQUIRE(-CANARD_ERROR_OUT_OF_MEMORY == + accept(0, 100'000'003, 0b100'10'0000111100'0011010'0011011, {5, 0b111'00001})); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 4); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == (2 * sizeof(RxSession) + 16 + 20)); + + // Response transfer not accepted due to OOM -- can't allocate the buffer (RX session is allocated OK). + ins.getAllocator().setAllocationCeiling(3 * sizeof(RxSession) + 16 + 20); + REQUIRE(-CANARD_ERROR_OUT_OF_MEMORY == + accept(0, 100'000'003, 0b100'10'0000111100'0011010'0011011, {5, 0b111'00010})); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 5); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == (3 * sizeof(RxSession) + 16 + 20)); + + // Destroy the message subscription and the buffer to free up memory. + REQUIRE(1 == ins.rxUnsubscribe(CanardTransferKindMessage, 0b110110011001100)); + REQUIRE(0 == ins.rxUnsubscribe(CanardTransferKindMessage, 0b110110011001100)); // Repeat, nothing to do. + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 4); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == (2 * sizeof(RxSession) + 16 + 20)); + ins.getAllocator().deallocate(msg_payload); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 3); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == (2 * sizeof(RxSession) + 20)); + + // Same response accepted now. We have to keep incrementing the transfer-ID though because it's tracked. + REQUIRE(1 == accept(0, 100'000'003, 0b100'10'0000111100'0011010'0011011, {5, 0b111'00011})); + REQUIRE(transfer.timestamp_usec == 100'000'003); + REQUIRE(transfer.priority == CanardPriorityNominal); + REQUIRE(transfer.transfer_kind == CanardTransferKindResponse); + REQUIRE(transfer.port_id == 0b0000111100); + REQUIRE(transfer.remote_node_id == 0b0011011); + REQUIRE(transfer.transfer_id == 3); + REQUIRE(transfer.payload_size == 1); + REQUIRE(0 == std::memcmp(transfer.payload, "\x05", 1)); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 4); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == (2 * sizeof(RxSession) + 10 + 20)); + REQUIRE(ins.getInstance()._rx_subscriptions[1]->_sessions[0b0011011] != nullptr); + + // Bad frames shall be rejected silently. + REQUIRE(0 == accept(0, 900'000'000, 0b100'10'0000111100'0011010'0011011, {5, 0b110'00011})); + REQUIRE(0 == accept(0, 900'000'001, 0b100'10'0000111100'0011010'0011011, {})); } TEST_CASE("RxSubscriptionErrors") From 64e60ceaf56e724b6d26ad475a20eaecedebc540 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 23 Feb 2020 04:18:25 +0200 Subject: [PATCH 059/100] Rework --- libcanard/canard.c | 147 +++++++++++++++++++------------------- libcanard/canard.h | 21 +++--- libcanard/canard_dsdl.c | 12 ++-- tests/exposed.hpp | 15 ++-- tests/helpers.hpp | 50 ++++++++----- tests/test_private_rx.cpp | 76 +++++++++++++------- tests/test_public_tx.cpp | 18 ++++- 7 files changed, 194 insertions(+), 145 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 7fbab3be..c85b0bc1 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -21,16 +21,8 @@ # define CANARD_PRIVATE static inline #endif -#if !defined(__STDC_VERSION__) || (__STDC_VERSION__ < 199901L) -# error "Unsupported language: ISO C99 or a newer version is required." -#endif - -#ifndef static_assert -// Intentional violation of MISRA: static assertion macro cannot be replaced with a function definition. -# define static_assert(x, ...) typedef char _static_assert_gl(_static_assertion_, __LINE__)[(x) ? 1 : -1] // NOSONAR -# define _static_assert_gl(a, b) _static_assert_gl_impl(a, b) // NOSONAR -// Intentional violation of MISRA: the paste operator ## cannot be avoided in this context. -# define _static_assert_gl_impl(a, b) a##b // NOSONAR +#if !defined(__STDC_VERSION__) || (__STDC_VERSION__ < 201112L) +# error "Unsupported language: ISO C11 or a newer version is required." #endif // --------------------------------------------- COMMON CONSTANTS --------------------------------------------- @@ -59,6 +51,8 @@ #define TAIL_END_OF_TRANSFER 64U #define TAIL_TOGGLE 32U +#define INITIAL_TOGGLE_STATE true + // --------------------------------------------- TRANSFER CRC --------------------------------------------- typedef uint16_t TransferCRC; @@ -97,24 +91,18 @@ CANARD_PRIVATE TransferCRC crcAdd(const TransferCRC crc, const size_t size, cons // --------------------------------------------- TRANSMISSION --------------------------------------------- /// The memory requirement model provided in the documentation assumes that the maximum size of this structure never -/// exceeds 32 bytes on any conventional platform. The sizeof() of this structure, per the C standard, assumes that +/// exceeds 40 bytes on any conventional platform. The sizeof() of this structure, per the C standard, assumes that /// the length of the flex array member is zero. /// A user that needs a detailed analysis of the worst-case memory consumption may compute the size of this structure /// for the particular platform at hand manually or by evaluating its sizeof(). /// The fields are ordered to minimize the amount of padding on all conventional platforms. typedef struct CanardInternalTxQueueItem { + CanardMicrosecond deadline_usec; struct CanardInternalTxQueueItem* next; - - CanardMicrosecond deadline_usec; - size_t payload_size; - uint32_t id; - - // Intentional violation of MISRA: this flex array is the lesser of three evils. The other two are: - // - Use pointer, make it point to the remainder of the allocated memory following this structure. - // The pointer is bad because it requires us to use pointer arithmetics and adds sizeof(void*) of waste per item. - // - Use a separate memory allocation for data. This is terribly wasteful (both time & memory). - uint8_t payload[]; // NOSONAR + uint8_t* payload; + size_t payload_size; + uint32_t id; } CanardInternalTxQueueItem; CANARD_PRIVATE uint32_t txMakeMessageSessionSpecifier(const CanardPortID subject_id, const CanardNodeID src_node_id); @@ -268,13 +256,30 @@ CANARD_PRIVATE CanardInternalTxQueueItem* txAllocateQueueItem(CanardInstance* co { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(payload_size > 0U); - CanardInternalTxQueueItem* const out = - (CanardInternalTxQueueItem*) ins->memory_allocate(ins, sizeof(CanardInternalTxQueueItem) + payload_size); - if (out != NULL) + + // Increase the size of the payload if necessary to ensure that the following structure is well-aligned. + static_assert((_Alignof(max_align_t) & (_Alignof(max_align_t) - 1U)) == 0U, "Max align shall be a power of 2"); + static const size_t max_align = _Alignof(max_align_t); + const size_t aligned_payload_size = (payload_size + max_align - 1U) & ~(max_align - 1U); + CANARD_ASSERT(aligned_payload_size >= payload_size); + CANARD_ASSERT(aligned_payload_size < (payload_size + max_align)); + + // Allocate one fragment of dynamic memory to store both the payload buffer and the metadata. + // The payload buffer is allocated in the beginning of the fragment. This allows us to hand it over to the + // application later so that when it is deallocated the metadata storage is also destroyed. + CanardInternalTxQueueItem* out = NULL; + uint8_t* const ptr = (uint8_t*) ins->memory_allocate(ins, sizeof(CanardInternalTxQueueItem) + aligned_payload_size); + if (ptr != NULL) { + out = (CanardInternalTxQueueItem*) (void*) (ptr + aligned_payload_size); + // Normally, we should check that the pointer is aligned here. We don't do that here because there is a bug + // in Glibc on x86: the alignment of malloc() is 8 bytes, whereas alignof(max_align_t) is 16 bytes. + // I tried filing it but their bug tracker requires registration via email, so I sent them one. + // Until the bug in Glibc is fixed, the assertion check should not be here because it would fail the unit tests. out->next = NULL; out->deadline_usec = deadline_usec; out->payload_size = payload_size; + out->payload = ptr; out->id = id; } return out; @@ -334,13 +339,11 @@ CANARD_PRIVATE int32_t txPushSingleFrame(CanardInstance* const ins, { CANARD_ASSERT(payload != NULL); // Clang-Tidy raises an error recommending the use of memcpy_s() instead. - // We ignore this recommendation because it is not available in C99. - (void) memcpy(&tqi->payload[0], payload, payload_size); // NOLINT + (void) memcpy(tqi->payload, payload, payload_size); // NOLINT } // Clang-Tidy raises an error recommending the use of memset_s() instead. - // We ignore this recommendation because it is not available in C99. - (void) memset(&tqi->payload[payload_size], PADDING_BYTE_VALUE, padding_size); // NOLINT + (void) memset(tqi->payload + payload_size, PADDING_BYTE_VALUE, padding_size); // NOLINT tqi->payload[frame_payload_size - 1U] = txMakeTailByte(true, true, true, transfer_id); CanardInternalTxQueueItem* const sup = txFindQueueSupremum(ins, can_id); @@ -394,7 +397,7 @@ CANARD_PRIVATE int32_t txPushMultiFrame(CanardInstance* const ins, size_t offset = 0U; TransferCRC crc = crcAdd(CRC_INITIAL, payload_size, payload); bool start_of_transfer = true; - bool toggle = true; + bool toggle = INITIAL_TOGGLE_STATE; const uint8_t* payload_ptr = (const uint8_t*) payload; while (offset < payload_size_with_crc) @@ -432,8 +435,7 @@ CANARD_PRIVATE int32_t txPushMultiFrame(CanardInstance* const ins, move_size = frame_payload_size; } // Clang-Tidy raises an error recommending the use of memcpy_s() instead. - // We ignore this recommendation because it is not available in C99. - (void) memcpy(&tail->payload[0], payload_ptr, move_size); // NOLINT + (void) memcpy(tail->payload, payload_ptr, move_size); // NOLINT frame_offset = frame_offset + move_size; offset += move_size; payload_ptr += move_size; @@ -509,18 +511,20 @@ CANARD_PRIVATE int32_t txPushMultiFrame(CanardInstance* const ins, #define RX_SESSIONS_PER_SUBSCRIPTION (CANARD_NODE_ID_MAX + 1U) /// The memory requirement model provided in the documentation assumes that the maximum size of this structure never -/// exceeds 32 bytes on any conventional platform. +/// exceeds 48 bytes on any conventional platform. /// A user that needs a detailed analysis of the worst-case memory consumption may compute the size of this structure /// for the particular platform at hand manually or by evaluating its sizeof(). /// The fields are ordered to minimize the amount of padding on all conventional platforms. typedef struct CanardInternalRxSession { CanardMicrosecond transfer_timestamp_usec; ///< Timestamp of the last received start-of-transfer. + size_t total_payload_size; ///< The payload size before the implicit truncation, including the CRC. size_t payload_size; ///< How many bytes received so far. uint8_t* payload; ///< Dynamically allocated and handed off to the application when done. TransferCRC calculated_crc; ///< Updated with the received payload in real time. - CanardTransferID toggle_and_transfer_id; ///< Toggle and transfer-ID combined into one field to reduce footprint. + CanardTransferID transfer_id; uint8_t redundant_transport_index; ///< Arbitrary value in [0, 255]. + bool toggle; } CanardInternalRxSession; /// High-level transport frame model. @@ -594,7 +598,7 @@ CANARD_PRIVATE bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel // Final validation. // Protocol version check: if SOT is set, then the toggle shall also be set. - valid = valid && ((!out->start_of_transfer) || out->toggle); + valid = valid && ((!out->start_of_transfer) || (out->toggle == INITIAL_TOGGLE_STATE)); // Anonymous transfers can be only single-frame transfers. valid = valid && ((out->start_of_transfer && out->end_of_transfer) || (CANARD_NODE_ID_UNSET != out->source_node_id)); @@ -626,6 +630,8 @@ CANARD_PRIVATE void rxInitTransferFromFrame(const RxFrameModel* const frame, Can CANARD_PRIVATE uint8_t rxComputeTransferIDDifference(const uint8_t a, const uint8_t b); CANARD_PRIVATE uint8_t rxComputeTransferIDDifference(const uint8_t a, const uint8_t b) { + CANARD_ASSERT(a <= CANARD_TRANSFER_ID_MAX); + CANARD_ASSERT(b <= CANARD_TRANSFER_ID_MAX); int16_t diff = (int16_t)(((int16_t) a) - ((int16_t) b)); if (diff < 0) { @@ -650,6 +656,9 @@ CANARD_PRIVATE int8_t rxSessionWritePayload(CanardInstance* const ins, CANARD_ASSERT(rxs != NULL); CANARD_ASSERT((payload != NULL) || (payload_size == 0U)); CANARD_ASSERT(rxs->payload_size <= payload_size_max); // This invariant is enforced by the subscription logic. + CANARD_ASSERT(rxs->payload_size <= rxs->total_payload_size); + + rxs->total_payload_size += payload_size; // Allocate the payload lazily, as late as possible. if ((NULL == rxs->payload) && (payload_size_max > 0U)) @@ -693,11 +702,13 @@ CANARD_PRIVATE void rxSessionRestart(CanardInstance* const ins, CanardInternalRx CANARD_ASSERT(ins != NULL); CANARD_ASSERT(rxs != NULL); ins->memory_free(ins, rxs->payload); // May be NULL, which is OK. - rxs->payload = NULL; - rxs->payload_size = 0U; - rxs->calculated_crc = CRC_INITIAL; - rxs->toggle_and_transfer_id = - (CanardTransferID)(TAIL_TOGGLE | ((rxs->toggle_and_transfer_id + 1U) & CANARD_TRANSFER_ID_MAX)); + rxs->total_payload_size = 0U; + rxs->payload_size = 0U; + rxs->payload = NULL; + rxs->calculated_crc = CRC_INITIAL; + rxs->transfer_id = (CanardTransferID)((rxs->transfer_id + 1U) & CANARD_TRANSFER_ID_MAX); + // The transport index is retained. + rxs->toggle = INITIAL_TOGGLE_STATE; } CANARD_PRIVATE int8_t rxSessionAcceptFrame(CanardInstance* const ins, @@ -723,35 +734,15 @@ CANARD_PRIVATE int8_t rxSessionAcceptFrame(CanardInstance* const ins, rxs->transfer_timestamp_usec = frame->timestamp_usec; } - size_t payload_size = frame->payload_size; const bool single_frame = frame->start_of_transfer && frame->end_of_transfer; if (!single_frame) { // Update the CRC. Observe that the implicit truncation rule may apply here: the payload may be // truncated, but its CRC is validated always anyway. rxs->calculated_crc = crcAdd(rxs->calculated_crc, frame->payload_size, frame->payload); - // Drop the last two bytes of the payload because we don't want to expose the transfer-CRC to the user. - if (frame->end_of_transfer) - { - CANARD_ASSERT(rxs->payload_size >= MFT_NON_LAST_FRAME_PAYLOAD_MIN); // Enforced in rxTryParseFrame(). - if (payload_size >= CRC_SIZE_BYTES) - { - payload_size -= CRC_SIZE_BYTES; - } - else - { - // Edge case: this frame contains only a part of the transfer CRC and no payload. - // This means that the last byte of the accepted payload is actually a part of the transfer CRC. - // Backtrack to drop it from the output. - const size_t backtrack = CRC_SIZE_BYTES - payload_size; - payload_size = 0U; - CANARD_ASSERT(rxs->payload_size >= backtrack); - rxs->payload_size -= backtrack; - } - } } - int8_t out = rxSessionWritePayload(ins, rxs, payload_size_max, payload_size, frame->payload); + int8_t out = rxSessionWritePayload(ins, rxs, payload_size_max, frame->payload_size, frame->payload); if (out < 0) { CANARD_ASSERT(-CANARD_ERROR_OUT_OF_MEMORY == out); @@ -762,21 +753,28 @@ CANARD_PRIVATE int8_t rxSessionAcceptFrame(CanardInstance* const ins, CANARD_ASSERT(0 == out); if (single_frame || (CRC_RESIDUE == rxs->calculated_crc)) { + out = 1; // One transfer received, notify the application. rxInitTransferFromFrame(frame, out_transfer); out_transfer->timestamp_usec = rxs->transfer_timestamp_usec; out_transfer->payload_size = rxs->payload_size; out_transfer->payload = rxs->payload; - rxs->payload = NULL; // Ownership passed over to the application, nullify to prevent freeing. + // Cut off the CRC from the payload if it's there -- we don't want to expose it to the user. + CANARD_ASSERT(rxs->total_payload_size >= rxs->payload_size); + const size_t truncated_amount = rxs->total_payload_size - rxs->payload_size; + if (!single_frame && (CRC_SIZE_BYTES > truncated_amount)) // Single-frame transfers don't have CRC. + { + CANARD_ASSERT(out_transfer->payload_size >= (CRC_SIZE_BYTES - truncated_amount)); + out_transfer->payload_size -= CRC_SIZE_BYTES - truncated_amount; + } - out = 1; // One transfer received, notify the application. + rxs->payload = NULL; // Ownership passed over to the application, nullify to prevent freeing. } rxSessionRestart(ins, rxs); // Successful completion. } else { - const CanardTransferID tog = (0 == (rxs->toggle_and_transfer_id & TAIL_TOGGLE)) ? TAIL_TOGGLE : 0U; - rxs->toggle_and_transfer_id = (CanardTransferID)(tog | (rxs->toggle_and_transfer_id & CANARD_TRANSFER_ID_MAX)); + rxs->toggle = !rxs->toggle; } return out; } @@ -807,21 +805,24 @@ CANARD_PRIVATE int8_t rxSessionUpdate(CanardInstance* const ins, CANARD_ASSERT(rxs != NULL); CANARD_ASSERT(frame != NULL); CANARD_ASSERT(out_transfer != NULL); + CANARD_ASSERT(rxs->transfer_id <= CANARD_TRANSFER_ID_MAX); + CANARD_ASSERT(frame->transfer_id <= CANARD_TRANSFER_ID_MAX); const bool tid_timed_out = (frame->timestamp_usec > rxs->transfer_timestamp_usec) && ((frame->timestamp_usec - rxs->transfer_timestamp_usec) > transfer_id_timeout_usec); - const bool not_previous_tid = - rxComputeTransferIDDifference(rxs->toggle_and_transfer_id & CANARD_TRANSFER_ID_MAX, frame->transfer_id) > 1; + const bool not_previous_tid = rxComputeTransferIDDifference(rxs->transfer_id, frame->transfer_id) > 1; const bool need_restart = tid_timed_out || ((rxs->redundant_transport_index == redundant_transport_index) && frame->start_of_transfer && not_previous_tid); if (need_restart) { + rxs->total_payload_size = 0U; rxs->payload_size = 0U; rxs->calculated_crc = CRC_INITIAL; - rxs->toggle_and_transfer_id = (CanardTransferID)(TAIL_TOGGLE | frame->transfer_id); + rxs->transfer_id = frame->transfer_id; + rxs->toggle = INITIAL_TOGGLE_STATE; rxs->redundant_transport_index = redundant_transport_index; } @@ -833,8 +834,8 @@ CANARD_PRIVATE int8_t rxSessionUpdate(CanardInstance* const ins, else { const bool correct_transport = (rxs->redundant_transport_index == redundant_transport_index); - const bool correct_toggle = (frame->toggle == ((rxs->toggle_and_transfer_id & TAIL_TOGGLE) != 0)); - const bool correct_tid = (frame->transfer_id == (rxs->toggle_and_transfer_id & CANARD_TRANSFER_ID_MAX)); + const bool correct_toggle = (frame->toggle == rxs->toggle); + const bool correct_tid = (frame->transfer_id == rxs->transfer_id); if (correct_transport && correct_toggle && correct_tid) { out = rxSessionAcceptFrame(ins, rxs, frame, payload_size_max, out_transfer); @@ -876,11 +877,13 @@ CANARD_PRIVATE int8_t rxAcceptFrame(CanardInstance* const ins, if (rxs != NULL) { rxs->transfer_timestamp_usec = frame->timestamp_usec; + rxs->total_payload_size = 0U; rxs->payload_size = 0U; rxs->payload = NULL; rxs->calculated_crc = CRC_INITIAL; - rxs->toggle_and_transfer_id = (CanardTransferID)(TAIL_TOGGLE | frame->transfer_id); + rxs->transfer_id = frame->transfer_id; rxs->redundant_transport_index = redundant_transport_index; + rxs->toggle = INITIAL_TOGGLE_STATE; } else { @@ -988,7 +991,7 @@ int8_t canardTxPeek(const CanardInstance* const ins, CanardFrame* const out_fram out_frame->timestamp_usec = tqi->deadline_usec; out_frame->extended_can_id = tqi->id; out_frame->payload_size = tqi->payload_size; - out_frame->payload = &tqi->payload[0]; + out_frame->payload = tqi->payload; out = 1; } else @@ -1004,7 +1007,7 @@ void canardTxPop(CanardInstance* const ins) if ((ins != NULL) && (ins->_tx_queue != NULL)) { CanardInternalTxQueueItem* const next = ins->_tx_queue->next; - ins->memory_free(ins, ins->_tx_queue); + // The memory is NOT deallocated. The application is responsible for that. ins->_tx_queue = next; } } diff --git a/libcanard/canard.h b/libcanard/canard.h index 8d090412..73f7c8d7 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -280,9 +280,9 @@ CanardInstance canardInit(const CanardMemoryAllocate memory_allocate, const Cana /// /// The memory allocation requirement is one allocation per transport frame. /// A single-frame transfer takes one allocation; a multi-frame transfer of N frames takes N allocations. -/// The maximum size of each allocation is sizeof(CanardInternalTxQueueItem) plus MTU, -/// where sizeof(CanardInternalTxQueueItem) is at most 32 bytes on any conventional platform (typically smaller). -/// For example, if the MTU is 64 bytes, the allocation size will never exceed 96 bytes on any conventional platform. +/// The maximum size of each allocation is sizeof(CanardInternalTxQueueItem) plus MTU size padded to max_align_t, +/// where sizeof(CanardInternalTxQueueItem) is at most 40 bytes on any conventional platform (typically smaller). +/// For example, if the MTU is 64 bytes, the allocation size will never exceed 104 bytes on any conventional platform. int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfer); /// Access the top element of the prioritized transmission queue. The queue itself is not modified (i.e., the @@ -301,11 +301,9 @@ int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const tran /// /// If the queue is non-empty, the return value is 1 (one) and the out_frame is populated with the data from /// the top element (i.e., the next frame awaiting transmission). -/// The payload pointer of the out_frame will point to the data buffer of the accessed frame; -/// the pointer retains validity until the element is removed from the queue by calling canardTxPop(). -/// The payload pointer retains validity even if more frames are added to the transmission queue. -/// If the returned frame instance is not needed, it can be dropped -- no deinitialization procedures are needed -/// since it does not own any memory itself. +/// The payload pointer of the out_frame will point to a dynamically allocated storage. +/// The payload pointer retains validity until explicitly freed by the application. +/// The payload pointer shall not be freed before the entry is removed from the queue by calling canardTxPop(). /// /// If either of the arguments are NULL, the negated invalid argument error code is returned and no other /// actions are performed. @@ -322,7 +320,10 @@ int8_t canardTxPeek(const CanardInstance* const ins, CanardFrame* const out_fram /// The calling code shall take that into account to eliminate the possibility of data loss due to the frame /// at the top of the queue being unexpectedly replaced between calls of canardTxPeek() and this function. /// -/// Invocation of this function invalidates the payload pointer of the top frame because the underlying buffer is freed. +/// AFTER this function is invoked, the application shall free the memory pointed to by the payload data pointer. +/// The time between invocation and the payload buffer deallocation can be arbitrary because the ownership is fully +/// transferred to the application. This design is intended to facilitate zero-copy enqueueing and transmission. +/// The payload memory SHALL NOT be freed UNTIL this function is invoked. /// /// If the input argument is NULL or if the transmission queue is empty, the function has no effect. /// @@ -401,7 +402,7 @@ int8_t canardRxAccept(CanardInstance* const ins, /// Once a new RX session is allocated, it will never be removed as long as the subscription is active. /// The rationale for this behavior is that real-time networks typically do not change their configuration at runtime; /// hence, it is possible to reduce the worst-case computational complexity of the library routines by never -/// deallocating sessions once allocated. The size of an RX state is at most 32 bytes on any conventional platform. +/// deallocating sessions once allocated. The size of an RX state is at most 48 bytes on any conventional platform. /// If this behavior is found to be undesirable, the application can force deallocation of all unused states by /// re-creating the subscription anew. /// diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index 9ee70b97..30a6cfb5 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -4,16 +4,12 @@ #include "canard_dsdl.h" #include -#ifndef CANARD_ASSERT -# define CANARD_ASSERT assert +#if !defined(__STDC_VERSION__) || (__STDC_VERSION__ < 201112L) +# error "Unsupported language: ISO C11 or a newer version is required." #endif -#ifndef static_assert -// Intentional violation of MISRA: static assertion macro cannot be replaced with a function definition. -# define static_assert(x, ...) typedef char _static_assert_gl(_static_assertion_, __LINE__)[(x) ? 1 : -1] // NOSONAR -# define _static_assert_gl(a, b) _static_assert_gl_impl(a, b) // NOSONAR -// Intentional violation of MISRA: the paste operator ## cannot be avoided in this context. -# define _static_assert_gl_impl(a, b) a##b // NOSONAR +#ifndef CANARD_ASSERT +# define CANARD_ASSERT assert #endif #if CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT diff --git a/tests/exposed.hpp b/tests/exposed.hpp index 94ce299b..60f9a10c 100644 --- a/tests/exposed.hpp +++ b/tests/exposed.hpp @@ -16,18 +16,13 @@ using TransferCRC = std::uint16_t; struct TxQueueItem final { - TxQueueItem* next = nullptr; - std::uint64_t deadline_usec = 0; + TxQueueItem* next = nullptr; + std::uint8_t* payload = nullptr; std::size_t payload_size = 0; std::uint32_t id = 0; - std::array payload{}; // The real definition has a flex array here. - - [[nodiscard]] auto getPayloadByte(const std::size_t offset) const -> std::uint8_t - { - return *(payload.data() + offset); - } + [[nodiscard]] auto getPayloadByte(const std::size_t offset) const -> std::uint8_t { return payload[offset]; } [[nodiscard]] auto getTailByte() const { @@ -49,11 +44,13 @@ struct TxQueueItem final struct RxSession { CanardMicrosecond transfer_timestamp_usec = std::numeric_limits::max(); + std::size_t total_payload_size = 0U; std::size_t payload_size = 0U; std::uint8_t* payload = nullptr; TransferCRC calculated_crc = 0U; - CanardTransferID toggle_and_transfer_id = std::numeric_limits::max(); + CanardTransferID transfer_id = std::numeric_limits::max(); std::uint8_t redundant_transport_index = std::numeric_limits::max(); + bool toggle = false; }; struct RxFrameModel diff --git a/tests/helpers.hpp b/tests/helpers.hpp index 585dd648..e4da8373 100644 --- a/tests/helpers.hpp +++ b/tests/helpers.hpp @@ -6,9 +6,11 @@ #include "canard.h" #include "exposed.hpp" #include +#include #include +#include +#include #include -#include #include namespace helpers @@ -29,20 +31,20 @@ inline void free(CanardInstance* const ins, void* const pointer) } } // namespace dummy_allocator +/// We can't use the recommended true random std::random because it cannot be seeded by Catch2 (the testing framework). +template +inline auto getRandomNatural(const T upper_open) -> T +{ + return static_cast(static_cast(std::rand()) % upper_open); // NOLINT +} + /// An allocator that sits on top of the standard malloc() providing additional testing capabilities. /// It allows the user to specify the maximum amount of memory that can be allocated; further requests will emulate OOM. class TestAllocator { + mutable std::recursive_mutex lock_; std::unordered_map allocated_; - std::size_t ceiling_ = std::numeric_limits::max(); - - static auto getRandomByte() - { - static std::random_device rd; - static std::mt19937 gen(rd()); - static std::uniform_int_distribution dis(0, 255U); - return static_cast(dis(gen)); - } + std::atomic ceiling_ = std::numeric_limits::max(); public: TestAllocator() = default; @@ -53,6 +55,7 @@ class TestAllocator virtual ~TestAllocator() { + std::unique_lock locker(lock_); for (auto [ptr, _] : allocated_) { // Clang-tidy complains about manual memory management. Suppressed because we need it for testing purposes. @@ -62,7 +65,8 @@ class TestAllocator [[nodiscard]] auto allocate(const std::size_t amount) { - void* p = nullptr; + std::unique_lock locker(lock_); + void* p = nullptr; if ((amount > 0U) && ((getTotalAllocatedAmount() + amount) <= ceiling_)) { // Clang-tidy complains about manual memory management. Suppressed because we need it for testing purposes. @@ -72,7 +76,10 @@ class TestAllocator throw std::bad_alloc(); // This is a test suite failure, not a failed test. Mind the difference. } // Random-fill the memory to make sure no assumptions are made about its contents. - std::generate_n(reinterpret_cast(p), amount, &TestAllocator::getRandomByte); + std::uniform_int_distribution dist(0, 255U); + std::generate_n(reinterpret_cast(p), amount, [&]() { + return static_cast(getRandomNatural(256U)); + }); allocated_.emplace(p, amount); } return p; @@ -90,24 +97,33 @@ class TestAllocator { if (pointer != nullptr) { - const auto it = allocated_.find(pointer); + std::unique_lock locker(lock_); + const auto it = allocated_.find(pointer); if (it == std::end(allocated_)) { throw std::logic_error("Heap corruption: an attempt to deallocate memory that is not allocated"); } // Damage the memory to make sure it's not used after deallocation. - std::generate_n(reinterpret_cast(pointer), it->second, &TestAllocator::getRandomByte); + std::uniform_int_distribution dist(0, 255U); + std::generate_n(reinterpret_cast(pointer), it->second, [&]() { + return static_cast(getRandomNatural(256U)); + }); // Clang-tidy complains about manual memory management. Suppressed because we need it for testing purposes. std::free(it->first); // NOLINT allocated_.erase(it); } } - [[nodiscard]] auto getNumAllocatedFragments() const { return std::size(allocated_); } + [[nodiscard]] auto getNumAllocatedFragments() const + { + std::unique_lock locker(lock_); + return std::size(allocated_); + } [[nodiscard]] auto getTotalAllocatedAmount() const -> std::size_t { - std::size_t out = 0U; + std::unique_lock locker(lock_); + std::size_t out = 0U; for (auto [_, size] : allocated_) { out += size; @@ -115,7 +131,7 @@ class TestAllocator return out; } - [[nodiscard]] auto getAllocationCeiling() const { return ceiling_; } + [[nodiscard]] auto getAllocationCeiling() const { return static_cast(ceiling_); } void setAllocationCeiling(const std::size_t amount) { ceiling_ = amount; } }; diff --git a/tests/test_private_rx.cpp b/tests/test_private_rx.cpp index 3e4be0ac..da514914 100644 --- a/tests/test_private_rx.cpp +++ b/tests/test_private_rx.cpp @@ -158,7 +158,7 @@ TEST_CASE("rxSessionWritePayload") Instance ins; RxSession rxs; - rxs.toggle_and_transfer_id = 0U; + rxs.transfer_id = 0U; REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 0); @@ -232,29 +232,34 @@ TEST_CASE("rxSessionWritePayload") REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); REQUIRE(rxs.calculated_crc == 0xFFFFU); - REQUIRE(rxs.toggle_and_transfer_id == 33); + REQUIRE(rxs.transfer_id == 1); + REQUIRE(rxs.toggle); // Double restart has no effect on memory. - rxs.calculated_crc = 0x1234U; - rxs.toggle_and_transfer_id = 23; + rxs.calculated_crc = 0x1234U; + rxs.transfer_id = 23; + rxs.toggle = false; rxSessionRestart(&ins.getInstance(), &rxs); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 0); REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); REQUIRE(rxs.calculated_crc == 0xFFFFU); - REQUIRE(rxs.toggle_and_transfer_id == (32U | 24U)); + REQUIRE(rxs.transfer_id == 24U); + REQUIRE(rxs.toggle); // Restart with a transfer-ID overflow. - rxs.calculated_crc = 0x1234U; - rxs.toggle_and_transfer_id = 31; + rxs.calculated_crc = 0x1234U; + rxs.transfer_id = 31; + rxs.toggle = false; rxSessionRestart(&ins.getInstance(), &rxs); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 0); REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); REQUIRE(rxs.calculated_crc == 0xFFFFU); - REQUIRE(rxs.toggle_and_transfer_id == 32U); + REQUIRE(rxs.transfer_id == 0U); + REQUIRE(rxs.toggle); // Write into a zero-capacity storage. NULL at the output. REQUIRE(0 == rxSessionWritePayload(&ins.getInstance(), &rxs, 0, 3, "\x00\x01\x02")); @@ -298,6 +303,7 @@ TEST_CASE("rxSessionUpdate") frame.payload = reinterpret_cast("\x01\x01\x01"); RxSession rxs; + rxs.transfer_id = 31; rxs.redundant_transport_index = 1; CanardTransfer transfer{}; @@ -322,7 +328,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 0); // Handed over to the output transfer. REQUIRE(rxs.payload == nullptr); // Handed over to the output transfer. REQUIRE(rxs.calculated_crc == 0xFFFF); - REQUIRE(rxs.toggle_and_transfer_id == (32U | 12U)); // Incremented. + REQUIRE(rxs.transfer_id == 12U); // Incremented. + REQUIRE(rxs.toggle); REQUIRE(rxs.redundant_transport_index == 1); REQUIRE(transfer.timestamp_usec == 10'000'000); REQUIRE(transfer.priority == CanardPrioritySlow); @@ -345,7 +352,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 0); // Handed over to the output transfer. REQUIRE(rxs.payload == nullptr); // Handed over to the output transfer. REQUIRE(rxs.calculated_crc == 0xFFFF); - REQUIRE(rxs.toggle_and_transfer_id == (32U | 12U)); // Incremented. + REQUIRE(rxs.transfer_id == 12U); // Incremented. + REQUIRE(rxs.toggle); REQUIRE(rxs.redundant_transport_index == 1); // Correct transport. @@ -356,7 +364,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); REQUIRE(rxs.calculated_crc == 0xFFFF); - REQUIRE(rxs.toggle_and_transfer_id == (32U | 13U)); + REQUIRE(rxs.transfer_id == 13U); + REQUIRE(rxs.toggle); REQUIRE(rxs.redundant_transport_index == 1); REQUIRE(transfer.timestamp_usec == 10'000'050); REQUIRE(transfer.priority == CanardPrioritySlow); @@ -379,7 +388,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); REQUIRE(rxs.calculated_crc == 0xFFFF); - REQUIRE(rxs.toggle_and_transfer_id == (32U | 13U)); + REQUIRE(rxs.transfer_id == 13U); + REQUIRE(rxs.toggle); REQUIRE(rxs.redundant_transport_index == 1); // Restart due to TID timeout, switch iface. @@ -391,7 +401,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); REQUIRE(rxs.calculated_crc == 0xFFFF); - REQUIRE(rxs.toggle_and_transfer_id == (32U | 13U)); + REQUIRE(rxs.transfer_id == 13U); + REQUIRE(rxs.toggle); REQUIRE(rxs.redundant_transport_index == 0); REQUIRE(transfer.timestamp_usec == 20'000'000); REQUIRE(transfer.priority == CanardPrioritySlow); @@ -416,7 +427,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 7); REQUIRE(0 == std::memcmp(rxs.payload, "\x06\x06\x06\x06\x06\x06\x06", 7)); REQUIRE(rxs.calculated_crc == crc("\x06\x06\x06\x06\x06\x06\x06")); - REQUIRE(rxs.toggle_and_transfer_id == (0U | 13U)); + REQUIRE(rxs.transfer_id == 13U); + REQUIRE(!rxs.toggle); REQUIRE(rxs.redundant_transport_index == 0); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); @@ -432,7 +444,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 14); REQUIRE(0 == std::memcmp(rxs.payload, "\x06\x06\x06\x06\x06\x06\x06\x07\x07\x07\x07\x07\x07\x07", 14)); REQUIRE(rxs.calculated_crc == crc("\x06\x06\x06\x06\x06\x06\x06\x07\x07\x07\x07\x07\x07\x07")); - REQUIRE(rxs.toggle_and_transfer_id == (32U | 13U)); + REQUIRE(rxs.transfer_id == 13U); + REQUIRE(rxs.toggle); REQUIRE(rxs.redundant_transport_index == 0); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); @@ -448,7 +461,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 14); REQUIRE(0 == std::memcmp(rxs.payload, "\x06\x06\x06\x06\x06\x06\x06\x07\x07\x07\x07\x07\x07\x07", 14)); REQUIRE(rxs.calculated_crc == crc("\x06\x06\x06\x06\x06\x06\x06\x07\x07\x07\x07\x07\x07\x07")); - REQUIRE(rxs.toggle_and_transfer_id == (32U | 13U)); + REQUIRE(rxs.transfer_id == 13U); + REQUIRE(rxs.toggle); REQUIRE(rxs.redundant_transport_index == 0); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); @@ -465,7 +479,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); REQUIRE(rxs.calculated_crc == 0xFFFF); - REQUIRE(rxs.toggle_and_transfer_id == (32U | 14U)); + REQUIRE(rxs.transfer_id == 14U); + REQUIRE(rxs.toggle); REQUIRE(rxs.redundant_transport_index == 0); REQUIRE(transfer.timestamp_usec == 20'000'100); REQUIRE(transfer.priority == CanardPrioritySlow); @@ -492,7 +507,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); REQUIRE(rxs.calculated_crc == 0xFFFF); - REQUIRE(rxs.toggle_and_transfer_id == (32U | 13U)); + REQUIRE(rxs.transfer_id == 13U); + REQUIRE(rxs.toggle); REQUIRE(rxs.redundant_transport_index == 2); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 0); @@ -510,7 +526,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 7); REQUIRE(0 == std::memcmp(rxs.payload, "\x0B\x0B\x0B\x0B\x0B\x0B\x0B", 7)); REQUIRE(rxs.calculated_crc == crc("\x0B\x0B\x0B\x0B\x0B\x0B\x0B")); - REQUIRE(rxs.toggle_and_transfer_id == (0U | 11U)); + REQUIRE(rxs.transfer_id == 11U); + REQUIRE(!rxs.toggle); REQUIRE(rxs.redundant_transport_index == 2); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); @@ -528,7 +545,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 7); REQUIRE(0 == std::memcmp(rxs.payload, "\x0B\x0B\x0B\x0B\x0B\x0B\x0B", 7)); REQUIRE(rxs.calculated_crc == crc("\x0B\x0B\x0B\x0B\x0B\x0B\x0B")); - REQUIRE(rxs.toggle_and_transfer_id == (0U | 11U)); + REQUIRE(rxs.transfer_id == 11U); + REQUIRE(!rxs.toggle); REQUIRE(rxs.redundant_transport_index == 2); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); @@ -546,7 +564,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); REQUIRE(rxs.calculated_crc == 0xFFFF); - REQUIRE(rxs.toggle_and_transfer_id == (32U | 12U)); + REQUIRE(rxs.transfer_id == 12U); + REQUIRE(rxs.toggle); REQUIRE(rxs.redundant_transport_index == 2); REQUIRE(transfer.timestamp_usec == 20'000'200); REQUIRE(transfer.priority == CanardPrioritySlow); @@ -573,7 +592,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 8); REQUIRE(0 == std::memcmp(rxs.payload, "\x0E\x0E\x0E\x0E\x0E\x0E\x0E\xF7", 8)); REQUIRE(rxs.calculated_crc == crc("\x0E\x0E\x0E\x0E\x0E\x0E\x0E\xF7")); - REQUIRE(rxs.toggle_and_transfer_id == 0); + REQUIRE(rxs.transfer_id == 0); + REQUIRE(!rxs.toggle); REQUIRE(rxs.redundant_transport_index == 0); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); @@ -591,7 +611,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); REQUIRE(rxs.calculated_crc == 0xFFFF); - REQUIRE(rxs.toggle_and_transfer_id == (32U | 1U)); + REQUIRE(rxs.transfer_id == 1U); + REQUIRE(rxs.toggle); REQUIRE(rxs.redundant_transport_index == 0); REQUIRE(transfer.timestamp_usec == 30'000'000); REQUIRE(transfer.priority == CanardPrioritySlow); @@ -618,7 +639,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 8); REQUIRE(0 == std::memcmp(rxs.payload, "\x0E\x0E\x0E\x0E\x0E\x0E\x0E\xF7", 8)); REQUIRE(rxs.calculated_crc == crc("\x0E\x0E\x0E\x0E\x0E\x0E\x0E\xF7")); - REQUIRE(rxs.toggle_and_transfer_id == 31); + REQUIRE(rxs.transfer_id == 31U); + REQUIRE(!rxs.toggle); REQUIRE(rxs.redundant_transport_index == 0); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); @@ -636,7 +658,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); REQUIRE(rxs.calculated_crc == 0xFFFF); - REQUIRE(rxs.toggle_and_transfer_id == (32U | 0U)); + REQUIRE(rxs.transfer_id == 0U); + REQUIRE(rxs.toggle); REQUIRE(rxs.redundant_transport_index == 0); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); // Deallocated on failure. REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 0); @@ -654,7 +677,8 @@ TEST_CASE("rxSessionUpdate") REQUIRE(rxs.payload_size == 0); REQUIRE(rxs.payload == nullptr); REQUIRE(rxs.calculated_crc == 0xFFFF); - REQUIRE(rxs.toggle_and_transfer_id == (32U | 31U)); // Reset. + REQUIRE(rxs.transfer_id == 31U); // Reset. + REQUIRE(rxs.toggle); REQUIRE(rxs.redundant_transport_index == 2); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 0); diff --git a/tests/test_public_tx.cpp b/tests/test_public_tx.cpp index 7a56833c..d8fdac87 100644 --- a/tests/test_public_tx.cpp +++ b/tests/test_public_tx.cpp @@ -141,6 +141,7 @@ TEST_CASE("TxBasic0") REQUIRE((0b11100000U | 21U) == reinterpret_cast(frame.payload)[11]); REQUIRE(frame.timestamp_usec == 1'000'000'000'000ULL); ins.txPop(); + ins.getAllocator().deallocate(frame.payload); REQUIRE(2 == ins.getTxQueueLength()); REQUIRE(2 == alloc.getNumAllocatedFragments()); REQUIRE(1 == ins.txPeek(frame)); @@ -149,6 +150,7 @@ TEST_CASE("TxBasic0") REQUIRE((0b10100000U | 22U) == reinterpret_cast(frame.payload)[7]); REQUIRE(frame.timestamp_usec == 1'000'000'000'100ULL); ins.txPop(); + ins.getAllocator().deallocate(frame.payload); REQUIRE(1 == ins.getTxQueueLength()); REQUIRE(1 == alloc.getNumAllocatedFragments()); REQUIRE(1 == ins.txPeek(frame)); @@ -159,6 +161,7 @@ TEST_CASE("TxBasic0") REQUIRE((0b01000000U | 22U) == reinterpret_cast(frame.payload)[3]); REQUIRE(frame.timestamp_usec == 1'000'000'000'100ULL); ins.txPop(); + ins.getAllocator().deallocate(frame.payload); REQUIRE(0 == ins.getTxQueueLength()); REQUIRE(nullptr == ins.getTxQueueRoot()); REQUIRE(0 == alloc.getNumAllocatedFragments()); @@ -184,7 +187,7 @@ TEST_CASE("TxBasic0") REQUIRE(3 == ins.getTxQueueLength()); REQUIRE(3 == alloc.getNumAllocatedFragments()); REQUIRE(40 < alloc.getTotalAllocatedAmount()); - REQUIRE(200 > alloc.getTotalAllocatedAmount()); + REQUIRE(220 > alloc.getTotalAllocatedAmount()); // Read the generated frames. REQUIRE(1 == ins.txPeek(frame)); REQUIRE(frame.payload_size == 32); @@ -192,6 +195,7 @@ TEST_CASE("TxBasic0") REQUIRE((0b10100000U | 25U) == reinterpret_cast(frame.payload)[31]); REQUIRE(frame.timestamp_usec == 1'000'000'001'000ULL); ins.txPop(); + ins.getAllocator().deallocate(frame.payload); REQUIRE(2 == ins.getTxQueueLength()); REQUIRE(2 == alloc.getNumAllocatedFragments()); REQUIRE(1 == ins.txPeek(frame)); @@ -201,6 +205,7 @@ TEST_CASE("TxBasic0") REQUIRE((0b00000000U | 25U) == reinterpret_cast(frame.payload)[31]); REQUIRE(frame.timestamp_usec == 1'000'000'001'000ULL); ins.txPop(); + ins.getAllocator().deallocate(frame.payload); REQUIRE(1 == ins.getTxQueueLength()); REQUIRE(1 == alloc.getNumAllocatedFragments()); REQUIRE(1 == ins.txPeek(frame)); @@ -209,6 +214,7 @@ TEST_CASE("TxBasic0") REQUIRE((0b01100000U | 25U) == reinterpret_cast(frame.payload)[1]); REQUIRE(frame.timestamp_usec == 1'000'000'001'000ULL); ins.txPop(); + ins.getAllocator().deallocate(frame.payload); REQUIRE(0 == ins.getTxQueueLength()); REQUIRE(0 == alloc.getNumAllocatedFragments()); @@ -224,7 +230,7 @@ TEST_CASE("TxBasic0") REQUIRE(3 == ins.getTxQueueLength()); REQUIRE(3 == alloc.getNumAllocatedFragments()); REQUIRE(40 < alloc.getTotalAllocatedAmount()); - REQUIRE(200 > alloc.getTotalAllocatedAmount()); + REQUIRE(220 > alloc.getTotalAllocatedAmount()); // Read the generated frames. REQUIRE(1 == ins.txPeek(frame)); REQUIRE(frame.payload_size == 32); @@ -232,6 +238,7 @@ TEST_CASE("TxBasic0") REQUIRE((0b10100000U | 26U) == reinterpret_cast(frame.payload)[31]); REQUIRE(frame.timestamp_usec == 1'000'000'002'000ULL); ins.txPop(); + ins.getAllocator().deallocate(frame.payload); REQUIRE(2 == ins.getTxQueueLength()); REQUIRE(2 == alloc.getNumAllocatedFragments()); REQUIRE(1 == ins.txPeek(frame)); @@ -240,6 +247,7 @@ TEST_CASE("TxBasic0") REQUIRE((0b00000000U | 26U) == reinterpret_cast(frame.payload)[31]); REQUIRE(frame.timestamp_usec == 1'000'000'002'000ULL); ins.txPop(); + ins.getAllocator().deallocate(frame.payload); REQUIRE(1 == ins.getTxQueueLength()); REQUIRE(1 == alloc.getNumAllocatedFragments()); REQUIRE(1 == ins.txPeek(frame)); @@ -249,6 +257,7 @@ TEST_CASE("TxBasic0") REQUIRE((0b01100000U | 26U) == reinterpret_cast(frame.payload)[2]); REQUIRE(frame.timestamp_usec == 1'000'000'002'000ULL); ins.txPop(); + ins.getAllocator().deallocate(frame.payload); REQUIRE(0 == ins.getTxQueueLength()); REQUIRE(0 == alloc.getNumAllocatedFragments()); @@ -270,6 +279,7 @@ TEST_CASE("TxBasic0") REQUIRE((0b10100000U | 27U) == reinterpret_cast(frame.payload)[63]); REQUIRE(frame.timestamp_usec == 1'000'000'003'000ULL); ins.txPop(); + ins.getAllocator().deallocate(frame.payload); REQUIRE(1 == ins.getTxQueueLength()); REQUIRE(1 == alloc.getNumAllocatedFragments()); REQUIRE(1 == ins.txPeek(frame)); @@ -283,6 +293,7 @@ TEST_CASE("TxBasic0") REQUIRE((0b01000000U | 27U) == reinterpret_cast(frame.payload)[63]); // Tail REQUIRE(frame.timestamp_usec == 1'000'000'003'000ULL); ins.txPop(); + ins.getAllocator().deallocate(frame.payload); REQUIRE(0 == ins.getTxQueueLength()); REQUIRE(0 == alloc.getNumAllocatedFragments()); @@ -294,7 +305,7 @@ TEST_CASE("TxBasic0") REQUIRE(1 == ins.txPush(transfer)); REQUIRE(1 == ins.getTxQueueLength()); REQUIRE(1 == alloc.getNumAllocatedFragments()); - REQUIRE(40 > alloc.getTotalAllocatedAmount()); + REQUIRE(60 > alloc.getTotalAllocatedAmount()); REQUIRE(ins.getTxQueueRoot()->deadline_usec == 1'000'000'004'000ULL); REQUIRE(ins.getTxQueueRoot()->payload_size == 1); REQUIRE(ins.getTxQueueRoot()->isStartOfTransfer()); @@ -305,6 +316,7 @@ TEST_CASE("TxBasic0") REQUIRE((0b11100000U | 28U) == reinterpret_cast(frame.payload)[0]); REQUIRE(frame.timestamp_usec == 1'000'000'004'000ULL); ins.txPop(); + ins.getAllocator().deallocate(frame.payload); REQUIRE(0 == ins.getTxQueueLength()); REQUIRE(0 == alloc.getNumAllocatedFragments()); From efaa25b94604bfe3ca373a8f0687e0f660249c83 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 23 Feb 2020 04:19:06 +0200 Subject: [PATCH 060/100] Randomized roundtrip test --- tests/CMakeLists.txt | 17 +-- tests/test_public_roundtrip.cpp | 243 ++++++++++++++++++++++++++++++++ 2 files changed, 252 insertions(+), 8 deletions(-) create mode 100644 tests/test_public_roundtrip.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 98768cde..317cc8db 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -37,6 +37,7 @@ if (NOT NO_STATIC_ANALYSIS) endif () # C options +set(CMAKE_C_STANDARD 11) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wextra -Werror -pedantic -fstrict-aliasing") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wdouble-promotion -Wswitch-enum -Wfloat-equal -Wundef") set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wconversion -Wtype-limits") @@ -56,25 +57,25 @@ add_definitions(-DCATCH_CONFIG_FAST_COMPILE=1) set(common_sources catch/main.cpp ${library_dir}/canard.c ${library_dir}/canard_dsdl.c) -function(gen_test name files compile_definitions compile_features compile_flags link_flags) +function(gen_test name files compile_definitions compile_flags link_flags) add_executable(${name} ${common_sources} ${files}) target_compile_definitions(${name} PUBLIC ${compile_definitions}) - target_compile_features(${name} PUBLIC ${compile_features}) + target_link_libraries(${name} pthread) set_target_properties(${name} PROPERTIES COMPILE_FLAGS "${compile_flags}" LINK_FLAGS "${link_flags}") add_test("run_${name}" "${name}" --rng-seed time) endfunction() function(gen_test_matrix name files compile_definitions) - gen_test("${name}_c99_x64" "${files}" "${compile_definitions}" c_std_99 "-m64" "-m64") - gen_test("${name}_c99_x32" "${files}" "${compile_definitions}" c_std_99 "-m32" "-m32") - gen_test("${name}_c11_x64" "${files}" "${compile_definitions}" c_std_11 "-m64" "-m64") - gen_test("${name}_c11_x32" "${files}" "${compile_definitions}" c_std_11 "-m32" "-m32") + gen_test("${name}_x64" "${files}" "${compile_definitions}" "-m64" "-m64") + gen_test("${name}_x32" "${files}" "${compile_definitions}" "-m32" "-m32") # Coverage is only available for GCC builds. if ((CMAKE_CXX_COMPILER_ID STREQUAL "GNU") AND (CMAKE_BUILD_TYPE STREQUAL "Debug")) - gen_test("${name}_cov" "${files}" "${compile_definitions}" c_std_11 "-g -O0 --coverage" "--coverage") + gen_test("${name}_cov" "${files}" "${compile_definitions}" "-g -O0 --coverage" "--coverage") endif () endfunction() gen_test_matrix(test_private "test_private_crc.cpp;test_private_rx.cpp;test_private_tx.cpp" CANARD_EXPOSE_PRIVATE=1) -gen_test_matrix(test_public "test_public_float16.cpp;test_public_tx.cpp;test_public_rx.cpp;test_self.cpp" "") +gen_test_matrix(test_public + "test_public_float16.cpp;test_public_tx.cpp;test_public_rx.cpp;test_public_roundtrip.cpp;test_self.cpp" + "") diff --git a/tests/test_public_roundtrip.cpp b/tests/test_public_roundtrip.cpp new file mode 100644 index 00000000..1ac199ae --- /dev/null +++ b/tests/test_public_roundtrip.cpp @@ -0,0 +1,243 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. + +#include "helpers.hpp" +#include "exposed.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +TEST_CASE("RoundtripSimple") +{ + using helpers::getRandomNatural; + + struct TxState + { + CanardTransferKind transfer_kind{}; + CanardPriority priority{}; + CanardPortID port_id{}; + std::size_t payload_size_max{}; + CanardTransferID transfer_id{}; + CanardRxSubscription subscription{}; + }; + + helpers::Instance ins_rx; + ins_rx.setNodeID(111); + + const auto get_random_priority = []() { + return static_cast(getRandomNatural(CANARD_PRIORITY_MAX + 1U)); + }; + std::array tx_states{ + TxState{CanardTransferKindMessage, get_random_priority(), 32767, 1000}, + TxState{CanardTransferKindMessage, get_random_priority(), 511, 0}, + TxState{CanardTransferKindMessage, get_random_priority(), 0, 13}, + TxState{CanardTransferKindRequest, get_random_priority(), 511, 900}, + TxState{CanardTransferKindRequest, get_random_priority(), 0, 0}, + TxState{CanardTransferKindResponse, get_random_priority(), 0, 1}, + }; + std::size_t rx_worst_case_memory_consumption = 0; + for (auto& s : tx_states) + { + REQUIRE(1 == ins_rx.rxSubscribe(s.transfer_kind, + s.port_id, + s.payload_size_max, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + s.subscription)); + // The true worst case is 128 times larger, but there is only one transmitting node. + rx_worst_case_memory_consumption += sizeof(exposed::RxSession) + s.payload_size_max; + } + ins_rx.getAllocator().setAllocationCeiling(rx_worst_case_memory_consumption); // This is guaranteed to be enough. + + helpers::Instance ins_tx; + ins_tx.setNodeID(99); + ins_tx.getAllocator().setAllocationCeiling(1024 * 1024 * 1024); + + std::unordered_map pending_transfers; + + std::atomic transfer_counter = 0; + std::atomic frames_in_flight = 0; + std::uint64_t peak_frames_in_flight = 0; + + std::mutex lock; + std::atomic keep_going = true; + + const auto writer_thread_fun = [&]() { + while (keep_going) + { + auto& st = tx_states.at(getRandomNatural(std::size(tx_states))); + + // Generate random payload. The size may be larger than expected to test the implicit truncation rule. + const auto payload_size = getRandomNatural(st.payload_size_max + 1024U); + auto* const payload = static_cast(std::malloc(payload_size)); // NOLINT + std::generate_n(payload, payload_size, [&]() { return static_cast(getRandomNatural(256U)); }); + + // Generate the transfer. + CanardTransfer tran{}; + tran.timestamp_usec = transfer_counter++; + tran.priority = st.priority; + tran.transfer_kind = st.transfer_kind; + tran.port_id = st.port_id; + tran.remote_node_id = + (tran.transfer_kind == CanardTransferKindMessage) ? CANARD_NODE_ID_UNSET : ins_rx.getNodeID(); + tran.transfer_id = (st.transfer_id++) & CANARD_TRANSFER_ID_MAX; + tran.payload_size = payload_size; + tran.payload = payload; + + // Use a random MTU. + ins_tx.setMTU(static_cast(getRandomNatural(256U))); + + // Push the transfer. + bool sleep = false; + { + std::lock_guard locker(lock); + const auto result = ins_tx.txPush(tran); + if (result > 0) + { + pending_transfers.emplace(tran.timestamp_usec, tran); + frames_in_flight += static_cast(result); + peak_frames_in_flight = std::max(peak_frames_in_flight, frames_in_flight); + } + else + { + REQUIRE(result == -CANARD_ERROR_OUT_OF_MEMORY); + sleep = true; + } + } + if (sleep) + { + std::cout << "TX OOM" << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + } + }; + + std::thread writer_thread(writer_thread_fun); + + std::ofstream log_file("roundtrip_frames.log", std::ios::trunc | std::ios::out); + REQUIRE(log_file.good()); + + try + { + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(10); + while (true) + { + std::int8_t result = 0; + CanardFrame frame{}; + { + std::lock_guard locker(lock); + result = ins_tx.txPeek(frame); // Peek-pop form an atomic transaction. + ins_tx.txPop(); // No effect if the queue is empty. + if (result > 0) + { + REQUIRE(frames_in_flight > 0); + --frames_in_flight; + } + } + + if (result == 1) + { + const auto tail = reinterpret_cast(frame.payload)[frame.payload_size - 1U]; + log_file << frame.timestamp_usec << " " // + << std::hex << std::setfill('0') << std::setw(8) << frame.extended_can_id // + << " [" << std::dec << std::setfill(' ') << std::setw(2) << frame.payload_size << "] " // + << (bool(tail & 128U) ? 'S' : ' ') // + << (bool(tail & 64U) ? 'E' : ' ') // + << (bool(tail & 32U) ? 'T' : ' ') // + << " " << std::uint16_t(tail & 31U) // + << '\n'; + + CanardTransfer transfer{}; + result = ins_rx.rxAccept(frame, 0, transfer); + REQUIRE(0 == ins_rx.rxAccept(frame, 1, transfer)); // Redundant interface will never be used here. + if (result == 1) + { + CanardTransfer reference{}; // Fetch the reference transfer from the list of pending. + { + std::lock_guard locker(lock); + const auto pt_it = pending_transfers.find(transfer.timestamp_usec); + REQUIRE(pt_it != pending_transfers.end()); + reference = pt_it->second; + pending_transfers.erase(pt_it); + } + + REQUIRE(transfer.timestamp_usec == reference.timestamp_usec); + REQUIRE(transfer.priority == reference.priority); + REQUIRE(transfer.transfer_kind == reference.transfer_kind); + REQUIRE(transfer.port_id == reference.port_id); + REQUIRE(transfer.remote_node_id == ins_tx.getNodeID()); + REQUIRE(transfer.transfer_id == reference.transfer_id); + // The payload size is not checked because the variance is huge due to padding and truncation. + if (transfer.payload != nullptr) + { + REQUIRE(0 == std::memcmp(transfer.payload, + reference.payload, + std::min(transfer.payload_size, reference.payload_size))); + } + else + { + REQUIRE(transfer.payload_size == 0U); + } + + ins_rx.getAllocator().deallocate(transfer.payload); + std::free(const_cast(reference.payload)); // NOLINT + } + else + { + REQUIRE(result == 0); + } + } + else + { + REQUIRE(result == 0); + if (!keep_going) + { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + ins_tx.getAllocator().deallocate(frame.payload); + + if (std::chrono::steady_clock::now() > deadline) + { + keep_going = false; + } + } + } + catch (...) + { + keep_going = false; + writer_thread.detach(); + throw; + } + + writer_thread.join(); + + REQUIRE(0 == frames_in_flight); + + std::cout << "TOTAL TRANSFERS: " << transfer_counter << "; OF THEM LOST: " << std::size(pending_transfers) + << std::endl; + std::cout << "PEAK FRAMES IN FLIGHT: " << peak_frames_in_flight << std::endl; + + std::size_t i = 0; + for (const auto [k, v] : pending_transfers) + { + REQUIRE(k == v.timestamp_usec); + std::cout << "#" << i << "/" << std::size(pending_transfers) << ":" // + << " ts=" << v.timestamp_usec // + << " prio=" << static_cast(v.priority) // + << " kind=" << static_cast(v.transfer_kind) // + << " port=" << v.port_id // + << " nid=" << static_cast(v.remote_node_id) // + << " tid=" << static_cast(v.transfer_id) // + << std::endl; + } + + REQUIRE(0 == std::size(pending_transfers)); +} From 894f3a12a772379f2bae62afd2b28c067dc51c96 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 23 Feb 2020 04:35:23 +0200 Subject: [PATCH 061/100] Fix AVR compile test --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 2cd66844..23a9d80d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -95,7 +95,7 @@ matrix: script: # This is a trivial test where we only check whether it compiles at all with all warnings enabled. # TODO: Write unit tests and run them on an emulator. - - avr-gcc libcanard/*.c -c -std=c99 -mmcu=at90can64 -Wall -Wextra -Werror -pedantic -Wconversion -Wtype-limits + - avr-gcc libcanard/*.c -c -std=c11 -mmcu=at90can64 -Wall -Wextra -Werror -pedantic -Wconversion -Wtype-limits git: depth: false # Disable shallow clone because it is incompatible with SonarCloud From c579ae231e9e365569642f4229d8fff98cff6833 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 23 Feb 2020 06:56:16 +0200 Subject: [PATCH 062/100] Refactor the TX memory management API --- libcanard/canard.c | 115 +++++++--------- libcanard/canard.h | 35 +++-- tests/exposed.hpp | 16 +-- tests/helpers.hpp | 7 +- tests/test_private_tx.cpp | 20 +-- tests/test_public_roundtrip.cpp | 34 +++-- tests/test_public_tx.cpp | 228 +++++++++++++++++--------------- tests/test_self.cpp | 6 - 8 files changed, 221 insertions(+), 240 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index c85b0bc1..943c8377 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -90,19 +90,21 @@ CANARD_PRIVATE TransferCRC crcAdd(const TransferCRC crc, const size_t size, cons // --------------------------------------------- TRANSMISSION --------------------------------------------- -/// The memory requirement model provided in the documentation assumes that the maximum size of this structure never -/// exceeds 40 bytes on any conventional platform. The sizeof() of this structure, per the C standard, assumes that -/// the length of the flex array member is zero. -/// A user that needs a detailed analysis of the worst-case memory consumption may compute the size of this structure -/// for the particular platform at hand manually or by evaluating its sizeof(). -/// The fields are ordered to minimize the amount of padding on all conventional platforms. +/// This is a subclass of CanardFrame. A pointer to this type can be cast to CanardFrame safely. +/// This is standard-compliant. The paragraph 6.7.2.1.15 says: +/// A pointer to a structure object, suitably converted, points to its initial member (or if that member is a +/// bit-field, then to the unit in which it resides), and vice versa. There may be unnamed padding within a +/// structure object, but not at its beginning. typedef struct CanardInternalTxQueueItem { - CanardMicrosecond deadline_usec; + CanardFrame frame; struct CanardInternalTxQueueItem* next; - uint8_t* payload; - size_t payload_size; - uint32_t id; + + // Intentional violation of MISRA: this flex array is the lesser of three evils. The other two are: + // - Make the payload pointer point to the remainder of the allocated memory following this structure. + // The pointer is bad because it requires us to use pointer arithmetics. + // - Use a separate memory allocation for data. This is terribly wasteful (both time & memory). + uint8_t payload_buffer[]; } CanardInternalTxQueueItem; CANARD_PRIVATE uint32_t txMakeMessageSessionSpecifier(const CanardPortID subject_id, const CanardNodeID src_node_id); @@ -256,31 +258,15 @@ CANARD_PRIVATE CanardInternalTxQueueItem* txAllocateQueueItem(CanardInstance* co { CANARD_ASSERT(ins != NULL); CANARD_ASSERT(payload_size > 0U); - - // Increase the size of the payload if necessary to ensure that the following structure is well-aligned. - static_assert((_Alignof(max_align_t) & (_Alignof(max_align_t) - 1U)) == 0U, "Max align shall be a power of 2"); - static const size_t max_align = _Alignof(max_align_t); - const size_t aligned_payload_size = (payload_size + max_align - 1U) & ~(max_align - 1U); - CANARD_ASSERT(aligned_payload_size >= payload_size); - CANARD_ASSERT(aligned_payload_size < (payload_size + max_align)); - - // Allocate one fragment of dynamic memory to store both the payload buffer and the metadata. - // The payload buffer is allocated in the beginning of the fragment. This allows us to hand it over to the - // application later so that when it is deallocated the metadata storage is also destroyed. - CanardInternalTxQueueItem* out = NULL; - uint8_t* const ptr = (uint8_t*) ins->memory_allocate(ins, sizeof(CanardInternalTxQueueItem) + aligned_payload_size); - if (ptr != NULL) + CanardInternalTxQueueItem* const out = + (CanardInternalTxQueueItem*) ins->memory_allocate(ins, sizeof(CanardInternalTxQueueItem) + payload_size); + if (out != NULL) { - out = (CanardInternalTxQueueItem*) (void*) (ptr + aligned_payload_size); - // Normally, we should check that the pointer is aligned here. We don't do that here because there is a bug - // in Glibc on x86: the alignment of malloc() is 8 bytes, whereas alignof(max_align_t) is 16 bytes. - // I tried filing it but their bug tracker requires registration via email, so I sent them one. - // Until the bug in Glibc is fixed, the assertion check should not be here because it would fail the unit tests. - out->next = NULL; - out->deadline_usec = deadline_usec; - out->payload_size = payload_size; - out->payload = ptr; - out->id = id; + out->next = NULL; + out->frame.timestamp_usec = deadline_usec; + out->frame.payload_size = payload_size; + out->frame.payload = out->payload_buffer; + out->frame.extended_can_id = id; } return out; } @@ -293,19 +279,19 @@ CANARD_PRIVATE CanardInternalTxQueueItem* txFindQueueSupremum(const CanardInstan CANARD_ASSERT(ins != NULL); CANARD_ASSERT(can_id <= CAN_EXT_ID_MASK); CanardInternalTxQueueItem* out = ins->_tx_queue; - if ((NULL == out) || (out->id > can_id)) + if ((NULL == out) || (out->frame.extended_can_id > can_id)) { out = NULL; } else { // TODO The linear search should be replaced with O(log n) at least. Please help us here. - while ((out != NULL) && (out->next != NULL) && (out->next->id <= can_id)) + while ((out != NULL) && (out->next != NULL) && (out->next->frame.extended_can_id <= can_id)) { out = out->next; } } - CANARD_ASSERT((out == NULL) || (out->id <= can_id)); + CANARD_ASSERT((out == NULL) || (out->frame.extended_can_id <= can_id)); return out; } @@ -339,14 +325,16 @@ CANARD_PRIVATE int32_t txPushSingleFrame(CanardInstance* const ins, { CANARD_ASSERT(payload != NULL); // Clang-Tidy raises an error recommending the use of memcpy_s() instead. - (void) memcpy(tqi->payload, payload, payload_size); // NOLINT + // We ignore it because the safe functions are poorly supported; reliance on them may limit the portability. + (void) memcpy(&tqi->payload_buffer[0], payload, payload_size); // NOLINT } // Clang-Tidy raises an error recommending the use of memset_s() instead. - (void) memset(tqi->payload + payload_size, PADDING_BYTE_VALUE, padding_size); // NOLINT + // We ignore it because the safe functions are poorly supported; reliance on them may limit the portability. + (void) memset(&tqi->payload_buffer[payload_size], PADDING_BYTE_VALUE, padding_size); // NOLINT - tqi->payload[frame_payload_size - 1U] = txMakeTailByte(true, true, true, transfer_id); - CanardInternalTxQueueItem* const sup = txFindQueueSupremum(ins, can_id); + tqi->payload_buffer[frame_payload_size - 1U] = txMakeTailByte(true, true, true, transfer_id); + CanardInternalTxQueueItem* const sup = txFindQueueSupremum(ins, can_id); if (sup != NULL) { tqi->next = sup->next; @@ -435,7 +423,8 @@ CANARD_PRIVATE int32_t txPushMultiFrame(CanardInstance* const ins, move_size = frame_payload_size; } // Clang-Tidy raises an error recommending the use of memcpy_s() instead. - (void) memcpy(tail->payload, payload_ptr, move_size); // NOLINT + // We ignore it because the safe functions are poorly supported; reliance on them may limit the portability. + (void) memcpy(&tail->payload_buffer[0], payload_ptr, move_size); // NOLINT frame_offset = frame_offset + move_size; offset += move_size; payload_ptr += move_size; @@ -447,7 +436,7 @@ CANARD_PRIVATE int32_t txPushMultiFrame(CanardInstance* const ins, // Insert padding -- only in the last frame. Don't forget to include padding into the CRC. while ((frame_offset + CRC_SIZE_BYTES) < frame_payload_size) { - tail->payload[frame_offset] = PADDING_BYTE_VALUE; + tail->payload_buffer[frame_offset] = PADDING_BYTE_VALUE; ++frame_offset; crc = crcAddByte(crc, PADDING_BYTE_VALUE); } @@ -455,21 +444,21 @@ CANARD_PRIVATE int32_t txPushMultiFrame(CanardInstance* const ins, // Insert the CRC. if ((frame_offset < frame_payload_size) && (offset == payload_size)) { - tail->payload[frame_offset] = (uint8_t)(crc >> BITS_PER_BYTE); + tail->payload_buffer[frame_offset] = (uint8_t)(crc >> BITS_PER_BYTE); ++frame_offset; ++offset; } if ((frame_offset < frame_payload_size) && (offset > payload_size)) { - tail->payload[frame_offset] = (uint8_t)(crc & BYTE_MAX); + tail->payload_buffer[frame_offset] = (uint8_t)(crc & BYTE_MAX); ++frame_offset; ++offset; } } // Finalize the frame. - CANARD_ASSERT((frame_offset + 1U) == tail->payload_size); - tail->payload[frame_offset] = + CANARD_ASSERT((frame_offset + 1U) == tail->frame.payload_size); + tail->payload_buffer[frame_offset] = txMakeTailByte(start_of_transfer, offset >= payload_size_with_crc, toggle, transfer_id); start_of_transfer = false; toggle = !toggle; @@ -683,6 +672,8 @@ CANARD_PRIVATE int8_t rxSessionWritePayload(CanardInstance* const ins, // the other one is the search of the matching subscription state. // Excepting these two cases, the entire RX pipeline contains neither loops nor recursion. // Intentional violation of MISRA: indexing on a pointer. This is done to avoid pointer arithmetics. + // Clang-Tidy raises an error recommending the use of memcpy_s() instead. + // We ignore it because the safe functions are poorly supported; reliance on them may limit the portability. (void) memcpy(&rxs->payload[rxs->payload_size], payload, bytes_to_copy); // NOLINT NOSONAR rxs->payload_size += bytes_to_copy; CANARD_ASSERT(rxs->payload_size <= payload_size_max); @@ -980,24 +971,19 @@ int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const tran return out; } -int8_t canardTxPeek(const CanardInstance* const ins, CanardFrame* const out_frame) +const CanardFrame* canardTxPeek(const CanardInstance* const ins) { - int8_t out = -CANARD_ERROR_INVALID_ARGUMENT; - if ((ins != NULL) && (out_frame != NULL)) + const CanardFrame* out = NULL; + if ((ins != NULL) && (ins->_tx_queue != NULL)) { - const CanardInternalTxQueueItem* const tqi = ins->_tx_queue; - if (tqi != NULL) - { - out_frame->timestamp_usec = tqi->deadline_usec; - out_frame->extended_can_id = tqi->id; - out_frame->payload_size = tqi->payload_size; - out_frame->payload = tqi->payload; - out = 1; - } - else - { - out = 0; - } + // Return pointer to the TX queue item typed as CanardFrame. Later, the application will be able to free + // the memory allocated for the TX queue item using this pointer typed as CanardFrame. Although it may look + // sketchy, this is actually safe and standard-compliant. The paragraph 6.7.2.1.15 of the C standard says: + // A pointer to a structure object, suitably converted, points to its initial member (or if that member is a + // bit-field, then to the unit in which it resides), and vice versa. There may be unnamed padding within a + // structure object, but not at its beginning. + out = &ins->_tx_queue->frame; + CANARD_ASSERT(((void*) out) == ((void*) ins->_tx_queue)); } return out; } @@ -1006,9 +992,8 @@ void canardTxPop(CanardInstance* const ins) { if ((ins != NULL) && (ins->_tx_queue != NULL)) { - CanardInternalTxQueueItem* const next = ins->_tx_queue->next; // The memory is NOT deallocated. The application is responsible for that. - ins->_tx_queue = next; + ins->_tx_queue = ins->_tx_queue->next; } } diff --git a/libcanard/canard.h b/libcanard/canard.h index 73f7c8d7..b207c136 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -280,9 +280,7 @@ CanardInstance canardInit(const CanardMemoryAllocate memory_allocate, const Cana /// /// The memory allocation requirement is one allocation per transport frame. /// A single-frame transfer takes one allocation; a multi-frame transfer of N frames takes N allocations. -/// The maximum size of each allocation is sizeof(CanardInternalTxQueueItem) plus MTU size padded to max_align_t, -/// where sizeof(CanardInternalTxQueueItem) is at most 40 bytes on any conventional platform (typically smaller). -/// For example, if the MTU is 64 bytes, the allocation size will never exceed 104 bytes on any conventional platform. +/// The maximum size of each allocation is sizeof(CanardFrame) plus a pointer size plus MTU size. int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfer); /// Access the top element of the prioritized transmission queue. The queue itself is not modified (i.e., the @@ -297,34 +295,31 @@ int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const tran /// the driver layer to implement the discardment of timed-out transport frames. The library does not check it, /// so a frame that is already timed out may be returned here. /// -/// If the queue is empty, the return value is zero and the out_frame is not modified. +/// If the queue is empty or if the argument is NULL, the returned value is NULL. /// -/// If the queue is non-empty, the return value is 1 (one) and the out_frame is populated with the data from -/// the top element (i.e., the next frame awaiting transmission). -/// The payload pointer of the out_frame will point to a dynamically allocated storage. -/// The payload pointer retains validity until explicitly freed by the application. -/// The payload pointer shall not be freed before the entry is removed from the queue by calling canardTxPop(). +/// If the queue is non-empty, the returned value is a pointer to its top element (i.e., the next frame to transmit). +/// The returned pointer points to an object allocated in the dynamic storage; it should be freed by the application +/// by calling CanardInstance::memory_free(). The memory shall not be freed before the entry is removed from the +/// queue by calling canardTxPop(); this is because until canardTxPop() is executed, the library retains ownership +/// of the object. The payload pointer retains validity until explicitly freed by the application; in other words, +/// calling canardTxPop() does not invalidate the object. /// -/// If either of the arguments are NULL, the negated invalid argument error code is returned and no other -/// actions are performed. +/// The payload buffer is located shortly after the object itself, in the same memory fragment. The application shall +/// not attempt to free its memory. /// /// The time complexity is constant. -int8_t canardTxPeek(const CanardInstance* const ins, CanardFrame* const out_frame); +const CanardFrame* canardTxPeek(const CanardInstance* const ins); -/// Remove and free the top element from the prioritized transmission queue. -/// The application should invoke this function after the top frame obtained through canardTxPeek() has been -/// processed and need not be kept anymore (e.g., transmitted successfully, timed out, errored, etc.). +/// Transfer the ownership of the top element of the prioritized transmission queue to the application. +/// The application should invoke this function to remove the top element from the prioritized transmission queue. +/// While the element is removed, it is not invalidated: it is the responsibility of the application to deallocate +/// the memory used by the object later. The object SHALL NOT be deallocated UNTIL this function is invoked. /// /// WARNING: /// Invocation of canardTxPush() may add new elements at the top of the prioritized transmission queue. /// The calling code shall take that into account to eliminate the possibility of data loss due to the frame /// at the top of the queue being unexpectedly replaced between calls of canardTxPeek() and this function. /// -/// AFTER this function is invoked, the application shall free the memory pointed to by the payload data pointer. -/// The time between invocation and the payload buffer deallocation can be arbitrary because the ownership is fully -/// transferred to the application. This design is intended to facilitate zero-copy enqueueing and transmission. -/// The payload memory SHALL NOT be freed UNTIL this function is invoked. -/// /// If the input argument is NULL or if the transmission queue is empty, the function has no effect. /// /// The time complexity is constant. diff --git a/tests/exposed.hpp b/tests/exposed.hpp index 60f9a10c..596ccaff 100644 --- a/tests/exposed.hpp +++ b/tests/exposed.hpp @@ -16,18 +16,18 @@ using TransferCRC = std::uint16_t; struct TxQueueItem final { - std::uint64_t deadline_usec = 0; - TxQueueItem* next = nullptr; - std::uint8_t* payload = nullptr; - std::size_t payload_size = 0; - std::uint32_t id = 0; + CanardFrame frame{}; + TxQueueItem* next = nullptr; - [[nodiscard]] auto getPayloadByte(const std::size_t offset) const -> std::uint8_t { return payload[offset]; } + [[nodiscard]] auto getPayloadByte(const std::size_t offset) const -> std::uint8_t + { + return reinterpret_cast(frame.payload)[offset]; + } [[nodiscard]] auto getTailByte() const { - REQUIRE(payload_size >= 1U); - return getPayloadByte(payload_size - 1U); + REQUIRE(frame.payload_size >= 1U); + return getPayloadByte(frame.payload_size - 1U); } [[nodiscard]] auto isStartOfTransfer() const { return (getTailByte() & 128U) != 0; } diff --git a/tests/helpers.hpp b/tests/helpers.hpp index e4da8373..7809f78e 100644 --- a/tests/helpers.hpp +++ b/tests/helpers.hpp @@ -99,10 +99,7 @@ class TestAllocator { std::unique_lock locker(lock_); const auto it = allocated_.find(pointer); - if (it == std::end(allocated_)) - { - throw std::logic_error("Heap corruption: an attempt to deallocate memory that is not allocated"); - } + REQUIRE(it != std::end(allocated_)); // Catch an attempt to deallocate memory that is not allocated. // Damage the memory to make sure it's not used after deallocation. std::uniform_int_distribution dist(0, 255U); std::generate_n(reinterpret_cast(pointer), it->second, [&]() { @@ -165,7 +162,7 @@ class Instance [[nodiscard]] auto txPush(const CanardTransfer& transfer) { return canardTxPush(&canard_, &transfer); } - [[nodiscard]] auto txPeek(CanardFrame& out_frame) const { return canardTxPeek(&canard_, &out_frame); } + [[nodiscard]] auto txPeek() const { return canardTxPeek(&canard_); } void txPop() { canardTxPop(&canard_); } diff --git a/tests/test_private_tx.cpp b/tests/test_private_tx.cpp index 08df4872..983d7da0 100644 --- a/tests/test_private_tx.cpp +++ b/tests/test_private_tx.cpp @@ -183,16 +183,16 @@ TEST_CASE("txFindQueueSupremum") REQUIRE(nullptr == find((1UL << 29U) - 1U)); TxQueueItem a{}; - a.id = 1000; - ins._tx_queue = reinterpret_cast(&a); + a.frame.extended_can_id = 1000; + ins._tx_queue = reinterpret_cast(&a); REQUIRE(nullptr == find(999)); REQUIRE(&a == find(1000)); REQUIRE(&a == find(1001)); TxQueueItem b{}; - b.id = 1010; - a.next = &b; + b.frame.extended_can_id = 1010; + a.next = &b; REQUIRE(nullptr == find(999)); REQUIRE(&a == find(1000)); @@ -202,12 +202,12 @@ TEST_CASE("txFindQueueSupremum") REQUIRE(&b == find(1011)); TxQueueItem c{}; - c.id = 990; - c.next = &a; - ins._tx_queue = reinterpret_cast(&c); - REQUIRE(reinterpret_cast(ins._tx_queue)->id == 990); // Make sure the list is assembled correctly. - REQUIRE(reinterpret_cast(ins._tx_queue)->next->id == 1000); - REQUIRE(reinterpret_cast(ins._tx_queue)->next->next->id == 1010); + c.frame.extended_can_id = 990; + c.next = &a; + ins._tx_queue = reinterpret_cast(&c); + REQUIRE(reinterpret_cast(ins._tx_queue)->frame.extended_can_id == 990); + REQUIRE(reinterpret_cast(ins._tx_queue)->next->frame.extended_can_id == 1000); + REQUIRE(reinterpret_cast(ins._tx_queue)->next->next->frame.extended_can_id == 1010); REQUIRE(reinterpret_cast(ins._tx_queue)->next->next->next == nullptr); REQUIRE(nullptr == find(989)); diff --git a/tests/test_public_roundtrip.cpp b/tests/test_public_roundtrip.cpp index 1ac199ae..efe51485 100644 --- a/tests/test_public_roundtrip.cpp +++ b/tests/test_public_roundtrip.cpp @@ -128,34 +128,33 @@ TEST_CASE("RoundtripSimple") const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(10); while (true) { - std::int8_t result = 0; - CanardFrame frame{}; + const CanardFrame* frame = nullptr; { std::lock_guard locker(lock); - result = ins_tx.txPeek(frame); // Peek-pop form an atomic transaction. - ins_tx.txPop(); // No effect if the queue is empty. - if (result > 0) + frame = ins_tx.txPeek(); // Peek-pop form an atomic transaction. + ins_tx.txPop(); // No effect if the queue is empty. + if (frame != nullptr) { REQUIRE(frames_in_flight > 0); --frames_in_flight; } } - if (result == 1) + if (frame != nullptr) { - const auto tail = reinterpret_cast(frame.payload)[frame.payload_size - 1U]; - log_file << frame.timestamp_usec << " " // - << std::hex << std::setfill('0') << std::setw(8) << frame.extended_can_id // - << " [" << std::dec << std::setfill(' ') << std::setw(2) << frame.payload_size << "] " // - << (bool(tail & 128U) ? 'S' : ' ') // - << (bool(tail & 64U) ? 'E' : ' ') // - << (bool(tail & 32U) ? 'T' : ' ') // - << " " << std::uint16_t(tail & 31U) // + const auto tail = reinterpret_cast(frame->payload)[frame->payload_size - 1U]; + log_file << frame->timestamp_usec << " " // + << std::hex << std::setfill('0') << std::setw(8) << frame->extended_can_id // + << " [" << std::dec << std::setfill(' ') << std::setw(2) << frame->payload_size << "] " // + << (bool(tail & 128U) ? 'S' : ' ') // + << (bool(tail & 64U) ? 'E' : ' ') // + << (bool(tail & 32U) ? 'T' : ' ') // + << " " << std::uint16_t(tail & 31U) // << '\n'; CanardTransfer transfer{}; - result = ins_rx.rxAccept(frame, 0, transfer); - REQUIRE(0 == ins_rx.rxAccept(frame, 1, transfer)); // Redundant interface will never be used here. + std::int8_t result = ins_rx.rxAccept(*frame, 0, transfer); + REQUIRE(0 == ins_rx.rxAccept(*frame, 1, transfer)); // Redundant interface will never be used here. if (result == 1) { CanardTransfer reference{}; // Fetch the reference transfer from the list of pending. @@ -195,14 +194,13 @@ TEST_CASE("RoundtripSimple") } else { - REQUIRE(result == 0); if (!keep_going) { break; } std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - ins_tx.getAllocator().deallocate(frame.payload); + ins_tx.getAllocator().deallocate(frame); if (std::chrono::steady_clock::now() > deadline) { diff --git a/tests/test_public_tx.cpp b/tests/test_public_tx.cpp index d8fdac87..4a7442d0 100644 --- a/tests/test_public_tx.cpp +++ b/tests/test_public_tx.cpp @@ -43,9 +43,9 @@ TEST_CASE("TxBasic0") REQUIRE(1 == alloc.getNumAllocatedFragments()); REQUIRE(10 < alloc.getTotalAllocatedAmount()); REQUIRE(80 > alloc.getTotalAllocatedAmount()); - REQUIRE(ins.getTxQueueRoot()->deadline_usec == 1'000'000'000'000ULL); - REQUIRE(ins.getTxQueueRoot()->payload_size == 12); // Three bytes of padding. - REQUIRE(ins.getTxQueueRoot()->getPayloadByte(0) == 0); // Payload start. + REQUIRE(ins.getTxQueueRoot()->frame.timestamp_usec == 1'000'000'000'000ULL); + REQUIRE(ins.getTxQueueRoot()->frame.payload_size == 12); // Three bytes of padding. + REQUIRE(ins.getTxQueueRoot()->getPayloadByte(0) == 0); // Payload start. REQUIRE(ins.getTxQueueRoot()->getPayloadByte(1) == 1); REQUIRE(ins.getTxQueueRoot()->getPayloadByte(2) == 2); REQUIRE(ins.getTxQueueRoot()->getPayloadByte(3) == 3); @@ -77,22 +77,22 @@ TEST_CASE("TxBasic0") { auto q = ins.getTxQueueRoot(); REQUIRE(q != nullptr); - REQUIRE(q->deadline_usec == 1'000'000'000'000ULL); - REQUIRE(q->payload_size == 12); + REQUIRE(q->frame.timestamp_usec == 1'000'000'000'000ULL); + REQUIRE(q->frame.payload_size == 12); REQUIRE(q->isStartOfTransfer()); REQUIRE(q->isEndOfTransfer()); REQUIRE(q->isToggleBitSet()); q = q->next; REQUIRE(q != nullptr); - REQUIRE(q->deadline_usec == 1'000'000'000'100ULL); - REQUIRE(q->payload_size == 8); + REQUIRE(q->frame.timestamp_usec == 1'000'000'000'100ULL); + REQUIRE(q->frame.payload_size == 8); REQUIRE(q->isStartOfTransfer()); REQUIRE(!q->isEndOfTransfer()); REQUIRE(q->isToggleBitSet()); q = q->next; REQUIRE(q != nullptr); - REQUIRE(q->deadline_usec == 1'000'000'000'100ULL); - REQUIRE(q->payload_size == 4); // One leftover, two CRC, one tail. + REQUIRE(q->frame.timestamp_usec == 1'000'000'000'100ULL); + REQUIRE(q->frame.payload_size == 4); // One leftover, two CRC, one tail. REQUIRE(!q->isStartOfTransfer()); REQUIRE(q->isEndOfTransfer()); REQUIRE(!q->isToggleBitSet()); @@ -124,54 +124,58 @@ TEST_CASE("TxBasic0") // Pop the queue. // hex(pyuavcan.transport.commons.crc.CRC16CCITT.new(list(range(8))).value) - constexpr std::uint16_t CRC8 = 0x178DU; - CanardFrame frame{}; - REQUIRE(1 == ins.txPeek(frame)); - REQUIRE(frame.payload_size == 12); - REQUIRE(0 == std::memcmp(frame.payload, payload.data(), 8)); - REQUIRE(0 == reinterpret_cast(frame.payload)[8]); // Padding. - REQUIRE(0 == reinterpret_cast(frame.payload)[9]); // Padding. - REQUIRE(0 == reinterpret_cast(frame.payload)[10]); // Padding. - REQUIRE((0b11100000U | 21U) == reinterpret_cast(frame.payload)[11]); - REQUIRE(frame.timestamp_usec == 1'000'000'000'000ULL); - frame = {}; - REQUIRE(1 == ins.txPeek(frame)); // Make sure we get the same frame again. - REQUIRE(frame.payload_size == 12); - REQUIRE(0 == std::memcmp(frame.payload, payload.data(), 8)); - REQUIRE((0b11100000U | 21U) == reinterpret_cast(frame.payload)[11]); - REQUIRE(frame.timestamp_usec == 1'000'000'000'000ULL); + constexpr std::uint16_t CRC8 = 0x178DU; + const CanardFrame* frame = ins.txPeek(); + REQUIRE(nullptr != frame); + REQUIRE(frame->payload_size == 12); + REQUIRE(0 == std::memcmp(frame->payload, payload.data(), 8)); + REQUIRE(0 == reinterpret_cast(frame->payload)[8]); // Padding. + REQUIRE(0 == reinterpret_cast(frame->payload)[9]); // Padding. + REQUIRE(0 == reinterpret_cast(frame->payload)[10]); // Padding. + REQUIRE((0b11100000U | 21U) == reinterpret_cast(frame->payload)[11]); + REQUIRE(frame->timestamp_usec == 1'000'000'000'000ULL); + frame = ins.txPeek(); + REQUIRE(nullptr != frame); // Make sure we get the same frame again. + REQUIRE(frame->payload_size == 12); + REQUIRE(0 == std::memcmp(frame->payload, payload.data(), 8)); + REQUIRE((0b11100000U | 21U) == reinterpret_cast(frame->payload)[11]); + REQUIRE(frame->timestamp_usec == 1'000'000'000'000ULL); ins.txPop(); - ins.getAllocator().deallocate(frame.payload); + ins.getAllocator().deallocate(frame); REQUIRE(2 == ins.getTxQueueLength()); REQUIRE(2 == alloc.getNumAllocatedFragments()); - REQUIRE(1 == ins.txPeek(frame)); - REQUIRE(frame.payload_size == 8); - REQUIRE(0 == std::memcmp(frame.payload, payload.data(), 7)); - REQUIRE((0b10100000U | 22U) == reinterpret_cast(frame.payload)[7]); - REQUIRE(frame.timestamp_usec == 1'000'000'000'100ULL); + frame = ins.txPeek(); + REQUIRE(nullptr != frame); + REQUIRE(frame->payload_size == 8); + REQUIRE(0 == std::memcmp(frame->payload, payload.data(), 7)); + REQUIRE((0b10100000U | 22U) == reinterpret_cast(frame->payload)[7]); + REQUIRE(frame->timestamp_usec == 1'000'000'000'100ULL); ins.txPop(); - ins.getAllocator().deallocate(frame.payload); + ins.getAllocator().deallocate(frame); REQUIRE(1 == ins.getTxQueueLength()); REQUIRE(1 == alloc.getNumAllocatedFragments()); - REQUIRE(1 == ins.txPeek(frame)); - REQUIRE(frame.payload_size == 4); - REQUIRE(0 == std::memcmp(frame.payload, payload.data() + 7U, 1)); - REQUIRE((CRC8 >> 8U) == reinterpret_cast(frame.payload)[1]); - REQUIRE((CRC8 & 0xFFU) == reinterpret_cast(frame.payload)[2]); - REQUIRE((0b01000000U | 22U) == reinterpret_cast(frame.payload)[3]); - REQUIRE(frame.timestamp_usec == 1'000'000'000'100ULL); + frame = ins.txPeek(); + REQUIRE(nullptr != frame); + REQUIRE(frame->payload_size == 4); + REQUIRE(0 == std::memcmp(frame->payload, payload.data() + 7U, 1)); + REQUIRE((CRC8 >> 8U) == reinterpret_cast(frame->payload)[1]); + REQUIRE((CRC8 & 0xFFU) == reinterpret_cast(frame->payload)[2]); + REQUIRE((0b01000000U | 22U) == reinterpret_cast(frame->payload)[3]); + REQUIRE(frame->timestamp_usec == 1'000'000'000'100ULL); ins.txPop(); - ins.getAllocator().deallocate(frame.payload); + ins.getAllocator().deallocate(frame); REQUIRE(0 == ins.getTxQueueLength()); REQUIRE(nullptr == ins.getTxQueueRoot()); REQUIRE(0 == alloc.getNumAllocatedFragments()); - REQUIRE(0 == ins.txPeek(frame)); + frame = ins.txPeek(); + REQUIRE(nullptr == frame); ins.txPop(); // Invocation when empty has no effect. ins.txPop(); REQUIRE(0 == ins.getTxQueueLength()); REQUIRE(nullptr == ins.getTxQueueRoot()); REQUIRE(0 == alloc.getNumAllocatedFragments()); - REQUIRE(0 == ins.txPeek(frame)); + frame = ins.txPeek(); + REQUIRE(nullptr == frame); alloc.setAllocationCeiling(1000); @@ -189,36 +193,39 @@ TEST_CASE("TxBasic0") REQUIRE(40 < alloc.getTotalAllocatedAmount()); REQUIRE(220 > alloc.getTotalAllocatedAmount()); // Read the generated frames. - REQUIRE(1 == ins.txPeek(frame)); - REQUIRE(frame.payload_size == 32); - REQUIRE(0 == std::memcmp(frame.payload, payload.data(), 31)); - REQUIRE((0b10100000U | 25U) == reinterpret_cast(frame.payload)[31]); - REQUIRE(frame.timestamp_usec == 1'000'000'001'000ULL); + frame = ins.txPeek(); + REQUIRE(nullptr != frame); + REQUIRE(frame->payload_size == 32); + REQUIRE(0 == std::memcmp(frame->payload, payload.data(), 31)); + REQUIRE((0b10100000U | 25U) == reinterpret_cast(frame->payload)[31]); + REQUIRE(frame->timestamp_usec == 1'000'000'001'000ULL); ins.txPop(); - ins.getAllocator().deallocate(frame.payload); + ins.getAllocator().deallocate(frame); REQUIRE(2 == ins.getTxQueueLength()); REQUIRE(2 == alloc.getNumAllocatedFragments()); - REQUIRE(1 == ins.txPeek(frame)); - REQUIRE(frame.payload_size == 32); - REQUIRE(0 == std::memcmp(frame.payload, payload.data() + 31U, 30)); - REQUIRE((CRC61 >> 8U) == reinterpret_cast(frame.payload)[30]); - REQUIRE((0b00000000U | 25U) == reinterpret_cast(frame.payload)[31]); - REQUIRE(frame.timestamp_usec == 1'000'000'001'000ULL); + frame = ins.txPeek(); + REQUIRE(nullptr != frame); + REQUIRE(frame->payload_size == 32); + REQUIRE(0 == std::memcmp(frame->payload, payload.data() + 31U, 30)); + REQUIRE((CRC61 >> 8U) == reinterpret_cast(frame->payload)[30]); + REQUIRE((0b00000000U | 25U) == reinterpret_cast(frame->payload)[31]); + REQUIRE(frame->timestamp_usec == 1'000'000'001'000ULL); ins.txPop(); - ins.getAllocator().deallocate(frame.payload); + ins.getAllocator().deallocate(frame); REQUIRE(1 == ins.getTxQueueLength()); REQUIRE(1 == alloc.getNumAllocatedFragments()); - REQUIRE(1 == ins.txPeek(frame)); - REQUIRE(frame.payload_size == 2); // The last byte of CRC plus the tail byte. - REQUIRE((CRC61 & 0xFFU) == reinterpret_cast(frame.payload)[0]); - REQUIRE((0b01100000U | 25U) == reinterpret_cast(frame.payload)[1]); - REQUIRE(frame.timestamp_usec == 1'000'000'001'000ULL); + frame = ins.txPeek(); + REQUIRE(nullptr != frame); + REQUIRE(frame->payload_size == 2); // The last byte of CRC plus the tail byte. + REQUIRE((CRC61 & 0xFFU) == reinterpret_cast(frame->payload)[0]); + REQUIRE((0b01100000U | 25U) == reinterpret_cast(frame->payload)[1]); + REQUIRE(frame->timestamp_usec == 1'000'000'001'000ULL); ins.txPop(); - ins.getAllocator().deallocate(frame.payload); + ins.getAllocator().deallocate(frame); REQUIRE(0 == ins.getTxQueueLength()); REQUIRE(0 == alloc.getNumAllocatedFragments()); - // Multi-frame, success. CRC is in the last frame. + // Multi-frame, success. CRC is in the last frame-> // hex(pyuavcan.transport.commons.crc.CRC16CCITT.new(list(range(62))).value) constexpr std::uint16_t CRC62 = 0xA3AEU; ins.setMTU(32); @@ -232,32 +239,35 @@ TEST_CASE("TxBasic0") REQUIRE(40 < alloc.getTotalAllocatedAmount()); REQUIRE(220 > alloc.getTotalAllocatedAmount()); // Read the generated frames. - REQUIRE(1 == ins.txPeek(frame)); - REQUIRE(frame.payload_size == 32); - REQUIRE(0 == std::memcmp(frame.payload, payload.data(), 31)); - REQUIRE((0b10100000U | 26U) == reinterpret_cast(frame.payload)[31]); - REQUIRE(frame.timestamp_usec == 1'000'000'002'000ULL); + frame = ins.txPeek(); + REQUIRE(nullptr != frame); + REQUIRE(frame->payload_size == 32); + REQUIRE(0 == std::memcmp(frame->payload, payload.data(), 31)); + REQUIRE((0b10100000U | 26U) == reinterpret_cast(frame->payload)[31]); + REQUIRE(frame->timestamp_usec == 1'000'000'002'000ULL); ins.txPop(); - ins.getAllocator().deallocate(frame.payload); + ins.getAllocator().deallocate(frame); REQUIRE(2 == ins.getTxQueueLength()); REQUIRE(2 == alloc.getNumAllocatedFragments()); - REQUIRE(1 == ins.txPeek(frame)); - REQUIRE(frame.payload_size == 32); - REQUIRE(0 == std::memcmp(frame.payload, payload.data() + 31U, 31)); - REQUIRE((0b00000000U | 26U) == reinterpret_cast(frame.payload)[31]); - REQUIRE(frame.timestamp_usec == 1'000'000'002'000ULL); + frame = ins.txPeek(); + REQUIRE(nullptr != frame); + REQUIRE(frame->payload_size == 32); + REQUIRE(0 == std::memcmp(frame->payload, payload.data() + 31U, 31)); + REQUIRE((0b00000000U | 26U) == reinterpret_cast(frame->payload)[31]); + REQUIRE(frame->timestamp_usec == 1'000'000'002'000ULL); ins.txPop(); - ins.getAllocator().deallocate(frame.payload); + ins.getAllocator().deallocate(frame); REQUIRE(1 == ins.getTxQueueLength()); REQUIRE(1 == alloc.getNumAllocatedFragments()); - REQUIRE(1 == ins.txPeek(frame)); - REQUIRE(frame.payload_size == 3); // The CRC plus the tail byte. - REQUIRE((CRC62 >> 8U) == reinterpret_cast(frame.payload)[0]); - REQUIRE((CRC62 & 0xFFU) == reinterpret_cast(frame.payload)[1]); - REQUIRE((0b01100000U | 26U) == reinterpret_cast(frame.payload)[2]); - REQUIRE(frame.timestamp_usec == 1'000'000'002'000ULL); + frame = ins.txPeek(); + REQUIRE(nullptr != frame); + REQUIRE(frame->payload_size == 3); // The CRC plus the tail byte. + REQUIRE((CRC62 >> 8U) == reinterpret_cast(frame->payload)[0]); + REQUIRE((CRC62 & 0xFFU) == reinterpret_cast(frame->payload)[1]); + REQUIRE((0b01100000U | 26U) == reinterpret_cast(frame->payload)[2]); + REQUIRE(frame->timestamp_usec == 1'000'000'002'000ULL); ins.txPop(); - ins.getAllocator().deallocate(frame.payload); + ins.getAllocator().deallocate(frame); REQUIRE(0 == ins.getTxQueueLength()); REQUIRE(0 == alloc.getNumAllocatedFragments()); @@ -273,27 +283,29 @@ TEST_CASE("TxBasic0") REQUIRE(2 == ins.getTxQueueLength()); REQUIRE(2 == alloc.getNumAllocatedFragments()); // Read the generated frames. - REQUIRE(1 == ins.txPeek(frame)); - REQUIRE(frame.payload_size == 64); - REQUIRE(0 == std::memcmp(frame.payload, payload.data(), 63)); - REQUIRE((0b10100000U | 27U) == reinterpret_cast(frame.payload)[63]); - REQUIRE(frame.timestamp_usec == 1'000'000'003'000ULL); + frame = ins.txPeek(); + REQUIRE(nullptr != frame); + REQUIRE(frame->payload_size == 64); + REQUIRE(0 == std::memcmp(frame->payload, payload.data(), 63)); + REQUIRE((0b10100000U | 27U) == reinterpret_cast(frame->payload)[63]); + REQUIRE(frame->timestamp_usec == 1'000'000'003'000ULL); ins.txPop(); - ins.getAllocator().deallocate(frame.payload); + ins.getAllocator().deallocate(frame); REQUIRE(1 == ins.getTxQueueLength()); REQUIRE(1 == alloc.getNumAllocatedFragments()); - REQUIRE(1 == ins.txPeek(frame)); - REQUIRE(frame.payload_size == 64); - REQUIRE(0 == std::memcmp(frame.payload, payload.data() + 63U, 49)); - REQUIRE(std::all_of(reinterpret_cast(frame.payload) + 49, // Check padding. - reinterpret_cast(frame.payload) + 61, + frame = ins.txPeek(); + REQUIRE(nullptr != frame); + REQUIRE(frame->payload_size == 64); + REQUIRE(0 == std::memcmp(frame->payload, payload.data() + 63U, 49)); + REQUIRE(std::all_of(reinterpret_cast(frame->payload) + 49, // Check padding. + reinterpret_cast(frame->payload) + 61, [](auto x) { return x == 0U; })); - REQUIRE((CRC112Padding12 >> 8U) == reinterpret_cast(frame.payload)[61]); // CRC - REQUIRE((CRC112Padding12 & 0xFFU) == reinterpret_cast(frame.payload)[62]); // CRC - REQUIRE((0b01000000U | 27U) == reinterpret_cast(frame.payload)[63]); // Tail - REQUIRE(frame.timestamp_usec == 1'000'000'003'000ULL); + REQUIRE((CRC112Padding12 >> 8U) == reinterpret_cast(frame->payload)[61]); // CRC + REQUIRE((CRC112Padding12 & 0xFFU) == reinterpret_cast(frame->payload)[62]); // CRC + REQUIRE((0b01000000U | 27U) == reinterpret_cast(frame->payload)[63]); // Tail + REQUIRE(frame->timestamp_usec == 1'000'000'003'000ULL); ins.txPop(); - ins.getAllocator().deallocate(frame.payload); + ins.getAllocator().deallocate(frame); REQUIRE(0 == ins.getTxQueueLength()); REQUIRE(0 == alloc.getNumAllocatedFragments()); @@ -306,22 +318,24 @@ TEST_CASE("TxBasic0") REQUIRE(1 == ins.getTxQueueLength()); REQUIRE(1 == alloc.getNumAllocatedFragments()); REQUIRE(60 > alloc.getTotalAllocatedAmount()); - REQUIRE(ins.getTxQueueRoot()->deadline_usec == 1'000'000'004'000ULL); - REQUIRE(ins.getTxQueueRoot()->payload_size == 1); + REQUIRE(ins.getTxQueueRoot()->frame.timestamp_usec == 1'000'000'004'000ULL); + REQUIRE(ins.getTxQueueRoot()->frame.payload_size == 1); REQUIRE(ins.getTxQueueRoot()->isStartOfTransfer()); REQUIRE(ins.getTxQueueRoot()->isEndOfTransfer()); REQUIRE(ins.getTxQueueRoot()->isToggleBitSet()); - REQUIRE(1 == ins.txPeek(frame)); - REQUIRE(frame.payload_size == 1); - REQUIRE((0b11100000U | 28U) == reinterpret_cast(frame.payload)[0]); - REQUIRE(frame.timestamp_usec == 1'000'000'004'000ULL); + frame = ins.txPeek(); + REQUIRE(nullptr != frame); + REQUIRE(frame->payload_size == 1); + REQUIRE((0b11100000U | 28U) == reinterpret_cast(frame->payload)[0]); + REQUIRE(frame->timestamp_usec == 1'000'000'004'000ULL); ins.txPop(); - ins.getAllocator().deallocate(frame.payload); + ins.getAllocator().deallocate(frame); REQUIRE(0 == ins.getTxQueueLength()); REQUIRE(0 == alloc.getNumAllocatedFragments()); // Nothing left to peek at. - REQUIRE(0 == ins.txPeek(frame)); + frame = ins.txPeek(); + REQUIRE(nullptr == frame); // Error handling. REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardTxPush(nullptr, nullptr)); @@ -331,9 +345,7 @@ TEST_CASE("TxBasic0") transfer.payload = nullptr; REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == ins.txPush(transfer)); - REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardTxPeek(nullptr, nullptr)); - REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardTxPeek(nullptr, &frame)); - REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardTxPeek(&ins.getInstance(), nullptr)); + REQUIRE(nullptr == canardTxPeek(nullptr)); canardTxPop(&ins.getInstance()); // No effect. } diff --git a/tests/test_self.cpp b/tests/test_self.cpp index 9237999b..a83dde5b 100644 --- a/tests/test_self.cpp +++ b/tests/test_self.cpp @@ -32,7 +32,6 @@ TEST_CASE("TestAllocator") al.deallocate(a); REQUIRE(2 == al.getNumAllocatedFragments()); REQUIRE(477 == al.getTotalAllocatedAmount()); - REQUIRE_THROWS_AS(al.deallocate(a), std::logic_error); auto d = al.allocate(100); REQUIRE(3 == al.getNumAllocatedFragments()); @@ -49,9 +48,4 @@ TEST_CASE("TestAllocator") al.deallocate(b); REQUIRE(0 == al.getNumAllocatedFragments()); REQUIRE(0 == al.getTotalAllocatedAmount()); - - REQUIRE_THROWS_AS(al.deallocate(a), std::logic_error); - REQUIRE_THROWS_AS(al.deallocate(b), std::logic_error); - REQUIRE_THROWS_AS(al.deallocate(c), std::logic_error); - REQUIRE_THROWS_AS(al.deallocate(d), std::logic_error); } From 82844f0717266254c03983a91e2d9d09718820df Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 23 Feb 2020 06:57:35 +0200 Subject: [PATCH 063/100] SonarQube --- libcanard/canard.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 943c8377..c8fa08b6 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -587,7 +587,7 @@ CANARD_PRIVATE bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel // Final validation. // Protocol version check: if SOT is set, then the toggle shall also be set. - valid = valid && ((!out->start_of_transfer) || (out->toggle == INITIAL_TOGGLE_STATE)); + valid = valid && ((!out->start_of_transfer) || (INITIAL_TOGGLE_STATE == out->toggle)); // Anonymous transfers can be only single-frame transfers. valid = valid && ((out->start_of_transfer && out->end_of_transfer) || (CANARD_NODE_ID_UNSET != out->source_node_id)); @@ -753,7 +753,7 @@ CANARD_PRIVATE int8_t rxSessionAcceptFrame(CanardInstance* const ins, // Cut off the CRC from the payload if it's there -- we don't want to expose it to the user. CANARD_ASSERT(rxs->total_payload_size >= rxs->payload_size); const size_t truncated_amount = rxs->total_payload_size - rxs->payload_size; - if (!single_frame && (CRC_SIZE_BYTES > truncated_amount)) // Single-frame transfers don't have CRC. + if ((!single_frame) && (CRC_SIZE_BYTES > truncated_amount)) // Single-frame transfers don't have CRC. { CANARD_ASSERT(out_transfer->payload_size >= (CRC_SIZE_BYTES - truncated_amount)); out_transfer->payload_size -= CRC_SIZE_BYTES - truncated_amount; From e9019d086d00e3bfb673f9a807b83754127edb38 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 23 Feb 2020 07:25:29 +0200 Subject: [PATCH 064/100] Fix the AVR build again --- libcanard/canard_dsdl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index 30a6cfb5..e3c2f0cf 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -27,7 +27,7 @@ typedef union // NOSONAR uint32_t bits; CanardDSDLFloatNative real; } Float32Bits; -static_assert(4 == sizeof(Float32Bits), "Unsupported float model"); +_Static_assert(4 == sizeof(Float32Bits), "Unsupported float model"); uint16_t canardDSDLFloat16Pack(const CanardDSDLFloatNative value) { From 5e1bca92d2197cdf85c021aad8de4c9ce6294192 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 23 Feb 2020 07:27:16 +0200 Subject: [PATCH 065/100] Suppress check near an intended MISRA violation --- libcanard/canard.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index c8fa08b6..348d9254 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -104,7 +104,7 @@ typedef struct CanardInternalTxQueueItem // - Make the payload pointer point to the remainder of the allocated memory following this structure. // The pointer is bad because it requires us to use pointer arithmetics. // - Use a separate memory allocation for data. This is terribly wasteful (both time & memory). - uint8_t payload_buffer[]; + uint8_t payload_buffer[]; // NOSONAR } CanardInternalTxQueueItem; CANARD_PRIVATE uint32_t txMakeMessageSessionSpecifier(const CanardPortID subject_id, const CanardNodeID src_node_id); From 9ca9be2e7a3f5b3ed953b9fc52216effa63ea99c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 23 Feb 2020 10:03:09 +0200 Subject: [PATCH 066/100] Started work on the DSDL helpers --- libcanard/canard_dsdl.c | 59 +++++++++++++++++-- libcanard/canard_dsdl.h | 17 ++++-- tests/CMakeLists.txt | 6 +- tests/exposed.hpp | 6 ++ ...{test_public_float16.cpp => test_dsdl.cpp} | 25 ++++++++ 5 files changed, 102 insertions(+), 11 deletions(-) rename tests/{test_public_float16.cpp => test_dsdl.cpp} (59%) diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index e3c2f0cf..887533e4 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -4,20 +4,71 @@ #include "canard_dsdl.h" #include +// --------------------------------------------- BUILD CONFIGURATION --------------------------------------------- + +/// By default, this macro resolves to the standard assert(). The user can redefine this if necessary. +/// To disable assertion checks completely, make it expand into `(void)(0)`. +#ifndef CANARD_ASSERT +// Intentional violation of MISRA: assertion macro cannot be replaced with a function definition. +# define CANARD_ASSERT(x) assert(x) // NOSONAR +#endif + +/// This macro is needed only for testing and for library development. Do not redefine this in production. +#if defined(CANARD_EXPOSE_PRIVATE) && CANARD_EXPOSE_PRIVATE +# define CANARD_PRIVATE +#else +# define CANARD_PRIVATE static inline +#endif + #if !defined(__STDC_VERSION__) || (__STDC_VERSION__ < 201112L) # error "Unsupported language: ISO C11 or a newer version is required." #endif -#ifndef CANARD_ASSERT -# define CANARD_ASSERT assert -#endif +// --------------------------------------------- COMMON CONSTANTS --------------------------------------------- + +#define BYTE_SIZE 8U +#define BYTE_MAX 0xFFU + +// --------------------------------------------- PRIMITIVE SERIALIZATION --------------------------------------------- #if CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT -// TODO implement +/// Per the DSDL specification, it is assumed that 1 byte = 8 bits. +CANARD_PRIVATE void copyBitArray(const size_t length_bit, + const size_t src_offset_bit, + const size_t dst_offset_bit, + const uint8_t* const src, + uint8_t* const dst) +{ + CANARD_ASSERT((src != NULL) || (length_bit == 0U)); + CANARD_ASSERT((dst != NULL) || (length_bit == 0U)); + size_t src_off = src_offset_bit; + size_t dst_off = dst_offset_bit; + const size_t last_bit = src_off + length_bit; + while (last_bit > src_off) + { + const uint8_t src_mod = (uint8_t)(src_off % BYTE_SIZE); + const uint8_t dst_mod = (uint8_t)(dst_off % BYTE_SIZE); + const uint8_t max_mod = (src_mod > dst_mod) ? src_mod : dst_mod; + size_t size = BYTE_SIZE - max_mod; + if (size > (last_bit - src_off)) + { + size = last_bit - src_off; + } + const uint8_t mask = (uint8_t)((((BYTE_MAX << BYTE_SIZE) >> size) & BYTE_MAX) >> dst_mod); + const uint8_t in = (uint8_t)(((uint32_t) src[src_off / BYTE_SIZE] << src_mod) >> dst_mod); + const uint8_t a = dst[dst_off / BYTE_SIZE] & (uint8_t) ~mask; + const uint8_t b = in & mask; + dst[dst_off / BYTE_SIZE] = a | b; + src_off += size; + dst_off += size; + } +} #endif // CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT +// --------------------------------------------- FLOAT16 SUPPORT --------------------------------------------- + #if CANARD_DSDL_PLATFORM_IEEE754 // Intentional violation of MISRA: we need this union because the alternative is far more error prone. diff --git a/libcanard/canard_dsdl.h b/libcanard/canard_dsdl.h index 21363c16..76c43e41 100644 --- a/libcanard/canard_dsdl.h +++ b/libcanard/canard_dsdl.h @@ -5,6 +5,7 @@ // It is intended for use in simple applications where auto-generated DSDL serialization logic is not available. // The functions are fully stateless (pure); read their documentation comments for usage information. // This is an optional part of libcanard that can be omitted if this functionality is not required by the application. +// High-integrity applications are not recommended to use this extension because it relies on unsafe memory operations. #ifndef CANARD_DSDL_H_INCLUDED #define CANARD_DSDL_H_INCLUDED @@ -28,12 +29,14 @@ extern "C" { #if CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT /// This function may be used to serialize values for later transmission in a UAVCAN transfer. -/// It serializes a primitive value -- boolean, integer, character, or floating point -- and puts it at the -/// specified bit offset in the specified contiguous buffer. -/// Simple objects can also be serialized manually instead of using this function. +/// It serializes a primitive value -- boolean, integer, character, or floating point -- following the DSDL +/// primitive serialization rules, and puts it at the specified bit offset in the destination buffer. /// /// The function is only available if the platform uses two's complement signed integer representation. /// +/// If any of the input pointers are NULL or the value of length_bit is not specified in the table, +/// the function has no effect. +/// /// The type of the value pointed to by 'value' is defined as follows: /// /// | bit_length | value points to | @@ -53,11 +56,15 @@ void canardDSDLPrimitiveSerialize(void* const destination, const uint8_t length_bit, const void* const value); -/// This function may be used to extract values from received UAVCAN transfers. It deserializes a scalar value -- -/// boolean, integer, character, or floating point -- from the specified bit position in the source buffer. +/// This function may be used to extract values from received UAVCAN transfers. +/// It deserializes a scalar value -- boolean, integer, character, or floating point -- from the specified +/// bit position in the source buffer. /// /// The function is only available if the platform uses two's complement signed integer representation. /// +/// If any of the input pointers are NULL or the value of length_bit is not specified in the table, +/// the function has no effect. +/// /// The type of the value pointed to by 'out_value' is defined as follows: /// /// | bit_length | is_signed | out_value points to | diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 317cc8db..a41e2bf2 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -74,8 +74,10 @@ function(gen_test_matrix name files compile_definitions) endif () endfunction() -gen_test_matrix(test_private "test_private_crc.cpp;test_private_rx.cpp;test_private_tx.cpp" CANARD_EXPOSE_PRIVATE=1) +gen_test_matrix(test_private + "test_private_crc.cpp;test_private_rx.cpp;test_private_tx.cpp;test_dsdl.cpp" + CANARD_EXPOSE_PRIVATE=1) gen_test_matrix(test_public - "test_public_float16.cpp;test_public_tx.cpp;test_public_rx.cpp;test_public_roundtrip.cpp;test_self.cpp" + "test_public_tx.cpp;test_public_rx.cpp;test_public_roundtrip.cpp;test_self.cpp" "") diff --git a/tests/exposed.hpp b/tests/exposed.hpp index 596ccaff..e8b857cf 100644 --- a/tests/exposed.hpp +++ b/tests/exposed.hpp @@ -112,5 +112,11 @@ auto rxSessionUpdate(CanardInstance* const ins, const CanardMicrosecond transfer_id_timeout_usec, const std::size_t payload_size_max, CanardTransfer* const out_transfer) -> std::int8_t; + +void copyBitArray(const std::size_t length_bit, + const std::size_t src_offset_bit, + const std::size_t dst_offset_bit, + const std::uint8_t* const src, + std::uint8_t* const dst); } } // namespace exposed diff --git a/tests/test_public_float16.cpp b/tests/test_dsdl.cpp similarity index 59% rename from tests/test_public_float16.cpp rename to tests/test_dsdl.cpp index fdf94064..27f73d97 100644 --- a/tests/test_public_float16.cpp +++ b/tests/test_dsdl.cpp @@ -3,6 +3,7 @@ #include "canard_dsdl.h" #include "exposed.hpp" +#include #include TEST_CASE("canardDSDLFloat16Pack") @@ -37,3 +38,27 @@ TEST_CASE("canardDSDLFloat16") x += 0.5F; } } + +TEST_CASE("copyBitArray") +{ + using exposed::copyBitArray; + + copyBitArray(0, 0, 0, nullptr, nullptr); + + const auto test = [&](const size_t length_bit, + const size_t src_offset_bit, + const size_t dst_offset_bit, + const std::vector& src, + const std::vector& dst, + const std::vector& ref) { + REQUIRE(length_bit <= (dst.size() * 8)); + REQUIRE(length_bit <= (src.size() * 8)); + std::vector result = dst; + copyBitArray(length_bit, src_offset_bit, dst_offset_bit, src.data(), result.data()); + return std::equal(std::begin(ref), std::end(ref), std::begin(result)); + }; + + REQUIRE(test(0, 0, 0, {}, {}, {})); + REQUIRE(test(8, 0, 0, {0xFF}, {0x00}, {0xFF})); + REQUIRE(test(16, 0, 0, {0xFF, 0xFF}, {0x00, 0x00}, {0xFF, 0xFF})); +} From 389cf17d3fd0e60164646f9ba7563ebf2f1a24ba Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 27 Feb 2020 21:48:27 +0200 Subject: [PATCH 067/100] bit copy implementation & minimal testing (to be extended) --- libcanard/canard_dsdl.c | 80 ++++++++++++++++++++++++++++++----------- tests/test_dsdl.cpp | 11 ++++-- 2 files changed, 68 insertions(+), 23 deletions(-) diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index 887533e4..439a3d5b 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -3,6 +3,7 @@ #include "canard_dsdl.h" #include +#include // --------------------------------------------- BUILD CONFIGURATION --------------------------------------------- @@ -26,42 +27,79 @@ // --------------------------------------------- COMMON CONSTANTS --------------------------------------------- -#define BYTE_SIZE 8U +/// Per the DSDL specification, it is assumed that 1 byte = 8 bits. +#define BYTE_WIDTH 8U #define BYTE_MAX 0xFFU // --------------------------------------------- PRIMITIVE SERIALIZATION --------------------------------------------- #if CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT -/// Per the DSDL specification, it is assumed that 1 byte = 8 bits. +CANARD_PRIVATE size_t chooseMin(size_t a, size_t b) +{ + return (a < b) ? a : b; +} + +/// The algorithm was originally designed by Ben Dyer for Libuavcan v0: +/// https://github.com/UAVCAN/libuavcan/blob/ba696029f9625d7ea3eb00/libuavcan/src/marshal/uc_bit_array_copy.cpp#L12-L58 +/// This version is modified for v1 where the bit order is the opposite. +/// If both offsets and the length are byte-aligned, the algorithm degenerates into memcpy(). +/// The source and the destination shall not overlap. CANARD_PRIVATE void copyBitArray(const size_t length_bit, const size_t src_offset_bit, const size_t dst_offset_bit, const uint8_t* const src, uint8_t* const dst) { - CANARD_ASSERT((src != NULL) || (length_bit == 0U)); - CANARD_ASSERT((dst != NULL) || (length_bit == 0U)); - size_t src_off = src_offset_bit; - size_t dst_off = dst_offset_bit; - const size_t last_bit = src_off + length_bit; - while (last_bit > src_off) + CANARD_ASSERT((src != NULL) && (dst != NULL) && (src != dst)); + CANARD_ASSERT((src < dst) ? ((src + ((src_offset_bit + length_bit + BYTE_WIDTH) / BYTE_WIDTH)) <= dst) + : ((dst + ((dst_offset_bit + length_bit + BYTE_WIDTH) / BYTE_WIDTH)) <= src)); + if (((length_bit % BYTE_WIDTH) == 0U) && // + ((src_offset_bit % BYTE_WIDTH) == 0U) && // + ((dst_offset_bit % BYTE_WIDTH) == 0U)) + { + // Intentional violation of MISRA: Pointer arithmetics. + // This is done to remove the API constraint that offsets be under 8 bits. + // Fewer constraints reduce the chance of API misuse. + (void) memcpy(dst + (dst_offset_bit / BYTE_WIDTH), // NOSONAR NOLINT + src + (src_offset_bit / BYTE_WIDTH), // NOSONAR + length_bit / BYTE_WIDTH); + } + else { - const uint8_t src_mod = (uint8_t)(src_off % BYTE_SIZE); - const uint8_t dst_mod = (uint8_t)(dst_off % BYTE_SIZE); - const uint8_t max_mod = (src_mod > dst_mod) ? src_mod : dst_mod; - size_t size = BYTE_SIZE - max_mod; - if (size > (last_bit - src_off)) + size_t src_off = src_offset_bit; + size_t dst_off = dst_offset_bit; + const size_t last_bit = src_off + length_bit; + while (last_bit > src_off) { - size = last_bit - src_off; + const uint8_t src_mod = (uint8_t)(src_off % BYTE_WIDTH); + const uint8_t dst_mod = (uint8_t)(dst_off % BYTE_WIDTH); + const uint8_t max_mod = (src_mod > dst_mod) ? src_mod : dst_mod; + + const size_t size = chooseMin(BYTE_WIDTH - max_mod, last_bit - src_off); + CANARD_ASSERT((size > 0U) && (size <= BYTE_WIDTH)); + + const uint8_t mask = (uint8_t)((((1U << size) - 1U) << dst_mod) & BYTE_MAX); + CANARD_ASSERT((mask > 0U) && (mask <= BYTE_MAX)); + + // Intentional violation of MISRA: indexing on a pointer. + // This simplifies the implementation greatly and avoids pointer arithmetics. + const uint8_t in = + (uint8_t)((uint8_t)(src[src_off / BYTE_WIDTH] >> src_mod) << dst_mod) & BYTE_MAX; // NOSONAR + + // Intentional violation of MISRA: indexing on a pointer. + // This simplifies the implementation greatly and avoids pointer arithmetics. + const uint8_t a = dst[dst_off / BYTE_WIDTH] & ((uint8_t) ~mask); // NOSONAR + const uint8_t b = in & mask; + + // Intentional violation of MISRA: indexing on a pointer. + // This simplifies the implementation greatly and avoids pointer arithmetics. + dst[dst_off / BYTE_WIDTH] = a | b; // NOSONAR + + src_off += size; + dst_off += size; } - const uint8_t mask = (uint8_t)((((BYTE_MAX << BYTE_SIZE) >> size) & BYTE_MAX) >> dst_mod); - const uint8_t in = (uint8_t)(((uint32_t) src[src_off / BYTE_SIZE] << src_mod) >> dst_mod); - const uint8_t a = dst[dst_off / BYTE_SIZE] & (uint8_t) ~mask; - const uint8_t b = in & mask; - dst[dst_off / BYTE_SIZE] = a | b; - src_off += size; - dst_off += size; + CANARD_ASSERT(last_bit == src_off); } } diff --git a/tests/test_dsdl.cpp b/tests/test_dsdl.cpp index 27f73d97..2a8628ef 100644 --- a/tests/test_dsdl.cpp +++ b/tests/test_dsdl.cpp @@ -43,7 +43,11 @@ TEST_CASE("copyBitArray") { using exposed::copyBitArray; - copyBitArray(0, 0, 0, nullptr, nullptr); + { + uint8_t a = 0; + uint8_t b = 0; + copyBitArray(0, 0, 0, &a, &b); + } const auto test = [&](const size_t length_bit, const size_t src_offset_bit, @@ -58,7 +62,10 @@ TEST_CASE("copyBitArray") return std::equal(std::begin(ref), std::end(ref), std::begin(result)); }; - REQUIRE(test(0, 0, 0, {}, {}, {})); REQUIRE(test(8, 0, 0, {0xFF}, {0x00}, {0xFF})); REQUIRE(test(16, 0, 0, {0xFF, 0xFF}, {0x00, 0x00}, {0xFF, 0xFF})); + REQUIRE(test(12, 0, 0, {0xFF, 0x0A}, {0x55, 0x00}, {0xFF, 0x0A})); + REQUIRE(test(12, 0, 0, {0xFF, 0x0A}, {0x00, 0xF0}, {0xFF, 0xFA})); + REQUIRE(test(12, 0, 4, {0xFF, 0x0A}, {0x53, 0x55}, {0xF3, 0xAF})); + REQUIRE(test(8, 4, 4, {0x55, 0x55}, {0xAA, 0xAA}, {0x5A, 0xA5})); } From 5c8bdef36d6809b267faac332237884ba00c0f1d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 28 Feb 2020 03:24:13 +0200 Subject: [PATCH 068/100] canardDSDLPrimitiveSerialize() --- libcanard/canard.c | 2 +- libcanard/canard_dsdl.c | 104 ++++++++++++++++++- libcanard/canard_dsdl.h | 44 ++++---- tests/CMakeLists.txt | 7 +- tests/test_dsdl.cpp | 220 ++++++++++++++++++++++++++++++++++++++++ 5 files changed, 355 insertions(+), 22 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 348d9254..ef63bc78 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -15,7 +15,7 @@ #endif /// This macro is needed only for testing and for library development. Do not redefine this in production. -#if defined(CANARD_EXPOSE_PRIVATE) && CANARD_EXPOSE_PRIVATE +#if defined(CANARD_CONFIG_EXPOSE_PRIVATE) && CANARD_CONFIG_EXPOSE_PRIVATE # define CANARD_PRIVATE #else # define CANARD_PRIVATE static inline diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index 439a3d5b..6cfb62cb 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -7,6 +7,17 @@ // --------------------------------------------- BUILD CONFIGURATION --------------------------------------------- +/// This option allows the user to improve the primitive (de-)serialization performance if the target platform +/// is little endian. +/// There are two implementations of the primitive (de-)serialization algorithms: a generic one, which is invariant +/// to the native byte order (and therefore compatible with any platform), and the optimized one which is compatible +/// with little-endian platforms only. +/// By default, the slow generic algorithm is used. +/// If the target platform is little-endian, the user can enable this option to use the optimized algorithm. +#ifndef CANARD_DSDL_CONFIG_LITTLE_ENDIAN +# define CANARD_DSDL_CONFIG_LITTLE_ENDIAN false +#endif + /// By default, this macro resolves to the standard assert(). The user can redefine this if necessary. /// To disable assertion checks completely, make it expand into `(void)(0)`. #ifndef CANARD_ASSERT @@ -15,7 +26,7 @@ #endif /// This macro is needed only for testing and for library development. Do not redefine this in production. -#if defined(CANARD_EXPOSE_PRIVATE) && CANARD_EXPOSE_PRIVATE +#if defined(CANARD_CONFIG_EXPOSE_PRIVATE) && CANARD_CONFIG_EXPOSE_PRIVATE # define CANARD_PRIVATE #else # define CANARD_PRIVATE static inline @@ -31,6 +42,13 @@ #define BYTE_WIDTH 8U #define BYTE_MAX 0xFFU +#define WIDTH_BIT 1U +#define WIDTH_8 8U +#define WIDTH_16 16U +#define WIDTH_32 32U +#define WIDTH_64 64U +#define WIDTH_MAX WIDTH_64 + // --------------------------------------------- PRIMITIVE SERIALIZATION --------------------------------------------- #if CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT @@ -103,6 +121,90 @@ CANARD_PRIVATE void copyBitArray(const size_t length_bit, } } +# if CANARD_DSDL_CONFIG_LITTLE_ENDIAN + +size_t canardDSDLPrimitiveSerialize(uint8_t* const destination, + const size_t offset_bit, + const uint8_t length_bit, + const void* const value) +{ + size_t out = 0U; + if ((destination != NULL) && (value != NULL) && (length_bit > 0U) && (length_bit <= WIDTH_MAX)) + { + // Intentional violation of MISRA: void cast is necessary due to inherent limitations of C. + copyBitArray(length_bit, 0U, offset_bit, (const uint8_t*) value, destination); // NOSONAR + out = offset_bit + length_bit; + } + return out; +} + +# else + +size_t canardDSDLPrimitiveSerialize(uint8_t* const destination, + const size_t offset_bit, + const uint8_t length_bit, + const void* const value) +{ + size_t out = 0U; + if ((destination != NULL) && (value != NULL)) + { + bool success = true; + uint8_t buffer[WIDTH_MAX / BYTE_WIDTH] = {0}; + if (length_bit == WIDTH_BIT) + { + // Intentional violation of MISRA: void cast is necessary due to inherent limitations of C. + buffer[0] = (*((const bool*) value) != 0) ? 1U : 0U; // NOSONAR + } + else if (length_bit <= WIDTH_8) + { + // Intentional violation of MISRA: void cast is necessary due to inherent limitations of C. + buffer[0] = *((const uint8_t*) value); // NOSONAR + } + else if (length_bit <= WIDTH_16) + { + uint16_t x = *(const uint16_t*) value; + buffer[0] = (uint8_t)(x & BYTE_MAX); + x >>= BYTE_WIDTH; + buffer[1] = (uint8_t)(x & BYTE_MAX); + } + else if (length_bit <= WIDTH_32) + { + uint32_t x = *(const uint32_t*) value; + size_t i = 0; + while (x > 0U) + { + buffer[i] = (uint8_t)(x & BYTE_MAX); + x >>= BYTE_WIDTH; + ++i; + } + } + else if (length_bit <= WIDTH_64) + { + uint64_t x = *(const uint64_t*) value; // Stuff like this makes one miss generics. + size_t i = 0; // What am I doing with my life? + while (x > 0U) // Copy-pasting code like we can't automate it away in 2020. + { + buffer[i] = (uint8_t)(x & BYTE_MAX); + x >>= BYTE_WIDTH; + ++i; + } + } + else + { + success = false; + } + + if (success) + { + copyBitArray(length_bit, 0U, offset_bit, &buffer[0], destination); + out = offset_bit + length_bit; + } + } + return out; +} + +# endif // CANARD_DSDL_CONFIG_LITTLE_ENDIAN + #endif // CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT // --------------------------------------------- FLOAT16 SUPPORT --------------------------------------------- diff --git a/libcanard/canard_dsdl.h b/libcanard/canard_dsdl.h index 76c43e41..53339354 100644 --- a/libcanard/canard_dsdl.h +++ b/libcanard/canard_dsdl.h @@ -34,8 +34,9 @@ extern "C" { /// /// The function is only available if the platform uses two's complement signed integer representation. /// -/// If any of the input pointers are NULL or the value of length_bit is not specified in the table, -/// the function has no effect. +/// If any of the input pointers are NULL or the value of length_bit is not specified in the table, zero is returned. +/// +/// If the source and the destination areas overlap, the behavior is undefined. /// /// The type of the value pointed to by 'value' is defined as follows: /// @@ -48,13 +49,14 @@ extern "C" { /// | [33, 64] | uint64_t, int64_t, or 64-bit float | /// /// @param destination Destination buffer where the result will be stored. -/// @param offset_bit Offset, in bits, from the beginning of the destination buffer. +/// @param offset_bit Offset, in bits, from the beginning of the destination buffer. May exceed one byte. /// @param length_bit Length of the value, in bits; see the table. /// @param value Pointer to the value; see the table. -void canardDSDLPrimitiveSerialize(void* const destination, - const size_t offset_bit, - const uint8_t length_bit, - const void* const value); +/// @returns (offset_bit + length_bit) on success, zero if any of the arguments are invalid. +size_t canardDSDLPrimitiveSerialize(uint8_t* const destination, + const size_t offset_bit, + const uint8_t length_bit, + const void* const value); /// This function may be used to extract values from received UAVCAN transfers. /// It deserializes a scalar value -- boolean, integer, character, or floating point -- from the specified @@ -62,8 +64,9 @@ void canardDSDLPrimitiveSerialize(void* const destination, /// /// The function is only available if the platform uses two's complement signed integer representation. /// -/// If any of the input pointers are NULL or the value of length_bit is not specified in the table, -/// the function has no effect. +/// If any of the input pointers are NULL or the value of length_bit is not specified in the table, zero is returned. +/// +/// If the source and the destination areas overlap, the behavior is undefined. /// /// The type of the value pointed to by 'out_value' is defined as follows: /// @@ -80,16 +83,19 @@ void canardDSDLPrimitiveSerialize(void* const destination, /// | [33, 64] | false | uint64_t | /// | [33, 64] | true | int64_t, or 64-bit float IEEE 754 | /// -/// @param source The source buffer where the data will be read from. -/// @param offset_bit Offset, in bits, from the beginning of the source buffer. -/// @param length_bit Length of the value, in bits; see the table. -/// @param is_signed True if the value can be negative (i.e., sign bit extension is needed); see the table. -/// @param out_value Pointer to the output storage; see the table. -void canardDSDLPrimitiveDeserialize(const void* const source, - const size_t offset_bit, - const uint8_t length_bit, - const bool is_signed, - void* const out_value); +/// @param source The source buffer where the data will be read from. +/// @param source_size_bytes Bytes above this size will be assumed to equal zero, per the zero extension rule. +/// @param offset_bit Offset, in bits, from the beginning of the source buffer. May exceed one byte. +/// @param length_bit Length of the value, in bits; see the table. +/// @param is_signed True if the value can be negative (i.e., sign bit extension is needed); see the table. +/// @param out_value Pointer to the output storage; see the table. +/// @returns (offset_bit + length_bit) on success, zero if any of the arguments are invalid. +size_t canardDSDLPrimitiveDeserialize(const uint8_t* const source, + const size_t source_size_bytes, + const size_t offset_bit, + const uint8_t length_bit, + const bool is_signed, + void* const out_value); #endif // CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index a41e2bf2..f98bd040 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -76,8 +76,13 @@ endfunction() gen_test_matrix(test_private "test_private_crc.cpp;test_private_rx.cpp;test_private_tx.cpp;test_dsdl.cpp" - CANARD_EXPOSE_PRIVATE=1) + CANARD_CONFIG_EXPOSE_PRIVATE=1) gen_test_matrix(test_public "test_public_tx.cpp;test_public_rx.cpp;test_public_roundtrip.cpp;test_self.cpp" "") + +# We assume here that the target platform is little-endian. If it's not, the test will fail. Perhaps this needs fixing. +gen_test_matrix(test_dsdl_little_endian + "test_dsdl.cpp" + "CANARD_CONFIG_EXPOSE_PRIVATE=1;CANARD_DSDL_CONFIG_LITTLE_ENDIAN=1") diff --git a/tests/test_dsdl.cpp b/tests/test_dsdl.cpp index 2a8628ef..16a946a4 100644 --- a/tests/test_dsdl.cpp +++ b/tests/test_dsdl.cpp @@ -5,6 +5,7 @@ #include "exposed.hpp" #include #include +#include TEST_CASE("canardDSDLFloat16Pack") { @@ -69,3 +70,222 @@ TEST_CASE("copyBitArray") REQUIRE(test(12, 0, 4, {0xFF, 0x0A}, {0x53, 0x55}, {0xF3, 0xAF})); REQUIRE(test(8, 4, 4, {0x55, 0x55}, {0xAA, 0xAA}, {0x5A, 0xA5})); } + +TEST_CASE("canardDSDLPrimitiveSerialize_aligned") +{ + // The reference values for the following test have been taken from the PyUAVCAN test suite. + const std::vector Reference({0xA7, 0xEF, 0xCD, 0xAB, 0x90, 0x78, 0x56, 0x34, 0x12, 0x88, 0xA9, 0xCB, + 0xED, 0xFE, 0xFF, 0x00, 0x7F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xF0, + 0x3F, 0x00, 0x00, 0x80, 0x3F, 0x00, 0x7C, 0xDA, 0x0E, 0xDA, 0xBE, 0xFE, + 0x01, 0xAD, 0xDE, 0xEF, 0xBE, 0xC5, 0x67, 0xC5, 0x0B}); + + std::vector dest(std::size(Reference)); + + std::uint8_t u8 = 0b1010'0111; + REQUIRE(8 == canardDSDLPrimitiveSerialize(dest.data(), 0, 8, &u8)); + + std::int64_t i64 = 0x1234'5678'90ab'cdef; + REQUIRE(72 == canardDSDLPrimitiveSerialize(dest.data(), 8, 64, &i64)); + + std::int32_t i32 = -0x1234'5678; + REQUIRE(104 == canardDSDLPrimitiveSerialize(dest.data(), 72, 32, &i32)); + + std::int32_t i16 = -2; + u8 = 0; + std::int8_t i8 = 127; + REQUIRE(120 == canardDSDLPrimitiveSerialize(dest.data(), 104, 16, &i16)); + REQUIRE(128 == canardDSDLPrimitiveSerialize(dest.data(), 120, 8, &u8)); + REQUIRE(136 == canardDSDLPrimitiveSerialize(dest.data(), 128, 8, &i8)); + + double f64 = 1.0; + float f32 = 1.0F; + std::uint16_t f16 = canardDSDLFloat16Pack(99999.9F); + REQUIRE(200 == canardDSDLPrimitiveSerialize(dest.data(), 136, 64, &f64)); + REQUIRE(232 == canardDSDLPrimitiveSerialize(dest.data(), 200, 32, &f32)); + REQUIRE(248 == canardDSDLPrimitiveSerialize(dest.data(), 232, 16, &f16)); + + std::uint16_t u16 = 0xBEDA; + u8 = 0; + REQUIRE(260 == canardDSDLPrimitiveSerialize(dest.data(), 248, 12, &u16)); + REQUIRE(264 == canardDSDLPrimitiveSerialize(dest.data(), 260, 4, &u8)); + REQUIRE(280 == canardDSDLPrimitiveSerialize(dest.data(), 264, 16, &u16)); + + i16 = -2; + u8 = 0; + REQUIRE(289 == canardDSDLPrimitiveSerialize(dest.data(), 280, 9, &i16)); + REQUIRE(296 == canardDSDLPrimitiveSerialize(dest.data(), 289, 7, &u8)); + + u16 = 0xDEAD; + REQUIRE(312 == canardDSDLPrimitiveSerialize(dest.data(), 296, 16, &u16)); + u16 = 0xBEEF; + REQUIRE(328 == canardDSDLPrimitiveSerialize(dest.data(), 312, 16, &u16)); + + std::size_t offset = 328; + const auto push_bit = [&](const bool value) { + REQUIRE((offset + 1U) == canardDSDLPrimitiveSerialize(dest.data(), offset, 1, &value)); + ++offset; + }; + + push_bit(true); + push_bit(false); + push_bit(true); + push_bit(false); + push_bit(false); + push_bit(false); + push_bit(true); + push_bit(true); + push_bit(true); + push_bit(true); + push_bit(true); + push_bit(false); + push_bit(false); + push_bit(true); + push_bit(true); + push_bit(false); + + push_bit(true); + push_bit(false); + push_bit(true); + push_bit(false); + push_bit(false); + push_bit(false); + push_bit(true); + push_bit(true); + push_bit(true); + push_bit(true); + push_bit(false); + push_bit(true); + push_bit(false); + + u8 = 0; + REQUIRE(360 == canardDSDLPrimitiveSerialize(dest.data(), 357, 3, &u8)); + + REQUIRE(std::size(dest) == std::size(Reference)); + REQUIRE_THAT(dest, Catch::Matchers::Equals(Reference)); +} + +TEST_CASE("canardDSDLPrimitiveSerialize_unaligned") +{ + // The reference values for the following test have been taken from the PyUAVCAN test suite. + const std::vector Reference({ + 0xC5, 0x2F, 0x57, 0x82, 0xC6, 0xCA, 0x12, 0x34, 0x56, 0xD9, 0xBF, 0xEC, 0x06, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x80, 0xFF, 0x01, 0x00, 0x00, 0xFC, 0x01, 0xE0, 0x6F, 0xF5, 0x7E, 0xF7, 0x05 // + }); + + std::vector dest(std::size(Reference)); + + std::size_t offset = 0; + const auto push_bit = [&](const bool value) { + REQUIRE((offset + 1U) == canardDSDLPrimitiveSerialize(dest.data(), offset, 1, &value)); + ++offset; + }; + + push_bit(true); + push_bit(false); + push_bit(true); + push_bit(false); + push_bit(false); + push_bit(false); + push_bit(true); + push_bit(true); + push_bit(true); + push_bit(true); + push_bit(true); + + push_bit(true); + push_bit(false); + push_bit(true); + push_bit(false); + push_bit(false); + push_bit(true); + push_bit(true); + push_bit(true); + push_bit(false); + push_bit(true); + + REQUIRE_THAT(std::vector(std::begin(dest), std::begin(dest) + 2), + Catch::Matchers::Equals(std::vector(std::begin(Reference), std::begin(Reference) + 2))); + + std::uint8_t u8 = 0x12; + REQUIRE(29 == canardDSDLPrimitiveSerialize(dest.data(), 21, 8, &u8)); + u8 = 0x34; + REQUIRE(37 == canardDSDLPrimitiveSerialize(dest.data(), 29, 8, &u8)); + u8 = 0x56; + REQUIRE(45 == canardDSDLPrimitiveSerialize(dest.data(), 37, 8, &u8)); + + REQUIRE_THAT(std::vector(std::begin(dest), std::begin(dest) + 5), + Catch::Matchers::Equals(std::vector(std::begin(Reference), std::begin(Reference) + 5))); + + offset = 45; + push_bit(false); + push_bit(true); + push_bit(true); + + u8 = 0x12; + REQUIRE(56 == canardDSDLPrimitiveSerialize(dest.data(), 48, 8, &u8)); + u8 = 0x34; + REQUIRE(64 == canardDSDLPrimitiveSerialize(dest.data(), 56, 8, &u8)); + u8 = 0x56; + REQUIRE(72 == canardDSDLPrimitiveSerialize(dest.data(), 64, 8, &u8)); + + offset = 72; + push_bit(true); + push_bit(false); + push_bit(false); + push_bit(true); + push_bit(true); + + REQUIRE_THAT(std::vector(std::begin(dest), std::begin(dest) + 9), + Catch::Matchers::Equals(std::vector(std::begin(Reference), std::begin(Reference) + 9))); + + std::int8_t i8 = -2; + REQUIRE(85 == canardDSDLPrimitiveSerialize(dest.data(), 77, 8, &i8)); + + std::uint16_t u16 = 0b11101100101; + REQUIRE(96 == canardDSDLPrimitiveSerialize(dest.data(), 85, 11, &u16)); + + u8 = 0b1110; + REQUIRE(99 == canardDSDLPrimitiveSerialize(dest.data(), 96, 3, &u8)); + + REQUIRE_THAT(std::vector(std::begin(dest), std::begin(dest) + 12), + Catch::Matchers::Equals(std::vector(std::begin(Reference), std::begin(Reference) + 12))); + + double f64 = 1.0; + float f32 = 1.0F; + std::uint16_t f16 = canardDSDLFloat16Pack(-99999.0F); + REQUIRE(163 == canardDSDLPrimitiveSerialize(dest.data(), 99, 64, &f64)); + REQUIRE(195 == canardDSDLPrimitiveSerialize(dest.data(), 163, 32, &f32)); + REQUIRE(211 == canardDSDLPrimitiveSerialize(dest.data(), 195, 16, &f16)); + + u16 = 0xDEAD; + REQUIRE(227 == canardDSDLPrimitiveSerialize(dest.data(), 211, 16, &u16)); + u16 = 0xBEEF; + REQUIRE(243 == canardDSDLPrimitiveSerialize(dest.data(), 227, 16, &u16)); + + u8 = 0; + REQUIRE(248 == canardDSDLPrimitiveSerialize(dest.data(), 243, 5, &u8)); + + REQUIRE(std::size(dest) == std::size(Reference)); + REQUIRE_THAT(dest, Catch::Matchers::Equals(Reference)); +} + +TEST_CASE("canardDSDLPrimitiveSerialize_heartbeat") +{ + // The reference values were taken from the PyUAVCAN test. + const std::vector Reference({239, 190, 173, 222, 234, 255, 255, 0}); + + std::vector dest(std::size(Reference)); + + std::uint32_t uptime = 0xdeadbeef; + std::uint8_t health = 2; + std::uint8_t mode = 2; + std::uint32_t vssc = 0x7FFFF; + + REQUIRE(37 == canardDSDLPrimitiveSerialize(dest.data(), 34, 3, &mode)); + REQUIRE(32 == canardDSDLPrimitiveSerialize(dest.data(), 0, 32, &uptime)); + REQUIRE(56 == canardDSDLPrimitiveSerialize(dest.data(), 37, 19, &vssc)); + REQUIRE(34 == canardDSDLPrimitiveSerialize(dest.data(), 32, 2, &health)); + + REQUIRE(std::size(dest) == std::size(Reference)); + REQUIRE_THAT(dest, Catch::Matchers::Equals(Reference)); +} From 54637ea4f5d5758b0a9006f52e5e605947816724 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 28 Feb 2020 10:57:22 +0200 Subject: [PATCH 069/100] Better DSDL serialization API --- libcanard/canard.c | 1 + libcanard/canard.h | 1 + libcanard/canard_dsdl.c | 290 +++++++++++++++++++++------------------- libcanard/canard_dsdl.h | 135 ++++++------------- tests/exposed.hpp | 3 + tests/test_dsdl.cpp | 202 ++++++++++++++-------------- 6 files changed, 297 insertions(+), 335 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index ef63bc78..00ba77b9 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -1,5 +1,6 @@ // This software is distributed under the terms of the MIT License. // Copyright (c) 2016-2020 UAVCAN Development Team. +// Author: Pavel Kirienko #include "canard.h" #include diff --git a/libcanard/canard.h b/libcanard/canard.h index b207c136..a083bdb6 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -1,5 +1,6 @@ // This software is distributed under the terms of the MIT License. // Copyright (c) 2016-2020 UAVCAN Development Team. +// Author: Pavel Kirienko // Contributors: https://github.com/UAVCAN/libcanard/contributors. // READ THE DOCUMENTATION IN README.md. diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index 6cfb62cb..8eec87a7 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -1,8 +1,10 @@ // This software is distributed under the terms of the MIT License. // Copyright (c) 2016-2020 UAVCAN Development Team. +// Author: Pavel Kirienko #include "canard_dsdl.h" #include +#include #include // --------------------------------------------- BUILD CONFIGURATION --------------------------------------------- @@ -36,23 +38,99 @@ # error "Unsupported language: ISO C11 or a newer version is required." #endif +#define CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT \ + ((INT8_MIN == -128) && (INT8_MAX == 127) && (INT16_MIN == -32768) && (INT16_MAX == 32767) && \ + (INT32_MIN == -0x80000000LL) && (INT32_MAX == 0x7FFFFFFFLL) && (INT64_MAX == 0x7FFFFFFFFFFFFFFFLL)) +_Static_assert(CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT, + "This component cannot be used on this platform because it uses a non-twos-complement signed integer " + "representation."); + +#define CANARD_DSDL_PLATFORM_IEEE754 \ + ((FLT_RADIX == 2) && (FLT_MANT_DIG == 24) && (FLT_MIN_EXP == -125) && (FLT_MAX_EXP == 128) && \ + (DBL_MANT_DIG == 53) && (DBL_MIN_EXP == -1021) && (DBL_MAX_EXP == 1024)) +_Static_assert(CANARD_DSDL_PLATFORM_IEEE754, + "Currently, the module requires that the target platform shall use an IEEE 754-compatible floating " + "point model. It is possible to support other platforms, but this has not been done yet. " + "If your platform is not IEEE 754-compatible, please reach the maintainers via http://forum.uavcan.org"); + // --------------------------------------------- COMMON CONSTANTS --------------------------------------------- /// Per the DSDL specification, it is assumed that 1 byte = 8 bits. #define BYTE_WIDTH 8U #define BYTE_MAX 0xFFU -#define WIDTH_BIT 1U -#define WIDTH_8 8U -#define WIDTH_16 16U -#define WIDTH_32 32U -#define WIDTH_64 64U -#define WIDTH_MAX WIDTH_64 +#define WIDTHBIT 1U +#define WIDTH8 8U +#define WIDTH16 16U +#define WIDTH32 32U +#define WIDTH64 64U -// --------------------------------------------- PRIMITIVE SERIALIZATION --------------------------------------------- +// --------------------------------------------- FLOAT16 SUPPORT --------------------------------------------- -#if CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT +_Static_assert(WIDTH32 == sizeof(CanardDSDLFloat32) * BYTE_WIDTH, "Unsupported floating point model"); +_Static_assert(WIDTH64 == sizeof(CanardDSDLFloat64) * BYTE_WIDTH, "Unsupported floating point model"); +// Intentional violation of MISRA: we need this union because the alternative is far more error prone. +// We have to rely on low-level data representation details to do the conversion; unions are helpful. +typedef union // NOSONAR +{ + uint32_t bits; + CanardDSDLFloat32 real; +} Float32Bits; +_Static_assert(4 == sizeof(Float32Bits), "Unsupported float model"); + +CANARD_PRIVATE uint16_t float16Pack(const CanardDSDLFloat32 value); +CANARD_PRIVATE uint16_t float16Pack(const CanardDSDLFloat32 value) +{ + // The no-lint statements suppress the warnings about magic numbers. + // The no-lint statements suppress the warning about the use of union. This is required for low-level bit access. + const uint32_t round_mask = ~(uint32_t) 0x0FFFU; // NOLINT NOSONAR + const Float32Bits f32inf = {.bits = ((uint32_t) 255U) << 23U}; // NOLINT NOSONAR + const Float32Bits f16inf = {.bits = ((uint32_t) 31U) << 23U}; // NOLINT NOSONAR + const Float32Bits magic = {.bits = ((uint32_t) 15U) << 23U}; // NOLINT NOSONAR + Float32Bits in = {.real = value}; // NOSONAR + const uint32_t sign = in.bits & (((uint32_t) 1U) << 31U); // NOLINT NOSONAR + in.bits ^= sign; + uint16_t out = 0; + if (in.bits >= f32inf.bits) + { + out = (in.bits > f32inf.bits) ? (uint16_t) 0x7FFFU : (uint16_t) 0x7C00U; // NOLINT NOSONAR + } + else + { + in.bits &= round_mask; + in.real *= magic.real; + in.bits -= round_mask; + if (in.bits > f16inf.bits) + { + in.bits = f16inf.bits; + } + out = (uint16_t)(in.bits >> 13U); // NOLINT NOSONAR + } + out |= (uint16_t)(sign >> 16U); // NOLINT NOSONAR + return out; +} + +CANARD_PRIVATE CanardDSDLFloat32 float16Unpack(const uint16_t value); +CANARD_PRIVATE CanardDSDLFloat32 float16Unpack(const uint16_t value) +{ + // The no-lint statements suppress the warnings about magic numbers. + // The no-lint statements suppress the warning about the use of union. This is required for low-level bit access. + const Float32Bits magic = {.bits = ((uint32_t) 0xEFU) << 23U}; // NOLINT NOSONAR + const Float32Bits inf_nan = {.bits = ((uint32_t) 0x8FU) << 23U}; // NOLINT NOSONAR + Float32Bits out = {.bits = ((uint32_t)(value & 0x7FFFU)) << 13U}; // NOLINT NOSONAR + out.real *= magic.real; + if (out.real >= inf_nan.real) + { + out.bits |= ((uint32_t) 0xFFU) << 23U; // NOLINT NOSONAR + } + out.bits |= ((uint32_t)(value & 0x8000U)) << 16U; // NOLINT NOSONAR + return out.real; +} + +// --------------------------------------------- PRIMITIVE SERIALIZATION --------------------------------------------- + +CANARD_PRIVATE size_t chooseMin(size_t a, size_t b); CANARD_PRIVATE size_t chooseMin(size_t a, size_t b) { return (a < b) ? a : b; @@ -61,8 +139,13 @@ CANARD_PRIVATE size_t chooseMin(size_t a, size_t b) /// The algorithm was originally designed by Ben Dyer for Libuavcan v0: /// https://github.com/UAVCAN/libuavcan/blob/ba696029f9625d7ea3eb00/libuavcan/src/marshal/uc_bit_array_copy.cpp#L12-L58 /// This version is modified for v1 where the bit order is the opposite. -/// If both offsets and the length are byte-aligned, the algorithm degenerates into memcpy(). +/// If both offsets and the length are byte-aligned, the algorithm degenerates to memcpy(). /// The source and the destination shall not overlap. +CANARD_PRIVATE void copyBitArray(const size_t length_bit, + const size_t src_offset_bit, + const size_t dst_offset_bit, + const uint8_t* const src, + uint8_t* const dst); CANARD_PRIVATE void copyBitArray(const size_t length_bit, const size_t src_offset_bit, const size_t dst_offset_bit, @@ -94,11 +177,11 @@ CANARD_PRIVATE void copyBitArray(const size_t length_bit, const uint8_t dst_mod = (uint8_t)(dst_off % BYTE_WIDTH); const uint8_t max_mod = (src_mod > dst_mod) ? src_mod : dst_mod; - const size_t size = chooseMin(BYTE_WIDTH - max_mod, last_bit - src_off); + const uint8_t size = (uint8_t) chooseMin(BYTE_WIDTH - max_mod, last_bit - src_off); CANARD_ASSERT((size > 0U) && (size <= BYTE_WIDTH)); const uint8_t mask = (uint8_t)((((1U << size) - 1U) << dst_mod) & BYTE_MAX); - CANARD_ASSERT((mask > 0U) && (mask <= BYTE_MAX)); + CANARD_ASSERT(mask > 0U); // Intentional violation of MISRA: indexing on a pointer. // This simplifies the implementation greatly and avoids pointer arithmetics. @@ -121,150 +204,79 @@ CANARD_PRIVATE void copyBitArray(const size_t length_bit, } } -# if CANARD_DSDL_CONFIG_LITTLE_ENDIAN +// --------------------------------------------- PUBLIC API --------------------------------------------- +// The following code relies on the C preprocessor, which is a violation of MISRA and other high-integrity coding +// guidelines. This is an intentional deviation because C does not support generics and the only viable alternative +// would be to perform the required operations at runtime sacrificing the scarce type safety provided by C. +// This deviation is therefore considered to be acceptable and superior to all alternative solutions (other than +// not using C at all). -size_t canardDSDLPrimitiveSerialize(uint8_t* const destination, - const size_t offset_bit, - const uint8_t length_bit, - const void* const value) +void canardDSDLSetBit(uint8_t* const buf, const size_t off_bit, const bool value) { - size_t out = 0U; - if ((destination != NULL) && (value != NULL) && (length_bit > 0U) && (length_bit <= WIDTH_MAX)) - { - // Intentional violation of MISRA: void cast is necessary due to inherent limitations of C. - copyBitArray(length_bit, 0U, offset_bit, (const uint8_t*) value, destination); // NOSONAR - out = offset_bit + length_bit; - } - return out; + CANARD_ASSERT(buf != NULL); + const uint8_t x = value ? 1U : 0U; + copyBitArray(1U, 0U, off_bit, &x, buf); } -# else - -size_t canardDSDLPrimitiveSerialize(uint8_t* const destination, - const size_t offset_bit, - const uint8_t length_bit, - const void* const value) +void canardDSDLSetUxx(uint8_t* const buf, const size_t off_bit, const uint64_t value, const uint8_t len_bit) { - size_t out = 0U; - if ((destination != NULL) && (value != NULL)) + _Static_assert(WIDTH64 == sizeof(uint64_t) * BYTE_WIDTH, "Unexpected size of uint64_t"); + CANARD_ASSERT(buf != NULL); +#if CANARD_DSDL_CONFIG_LITTLE_ENDIAN + copyBitArray(len_bit, 0U, off_bit, (const uint8_t*) &value, buf); +#else + uint8_t tmp[sizeof(uint64_t)] = {0}; + uint64_t x = value; + size_t i = 0; + while (x > 0U) // This conversion is independent of the native byte order. Slow but works everywhere. { - bool success = true; - uint8_t buffer[WIDTH_MAX / BYTE_WIDTH] = {0}; - if (length_bit == WIDTH_BIT) - { - // Intentional violation of MISRA: void cast is necessary due to inherent limitations of C. - buffer[0] = (*((const bool*) value) != 0) ? 1U : 0U; // NOSONAR - } - else if (length_bit <= WIDTH_8) - { - // Intentional violation of MISRA: void cast is necessary due to inherent limitations of C. - buffer[0] = *((const uint8_t*) value); // NOSONAR - } - else if (length_bit <= WIDTH_16) - { - uint16_t x = *(const uint16_t*) value; - buffer[0] = (uint8_t)(x & BYTE_MAX); - x >>= BYTE_WIDTH; - buffer[1] = (uint8_t)(x & BYTE_MAX); - } - else if (length_bit <= WIDTH_32) - { - uint32_t x = *(const uint32_t*) value; - size_t i = 0; - while (x > 0U) - { - buffer[i] = (uint8_t)(x & BYTE_MAX); - x >>= BYTE_WIDTH; - ++i; - } - } - else if (length_bit <= WIDTH_64) - { - uint64_t x = *(const uint64_t*) value; // Stuff like this makes one miss generics. - size_t i = 0; // What am I doing with my life? - while (x > 0U) // Copy-pasting code like we can't automate it away in 2020. - { - buffer[i] = (uint8_t)(x & BYTE_MAX); - x >>= BYTE_WIDTH; - ++i; - } - } - else - { - success = false; - } - - if (success) - { - copyBitArray(length_bit, 0U, offset_bit, &buffer[0], destination); - out = offset_bit + length_bit; - } + tmp[i] = (uint8_t)(x & BYTE_MAX); + x >>= BYTE_WIDTH; + ++i; } - return out; + copyBitArray(len_bit, 0U, off_bit, &tmp[0], buf); +#endif } -# endif // CANARD_DSDL_CONFIG_LITTLE_ENDIAN - -#endif // CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT - -// --------------------------------------------- FLOAT16 SUPPORT --------------------------------------------- - -#if CANARD_DSDL_PLATFORM_IEEE754 +void canardDSDLSetIxx(uint8_t* const buf, const size_t off_bit, const int64_t value, const uint8_t len_bit) +{ + // The naive sign conversion seems to be safe and portable according to the C standard: + // 6.3.1.3.3: if the new type is unsigned, the value is converted by repeatedly adding or subtracting one more + // than the maximum value that can be represented in the new type until the value is in the range of the new type. + canardDSDLSetUxx(buf, off_bit, (uint64_t) value, len_bit); +} -// Intentional violation of MISRA: we need this union because the alternative is far more error prone. -// We have to rely on low-level data representation details to do the conversion; unions are helpful. -typedef union // NOSONAR +void canardDSDLSetF16(uint8_t* const buf, const size_t off_bit, const CanardDSDLFloat32 value) { - uint32_t bits; - CanardDSDLFloatNative real; -} Float32Bits; -_Static_assert(4 == sizeof(Float32Bits), "Unsupported float model"); + canardDSDLSetUxx(buf, off_bit, float16Pack(value), WIDTH16); +} -uint16_t canardDSDLFloat16Pack(const CanardDSDLFloatNative value) +void canardDSDLSetF32(uint8_t* const buf, const size_t off_bit, const CanardDSDLFloat32 value) { - // The no-lint statements suppress the warnings about magic numbers. - // The no-lint statements suppress the warning about the use of union. This is required for low-level bit access. - const uint32_t round_mask = ~(uint32_t) 0x0FFFU; // NOLINT NOSONAR - const Float32Bits f32inf = {.bits = ((uint32_t) 255U) << 23U}; // NOLINT NOSONAR - const Float32Bits f16inf = {.bits = ((uint32_t) 31U) << 23U}; // NOLINT NOSONAR - const Float32Bits magic = {.bits = ((uint32_t) 15U) << 23U}; // NOLINT NOSONAR - Float32Bits in = {.real = value}; // NOSONAR - const uint32_t sign = in.bits & (((uint32_t) 1U) << 31U); // NOLINT NOSONAR - in.bits ^= sign; - uint16_t out = 0; - if (in.bits >= f32inf.bits) + // Intentional violation of MISRA: use union to perform fast conversion from an IEEE 754-compatible native + // representation into a serializable integer. The assumptions about the target platform properties are made + // clear. In the future we may add a more generic conversion that is platform-invariant. + _Static_assert(CANARD_DSDL_PLATFORM_IEEE754, "IEEE 754 required"); + union // NOSONAR { - out = (in.bits > f32inf.bits) ? (uint16_t) 0x7FFFU : (uint16_t) 0x7C00U; // NOLINT NOSONAR - } - else - { - in.bits &= round_mask; - in.real *= magic.real; - in.bits -= round_mask; - if (in.bits > f16inf.bits) - { - in.bits = f16inf.bits; - } - out = (uint16_t)(in.bits >> 13U); // NOLINT NOSONAR - } - out |= (uint16_t)(sign >> 16U); // NOLINT NOSONAR - return out; + CanardDSDLFloat32 fl; + uint32_t in; + } tmp = {value}; // NOSONAR + _Static_assert(WIDTH32 == sizeof(tmp) * BYTE_WIDTH, "IEEE 754 required"); + canardDSDLSetUxx(buf, off_bit, tmp.in, sizeof(tmp) * BYTE_WIDTH); } -CanardDSDLFloatNative canardDSDLFloat16Unpack(const uint16_t value) +void canardDSDLSetF64(uint8_t* const buf, const size_t off_bit, const CanardDSDLFloat64 value) { - // The no-lint statements suppress the warnings about magic numbers. - // The no-lint statements suppress the warning about the use of union. This is required for low-level bit access. - const Float32Bits magic = {.bits = ((uint32_t) 0xEFU) << 23U}; // NOLINT NOSONAR - const Float32Bits inf_nan = {.bits = ((uint32_t) 0x8FU) << 23U}; // NOLINT NOSONAR - Float32Bits out = {.bits = ((uint32_t)(value & 0x7FFFU)) << 13U}; // NOLINT NOSONAR - out.real *= magic.real; - if (out.real >= inf_nan.real) + // Intentional violation of MISRA: use union to perform fast conversion from an IEEE 754-compatible native + // representation into a serializable integer. The assumptions about the target platform properties are made + // clear. In the future we may add a more generic conversion that is platform-invariant. + _Static_assert(CANARD_DSDL_PLATFORM_IEEE754, "IEEE 754 required"); + union // NOSONAR { - out.bits |= ((uint32_t) 0xFFU) << 23U; // NOLINT NOSONAR - } - out.bits |= ((uint32_t)(value & 0x8000U)) << 16U; // NOLINT NOSONAR - return out.real; + CanardDSDLFloat64 fl; + uint64_t in; + } tmp = {value}; // NOSONAR + _Static_assert(WIDTH64 == sizeof(tmp) * BYTE_WIDTH, "IEEE 754 required"); + canardDSDLSetUxx(buf, off_bit, tmp.in, sizeof(tmp) * BYTE_WIDTH); } - -#endif // CANARD_DSDL_PLATFORM_IEEE754 diff --git a/libcanard/canard_dsdl.h b/libcanard/canard_dsdl.h index 53339354..f1a18462 100644 --- a/libcanard/canard_dsdl.h +++ b/libcanard/canard_dsdl.h @@ -1,7 +1,8 @@ // This software is distributed under the terms of the MIT License. // Copyright (c) 2016-2020 UAVCAN Development Team. +// Author: Pavel Kirienko // -// This is a trivial optional extension library that contains basic DSDL primitive serialization routines. +// This is a trivial optional extension library that contains basic DSDL serialization routines. // It is intended for use in simple applications where auto-generated DSDL serialization logic is not available. // The functions are fully stateless (pure); read their documentation comments for usage information. // This is an optional part of libcanard that can be omitted if this functionality is not required by the application. @@ -10,7 +11,6 @@ #ifndef CANARD_DSDL_H_INCLUDED #define CANARD_DSDL_H_INCLUDED -#include #include #include #include @@ -19,101 +19,46 @@ extern "C" { #endif -#define CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT \ - ((INT8_MIN == -128) && (INT8_MAX == 127) && (INT16_MIN == -32768) && (INT16_MAX == 32767) && \ - (INT32_MIN == -0x80000000LL) && (INT32_MAX == 0x7FFFFFFFLL) && (INT64_MAX == 0x7FFFFFFFFFFFFFFFLL)) +typedef float CanardDSDLFloat32; +typedef double CanardDSDLFloat64; -#define CANARD_DSDL_PLATFORM_IEEE754 \ - ((FLT_RADIX == 2) && (FLT_MANT_DIG == 24) && (FLT_MIN_EXP == -125) && (FLT_MAX_EXP == 128)) +/// Serialize a DSDL field value at the specified bit offset from the beginning of the destination buffer. +/// The behavior is undefined if the input pointer is NULL. The time complexity is linear of the bit length. +/// Arguments: +/// buf Destination buffer where the result will be stored. +/// off_bit Offset, in bits, from the beginning of the buffer. May exceed one byte. +/// value The value itself (promoted to 64-bit for unification). +/// len_bit Length of the serialized representation, in bits. Zero has no effect. +void canardDSDLSetBit(uint8_t* const buf, const size_t off_bit, const bool value); +void canardDSDLSetUxx(uint8_t* const buf, const size_t off_bit, const uint64_t value, const uint8_t len_bit); +void canardDSDLSetIxx(uint8_t* const buf, const size_t off_bit, const int64_t value, const uint8_t len_bit); +void canardDSDLSetF16(uint8_t* const buf, const size_t off_bit, const CanardDSDLFloat32 value); +void canardDSDLSetF32(uint8_t* const buf, const size_t off_bit, const CanardDSDLFloat32 value); +void canardDSDLSetF64(uint8_t* const buf, const size_t off_bit, const CanardDSDLFloat64 value); -#if CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT - -/// This function may be used to serialize values for later transmission in a UAVCAN transfer. -/// It serializes a primitive value -- boolean, integer, character, or floating point -- following the DSDL -/// primitive serialization rules, and puts it at the specified bit offset in the destination buffer. -/// -/// The function is only available if the platform uses two's complement signed integer representation. -/// -/// If any of the input pointers are NULL or the value of length_bit is not specified in the table, zero is returned. -/// -/// If the source and the destination areas overlap, the behavior is undefined. -/// -/// The type of the value pointed to by 'value' is defined as follows: -/// -/// | bit_length | value points to | -/// |------------|------------------------------------------| -/// | 1 | bool (may be incompatible with uint8_t!) | -/// | [2, 8] | uint8_t, int8_t, or char | -/// | [9, 16] | uint16_t, int16_t | -/// | [17, 32] | uint32_t, int32_t, or 32-bit float | -/// | [33, 64] | uint64_t, int64_t, or 64-bit float | -/// -/// @param destination Destination buffer where the result will be stored. -/// @param offset_bit Offset, in bits, from the beginning of the destination buffer. May exceed one byte. -/// @param length_bit Length of the value, in bits; see the table. -/// @param value Pointer to the value; see the table. -/// @returns (offset_bit + length_bit) on success, zero if any of the arguments are invalid. -size_t canardDSDLPrimitiveSerialize(uint8_t* const destination, - const size_t offset_bit, - const uint8_t length_bit, - const void* const value); - -/// This function may be used to extract values from received UAVCAN transfers. -/// It deserializes a scalar value -- boolean, integer, character, or floating point -- from the specified -/// bit position in the source buffer. -/// -/// The function is only available if the platform uses two's complement signed integer representation. -/// -/// If any of the input pointers are NULL or the value of length_bit is not specified in the table, zero is returned. -/// -/// If the source and the destination areas overlap, the behavior is undefined. -/// -/// The type of the value pointed to by 'out_value' is defined as follows: -/// -/// | bit_length | is_signed | out_value points to | -/// |------------|-------------|------------------------------------------| -/// | 1 | false | bool (may be incompatible with uint8_t!) | -/// | 1 | true | N/A | -/// | [2, 8] | false | uint8_t, or char | -/// | [2, 8] | true | int8_t, or char | -/// | [9, 16] | false | uint16_t | -/// | [9, 16] | true | int16_t | -/// | [17, 32] | false | uint32_t | -/// | [17, 32] | true | int32_t, or 32-bit float IEEE 754 | -/// | [33, 64] | false | uint64_t | -/// | [33, 64] | true | int64_t, or 64-bit float IEEE 754 | -/// -/// @param source The source buffer where the data will be read from. -/// @param source_size_bytes Bytes above this size will be assumed to equal zero, per the zero extension rule. -/// @param offset_bit Offset, in bits, from the beginning of the source buffer. May exceed one byte. -/// @param length_bit Length of the value, in bits; see the table. -/// @param is_signed True if the value can be negative (i.e., sign bit extension is needed); see the table. -/// @param out_value Pointer to the output storage; see the table. -/// @returns (offset_bit + length_bit) on success, zero if any of the arguments are invalid. -size_t canardDSDLPrimitiveDeserialize(const uint8_t* const source, - const size_t source_size_bytes, - const size_t offset_bit, - const uint8_t length_bit, - const bool is_signed, - void* const out_value); - -#endif // CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT - -#if CANARD_DSDL_PLATFORM_IEEE754 - -/// This alias for the native float is required to comply with MISRA. -typedef float CanardDSDLFloatNative; - -/// Convert a native float into the standard IEEE 754 binary16 format. The byte order is native. -/// Overflow collapses into infinity with the same sign. -/// This function is only available if the native float format is IEEE 754 binary32. -uint16_t canardDSDLFloat16Pack(const CanardDSDLFloatNative value); - -/// Convert a standard IEEE 754 binary16 value into the native float format. The byte order is native. -/// This function is only available if the native float format is IEEE 754 binary32. -CanardDSDLFloatNative canardDSDLFloat16Unpack(const uint16_t value); - -#endif // CANARD_DSDL_PLATFORM_IEEE754 +/// Deserialize a DSDL field value located at the specified bit offset from the beginning of the source buffer. +/// If the deserialized value extends beyond the end of the buffer, the missing bits are taken as zero, as required +/// by the DSDL specification (see Implicit Zero Extension Rule, IZER). +/// If len_bit is greater than the return type, extra bits will be truncated per regular narrowing conversion rules. +/// The behavior is undefined if the input pointer is NULL. The time complexity is linear of the bit length. +/// Returns the deserialized value. If the value spills over the buffer boundary, the spilled bits are taken as zero. +/// Arguments: +/// buf Source buffer where the serialized representation will be read from. +/// buf_size The size of the source buffer, in bytes. Reads past this limit will be assumed to return zero bits. +/// off_bit Offset, in bits, from the beginning of the buffer. May exceed one byte. +/// len_bit Length of the serialized representation, in bits. Zero returns zero. +bool canardDSDLGetBit(const uint8_t* const buf, const size_t buf_size, const size_t off_bit); +uint8_t canardDSDLGetU08(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); +uint16_t canardDSDLGetU16(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); +uint32_t canardDSDLGetU32(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); +uint64_t canardDSDLGetU64(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); +int8_t canardDSDLGetI08(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); +int16_t canardDSDLGetI16(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); +int32_t canardDSDLGetI32(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); +int64_t canardDSDLGetI64(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); +CanardDSDLFloat32 canardDSDLGetF16(const uint8_t* const buf, const size_t buf_size, const size_t off_bit); +CanardDSDLFloat32 canardDSDLGetF32(const uint8_t* const buf, const size_t buf_size, const size_t off_bit); +CanardDSDLFloat64 canardDSDLGetF64(const uint8_t* const buf, const size_t buf_size, const size_t off_bit); #ifdef __cplusplus } diff --git a/tests/exposed.hpp b/tests/exposed.hpp index e8b857cf..175c754f 100644 --- a/tests/exposed.hpp +++ b/tests/exposed.hpp @@ -118,5 +118,8 @@ void copyBitArray(const std::size_t length_bit, const std::size_t dst_offset_bit, const std::uint8_t* const src, std::uint8_t* const dst); + +auto float16Pack(const float value) -> std::uint16_t; +auto float16Unpack(const std::uint16_t value) -> float; } } // namespace exposed diff --git a/tests/test_dsdl.cpp b/tests/test_dsdl.cpp index 16a946a4..5f2c8cc3 100644 --- a/tests/test_dsdl.cpp +++ b/tests/test_dsdl.cpp @@ -7,35 +7,39 @@ #include #include -TEST_CASE("canardDSDLFloat16Pack") +TEST_CASE("float16Pack") { + using exposed::float16Pack; // Reference values were generated manually with libuavcan and numpy.float16(). - REQUIRE(0b0000000000000000 == canardDSDLFloat16Pack(0.0F)); - REQUIRE(0b0011110000000000 == canardDSDLFloat16Pack(1.0F)); - REQUIRE(0b1100000000000000 == canardDSDLFloat16Pack(-2.0F)); - REQUIRE(0b0111110000000000 == canardDSDLFloat16Pack(999999.0F)); // +inf - REQUIRE(0b1111101111111111 == canardDSDLFloat16Pack(-65519.0F)); // -max - REQUIRE(0b0111111111111111 == canardDSDLFloat16Pack(std::nanf(""))); // nan + REQUIRE(0b0000000000000000 == float16Pack(0.0F)); + REQUIRE(0b0011110000000000 == float16Pack(1.0F)); + REQUIRE(0b1100000000000000 == float16Pack(-2.0F)); + REQUIRE(0b0111110000000000 == float16Pack(999999.0F)); // +inf + REQUIRE(0b1111101111111111 == float16Pack(-65519.0F)); // -max + REQUIRE(0b0111111111111111 == float16Pack(std::nanf(""))); // nan } -TEST_CASE("canardDSDLFloat16Unpack") +TEST_CASE("float16Unpack") { + using exposed::float16Unpack; // Reference values were generated manually with libuavcan and numpy.float16(). - REQUIRE(Approx(0.0F) == canardDSDLFloat16Unpack(0b0000000000000000)); - REQUIRE(Approx(1.0F) == canardDSDLFloat16Unpack(0b0011110000000000)); - REQUIRE(Approx(-2.0F) == canardDSDLFloat16Unpack(0b1100000000000000)); - REQUIRE(Approx(-65504.0F) == canardDSDLFloat16Unpack(0b1111101111111111)); - REQUIRE(std::isinf(canardDSDLFloat16Unpack(0b0111110000000000))); + REQUIRE(Approx(0.0F) == float16Unpack(0b0000000000000000)); + REQUIRE(Approx(1.0F) == float16Unpack(0b0011110000000000)); + REQUIRE(Approx(-2.0F) == float16Unpack(0b1100000000000000)); + REQUIRE(Approx(-65504.0F) == float16Unpack(0b1111101111111111)); + REQUIRE(std::isinf(float16Unpack(0b0111110000000000))); - REQUIRE(bool(std::isnan(canardDSDLFloat16Unpack(0b0111111111111111)))); + REQUIRE(bool(std::isnan(float16Unpack(0b0111111111111111)))); } TEST_CASE("canardDSDLFloat16") { + using exposed::float16Pack; + using exposed::float16Unpack; float x = -1000.0F; while (x <= 1000.0F) { - REQUIRE(Approx(x) == canardDSDLFloat16Unpack(canardDSDLFloat16Pack(x))); + REQUIRE(Approx(x) == float16Unpack(float16Pack(x))); x += 0.5F; } } @@ -71,7 +75,7 @@ TEST_CASE("copyBitArray") REQUIRE(test(8, 4, 4, {0x55, 0x55}, {0xAA, 0xAA}, {0x5A, 0xA5})); } -TEST_CASE("canardDSDLPrimitiveSerialize_aligned") +TEST_CASE("canardDSDLSerialize_aligned") { // The reference values for the following test have been taken from the PyUAVCAN test suite. const std::vector Reference({0xA7, 0xEF, 0xCD, 0xAB, 0x90, 0x78, 0x56, 0x34, 0x12, 0x88, 0xA9, 0xCB, @@ -81,48 +85,45 @@ TEST_CASE("canardDSDLPrimitiveSerialize_aligned") std::vector dest(std::size(Reference)); - std::uint8_t u8 = 0b1010'0111; - REQUIRE(8 == canardDSDLPrimitiveSerialize(dest.data(), 0, 8, &u8)); - - std::int64_t i64 = 0x1234'5678'90ab'cdef; - REQUIRE(72 == canardDSDLPrimitiveSerialize(dest.data(), 8, 64, &i64)); - - std::int32_t i32 = -0x1234'5678; - REQUIRE(104 == canardDSDLPrimitiveSerialize(dest.data(), 72, 32, &i32)); - - std::int32_t i16 = -2; - u8 = 0; - std::int8_t i8 = 127; - REQUIRE(120 == canardDSDLPrimitiveSerialize(dest.data(), 104, 16, &i16)); - REQUIRE(128 == canardDSDLPrimitiveSerialize(dest.data(), 120, 8, &u8)); - REQUIRE(136 == canardDSDLPrimitiveSerialize(dest.data(), 128, 8, &i8)); - - double f64 = 1.0; - float f32 = 1.0F; - std::uint16_t f16 = canardDSDLFloat16Pack(99999.9F); - REQUIRE(200 == canardDSDLPrimitiveSerialize(dest.data(), 136, 64, &f64)); - REQUIRE(232 == canardDSDLPrimitiveSerialize(dest.data(), 200, 32, &f32)); - REQUIRE(248 == canardDSDLPrimitiveSerialize(dest.data(), 232, 16, &f16)); - - std::uint16_t u16 = 0xBEDA; - u8 = 0; - REQUIRE(260 == canardDSDLPrimitiveSerialize(dest.data(), 248, 12, &u16)); - REQUIRE(264 == canardDSDLPrimitiveSerialize(dest.data(), 260, 4, &u8)); - REQUIRE(280 == canardDSDLPrimitiveSerialize(dest.data(), 264, 16, &u16)); - - i16 = -2; - u8 = 0; - REQUIRE(289 == canardDSDLPrimitiveSerialize(dest.data(), 280, 9, &i16)); - REQUIRE(296 == canardDSDLPrimitiveSerialize(dest.data(), 289, 7, &u8)); - - u16 = 0xDEAD; - REQUIRE(312 == canardDSDLPrimitiveSerialize(dest.data(), 296, 16, &u16)); - u16 = 0xBEEF; - REQUIRE(328 == canardDSDLPrimitiveSerialize(dest.data(), 312, 16, &u16)); + const auto set_b = [&](const std::size_t offset_bit, const bool value) { + canardDSDLSetBit(dest.data(), offset_bit, value); + }; + const auto set_u = [&](const std::size_t offset_bit, const std::uint64_t value, const std::uint8_t length_bit) { + canardDSDLSetUxx(dest.data(), offset_bit, value, length_bit); + }; + const auto set_i = [&](const std::size_t offset_bit, const std::int64_t value, const std::uint8_t length_bit) { + canardDSDLSetIxx(dest.data(), offset_bit, value, length_bit); + }; + const auto set_f16 = [&](const std::size_t offset_bit, const float value) { + canardDSDLSetF16(dest.data(), offset_bit, value); + }; + const auto set_f32 = [&](const std::size_t offset_bit, const float value) { + canardDSDLSetF32(dest.data(), offset_bit, value); + }; + const auto set_f64 = [&](const std::size_t offset_bit, const double value) { + canardDSDLSetF64(dest.data(), offset_bit, value); + }; + + set_u(0, 0b1010'0111, 8); + set_i(8, 0x1234'5678'90ab'cdef, 64); + set_i(72, -0x1234'5678, 32); + set_i(104, -2, 16); + set_u(120, 0, 8); + set_i(128, 127, 8); + set_f64(136, 1.0); + set_f32(200, 1.0F); + set_f16(232, 99999.9F); + set_u(248, 0xBEDA, 12); // Truncation + set_u(260, 0, 4); + set_u(264, 0xBEDA, 16); + set_i(280, -2, 9); + set_i(289, 0, 7); + set_u(296, 0xDEAD, 16); + set_u(312, 0xBEEF, 16); std::size_t offset = 328; const auto push_bit = [&](const bool value) { - REQUIRE((offset + 1U) == canardDSDLPrimitiveSerialize(dest.data(), offset, 1, &value)); + set_b(offset, value); ++offset; }; @@ -157,14 +158,13 @@ TEST_CASE("canardDSDLPrimitiveSerialize_aligned") push_bit(true); push_bit(false); - u8 = 0; - REQUIRE(360 == canardDSDLPrimitiveSerialize(dest.data(), 357, 3, &u8)); + set_u(357, 0, 3); REQUIRE(std::size(dest) == std::size(Reference)); REQUIRE_THAT(dest, Catch::Matchers::Equals(Reference)); } -TEST_CASE("canardDSDLPrimitiveSerialize_unaligned") +TEST_CASE("canardDSDLSerialize_unaligned") { // The reference values for the following test have been taken from the PyUAVCAN test suite. const std::vector Reference({ @@ -174,9 +174,28 @@ TEST_CASE("canardDSDLPrimitiveSerialize_unaligned") std::vector dest(std::size(Reference)); + const auto set_b = [&](const std::size_t offset_bit, const bool value) { + canardDSDLSetBit(dest.data(), offset_bit, value); + }; + const auto set_u = [&](const std::size_t offset_bit, const std::uint64_t value, const std::uint8_t length_bit) { + canardDSDLSetUxx(dest.data(), offset_bit, value, length_bit); + }; + const auto set_i = [&](const std::size_t offset_bit, const std::int64_t value, const std::uint8_t length_bit) { + canardDSDLSetIxx(dest.data(), offset_bit, value, length_bit); + }; + const auto set_f16 = [&](const std::size_t offset_bit, const float value) { + canardDSDLSetF16(dest.data(), offset_bit, value); + }; + const auto set_f32 = [&](const std::size_t offset_bit, const float value) { + canardDSDLSetF32(dest.data(), offset_bit, value); + }; + const auto set_f64 = [&](const std::size_t offset_bit, const double value) { + canardDSDLSetF64(dest.data(), offset_bit, value); + }; + std::size_t offset = 0; const auto push_bit = [&](const bool value) { - REQUIRE((offset + 1U) == canardDSDLPrimitiveSerialize(dest.data(), offset, 1, &value)); + set_b(offset, value); ++offset; }; @@ -206,12 +225,9 @@ TEST_CASE("canardDSDLPrimitiveSerialize_unaligned") REQUIRE_THAT(std::vector(std::begin(dest), std::begin(dest) + 2), Catch::Matchers::Equals(std::vector(std::begin(Reference), std::begin(Reference) + 2))); - std::uint8_t u8 = 0x12; - REQUIRE(29 == canardDSDLPrimitiveSerialize(dest.data(), 21, 8, &u8)); - u8 = 0x34; - REQUIRE(37 == canardDSDLPrimitiveSerialize(dest.data(), 29, 8, &u8)); - u8 = 0x56; - REQUIRE(45 == canardDSDLPrimitiveSerialize(dest.data(), 37, 8, &u8)); + set_u(21, 0x12, 8); + set_u(29, 0x34, 8); + set_u(37, 0x56, 8); REQUIRE_THAT(std::vector(std::begin(dest), std::begin(dest) + 5), Catch::Matchers::Equals(std::vector(std::begin(Reference), std::begin(Reference) + 5))); @@ -221,12 +237,9 @@ TEST_CASE("canardDSDLPrimitiveSerialize_unaligned") push_bit(true); push_bit(true); - u8 = 0x12; - REQUIRE(56 == canardDSDLPrimitiveSerialize(dest.data(), 48, 8, &u8)); - u8 = 0x34; - REQUIRE(64 == canardDSDLPrimitiveSerialize(dest.data(), 56, 8, &u8)); - u8 = 0x56; - REQUIRE(72 == canardDSDLPrimitiveSerialize(dest.data(), 64, 8, &u8)); + set_u(48, 0x12, 8); + set_u(56, 0x34, 8); + set_u(64, 0x56, 8); offset = 72; push_bit(true); @@ -238,53 +251,40 @@ TEST_CASE("canardDSDLPrimitiveSerialize_unaligned") REQUIRE_THAT(std::vector(std::begin(dest), std::begin(dest) + 9), Catch::Matchers::Equals(std::vector(std::begin(Reference), std::begin(Reference) + 9))); - std::int8_t i8 = -2; - REQUIRE(85 == canardDSDLPrimitiveSerialize(dest.data(), 77, 8, &i8)); - - std::uint16_t u16 = 0b11101100101; - REQUIRE(96 == canardDSDLPrimitiveSerialize(dest.data(), 85, 11, &u16)); - - u8 = 0b1110; - REQUIRE(99 == canardDSDLPrimitiveSerialize(dest.data(), 96, 3, &u8)); + set_i(77, -2, 8); + set_u(85, 0b11101100101, 11); + set_u(96, 0b1110, 3); // Truncation REQUIRE_THAT(std::vector(std::begin(dest), std::begin(dest) + 12), Catch::Matchers::Equals(std::vector(std::begin(Reference), std::begin(Reference) + 12))); - double f64 = 1.0; - float f32 = 1.0F; - std::uint16_t f16 = canardDSDLFloat16Pack(-99999.0F); - REQUIRE(163 == canardDSDLPrimitiveSerialize(dest.data(), 99, 64, &f64)); - REQUIRE(195 == canardDSDLPrimitiveSerialize(dest.data(), 163, 32, &f32)); - REQUIRE(211 == canardDSDLPrimitiveSerialize(dest.data(), 195, 16, &f16)); + set_f64(99, 1.0); + set_f32(163, 1.0F); + set_f16(195, -99999.0F); - u16 = 0xDEAD; - REQUIRE(227 == canardDSDLPrimitiveSerialize(dest.data(), 211, 16, &u16)); - u16 = 0xBEEF; - REQUIRE(243 == canardDSDLPrimitiveSerialize(dest.data(), 227, 16, &u16)); - - u8 = 0; - REQUIRE(248 == canardDSDLPrimitiveSerialize(dest.data(), 243, 5, &u8)); + set_u(211, 0xDEAD, 16); + set_u(227, 0xBEEF, 16); + set_u(243, 0, 5); REQUIRE(std::size(dest) == std::size(Reference)); REQUIRE_THAT(dest, Catch::Matchers::Equals(Reference)); } -TEST_CASE("canardDSDLPrimitiveSerialize_heartbeat") +TEST_CASE("canardDSDLSerialize_heartbeat") { // The reference values were taken from the PyUAVCAN test. const std::vector Reference({239, 190, 173, 222, 234, 255, 255, 0}); std::vector dest(std::size(Reference)); - std::uint32_t uptime = 0xdeadbeef; - std::uint8_t health = 2; - std::uint8_t mode = 2; - std::uint32_t vssc = 0x7FFFF; + const auto set_u = [&](const std::size_t offset_bit, const std::uint64_t value, const std::uint8_t length_bit) { + canardDSDLSetUxx(dest.data(), offset_bit, value, length_bit); + }; - REQUIRE(37 == canardDSDLPrimitiveSerialize(dest.data(), 34, 3, &mode)); - REQUIRE(32 == canardDSDLPrimitiveSerialize(dest.data(), 0, 32, &uptime)); - REQUIRE(56 == canardDSDLPrimitiveSerialize(dest.data(), 37, 19, &vssc)); - REQUIRE(34 == canardDSDLPrimitiveSerialize(dest.data(), 32, 2, &health)); + set_u(34, 2, 3); // mode + set_u(0, 0xdeadbeef, 32); // uptime + set_u(37, 0x7FFFF, 19); // vssc + set_u(32, 2, 2); // health REQUIRE(std::size(dest) == std::size(Reference)); REQUIRE_THAT(dest, Catch::Matchers::Equals(Reference)); From 6aa8de0fda57e34780f1e7c4ea5538981142f7f6 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 28 Feb 2020 17:24:49 +0200 Subject: [PATCH 070/100] Working on the deserialization logic and its tests --- libcanard/canard_dsdl.c | 167 ++++++++++++++++++++++++++++++++++++---- libcanard/canard_dsdl.h | 10 ++- tests/test_dsdl.cpp | 25 ++++++ 3 files changed, 181 insertions(+), 21 deletions(-) diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index 8eec87a7..e5590bd3 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -38,13 +38,6 @@ # error "Unsupported language: ISO C11 or a newer version is required." #endif -#define CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT \ - ((INT8_MIN == -128) && (INT8_MAX == 127) && (INT16_MIN == -32768) && (INT16_MAX == 32767) && \ - (INT32_MIN == -0x80000000LL) && (INT32_MAX == 0x7FFFFFFFLL) && (INT64_MAX == 0x7FFFFFFFFFFFFFFFLL)) -_Static_assert(CANARD_DSDL_PLATFORM_TWOS_COMPLEMENT, - "This component cannot be used on this platform because it uses a non-twos-complement signed integer " - "representation."); - #define CANARD_DSDL_PLATFORM_IEEE754 \ ((FLT_RADIX == 2) && (FLT_MANT_DIG == 24) && (FLT_MIN_EXP == -125) && (FLT_MAX_EXP == 128) && \ (DBL_MANT_DIG == 53) && (DBL_MIN_EXP == -1021) && (DBL_MAX_EXP == 1024)) @@ -53,14 +46,12 @@ _Static_assert(CANARD_DSDL_PLATFORM_IEEE754, "point model. It is possible to support other platforms, but this has not been done yet. " "If your platform is not IEEE 754-compatible, please reach the maintainers via http://forum.uavcan.org"); -// --------------------------------------------- COMMON CONSTANTS --------------------------------------------- +// --------------------------------------------- COMMON ITEMS --------------------------------------------- /// Per the DSDL specification, it is assumed that 1 byte = 8 bits. #define BYTE_WIDTH 8U #define BYTE_MAX 0xFFU -#define WIDTHBIT 1U -#define WIDTH8 8U #define WIDTH16 16U #define WIDTH32 32U #define WIDTH64 64U @@ -204,12 +195,17 @@ CANARD_PRIVATE void copyBitArray(const size_t length_bit, } } +CANARD_PRIVATE size_t getBitCopySize(const size_t buf_size_bytes, + const size_t offset_bit, + const size_t copy_length_bit); +CANARD_PRIVATE size_t getBitCopySize(const size_t buf_size_bytes, const size_t offset_bit, const size_t copy_length_bit) +{ + const size_t buf_size_bit = buf_size_bytes * BYTE_WIDTH; + const size_t remaining_bit = buf_size_bit - chooseMin(buf_size_bit, offset_bit); + return chooseMin(remaining_bit, copy_length_bit); +} + // --------------------------------------------- PUBLIC API --------------------------------------------- -// The following code relies on the C preprocessor, which is a violation of MISRA and other high-integrity coding -// guidelines. This is an intentional deviation because C does not support generics and the only viable alternative -// would be to perform the required operations at runtime sacrificing the scarce type safety provided by C. -// This deviation is therefore considered to be acceptable and superior to all alternative solutions (other than -// not using C at all). void canardDSDLSetBit(uint8_t* const buf, const size_t off_bit, const bool value) { @@ -222,8 +218,9 @@ void canardDSDLSetUxx(uint8_t* const buf, const size_t off_bit, const uint64_t v { _Static_assert(WIDTH64 == sizeof(uint64_t) * BYTE_WIDTH, "Unexpected size of uint64_t"); CANARD_ASSERT(buf != NULL); + const size_t saturated_len_bit = chooseMin(len_bit, WIDTH64); #if CANARD_DSDL_CONFIG_LITTLE_ENDIAN - copyBitArray(len_bit, 0U, off_bit, (const uint8_t*) &value, buf); + copyBitArray(saturated_len_bit, 0U, off_bit, (const uint8_t*) &value, buf); #else uint8_t tmp[sizeof(uint64_t)] = {0}; uint64_t x = value; @@ -234,7 +231,7 @@ void canardDSDLSetUxx(uint8_t* const buf, const size_t off_bit, const uint64_t v x >>= BYTE_WIDTH; ++i; } - copyBitArray(len_bit, 0U, off_bit, &tmp[0], buf); + copyBitArray(saturated_len_bit, 0U, off_bit, &tmp[0], buf); #endif } @@ -280,3 +277,139 @@ void canardDSDLSetF64(uint8_t* const buf, const size_t off_bit, const CanardDSDL _Static_assert(WIDTH64 == sizeof(tmp) * BYTE_WIDTH, "IEEE 754 required"); canardDSDLSetUxx(buf, off_bit, tmp.in, sizeof(tmp) * BYTE_WIDTH); } + +bool canardDSDLGetBit(const uint8_t* const buf, const size_t buf_size, const size_t off_bit) +{ + return canardDSDLGetU8(buf, buf_size, off_bit, 1U) == 1U; +} + +uint8_t canardDSDLGetU8(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) +{ + CANARD_ASSERT(buf != NULL); + const size_t copy_size = getBitCopySize(buf_size, off_bit, chooseMin(len_bit, BYTE_WIDTH)); + uint8_t x = 0; + copyBitArray(copy_size, off_bit, 0U, buf, &x); + return x; +} + +uint16_t canardDSDLGetU16(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) +{ + CANARD_ASSERT(buf != NULL); + const size_t copy_size = getBitCopySize(buf_size, off_bit, chooseMin(len_bit, WIDTH16)); +#if CANARD_DSDL_CONFIG_LITTLE_ENDIAN + uint16_t x = 0U; + copyBitArray(copy_size, off_bit, 0U, buf, (uint8_t*) &x); + return x; +#else + uint8_t tmp[sizeof(uint16_t)] = {0}; + copyBitArray(copy_size, off_bit, 0U, buf, &tmp[0]); + return tmp[0] | (uint16_t)(((uint16_t) tmp[1]) << BYTE_WIDTH); +#endif +} + +uint32_t canardDSDLGetU32(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) +{ + CANARD_ASSERT(buf != NULL); + const size_t copy_size = getBitCopySize(buf_size, off_bit, chooseMin(len_bit, WIDTH32)); + uint32_t x = 0U; +#if CANARD_DSDL_CONFIG_LITTLE_ENDIAN + copyBitArray(copy_size, off_bit, 0U, buf, (uint8_t*) &x); +#else + uint8_t tmp[sizeof(uint32_t)] = {0}; + copyBitArray(copy_size, off_bit, 0U, buf, &tmp[0]); + for (size_t i = sizeof(tmp); i > 0U; --i) + { + x <<= BYTE_WIDTH; + CANARD_ASSERT(i > 0U); + x |= tmp[i - 1U]; + } +#endif + return x; +} + +uint64_t canardDSDLGetU64(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) +{ + CANARD_ASSERT(buf != NULL); + const size_t copy_size = getBitCopySize(buf_size, off_bit, chooseMin(len_bit, WIDTH64)); + uint64_t x = 0U; +#if CANARD_DSDL_CONFIG_LITTLE_ENDIAN + copyBitArray(copy_size, off_bit, 0U, buf, (uint8_t*) &x); +#else + uint8_t tmp[sizeof(uint64_t)] = {0}; + copyBitArray(copy_size, off_bit, 0U, buf, &tmp[0]); + for (size_t i = sizeof(tmp); i > 0U; --i) + { + x <<= BYTE_WIDTH; + CANARD_ASSERT(i > 0U); + x |= tmp[i - 1U]; + } +#endif + return x; +} + +int8_t canardDSDLGetI8(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) +{ + uint8_t u = canardDSDLGetU8(buf, buf_size, off_bit, len_bit); + const bool negative = (len_bit > 0U) && ((u & (1ULL << (len_bit - 1U))) != 0U); + u |= ((len_bit < BYTE_WIDTH) && negative) ? ((uint8_t) ~((1ULL << len_bit) - 1U)) : 0U; + return negative ? ((-(int8_t) ~u) - 1) : (int8_t) u; +} + +int16_t canardDSDLGetI16(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) +{ + uint16_t u = canardDSDLGetU16(buf, buf_size, off_bit, len_bit); + const bool negative = (len_bit > 0U) && ((u & (1ULL << (len_bit - 1U))) != 0U); + u |= ((len_bit < WIDTH16) && negative) ? ((uint16_t) ~((1ULL << len_bit) - 1U)) : 0U; + return negative ? ((-(int16_t) ~u) - 1) : (int16_t) u; +} + +int32_t canardDSDLGetI32(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) +{ + uint32_t u = canardDSDLGetU32(buf, buf_size, off_bit, len_bit); + const bool negative = (len_bit > 0U) && ((u & (1ULL << (len_bit - 1U))) != 0U); + u |= ((len_bit < WIDTH32) && negative) ? ((uint32_t) ~((1ULL << len_bit) - 1U)) : 0U; + return negative ? ((-(int32_t) ~u) - 1) : (int32_t) u; +} + +int64_t canardDSDLGetI64(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) +{ + uint64_t u = canardDSDLGetU64(buf, buf_size, off_bit, len_bit); + const bool negative = (len_bit > 0U) && ((u & (1ULL << (len_bit - 1U))) != 0U); + u |= ((len_bit < WIDTH64) && negative) ? ((uint64_t) ~((1ULL << len_bit) - 1U)) : 0U; + return negative ? ((-(int64_t) ~u) - 1) : (int64_t) u; +} + +CanardDSDLFloat32 canardDSDLGetF16(const uint8_t* const buf, const size_t buf_size, const size_t off_bit) +{ + return float16Unpack(canardDSDLGetU16(buf, buf_size, off_bit, WIDTH16)); +} + +CanardDSDLFloat32 canardDSDLGetF32(const uint8_t* const buf, const size_t buf_size, const size_t off_bit) +{ + // Intentional violation of MISRA: use union to perform fast conversion to an IEEE 754-compatible native + // representation into a serializable integer. The assumptions about the target platform properties are made + // clear. In the future we may add a more generic conversion that is platform-invariant. + _Static_assert(CANARD_DSDL_PLATFORM_IEEE754, "IEEE 754 required"); + union // NOSONAR + { + uint32_t in; + CanardDSDLFloat32 fl; + } tmp = {canardDSDLGetU32(buf, buf_size, off_bit, WIDTH32)}; // NOSONAR + _Static_assert(WIDTH32 == sizeof(tmp) * BYTE_WIDTH, "IEEE 754 required"); + return tmp.fl; +} + +CanardDSDLFloat64 canardDSDLGetF64(const uint8_t* const buf, const size_t buf_size, const size_t off_bit) +{ + // Intentional violation of MISRA: use union to perform fast conversion to an IEEE 754-compatible native + // representation into a serializable integer. The assumptions about the target platform properties are made + // clear. In the future we may add a more generic conversion that is platform-invariant. + _Static_assert(CANARD_DSDL_PLATFORM_IEEE754, "IEEE 754 required"); + union // NOSONAR + { + uint64_t in; + CanardDSDLFloat64 fl; + } tmp = {canardDSDLGetU64(buf, buf_size, off_bit, WIDTH64)}; // NOSONAR + _Static_assert(WIDTH64 == sizeof(tmp) * BYTE_WIDTH, "IEEE 754 required"); + return tmp.fl; +} diff --git a/libcanard/canard_dsdl.h b/libcanard/canard_dsdl.h index f1a18462..fef31cd4 100644 --- a/libcanard/canard_dsdl.h +++ b/libcanard/canard_dsdl.h @@ -24,11 +24,12 @@ typedef double CanardDSDLFloat64; /// Serialize a DSDL field value at the specified bit offset from the beginning of the destination buffer. /// The behavior is undefined if the input pointer is NULL. The time complexity is linear of the bit length. +/// One-bit-wide signed integers are processed without raising an error or a deviation but the result is unspecified. /// Arguments: /// buf Destination buffer where the result will be stored. /// off_bit Offset, in bits, from the beginning of the buffer. May exceed one byte. /// value The value itself (promoted to 64-bit for unification). -/// len_bit Length of the serialized representation, in bits. Zero has no effect. +/// len_bit Length of the serialized representation, in bits. Zero has no effect. Values above 64 are saturated. void canardDSDLSetBit(uint8_t* const buf, const size_t off_bit, const bool value); void canardDSDLSetUxx(uint8_t* const buf, const size_t off_bit, const uint64_t value, const uint8_t len_bit); void canardDSDLSetIxx(uint8_t* const buf, const size_t off_bit, const int64_t value, const uint8_t len_bit); @@ -41,18 +42,19 @@ void canardDSDLSetF64(uint8_t* const buf, const size_t off_bit, const CanardDSDL /// by the DSDL specification (see Implicit Zero Extension Rule, IZER). /// If len_bit is greater than the return type, extra bits will be truncated per regular narrowing conversion rules. /// The behavior is undefined if the input pointer is NULL. The time complexity is linear of the bit length. +/// One-bit-wide signed integers are processed without raising an error or a deviation but the result is unspecified. /// Returns the deserialized value. If the value spills over the buffer boundary, the spilled bits are taken as zero. /// Arguments: /// buf Source buffer where the serialized representation will be read from. /// buf_size The size of the source buffer, in bytes. Reads past this limit will be assumed to return zero bits. /// off_bit Offset, in bits, from the beginning of the buffer. May exceed one byte. -/// len_bit Length of the serialized representation, in bits. Zero returns zero. +/// len_bit Length of the serialized representation, in bits. Zero returns zero. Out-of-range values are saturated. bool canardDSDLGetBit(const uint8_t* const buf, const size_t buf_size, const size_t off_bit); -uint8_t canardDSDLGetU08(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); +uint8_t canardDSDLGetU8(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); uint16_t canardDSDLGetU16(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); uint32_t canardDSDLGetU32(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); uint64_t canardDSDLGetU64(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); -int8_t canardDSDLGetI08(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); +int8_t canardDSDLGetI8(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); int16_t canardDSDLGetI16(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); int32_t canardDSDLGetI32(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); int64_t canardDSDLGetI64(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit); diff --git a/tests/test_dsdl.cpp b/tests/test_dsdl.cpp index 5f2c8cc3..63a11e53 100644 --- a/tests/test_dsdl.cpp +++ b/tests/test_dsdl.cpp @@ -289,3 +289,28 @@ TEST_CASE("canardDSDLSerialize_heartbeat") REQUIRE(std::size(dest) == std::size(Reference)); REQUIRE_THAT(dest, Catch::Matchers::Equals(Reference)); } + +TEST_CASE("canardDSDLDeserialize_aligned") +{ + // The reference values for the following test have been taken from the PyUAVCAN test suite. + const std::vector Reference({0xA7, 0xEF, 0xCD, 0xAB, 0x90, 0x78, 0x56, 0x34, 0x12, 0x88, 0xA9, 0xCB, + 0xED, 0xFE, 0xFF, 0x00, 0x7F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xF0, + 0x3F, 0x00, 0x00, 0x80, 0x3F, 0x00, 0x7C, 0xDA, 0x0E, 0xDA, 0xBE, 0xFE, + 0x01, 0xAD, 0xDE, 0xEF, 0xBE, 0xC5, 0x67, 0xC5, 0x0B}); + const std::uint8_t* const buf = Reference.data(); + + REQUIRE(canardDSDLGetBit(buf, 1, 0)); + REQUIRE(!canardDSDLGetBit(buf, 1, 3)); + REQUIRE(!canardDSDLGetBit(buf, 0, 0)); // IZER + + REQUIRE(0b1010'0111 == canardDSDLGetU8(buf, 45, 0, 8)); + + REQUIRE(0x1234'5678'90ab'cdef == canardDSDLGetI64(buf, 45, 8, 64)); + REQUIRE(0x1234'5678'90ab'cdef == canardDSDLGetU64(buf, 45, 8, 64)); + REQUIRE(0xef == canardDSDLGetU8(buf, 45, 8, 64)); + + REQUIRE(-0x1234'5678 == canardDSDLGetI32(buf, 45, 72, 32)); + REQUIRE(-2 == canardDSDLGetI16(buf, 45, 104, 16)); + REQUIRE(0 == canardDSDLGetU8(buf, 45, 120, 8)); + REQUIRE(127 == canardDSDLGetI8(buf, 45, 128, 8)); +} From 357d4d1ee07f45a8811cf6f0a3280dd553d61123 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 29 Feb 2020 20:08:08 +0200 Subject: [PATCH 071/100] Finished the tests for the deserializer routines --- libcanard/canard_dsdl.c | 137 +++++++++++++++++++++------------------- libcanard/canard_dsdl.h | 14 ++-- tests/test_dsdl.cpp | 135 ++++++++++++++++++++++++++++++++++++++- 3 files changed, 217 insertions(+), 69 deletions(-) diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index e5590bd3..cac26c25 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -195,14 +195,18 @@ CANARD_PRIVATE void copyBitArray(const size_t length_bit, } } -CANARD_PRIVATE size_t getBitCopySize(const size_t buf_size_bytes, - const size_t offset_bit, - const size_t copy_length_bit); -CANARD_PRIVATE size_t getBitCopySize(const size_t buf_size_bytes, const size_t offset_bit, const size_t copy_length_bit) +CANARD_PRIVATE size_t getBitCopySize(const size_t buf_size_bytes, + const size_t offset_bit, + const size_t requested_length_bit, + const uint8_t value_length_bit); +CANARD_PRIVATE size_t getBitCopySize(const size_t buf_size_bytes, + const size_t offset_bit, + const size_t requested_length_bit, + const uint8_t value_length_bit) { const size_t buf_size_bit = buf_size_bytes * BYTE_WIDTH; const size_t remaining_bit = buf_size_bit - chooseMin(buf_size_bit, offset_bit); - return chooseMin(remaining_bit, copy_length_bit); + return chooseMin(remaining_bit, chooseMin(requested_length_bit, value_length_bit)); } // --------------------------------------------- PUBLIC API --------------------------------------------- @@ -210,8 +214,8 @@ CANARD_PRIVATE size_t getBitCopySize(const size_t buf_size_bytes, const size_t o void canardDSDLSetBit(uint8_t* const buf, const size_t off_bit, const bool value) { CANARD_ASSERT(buf != NULL); - const uint8_t x = value ? 1U : 0U; - copyBitArray(1U, 0U, off_bit, &x, buf); + const uint8_t val = value ? 1U : 0U; + copyBitArray(1U, 0U, off_bit, &val, buf); } void canardDSDLSetUxx(uint8_t* const buf, const size_t off_bit, const uint64_t value, const uint8_t len_bit) @@ -222,15 +226,16 @@ void canardDSDLSetUxx(uint8_t* const buf, const size_t off_bit, const uint64_t v #if CANARD_DSDL_CONFIG_LITTLE_ENDIAN copyBitArray(saturated_len_bit, 0U, off_bit, (const uint8_t*) &value, buf); #else - uint8_t tmp[sizeof(uint64_t)] = {0}; - uint64_t x = value; - size_t i = 0; - while (x > 0U) // This conversion is independent of the native byte order. Slow but works everywhere. - { - tmp[i] = (uint8_t)(x & BYTE_MAX); - x >>= BYTE_WIDTH; - ++i; - } + const uint8_t tmp[sizeof(uint64_t)] = { + (uint8_t)((value >> 0U) & BYTE_MAX), // Suppress warnings about the magic numbers. Their purpose is clear. + (uint8_t)((value >> 8U) & BYTE_MAX), // NOLINT NOSONAR + (uint8_t)((value >> 16U) & BYTE_MAX), // NOLINT NOSONAR + (uint8_t)((value >> 24U) & BYTE_MAX), // NOLINT NOSONAR + (uint8_t)((value >> 32U) & BYTE_MAX), // NOLINT NOSONAR + (uint8_t)((value >> 40U) & BYTE_MAX), // NOLINT NOSONAR + (uint8_t)((value >> 48U) & BYTE_MAX), // NOLINT NOSONAR + (uint8_t)((value >> 56U) & BYTE_MAX), // NOLINT NOSONAR + }; copyBitArray(saturated_len_bit, 0U, off_bit, &tmp[0], buf); #endif } @@ -258,7 +263,7 @@ void canardDSDLSetF32(uint8_t* const buf, const size_t off_bit, const CanardDSDL { CanardDSDLFloat32 fl; uint32_t in; - } tmp = {value}; // NOSONAR + } const tmp = {value}; // NOSONAR _Static_assert(WIDTH32 == sizeof(tmp) * BYTE_WIDTH, "IEEE 754 required"); canardDSDLSetUxx(buf, off_bit, tmp.in, sizeof(tmp) * BYTE_WIDTH); } @@ -273,7 +278,7 @@ void canardDSDLSetF64(uint8_t* const buf, const size_t off_bit, const CanardDSDL { CanardDSDLFloat64 fl; uint64_t in; - } tmp = {value}; // NOSONAR + } const tmp = {value}; // NOSONAR _Static_assert(WIDTH64 == sizeof(tmp) * BYTE_WIDTH, "IEEE 754 required"); canardDSDLSetUxx(buf, off_bit, tmp.in, sizeof(tmp) * BYTE_WIDTH); } @@ -286,20 +291,22 @@ bool canardDSDLGetBit(const uint8_t* const buf, const size_t buf_size, const siz uint8_t canardDSDLGetU8(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) { CANARD_ASSERT(buf != NULL); - const size_t copy_size = getBitCopySize(buf_size, off_bit, chooseMin(len_bit, BYTE_WIDTH)); - uint8_t x = 0; - copyBitArray(copy_size, off_bit, 0U, buf, &x); - return x; + const size_t copy_size = getBitCopySize(buf_size, off_bit, len_bit, BYTE_WIDTH); + CANARD_ASSERT(copy_size <= (sizeof(uint8_t) * BYTE_WIDTH)); + uint8_t val = 0; + copyBitArray(copy_size, off_bit, 0U, buf, &val); + return val; } uint16_t canardDSDLGetU16(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) { CANARD_ASSERT(buf != NULL); - const size_t copy_size = getBitCopySize(buf_size, off_bit, chooseMin(len_bit, WIDTH16)); + const size_t copy_size = getBitCopySize(buf_size, off_bit, len_bit, WIDTH16); + CANARD_ASSERT(copy_size <= (sizeof(uint16_t) * BYTE_WIDTH)); #if CANARD_DSDL_CONFIG_LITTLE_ENDIAN - uint16_t x = 0U; - copyBitArray(copy_size, off_bit, 0U, buf, (uint8_t*) &x); - return x; + uint16_t val = 0U; + copyBitArray(copy_size, off_bit, 0U, buf, (uint8_t*) &val); + return val; #else uint8_t tmp[sizeof(uint16_t)] = {0}; copyBitArray(copy_size, off_bit, 0U, buf, &tmp[0]); @@ -310,73 +317,75 @@ uint16_t canardDSDLGetU16(const uint8_t* const buf, const size_t buf_size, const uint32_t canardDSDLGetU32(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) { CANARD_ASSERT(buf != NULL); - const size_t copy_size = getBitCopySize(buf_size, off_bit, chooseMin(len_bit, WIDTH32)); - uint32_t x = 0U; + const size_t copy_size = getBitCopySize(buf_size, off_bit, len_bit, WIDTH32); + CANARD_ASSERT(copy_size <= (sizeof(uint32_t) * BYTE_WIDTH)); #if CANARD_DSDL_CONFIG_LITTLE_ENDIAN - copyBitArray(copy_size, off_bit, 0U, buf, (uint8_t*) &x); + uint32_t val = 0U; + copyBitArray(copy_size, off_bit, 0U, buf, (uint8_t*) &val); + return val; #else uint8_t tmp[sizeof(uint32_t)] = {0}; copyBitArray(copy_size, off_bit, 0U, buf, &tmp[0]); - for (size_t i = sizeof(tmp); i > 0U; --i) - { - x <<= BYTE_WIDTH; - CANARD_ASSERT(i > 0U); - x |= tmp[i - 1U]; - } + return tmp[0] | // Suppress warnings about the magic numbers. Their purpose is clear here. + ((uint32_t) tmp[1] << 8U) | // NOLINT NOSONAR + ((uint32_t) tmp[2] << 16U) | // NOLINT NOSONAR + ((uint32_t) tmp[3] << 24U); // NOLINT NOSONAR #endif - return x; } uint64_t canardDSDLGetU64(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) { CANARD_ASSERT(buf != NULL); - const size_t copy_size = getBitCopySize(buf_size, off_bit, chooseMin(len_bit, WIDTH64)); - uint64_t x = 0U; + const size_t copy_size = getBitCopySize(buf_size, off_bit, len_bit, WIDTH64); + CANARD_ASSERT(copy_size <= (sizeof(uint64_t) * BYTE_WIDTH)); #if CANARD_DSDL_CONFIG_LITTLE_ENDIAN - copyBitArray(copy_size, off_bit, 0U, buf, (uint8_t*) &x); + uint64_t val = 0U; + copyBitArray(copy_size, off_bit, 0U, buf, (uint8_t*) &val); + return val; #else uint8_t tmp[sizeof(uint64_t)] = {0}; copyBitArray(copy_size, off_bit, 0U, buf, &tmp[0]); - for (size_t i = sizeof(tmp); i > 0U; --i) - { - x <<= BYTE_WIDTH; - CANARD_ASSERT(i > 0U); - x |= tmp[i - 1U]; - } + return tmp[0] | // Suppress warnings about the magic numbers. Their purpose is clear here. + ((uint64_t) tmp[1] << 8U) | // NOLINT NOSONAR + ((uint64_t) tmp[2] << 16U) | // NOLINT NOSONAR + ((uint64_t) tmp[3] << 24U) | // NOLINT NOSONAR + ((uint64_t) tmp[4] << 32U) | // NOLINT NOSONAR + ((uint64_t) tmp[5] << 40U) | // NOLINT NOSONAR + ((uint64_t) tmp[6] << 48U) | // NOLINT NOSONAR + ((uint64_t) tmp[7] << 56U); // NOLINT NOSONAR #endif - return x; } int8_t canardDSDLGetI8(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) { - uint8_t u = canardDSDLGetU8(buf, buf_size, off_bit, len_bit); - const bool negative = (len_bit > 0U) && ((u & (1ULL << (len_bit - 1U))) != 0U); - u |= ((len_bit < BYTE_WIDTH) && negative) ? ((uint8_t) ~((1ULL << len_bit) - 1U)) : 0U; - return negative ? ((-(int8_t) ~u) - 1) : (int8_t) u; + uint8_t val = canardDSDLGetU8(buf, buf_size, off_bit, len_bit); + const bool neg = (len_bit > 0U) && ((val & (1ULL << (len_bit - 1U))) != 0U); + val |= ((len_bit < BYTE_WIDTH) && neg) ? ((uint8_t) ~((1ULL << len_bit) - 1U)) : 0U; + return neg ? ((-(int8_t) ~val) - 1) : (int8_t) val; } int16_t canardDSDLGetI16(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) { - uint16_t u = canardDSDLGetU16(buf, buf_size, off_bit, len_bit); - const bool negative = (len_bit > 0U) && ((u & (1ULL << (len_bit - 1U))) != 0U); - u |= ((len_bit < WIDTH16) && negative) ? ((uint16_t) ~((1ULL << len_bit) - 1U)) : 0U; - return negative ? ((-(int16_t) ~u) - 1) : (int16_t) u; + uint16_t val = canardDSDLGetU16(buf, buf_size, off_bit, len_bit); + const bool neg = (len_bit > 0U) && ((val & (1ULL << (len_bit - 1U))) != 0U); + val |= ((len_bit < WIDTH16) && neg) ? ((uint16_t) ~((1ULL << len_bit) - 1U)) : 0U; + return neg ? ((-(int16_t) ~val) - 1) : (int16_t) val; } int32_t canardDSDLGetI32(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) { - uint32_t u = canardDSDLGetU32(buf, buf_size, off_bit, len_bit); - const bool negative = (len_bit > 0U) && ((u & (1ULL << (len_bit - 1U))) != 0U); - u |= ((len_bit < WIDTH32) && negative) ? ((uint32_t) ~((1ULL << len_bit) - 1U)) : 0U; - return negative ? ((-(int32_t) ~u) - 1) : (int32_t) u; + uint32_t val = canardDSDLGetU32(buf, buf_size, off_bit, len_bit); + const bool neg = (len_bit > 0U) && ((val & (1ULL << (len_bit - 1U))) != 0U); + val |= ((len_bit < WIDTH32) && neg) ? ((uint32_t) ~((1ULL << len_bit) - 1U)) : 0U; + return neg ? ((-(int32_t) ~val) - 1) : (int32_t) val; } int64_t canardDSDLGetI64(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) { - uint64_t u = canardDSDLGetU64(buf, buf_size, off_bit, len_bit); - const bool negative = (len_bit > 0U) && ((u & (1ULL << (len_bit - 1U))) != 0U); - u |= ((len_bit < WIDTH64) && negative) ? ((uint64_t) ~((1ULL << len_bit) - 1U)) : 0U; - return negative ? ((-(int64_t) ~u) - 1) : (int64_t) u; + uint64_t val = canardDSDLGetU64(buf, buf_size, off_bit, len_bit); + const bool neg = (len_bit > 0U) && ((val & (1ULL << (len_bit - 1U))) != 0U); + val |= ((len_bit < WIDTH64) && neg) ? ((uint64_t) ~((1ULL << len_bit) - 1U)) : 0U; + return neg ? ((-(int64_t) ~val) - 1) : (int64_t) val; } CanardDSDLFloat32 canardDSDLGetF16(const uint8_t* const buf, const size_t buf_size, const size_t off_bit) @@ -394,7 +403,7 @@ CanardDSDLFloat32 canardDSDLGetF32(const uint8_t* const buf, const size_t buf_si { uint32_t in; CanardDSDLFloat32 fl; - } tmp = {canardDSDLGetU32(buf, buf_size, off_bit, WIDTH32)}; // NOSONAR + } const tmp = {canardDSDLGetU32(buf, buf_size, off_bit, WIDTH32)}; // NOSONAR _Static_assert(WIDTH32 == sizeof(tmp) * BYTE_WIDTH, "IEEE 754 required"); return tmp.fl; } @@ -409,7 +418,7 @@ CanardDSDLFloat64 canardDSDLGetF64(const uint8_t* const buf, const size_t buf_si { uint64_t in; CanardDSDLFloat64 fl; - } tmp = {canardDSDLGetU64(buf, buf_size, off_bit, WIDTH64)}; // NOSONAR + } const tmp = {canardDSDLGetU64(buf, buf_size, off_bit, WIDTH64)}; // NOSONAR _Static_assert(WIDTH64 == sizeof(tmp) * BYTE_WIDTH, "IEEE 754 required"); return tmp.fl; } diff --git a/libcanard/canard_dsdl.h b/libcanard/canard_dsdl.h index fef31cd4..803a26e2 100644 --- a/libcanard/canard_dsdl.h +++ b/libcanard/canard_dsdl.h @@ -24,11 +24,12 @@ typedef double CanardDSDLFloat64; /// Serialize a DSDL field value at the specified bit offset from the beginning of the destination buffer. /// The behavior is undefined if the input pointer is NULL. The time complexity is linear of the bit length. -/// One-bit-wide signed integers are processed without raising an error or a deviation but the result is unspecified. +/// One-bit-wide signed integers are processed without raising an error but the result is unspecified. +/// /// Arguments: /// buf Destination buffer where the result will be stored. /// off_bit Offset, in bits, from the beginning of the buffer. May exceed one byte. -/// value The value itself (promoted to 64-bit for unification). +/// value The value itself (in case of integers it is promoted to 64-bit for unification). /// len_bit Length of the serialized representation, in bits. Zero has no effect. Values above 64 are saturated. void canardDSDLSetBit(uint8_t* const buf, const size_t off_bit, const bool value); void canardDSDLSetUxx(uint8_t* const buf, const size_t off_bit, const uint64_t value, const uint8_t len_bit); @@ -40,10 +41,15 @@ void canardDSDLSetF64(uint8_t* const buf, const size_t off_bit, const CanardDSDL /// Deserialize a DSDL field value located at the specified bit offset from the beginning of the source buffer. /// If the deserialized value extends beyond the end of the buffer, the missing bits are taken as zero, as required /// by the DSDL specification (see Implicit Zero Extension Rule, IZER). +/// /// If len_bit is greater than the return type, extra bits will be truncated per regular narrowing conversion rules. +/// If len_bit is shorter than the return type, missing bits will be zero per regular integer promotion rules. +/// Essentially, for integers, it would be enough to have 64-bit versions only; narrower variants exist only to avoid +/// narrowing type conversions of the result (and some small performance gains). +/// /// The behavior is undefined if the input pointer is NULL. The time complexity is linear of the bit length. -/// One-bit-wide signed integers are processed without raising an error or a deviation but the result is unspecified. -/// Returns the deserialized value. If the value spills over the buffer boundary, the spilled bits are taken as zero. +/// One-bit-wide signed integers are processed without raising an error but the result is unspecified. +/// /// Arguments: /// buf Source buffer where the serialized representation will be read from. /// buf_size The size of the source buffer, in bytes. Reads past this limit will be assumed to return zero bits. diff --git a/tests/test_dsdl.cpp b/tests/test_dsdl.cpp index 63a11e53..b37e48f0 100644 --- a/tests/test_dsdl.cpp +++ b/tests/test_dsdl.cpp @@ -282,7 +282,7 @@ TEST_CASE("canardDSDLSerialize_heartbeat") }; set_u(34, 2, 3); // mode - set_u(0, 0xdeadbeef, 32); // uptime + set_u(0, 0xDEADBEEF, 32); // uptime set_u(37, 0x7FFFF, 19); // vssc set_u(32, 2, 2); // health @@ -313,4 +313,137 @@ TEST_CASE("canardDSDLDeserialize_aligned") REQUIRE(-2 == canardDSDLGetI16(buf, 45, 104, 16)); REQUIRE(0 == canardDSDLGetU8(buf, 45, 120, 8)); REQUIRE(127 == canardDSDLGetI8(buf, 45, 128, 8)); + REQUIRE(Approx(1.0) == canardDSDLGetF64(buf, 45, 136)); + REQUIRE(Approx(1.0F) == canardDSDLGetF32(buf, 45, 200)); + REQUIRE(std::isinf(canardDSDLGetF16(buf, 45, 232))); + + REQUIRE(0x0EDA == canardDSDLGetU16(buf, 45, 248, 12)); + REQUIRE(0 == canardDSDLGetU8(buf, 45, 260, 4)); + REQUIRE(0xBEDA == canardDSDLGetU16(buf, 45, 264, 16)); + REQUIRE(-2 == canardDSDLGetI16(buf, 45, 280, 9)); + REQUIRE(0 == canardDSDLGetI16(buf, 45, 289, 7)); + REQUIRE(0 == canardDSDLGetU16(buf, 45, 289, 7)); + REQUIRE(0 == canardDSDLGetI8(buf, 45, 289, 7)); + REQUIRE(0 == canardDSDLGetU8(buf, 45, 289, 7)); + + REQUIRE(0xDEAD == canardDSDLGetU16(buf, 45, 296, 16)); + REQUIRE(0xBEEF == canardDSDLGetU16(buf, 45, 312, 16)); + REQUIRE(0xDEAD == canardDSDLGetU32(buf, 45, 296, 16)); + REQUIRE(0xBEEF == canardDSDLGetU32(buf, 45, 312, 16)); + REQUIRE(0xDEAD == canardDSDLGetU64(buf, 45, 296, 16)); + REQUIRE(0xBEEF == canardDSDLGetU64(buf, 45, 312, 16)); + REQUIRE(0xAD == canardDSDLGetU8(buf, 45, 296, 16)); + REQUIRE(0xEF == canardDSDLGetU8(buf, 45, 312, 16)); + REQUIRE(0x00AD == canardDSDLGetU16(buf, 38, 296, 16)); + REQUIRE(0x0000 == canardDSDLGetU32(buf, 37, 296, 16)); + + REQUIRE(canardDSDLGetBit(buf, 45, 328)); + REQUIRE(!canardDSDLGetBit(buf, 45, 329)); + REQUIRE(canardDSDLGetBit(buf, 45, 330)); + REQUIRE(!canardDSDLGetBit(buf, 45, 331)); + REQUIRE(!canardDSDLGetBit(buf, 45, 332)); + REQUIRE(!canardDSDLGetBit(buf, 45, 333)); + REQUIRE(canardDSDLGetBit(buf, 45, 334)); + REQUIRE(canardDSDLGetBit(buf, 45, 335)); + REQUIRE(canardDSDLGetBit(buf, 45, 336)); + REQUIRE(canardDSDLGetBit(buf, 45, 337)); + REQUIRE(canardDSDLGetBit(buf, 45, 338)); + REQUIRE(!canardDSDLGetBit(buf, 45, 339)); + REQUIRE(!canardDSDLGetBit(buf, 45, 340)); + REQUIRE(canardDSDLGetBit(buf, 45, 341)); + REQUIRE(canardDSDLGetBit(buf, 45, 342)); + REQUIRE(!canardDSDLGetBit(buf, 45, 343)); + + REQUIRE(canardDSDLGetBit(buf, 45, 344)); + REQUIRE(!canardDSDLGetBit(buf, 45, 345)); + REQUIRE(canardDSDLGetBit(buf, 45, 346)); + REQUIRE(!canardDSDLGetBit(buf, 45, 347)); + REQUIRE(!canardDSDLGetBit(buf, 45, 348)); + REQUIRE(!canardDSDLGetBit(buf, 45, 349)); + REQUIRE(canardDSDLGetBit(buf, 45, 350)); + REQUIRE(canardDSDLGetBit(buf, 45, 351)); + REQUIRE(canardDSDLGetBit(buf, 45, 352)); + REQUIRE(canardDSDLGetBit(buf, 45, 353)); + REQUIRE(!canardDSDLGetBit(buf, 45, 354)); + REQUIRE(canardDSDLGetBit(buf, 45, 355)); + REQUIRE(!canardDSDLGetBit(buf, 45, 356)); + + REQUIRE(0 == canardDSDLGetU8(buf, 45, 357, 3)); + + REQUIRE(!canardDSDLGetBit(buf, 44, 355)); +} + +TEST_CASE("canardDSDLDeserialize_unaligned") +{ + // The reference values for the following test have been taken from the PyUAVCAN test suite. + const std::vector Reference({ + 0xC5, 0x2F, 0x57, 0x82, 0xC6, 0xCA, 0x12, 0x34, 0x56, 0xD9, 0xBF, 0xEC, 0x06, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x80, 0xFF, 0x01, 0x00, 0x00, 0xFC, 0x01, 0xE0, 0x6F, 0xF5, 0x7E, 0xF7, 0x05 // + }); + const std::uint8_t* const buf = Reference.data(); + + REQUIRE(canardDSDLGetBit(buf, 31, 0)); + REQUIRE(!canardDSDLGetBit(buf, 31, 1)); + REQUIRE(canardDSDLGetBit(buf, 31, 2)); + REQUIRE(!canardDSDLGetBit(buf, 31, 3)); + REQUIRE(!canardDSDLGetBit(buf, 31, 4)); + REQUIRE(!canardDSDLGetBit(buf, 31, 5)); + REQUIRE(canardDSDLGetBit(buf, 31, 6)); + REQUIRE(canardDSDLGetBit(buf, 31, 7)); + REQUIRE(canardDSDLGetBit(buf, 31, 8)); + REQUIRE(canardDSDLGetBit(buf, 31, 9)); + REQUIRE(canardDSDLGetBit(buf, 31, 10)); + + REQUIRE(canardDSDLGetBit(buf, 31, 11)); + REQUIRE(!canardDSDLGetBit(buf, 31, 12)); + REQUIRE(canardDSDLGetBit(buf, 31, 13)); + REQUIRE(!canardDSDLGetBit(buf, 31, 14)); + REQUIRE(!canardDSDLGetBit(buf, 31, 15)); + REQUIRE(canardDSDLGetBit(buf, 31, 16)); + REQUIRE(canardDSDLGetBit(buf, 31, 17)); + REQUIRE(canardDSDLGetBit(buf, 31, 18)); + REQUIRE(!canardDSDLGetBit(buf, 31, 19)); + REQUIRE(canardDSDLGetBit(buf, 31, 20)); + + REQUIRE(0x12 == canardDSDLGetU8(buf, 31, 21, 8)); + REQUIRE(0x34 == canardDSDLGetU8(buf, 31, 29, 8)); + REQUIRE(0x56 == canardDSDLGetU8(buf, 31, 37, 8)); + + REQUIRE(!canardDSDLGetBit(buf, 31, 45)); + REQUIRE(canardDSDLGetBit(buf, 31, 46)); + REQUIRE(canardDSDLGetBit(buf, 31, 47)); + + REQUIRE(0x12 == canardDSDLGetU8(buf, 31, 48, 8)); + REQUIRE(0x34 == canardDSDLGetU8(buf, 31, 56, 8)); + REQUIRE(0x56 == canardDSDLGetU8(buf, 31, 64, 8)); + + REQUIRE(canardDSDLGetBit(buf, 31, 72)); + REQUIRE(!canardDSDLGetBit(buf, 31, 73)); + REQUIRE(!canardDSDLGetBit(buf, 31, 74)); + REQUIRE(canardDSDLGetBit(buf, 31, 75)); + REQUIRE(canardDSDLGetBit(buf, 31, 76)); + + REQUIRE(-2 == canardDSDLGetI8(buf, 31, 77, 8)); + REQUIRE(0b11101100101 == canardDSDLGetU16(buf, 31, 85, 11)); + REQUIRE(0b110 == canardDSDLGetU8(buf, 31, 96, 3)); + + REQUIRE(Approx(1.0) == canardDSDLGetF64(buf, 31, 99)); + REQUIRE(Approx(1.0F) == canardDSDLGetF32(buf, 31, 163)); + REQUIRE(std::isinf(canardDSDLGetF16(buf, 31, 195))); + REQUIRE(0.0F > canardDSDLGetF16(buf, 31, 195)); + + REQUIRE(0xDEAD == canardDSDLGetU16(buf, 31, 211, 16)); + REQUIRE(0xBEEF == canardDSDLGetU16(buf, 31, 227, 16)); + REQUIRE(0 == canardDSDLGetU8(buf, 31, 243, 5)); +} + +TEST_CASE("canardDSDLDeserialize_heartbeat") +{ + // The reference values were taken from the PyUAVCAN test. + const std::vector Reference({239, 190, 173, 222, 234, 255, 255}); + const std::uint8_t* const buf = Reference.data(); + REQUIRE(2 == canardDSDLGetU8(buf, 7, 34, 3)); // mode + REQUIRE(0xDEADBEEF == canardDSDLGetU32(buf, 7, 0, 32)); // uptime + REQUIRE(0x7FFFF == canardDSDLGetU32(buf, 7, 37, 19)); // vssc + REQUIRE(2 == canardDSDLGetU8(buf, 7, 32, 2)); // health } From 123ff12e463ab1f943a261effd24adc30c8f2cb9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 29 Feb 2020 20:30:35 +0200 Subject: [PATCH 072/100] Fix warnings --- libcanard/canard_dsdl.c | 42 ++++++++++++++++++++--------------------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index cac26c25..d4049d22 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -310,7 +310,7 @@ uint16_t canardDSDLGetU16(const uint8_t* const buf, const size_t buf_size, const #else uint8_t tmp[sizeof(uint16_t)] = {0}; copyBitArray(copy_size, off_bit, 0U, buf, &tmp[0]); - return tmp[0] | (uint16_t)(((uint16_t) tmp[1]) << BYTE_WIDTH); + return (uint16_t)(tmp[0] | (uint16_t)(((uint16_t) tmp[1]) << BYTE_WIDTH)); #endif } @@ -326,10 +326,10 @@ uint32_t canardDSDLGetU32(const uint8_t* const buf, const size_t buf_size, const #else uint8_t tmp[sizeof(uint32_t)] = {0}; copyBitArray(copy_size, off_bit, 0U, buf, &tmp[0]); - return tmp[0] | // Suppress warnings about the magic numbers. Their purpose is clear here. - ((uint32_t) tmp[1] << 8U) | // NOLINT NOSONAR - ((uint32_t) tmp[2] << 16U) | // NOLINT NOSONAR - ((uint32_t) tmp[3] << 24U); // NOLINT NOSONAR + return (uint32_t)(tmp[0] | // Suppress warnings about the magic numbers. + ((uint32_t) tmp[1] << 8U) | // NOLINT NOSONAR + ((uint32_t) tmp[2] << 16U) | // NOLINT NOSONAR + ((uint32_t) tmp[3] << 24U)); // NOLINT NOSONAR #endif } @@ -345,14 +345,14 @@ uint64_t canardDSDLGetU64(const uint8_t* const buf, const size_t buf_size, const #else uint8_t tmp[sizeof(uint64_t)] = {0}; copyBitArray(copy_size, off_bit, 0U, buf, &tmp[0]); - return tmp[0] | // Suppress warnings about the magic numbers. Their purpose is clear here. - ((uint64_t) tmp[1] << 8U) | // NOLINT NOSONAR - ((uint64_t) tmp[2] << 16U) | // NOLINT NOSONAR - ((uint64_t) tmp[3] << 24U) | // NOLINT NOSONAR - ((uint64_t) tmp[4] << 32U) | // NOLINT NOSONAR - ((uint64_t) tmp[5] << 40U) | // NOLINT NOSONAR - ((uint64_t) tmp[6] << 48U) | // NOLINT NOSONAR - ((uint64_t) tmp[7] << 56U); // NOLINT NOSONAR + return (uint64_t)(tmp[0] | // Suppress warnings about the magic numbers. + ((uint64_t) tmp[1] << 8U) | // NOLINT NOSONAR + ((uint64_t) tmp[2] << 16U) | // NOLINT NOSONAR + ((uint64_t) tmp[3] << 24U) | // NOLINT NOSONAR + ((uint64_t) tmp[4] << 32U) | // NOLINT NOSONAR + ((uint64_t) tmp[5] << 40U) | // NOLINT NOSONAR + ((uint64_t) tmp[6] << 48U) | // NOLINT NOSONAR + ((uint64_t) tmp[7] << 56U)); // NOLINT NOSONAR #endif } @@ -360,32 +360,32 @@ int8_t canardDSDLGetI8(const uint8_t* const buf, const size_t buf_size, const si { uint8_t val = canardDSDLGetU8(buf, buf_size, off_bit, len_bit); const bool neg = (len_bit > 0U) && ((val & (1ULL << (len_bit - 1U))) != 0U); - val |= ((len_bit < BYTE_WIDTH) && neg) ? ((uint8_t) ~((1ULL << len_bit) - 1U)) : 0U; - return neg ? ((-(int8_t) ~val) - 1) : (int8_t) val; + val = ((len_bit < BYTE_WIDTH) && neg) ? (uint8_t)(val | ~((1ULL << len_bit) - 1U)) : val; + return neg ? (int8_t)((-(int8_t) ~val) - 1) : (int8_t) val; } int16_t canardDSDLGetI16(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) { uint16_t val = canardDSDLGetU16(buf, buf_size, off_bit, len_bit); const bool neg = (len_bit > 0U) && ((val & (1ULL << (len_bit - 1U))) != 0U); - val |= ((len_bit < WIDTH16) && neg) ? ((uint16_t) ~((1ULL << len_bit) - 1U)) : 0U; - return neg ? ((-(int16_t) ~val) - 1) : (int16_t) val; + val = ((len_bit < WIDTH16) && neg) ? (uint16_t)(val | ~((1ULL << len_bit) - 1U)) : val; + return neg ? (int16_t)((-(int16_t) ~val) - 1) : (int16_t) val; } int32_t canardDSDLGetI32(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) { uint32_t val = canardDSDLGetU32(buf, buf_size, off_bit, len_bit); const bool neg = (len_bit > 0U) && ((val & (1ULL << (len_bit - 1U))) != 0U); - val |= ((len_bit < WIDTH32) && neg) ? ((uint32_t) ~((1ULL << len_bit) - 1U)) : 0U; - return neg ? ((-(int32_t) ~val) - 1) : (int32_t) val; + val = ((len_bit < WIDTH32) && neg) ? (uint32_t)(val | ~((1ULL << len_bit) - 1U)) : val; + return neg ? (int32_t)((-(int32_t) ~val) - 1) : (int32_t) val; } int64_t canardDSDLGetI64(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) { uint64_t val = canardDSDLGetU64(buf, buf_size, off_bit, len_bit); const bool neg = (len_bit > 0U) && ((val & (1ULL << (len_bit - 1U))) != 0U); - val |= ((len_bit < WIDTH64) && neg) ? ((uint64_t) ~((1ULL << len_bit) - 1U)) : 0U; - return neg ? ((-(int64_t) ~val) - 1) : (int64_t) val; + val = ((len_bit < WIDTH64) && neg) ? (uint64_t)(val | ~((1ULL << len_bit) - 1U)) : val; + return neg ? (int64_t)((-(int64_t) ~val) - 1) : (int64_t) val; } CanardDSDLFloat32 canardDSDLGetF16(const uint8_t* const buf, const size_t buf_size, const size_t off_bit) From b9497e7db6b31be99086c122fec9cb36b92c992b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 29 Feb 2020 20:51:44 +0200 Subject: [PATCH 073/100] Make the floating point support optional depending on the properties of the target platform. --- libcanard/canard_dsdl.c | 228 +++++++++++++++++++++------------------- libcanard/canard_dsdl.h | 2 + 2 files changed, 120 insertions(+), 110 deletions(-) diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index d4049d22..1e142e3e 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -38,13 +38,11 @@ # error "Unsupported language: ISO C11 or a newer version is required." #endif -#define CANARD_DSDL_PLATFORM_IEEE754 \ - ((FLT_RADIX == 2) && (FLT_MANT_DIG == 24) && (FLT_MIN_EXP == -125) && (FLT_MAX_EXP == 128) && \ - (DBL_MANT_DIG == 53) && (DBL_MIN_EXP == -1021) && (DBL_MAX_EXP == 1024)) -_Static_assert(CANARD_DSDL_PLATFORM_IEEE754, - "Currently, the module requires that the target platform shall use an IEEE 754-compatible floating " - "point model. It is possible to support other platforms, but this has not been done yet. " - "If your platform is not IEEE 754-compatible, please reach the maintainers via http://forum.uavcan.org"); +/// If your platform is not IEEE 754-compatible and you need floats, please reach https://forum.uavcan.org. +#define CANARD_DSDL_PLATFORM_IEEE754_FLOAT \ + ((FLT_RADIX == 2) && (FLT_MANT_DIG == 24) && (FLT_MIN_EXP == -125) && (FLT_MAX_EXP == 128)) +#define CANARD_DSDL_PLATFORM_IEEE754_DOUBLE \ + ((FLT_RADIX == 2) && (DBL_MANT_DIG == 53) && (DBL_MIN_EXP == -1021) && (DBL_MAX_EXP == 1024)) // --------------------------------------------- COMMON ITEMS --------------------------------------------- @@ -56,69 +54,6 @@ _Static_assert(CANARD_DSDL_PLATFORM_IEEE754, #define WIDTH32 32U #define WIDTH64 64U -// --------------------------------------------- FLOAT16 SUPPORT --------------------------------------------- - -_Static_assert(WIDTH32 == sizeof(CanardDSDLFloat32) * BYTE_WIDTH, "Unsupported floating point model"); -_Static_assert(WIDTH64 == sizeof(CanardDSDLFloat64) * BYTE_WIDTH, "Unsupported floating point model"); - -// Intentional violation of MISRA: we need this union because the alternative is far more error prone. -// We have to rely on low-level data representation details to do the conversion; unions are helpful. -typedef union // NOSONAR -{ - uint32_t bits; - CanardDSDLFloat32 real; -} Float32Bits; -_Static_assert(4 == sizeof(Float32Bits), "Unsupported float model"); - -CANARD_PRIVATE uint16_t float16Pack(const CanardDSDLFloat32 value); -CANARD_PRIVATE uint16_t float16Pack(const CanardDSDLFloat32 value) -{ - // The no-lint statements suppress the warnings about magic numbers. - // The no-lint statements suppress the warning about the use of union. This is required for low-level bit access. - const uint32_t round_mask = ~(uint32_t) 0x0FFFU; // NOLINT NOSONAR - const Float32Bits f32inf = {.bits = ((uint32_t) 255U) << 23U}; // NOLINT NOSONAR - const Float32Bits f16inf = {.bits = ((uint32_t) 31U) << 23U}; // NOLINT NOSONAR - const Float32Bits magic = {.bits = ((uint32_t) 15U) << 23U}; // NOLINT NOSONAR - Float32Bits in = {.real = value}; // NOSONAR - const uint32_t sign = in.bits & (((uint32_t) 1U) << 31U); // NOLINT NOSONAR - in.bits ^= sign; - uint16_t out = 0; - if (in.bits >= f32inf.bits) - { - out = (in.bits > f32inf.bits) ? (uint16_t) 0x7FFFU : (uint16_t) 0x7C00U; // NOLINT NOSONAR - } - else - { - in.bits &= round_mask; - in.real *= magic.real; - in.bits -= round_mask; - if (in.bits > f16inf.bits) - { - in.bits = f16inf.bits; - } - out = (uint16_t)(in.bits >> 13U); // NOLINT NOSONAR - } - out |= (uint16_t)(sign >> 16U); // NOLINT NOSONAR - return out; -} - -CANARD_PRIVATE CanardDSDLFloat32 float16Unpack(const uint16_t value); -CANARD_PRIVATE CanardDSDLFloat32 float16Unpack(const uint16_t value) -{ - // The no-lint statements suppress the warnings about magic numbers. - // The no-lint statements suppress the warning about the use of union. This is required for low-level bit access. - const Float32Bits magic = {.bits = ((uint32_t) 0xEFU) << 23U}; // NOLINT NOSONAR - const Float32Bits inf_nan = {.bits = ((uint32_t) 0x8FU) << 23U}; // NOLINT NOSONAR - Float32Bits out = {.bits = ((uint32_t)(value & 0x7FFFU)) << 13U}; // NOLINT NOSONAR - out.real *= magic.real; - if (out.real >= inf_nan.real) - { - out.bits |= ((uint32_t) 0xFFU) << 23U; // NOLINT NOSONAR - } - out.bits |= ((uint32_t)(value & 0x8000U)) << 16U; // NOLINT NOSONAR - return out.real; -} - // --------------------------------------------- PRIMITIVE SERIALIZATION --------------------------------------------- CANARD_PRIVATE size_t chooseMin(size_t a, size_t b); @@ -209,7 +144,7 @@ CANARD_PRIVATE size_t getBitCopySize(const size_t buf_size_bytes, return chooseMin(remaining_bit, chooseMin(requested_length_bit, value_length_bit)); } -// --------------------------------------------- PUBLIC API --------------------------------------------- +// --------------------------------------------- PUBLIC API - INTEGER --------------------------------------------- void canardDSDLSetBit(uint8_t* const buf, const size_t off_bit, const bool value) { @@ -248,41 +183,6 @@ void canardDSDLSetIxx(uint8_t* const buf, const size_t off_bit, const int64_t va canardDSDLSetUxx(buf, off_bit, (uint64_t) value, len_bit); } -void canardDSDLSetF16(uint8_t* const buf, const size_t off_bit, const CanardDSDLFloat32 value) -{ - canardDSDLSetUxx(buf, off_bit, float16Pack(value), WIDTH16); -} - -void canardDSDLSetF32(uint8_t* const buf, const size_t off_bit, const CanardDSDLFloat32 value) -{ - // Intentional violation of MISRA: use union to perform fast conversion from an IEEE 754-compatible native - // representation into a serializable integer. The assumptions about the target platform properties are made - // clear. In the future we may add a more generic conversion that is platform-invariant. - _Static_assert(CANARD_DSDL_PLATFORM_IEEE754, "IEEE 754 required"); - union // NOSONAR - { - CanardDSDLFloat32 fl; - uint32_t in; - } const tmp = {value}; // NOSONAR - _Static_assert(WIDTH32 == sizeof(tmp) * BYTE_WIDTH, "IEEE 754 required"); - canardDSDLSetUxx(buf, off_bit, tmp.in, sizeof(tmp) * BYTE_WIDTH); -} - -void canardDSDLSetF64(uint8_t* const buf, const size_t off_bit, const CanardDSDLFloat64 value) -{ - // Intentional violation of MISRA: use union to perform fast conversion from an IEEE 754-compatible native - // representation into a serializable integer. The assumptions about the target platform properties are made - // clear. In the future we may add a more generic conversion that is platform-invariant. - _Static_assert(CANARD_DSDL_PLATFORM_IEEE754, "IEEE 754 required"); - union // NOSONAR - { - CanardDSDLFloat64 fl; - uint64_t in; - } const tmp = {value}; // NOSONAR - _Static_assert(WIDTH64 == sizeof(tmp) * BYTE_WIDTH, "IEEE 754 required"); - canardDSDLSetUxx(buf, off_bit, tmp.in, sizeof(tmp) * BYTE_WIDTH); -} - bool canardDSDLGetBit(const uint8_t* const buf, const size_t buf_size, const size_t off_bit) { return canardDSDLGetU8(buf, buf_size, off_bit, 1U) == 1U; @@ -388,37 +288,145 @@ int64_t canardDSDLGetI64(const uint8_t* const buf, const size_t buf_size, const return neg ? (int64_t)((-(int64_t) ~val) - 1) : (int64_t) val; } +// --------------------------------------------- PUBLIC API - FLOAT16 --------------------------------------------- + +#if CANARD_DSDL_PLATFORM_IEEE754_FLOAT + +_Static_assert(WIDTH32 == sizeof(CanardDSDLFloat32) * BYTE_WIDTH, "Unsupported floating point model"); + +// Intentional violation of MISRA: we need this union because the alternative is far more error prone. +// We have to rely on low-level data representation details to do the conversion; unions are helpful. +typedef union // NOSONAR +{ + uint32_t bits; + CanardDSDLFloat32 real; +} Float32Bits; + +CANARD_PRIVATE uint16_t float16Pack(const CanardDSDLFloat32 value); +CANARD_PRIVATE uint16_t float16Pack(const CanardDSDLFloat32 value) +{ + // The no-lint statements suppress the warnings about magic numbers. + // The no-lint statements suppress the warning about the use of union. This is required for low-level bit access. + const uint32_t round_mask = ~(uint32_t) 0x0FFFU; // NOLINT NOSONAR + const Float32Bits f32inf = {.bits = ((uint32_t) 255U) << 23U}; // NOLINT NOSONAR + const Float32Bits f16inf = {.bits = ((uint32_t) 31U) << 23U}; // NOLINT NOSONAR + const Float32Bits magic = {.bits = ((uint32_t) 15U) << 23U}; // NOLINT NOSONAR + Float32Bits in = {.real = value}; // NOSONAR + const uint32_t sign = in.bits & (((uint32_t) 1U) << 31U); // NOLINT NOSONAR + in.bits ^= sign; + uint16_t out = 0; + if (in.bits >= f32inf.bits) + { + out = (in.bits > f32inf.bits) ? (uint16_t) 0x7FFFU : (uint16_t) 0x7C00U; // NOLINT NOSONAR + } + else + { + in.bits &= round_mask; + in.real *= magic.real; + in.bits -= round_mask; + if (in.bits > f16inf.bits) + { + in.bits = f16inf.bits; + } + out = (uint16_t)(in.bits >> 13U); // NOLINT NOSONAR + } + out |= (uint16_t)(sign >> 16U); // NOLINT NOSONAR + return out; +} + +CANARD_PRIVATE CanardDSDLFloat32 float16Unpack(const uint16_t value); +CANARD_PRIVATE CanardDSDLFloat32 float16Unpack(const uint16_t value) +{ + // The no-lint statements suppress the warnings about magic numbers. + // The no-lint statements suppress the warning about the use of union. This is required for low-level bit access. + const Float32Bits magic = {.bits = ((uint32_t) 0xEFU) << 23U}; // NOLINT NOSONAR + const Float32Bits inf_nan = {.bits = ((uint32_t) 0x8FU) << 23U}; // NOLINT NOSONAR + Float32Bits out = {.bits = ((uint32_t)(value & 0x7FFFU)) << 13U}; // NOLINT NOSONAR + out.real *= magic.real; + if (out.real >= inf_nan.real) + { + out.bits |= ((uint32_t) 0xFFU) << 23U; // NOLINT NOSONAR + } + out.bits |= ((uint32_t)(value & 0x8000U)) << 16U; // NOLINT NOSONAR + return out.real; +} + +void canardDSDLSetF16(uint8_t* const buf, const size_t off_bit, const CanardDSDLFloat32 value) +{ + canardDSDLSetUxx(buf, off_bit, float16Pack(value), WIDTH16); +} + CanardDSDLFloat32 canardDSDLGetF16(const uint8_t* const buf, const size_t buf_size, const size_t off_bit) { return float16Unpack(canardDSDLGetU16(buf, buf_size, off_bit, WIDTH16)); } +#endif // CANARD_DSDL_PLATFORM_IEEE754_FLOAT + +// --------------------------------------------- PUBLIC API - FLOAT32 --------------------------------------------- + +#if CANARD_DSDL_PLATFORM_IEEE754_FLOAT + +_Static_assert(WIDTH32 == sizeof(CanardDSDLFloat32) * BYTE_WIDTH, "Unsupported floating point model"); + +void canardDSDLSetF32(uint8_t* const buf, const size_t off_bit, const CanardDSDLFloat32 value) +{ + // Intentional violation of MISRA: use union to perform fast conversion from an IEEE 754-compatible native + // representation into a serializable integer. The assumptions about the target platform properties are made + // clear. In the future we may add a more generic conversion that is platform-invariant. + union // NOSONAR + { + CanardDSDLFloat32 fl; + uint32_t in; + } const tmp = {value}; // NOSONAR + canardDSDLSetUxx(buf, off_bit, tmp.in, sizeof(tmp) * BYTE_WIDTH); +} + CanardDSDLFloat32 canardDSDLGetF32(const uint8_t* const buf, const size_t buf_size, const size_t off_bit) { // Intentional violation of MISRA: use union to perform fast conversion to an IEEE 754-compatible native // representation into a serializable integer. The assumptions about the target platform properties are made // clear. In the future we may add a more generic conversion that is platform-invariant. - _Static_assert(CANARD_DSDL_PLATFORM_IEEE754, "IEEE 754 required"); union // NOSONAR { uint32_t in; CanardDSDLFloat32 fl; } const tmp = {canardDSDLGetU32(buf, buf_size, off_bit, WIDTH32)}; // NOSONAR - _Static_assert(WIDTH32 == sizeof(tmp) * BYTE_WIDTH, "IEEE 754 required"); return tmp.fl; } +#endif // CANARD_DSDL_PLATFORM_IEEE754_FLOAT + +// --------------------------------------------- PUBLIC API - FLOAT64 --------------------------------------------- + +#if CANARD_DSDL_PLATFORM_IEEE754_DOUBLE + +_Static_assert(WIDTH64 == sizeof(CanardDSDLFloat64) * BYTE_WIDTH, "Unsupported floating point model"); + CanardDSDLFloat64 canardDSDLGetF64(const uint8_t* const buf, const size_t buf_size, const size_t off_bit) { // Intentional violation of MISRA: use union to perform fast conversion to an IEEE 754-compatible native // representation into a serializable integer. The assumptions about the target platform properties are made // clear. In the future we may add a more generic conversion that is platform-invariant. - _Static_assert(CANARD_DSDL_PLATFORM_IEEE754, "IEEE 754 required"); union // NOSONAR { uint64_t in; CanardDSDLFloat64 fl; } const tmp = {canardDSDLGetU64(buf, buf_size, off_bit, WIDTH64)}; // NOSONAR - _Static_assert(WIDTH64 == sizeof(tmp) * BYTE_WIDTH, "IEEE 754 required"); return tmp.fl; } + +void canardDSDLSetF64(uint8_t* const buf, const size_t off_bit, const CanardDSDLFloat64 value) +{ + // Intentional violation of MISRA: use union to perform fast conversion from an IEEE 754-compatible native + // representation into a serializable integer. The assumptions about the target platform properties are made + // clear. In the future we may add a more generic conversion that is platform-invariant. + union // NOSONAR + { + CanardDSDLFloat64 fl; + uint64_t in; + } const tmp = {value}; // NOSONAR + canardDSDLSetUxx(buf, off_bit, tmp.in, sizeof(tmp) * BYTE_WIDTH); +} + +#endif // CANARD_DSDL_PLATFORM_IEEE754_DOUBLE diff --git a/libcanard/canard_dsdl.h b/libcanard/canard_dsdl.h index 803a26e2..6d157504 100644 --- a/libcanard/canard_dsdl.h +++ b/libcanard/canard_dsdl.h @@ -25,6 +25,7 @@ typedef double CanardDSDLFloat64; /// Serialize a DSDL field value at the specified bit offset from the beginning of the destination buffer. /// The behavior is undefined if the input pointer is NULL. The time complexity is linear of the bit length. /// One-bit-wide signed integers are processed without raising an error but the result is unspecified. +/// The floating point functions are only available if the target platform has an IEEE 754-compatible float model. /// /// Arguments: /// buf Destination buffer where the result will be stored. @@ -41,6 +42,7 @@ void canardDSDLSetF64(uint8_t* const buf, const size_t off_bit, const CanardDSDL /// Deserialize a DSDL field value located at the specified bit offset from the beginning of the source buffer. /// If the deserialized value extends beyond the end of the buffer, the missing bits are taken as zero, as required /// by the DSDL specification (see Implicit Zero Extension Rule, IZER). +/// The floating point functions are only available if the target platform has an IEEE 754-compatible float model. /// /// If len_bit is greater than the return type, extra bits will be truncated per regular narrowing conversion rules. /// If len_bit is shorter than the return type, missing bits will be zero per regular integer promotion rules. From 130862ce99c13c858f1922bf7237e8e01e7bc300 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 29 Feb 2020 20:57:09 +0200 Subject: [PATCH 074/100] Fix warnings from SonarQube --- libcanard/canard_dsdl.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index 1e142e3e..ee686509 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -155,7 +155,7 @@ void canardDSDLSetBit(uint8_t* const buf, const size_t off_bit, const bool value void canardDSDLSetUxx(uint8_t* const buf, const size_t off_bit, const uint64_t value, const uint8_t len_bit) { - _Static_assert(WIDTH64 == sizeof(uint64_t) * BYTE_WIDTH, "Unexpected size of uint64_t"); + _Static_assert(WIDTH64 == (sizeof(uint64_t) * BYTE_WIDTH), "Unexpected size of uint64_t"); CANARD_ASSERT(buf != NULL); const size_t saturated_len_bit = chooseMin(len_bit, WIDTH64); #if CANARD_DSDL_CONFIG_LITTLE_ENDIAN @@ -185,7 +185,7 @@ void canardDSDLSetIxx(uint8_t* const buf, const size_t off_bit, const int64_t va bool canardDSDLGetBit(const uint8_t* const buf, const size_t buf_size, const size_t off_bit) { - return canardDSDLGetU8(buf, buf_size, off_bit, 1U) == 1U; + return 1U == canardDSDLGetU8(buf, buf_size, off_bit, 1U); } uint8_t canardDSDLGetU8(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) @@ -261,7 +261,7 @@ int8_t canardDSDLGetI8(const uint8_t* const buf, const size_t buf_size, const si uint8_t val = canardDSDLGetU8(buf, buf_size, off_bit, len_bit); const bool neg = (len_bit > 0U) && ((val & (1ULL << (len_bit - 1U))) != 0U); val = ((len_bit < BYTE_WIDTH) && neg) ? (uint8_t)(val | ~((1ULL << len_bit) - 1U)) : val; - return neg ? (int8_t)((-(int8_t) ~val) - 1) : (int8_t) val; + return neg ? (int8_t)((-(int8_t)(uint8_t) ~val) - 1) : (int8_t) val; } int16_t canardDSDLGetI16(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) @@ -269,7 +269,7 @@ int16_t canardDSDLGetI16(const uint8_t* const buf, const size_t buf_size, const uint16_t val = canardDSDLGetU16(buf, buf_size, off_bit, len_bit); const bool neg = (len_bit > 0U) && ((val & (1ULL << (len_bit - 1U))) != 0U); val = ((len_bit < WIDTH16) && neg) ? (uint16_t)(val | ~((1ULL << len_bit) - 1U)) : val; - return neg ? (int16_t)((-(int16_t) ~val) - 1) : (int16_t) val; + return neg ? (int16_t)((-(int16_t)(uint16_t) ~val) - 1) : (int16_t) val; } int32_t canardDSDLGetI32(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) @@ -292,7 +292,7 @@ int64_t canardDSDLGetI64(const uint8_t* const buf, const size_t buf_size, const #if CANARD_DSDL_PLATFORM_IEEE754_FLOAT -_Static_assert(WIDTH32 == sizeof(CanardDSDLFloat32) * BYTE_WIDTH, "Unsupported floating point model"); +_Static_assert(WIDTH32 == (sizeof(CanardDSDLFloat32) * BYTE_WIDTH), "Unsupported floating point model"); // Intentional violation of MISRA: we need this union because the alternative is far more error prone. // We have to rely on low-level data representation details to do the conversion; unions are helpful. @@ -367,7 +367,7 @@ CanardDSDLFloat32 canardDSDLGetF16(const uint8_t* const buf, const size_t buf_si #if CANARD_DSDL_PLATFORM_IEEE754_FLOAT -_Static_assert(WIDTH32 == sizeof(CanardDSDLFloat32) * BYTE_WIDTH, "Unsupported floating point model"); +_Static_assert(WIDTH32 == (sizeof(CanardDSDLFloat32) * BYTE_WIDTH), "Unsupported floating point model"); void canardDSDLSetF32(uint8_t* const buf, const size_t off_bit, const CanardDSDLFloat32 value) { @@ -401,7 +401,7 @@ CanardDSDLFloat32 canardDSDLGetF32(const uint8_t* const buf, const size_t buf_si #if CANARD_DSDL_PLATFORM_IEEE754_DOUBLE -_Static_assert(WIDTH64 == sizeof(CanardDSDLFloat64) * BYTE_WIDTH, "Unsupported floating point model"); +_Static_assert(WIDTH64 == (sizeof(CanardDSDLFloat64) * BYTE_WIDTH), "Unsupported floating point model"); CanardDSDLFloat64 canardDSDLGetF64(const uint8_t* const buf, const size_t buf_size, const size_t off_bit) { From 57c328ba34f2b95e1ef6cc377bf6ba69b456e7dc Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 29 Feb 2020 21:09:11 +0200 Subject: [PATCH 075/100] Nits from SonarQube --- libcanard/canard_dsdl.c | 6 +++--- libcanard/canard_dsdl.h | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index ee686509..59a2d343 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -81,9 +81,9 @@ CANARD_PRIVATE void copyBitArray(const size_t length_bit, CANARD_ASSERT((src != NULL) && (dst != NULL) && (src != dst)); CANARD_ASSERT((src < dst) ? ((src + ((src_offset_bit + length_bit + BYTE_WIDTH) / BYTE_WIDTH)) <= dst) : ((dst + ((dst_offset_bit + length_bit + BYTE_WIDTH) / BYTE_WIDTH)) <= src)); - if (((length_bit % BYTE_WIDTH) == 0U) && // - ((src_offset_bit % BYTE_WIDTH) == 0U) && // - ((dst_offset_bit % BYTE_WIDTH) == 0U)) + if ((0U == (length_bit % BYTE_WIDTH)) && // + (0U == (src_offset_bit % BYTE_WIDTH)) && // + (0U == (dst_offset_bit % BYTE_WIDTH))) { // Intentional violation of MISRA: Pointer arithmetics. // This is done to remove the API constraint that offsets be under 8 bits. diff --git a/libcanard/canard_dsdl.h b/libcanard/canard_dsdl.h index 6d157504..f59dabbb 100644 --- a/libcanard/canard_dsdl.h +++ b/libcanard/canard_dsdl.h @@ -2,11 +2,11 @@ // Copyright (c) 2016-2020 UAVCAN Development Team. // Author: Pavel Kirienko // -// This is a trivial optional extension library that contains basic DSDL serialization routines. +// This is a trivial optional extension library that contains basic DSDL serialization routines. // It is intended for use in simple applications where auto-generated DSDL serialization logic is not available. // The functions are fully stateless (pure); read their documentation comments for usage information. // This is an optional part of libcanard that can be omitted if this functionality is not required by the application. -// High-integrity applications are not recommended to use this extension because it relies on unsafe memory operations. +// Some high-integrity applications may prefer avoid this extension because it relies on unsafe memory operations. #ifndef CANARD_DSDL_H_INCLUDED #define CANARD_DSDL_H_INCLUDED From 162a5a0e5e9ec7b98a3ac8b92d1fba660032292e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 29 Feb 2020 21:13:16 +0200 Subject: [PATCH 076/100] Suppress a false-positive from Clang-Tidy --- libcanard/canard_dsdl.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index 59a2d343..ba8a1c94 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -106,7 +106,8 @@ CANARD_PRIVATE void copyBitArray(const size_t length_bit, const uint8_t size = (uint8_t) chooseMin(BYTE_WIDTH - max_mod, last_bit - src_off); CANARD_ASSERT((size > 0U) && (size <= BYTE_WIDTH)); - const uint8_t mask = (uint8_t)((((1U << size) - 1U) << dst_mod) & BYTE_MAX); + // Suppress a false error from Clang-Tidy that size is being over-shifted. It's not, we check for that. + const uint8_t mask = (uint8_t)((((1U << size) - 1U) << dst_mod) & BYTE_MAX); // NOLINT CANARD_ASSERT(mask > 0U); // Intentional violation of MISRA: indexing on a pointer. From 0d665917faf6f0b1bfa90a861dfad68ded5e13a6 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 1 Mar 2020 01:31:24 +0200 Subject: [PATCH 077/100] Fix the hack in the CI coverage tracking --- .travis.yml | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/.travis.yml b/.travis.yml index 23a9d80d..ec4e08b8 100644 --- a/.travis.yml +++ b/.travis.yml @@ -25,13 +25,9 @@ matrix: - build-wrapper-linux-x86-64 --out-dir sonar-dump make all # The build wrapper comes from Sonar Cloud. - make test - # COVERAGE (this is a quick hack, please fix) - - find CMakeFiles/test_public_cov.dir/ -name 'canard*.c.*' -type f -print -exec mv -f {} . \; - - gcov-9 libcanard/canard.c -o canard.c.gcda -o canard.c.gcno -t > public_canard.gcov - - gcov-9 libcanard/canard_dsdl.c -o canard_dsdl.c.gcda -o canard_dsdl.c.gcno -t > public_canard_dsdl.gcov - - find CMakeFiles/test_private_cov.dir/ -name 'canard*.c.*' -type f -print -exec mv -f {} . \; - - gcov-9 libcanard/canard.c -o canard.c.gcda -o canard.c.gcno -t > private_canard.gcov - - gcov-9 libcanard/canard_dsdl.c -o canard_dsdl.c.gcda -o canard_dsdl.c.gcno -t > private_canard_dsdl.gcov + # COVERAGE + - gcov-9 CMakeFiles/test_private_cov.dir/**/*.gcno -t > private.gcov + - gcov-9 CMakeFiles/test_public_cov.dir/**/*.gcno -t > public.gcov # RELEASE - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Release -DNO_STATIC_ANALYSIS=1 From e1caf06772035dd3e776f1ae8ea00519797fbda0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 1 Mar 2020 01:51:46 +0200 Subject: [PATCH 078/100] Fixing combined coverage reporting --- .travis.yml | 8 ++++++-- tests/CMakeLists.txt | 11 ++++++----- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/.travis.yml b/.travis.yml index ec4e08b8..38129ebc 100644 --- a/.travis.yml +++ b/.travis.yml @@ -26,8 +26,12 @@ matrix: - make test # COVERAGE - - gcov-9 CMakeFiles/test_private_cov.dir/**/*.gcno -t > private.gcov - - gcov-9 CMakeFiles/test_public_cov.dir/**/*.gcno -t > public.gcov + - gcov-9 -t CMakeFiles/test_private_cov.dir/**/canard.*gcno > private_canard.gcov + - gcov-9 -t CMakeFiles/test_private_cov.dir/**/canard_dsdl.*gcno > private_canard_dsdl.gcov + - gcov-9 -t CMakeFiles/test_private_le_cov.dir/**/canard.*gcno > private_le_canard.gcov + - gcov-9 -t CMakeFiles/test_private_le_cov.dir/**/canard_dsdl.*gcno > private_le_canard_dsdl.gcov + - gcov-9 -t CMakeFiles/test_public_cov.dir/**/canard.*gcno > public_canard.gcov + - gcov-9 -t CMakeFiles/test_public_cov.dir/**/canard_dsdl.*gcno > public_canard_dsdl.gcov # RELEASE - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Release -DNO_STATIC_ANALYSIS=1 diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index f98bd040..094b7ff8 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -78,11 +78,12 @@ gen_test_matrix(test_private "test_private_crc.cpp;test_private_rx.cpp;test_private_tx.cpp;test_dsdl.cpp" CANARD_CONFIG_EXPOSE_PRIVATE=1) +# We assume here that the target platform is little-endian. If it's not, the test will fail. +# Perhaps this needs fixing: these tests need not be run if the target is not little-endian. +gen_test_matrix(test_private_le + "test_private_crc.cpp;test_private_rx.cpp;test_private_tx.cpp;test_dsdl.cpp" + "CANARD_CONFIG_EXPOSE_PRIVATE=1;CANARD_DSDL_CONFIG_LITTLE_ENDIAN=1") + gen_test_matrix(test_public "test_public_tx.cpp;test_public_rx.cpp;test_public_roundtrip.cpp;test_self.cpp" "") - -# We assume here that the target platform is little-endian. If it's not, the test will fail. Perhaps this needs fixing. -gen_test_matrix(test_dsdl_little_endian - "test_dsdl.cpp" - "CANARD_CONFIG_EXPOSE_PRIVATE=1;CANARD_DSDL_CONFIG_LITTLE_ENDIAN=1") From 966d10eef89b60cb48409865ed2a36c08669c3c1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 1 Mar 2020 04:19:44 +0200 Subject: [PATCH 079/100] Bump coverage --- libcanard/canard_dsdl.c | 28 +++++++++------- tests/test_dsdl.cpp | 71 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 87 insertions(+), 12 deletions(-) diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index ba8a1c94..fbc98894 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -259,33 +259,37 @@ uint64_t canardDSDLGetU64(const uint8_t* const buf, const size_t buf_size, const int8_t canardDSDLGetI8(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) { - uint8_t val = canardDSDLGetU8(buf, buf_size, off_bit, len_bit); - const bool neg = (len_bit > 0U) && ((val & (1ULL << (len_bit - 1U))) != 0U); - val = ((len_bit < BYTE_WIDTH) && neg) ? (uint8_t)(val | ~((1ULL << len_bit) - 1U)) : val; + const uint8_t sat = (uint8_t) chooseMin(len_bit, BYTE_WIDTH); + uint8_t val = canardDSDLGetU8(buf, buf_size, off_bit, sat); + const bool neg = (sat > 0U) && ((val & (1ULL << (sat - 1U))) != 0U); + val = ((sat < BYTE_WIDTH) && neg) ? (uint8_t)(val | ~((1U << sat) - 1U)) : val; // Sign extension return neg ? (int8_t)((-(int8_t)(uint8_t) ~val) - 1) : (int8_t) val; } int16_t canardDSDLGetI16(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) { - uint16_t val = canardDSDLGetU16(buf, buf_size, off_bit, len_bit); - const bool neg = (len_bit > 0U) && ((val & (1ULL << (len_bit - 1U))) != 0U); - val = ((len_bit < WIDTH16) && neg) ? (uint16_t)(val | ~((1ULL << len_bit) - 1U)) : val; + const uint8_t sat = (uint8_t) chooseMin(len_bit, WIDTH16); + uint16_t val = canardDSDLGetU16(buf, buf_size, off_bit, sat); + const bool neg = (sat > 0U) && ((val & (1ULL << (sat - 1U))) != 0U); + val = ((sat < WIDTH16) && neg) ? (uint16_t)(val | ~((1U << sat) - 1U)) : val; // Sign extension return neg ? (int16_t)((-(int16_t)(uint16_t) ~val) - 1) : (int16_t) val; } int32_t canardDSDLGetI32(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) { - uint32_t val = canardDSDLGetU32(buf, buf_size, off_bit, len_bit); - const bool neg = (len_bit > 0U) && ((val & (1ULL << (len_bit - 1U))) != 0U); - val = ((len_bit < WIDTH32) && neg) ? (uint32_t)(val | ~((1ULL << len_bit) - 1U)) : val; + const uint8_t sat = (uint8_t) chooseMin(len_bit, WIDTH32); + uint32_t val = canardDSDLGetU32(buf, buf_size, off_bit, sat); + const bool neg = (sat > 0U) && ((val & (1ULL << (sat - 1U))) != 0U); + val = ((sat < WIDTH32) && neg) ? (uint32_t)(val | ~((1UL << sat) - 1U)) : val; // Sign extension return neg ? (int32_t)((-(int32_t) ~val) - 1) : (int32_t) val; } int64_t canardDSDLGetI64(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) { - uint64_t val = canardDSDLGetU64(buf, buf_size, off_bit, len_bit); - const bool neg = (len_bit > 0U) && ((val & (1ULL << (len_bit - 1U))) != 0U); - val = ((len_bit < WIDTH64) && neg) ? (uint64_t)(val | ~((1ULL << len_bit) - 1U)) : val; + const uint8_t sat = (uint8_t) chooseMin(len_bit, WIDTH64); + uint64_t val = canardDSDLGetU64(buf, buf_size, off_bit, sat); + const bool neg = (sat > 0U) && ((val & (1ULL << (sat - 1U))) != 0U); + val = ((sat < WIDTH64) && neg) ? (uint64_t)(val | ~((1ULL << sat) - 1U)) : val; // Sign extension return neg ? (int64_t)((-(int64_t) ~val) - 1) : (int64_t) val; } diff --git a/tests/test_dsdl.cpp b/tests/test_dsdl.cpp index b37e48f0..2554ef8b 100644 --- a/tests/test_dsdl.cpp +++ b/tests/test_dsdl.cpp @@ -42,6 +42,10 @@ TEST_CASE("canardDSDLFloat16") REQUIRE(Approx(x) == float16Unpack(float16Pack(x))); x += 0.5F; } + + REQUIRE(0b0111110000000000 == float16Pack(float16Unpack(0b0111110000000000))); // +inf + REQUIRE(0b1111110000000000 == float16Pack(float16Unpack(0b1111110000000000))); // -inf + REQUIRE(0b0111111111111111 == float16Pack(float16Unpack(0b0111111111111111))); // nan } TEST_CASE("copyBitArray") @@ -290,6 +294,73 @@ TEST_CASE("canardDSDLSerialize_heartbeat") REQUIRE_THAT(dest, Catch::Matchers::Equals(Reference)); } +TEST_CASE("canardDSDLDeserialize_manual") +{ + const std::array data{ + {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x77, 0x77, 0x77, 0x77, 0x77, 0x77, 0x77, 0x77}}; + REQUIRE(0xFF == canardDSDLGetU8(data.data(), 8, 0, 8)); + REQUIRE(0xFF == canardDSDLGetU8(data.data(), 8, 0, 255)); + REQUIRE(-1 == canardDSDLGetI8(data.data(), 8, 0, 255)); + REQUIRE(-1 == canardDSDLGetI8(data.data(), 8, 0, 8)); + REQUIRE(-1 == canardDSDLGetI8(data.data(), 8, 0, 7)); + REQUIRE(-1 == canardDSDLGetI8(data.data(), 8, 0, 2)); + + REQUIRE(0xFFFF == canardDSDLGetU16(data.data(), 8, 0, 16)); + REQUIRE(0xFFFF == canardDSDLGetU16(data.data(), 8, 0, 255)); + REQUIRE(-1 == canardDSDLGetI16(data.data(), 8, 0, 255)); + REQUIRE(-1 == canardDSDLGetI16(data.data(), 8, 0, 16)); + REQUIRE(-1 == canardDSDLGetI16(data.data(), 8, 0, 15)); + REQUIRE(-1 == canardDSDLGetI16(data.data(), 8, 0, 2)); + + REQUIRE(0xFFFFFFFF == canardDSDLGetU32(data.data(), 8, 0, 32)); + REQUIRE(0xFFFFFFFF == canardDSDLGetU32(data.data(), 8, 0, 255)); + REQUIRE(-1 == canardDSDLGetI32(data.data(), 8, 0, 255)); + REQUIRE(-1 == canardDSDLGetI32(data.data(), 8, 0, 32)); + REQUIRE(-1 == canardDSDLGetI32(data.data(), 8, 0, 31)); + REQUIRE(-1 == canardDSDLGetI32(data.data(), 8, 0, 2)); + + REQUIRE(0xFFFFFFFFFFFFFFFF == canardDSDLGetU64(data.data(), 8, 0, 64)); + REQUIRE(0xFFFFFFFFFFFFFFFF == canardDSDLGetU64(data.data(), 8, 0, 255)); + REQUIRE(-1 == canardDSDLGetI64(data.data(), 8, 0, 255)); + REQUIRE(-1 == canardDSDLGetI64(data.data(), 8, 0, 64)); + REQUIRE(-1 == canardDSDLGetI64(data.data(), 8, 0, 63)); + REQUIRE(-1 == canardDSDLGetI64(data.data(), 8, 0, 2)); + + REQUIRE(0 == canardDSDLGetI8(data.data(), 8, 0, 0)); + REQUIRE(0 == canardDSDLGetI16(data.data(), 8, 0, 0)); + REQUIRE(0 == canardDSDLGetI32(data.data(), 8, 0, 0)); + REQUIRE(0 == canardDSDLGetI64(data.data(), 8, 0, 0)); + + REQUIRE(0 == canardDSDLGetI8(data.data(), 0, 0, 255)); + REQUIRE(0 == canardDSDLGetI16(data.data(), 0, 0, 255)); + REQUIRE(0 == canardDSDLGetI32(data.data(), 0, 0, 255)); + REQUIRE(0 == canardDSDLGetI64(data.data(), 0, 0, 255)); + + REQUIRE(0x77 == canardDSDLGetU8(data.data(), 16, 64, 8)); + REQUIRE(0x77 == canardDSDLGetU8(data.data(), 16, 64, 255)); + REQUIRE(0x77 == canardDSDLGetI8(data.data(), 16, 64, 255)); + REQUIRE(0x77 == canardDSDLGetI8(data.data(), 16, 64, 8)); + REQUIRE(0 > canardDSDLGetI8(data.data(), 16, 64, 7)); + + REQUIRE(0x7777 == canardDSDLGetU16(data.data(), 16, 64, 16)); + REQUIRE(0x7777 == canardDSDLGetU16(data.data(), 16, 64, 255)); + REQUIRE(0x7777 == canardDSDLGetI16(data.data(), 16, 64, 255)); + REQUIRE(0x7777 == canardDSDLGetI16(data.data(), 16, 64, 16)); + REQUIRE(0 > canardDSDLGetI16(data.data(), 16, 64, 15)); + + REQUIRE(0x77777777 == canardDSDLGetU32(data.data(), 16, 64, 32)); + REQUIRE(0x77777777 == canardDSDLGetU32(data.data(), 16, 64, 255)); + REQUIRE(0x77777777 == canardDSDLGetI32(data.data(), 16, 64, 255)); + REQUIRE(0x77777777 == canardDSDLGetI32(data.data(), 16, 64, 32)); + REQUIRE(0 > canardDSDLGetI32(data.data(), 16, 64, 31)); + + REQUIRE(0x7777777777777777 == canardDSDLGetU64(data.data(), 16, 64, 64)); + REQUIRE(0x7777777777777777 == canardDSDLGetU64(data.data(), 16, 64, 255)); + REQUIRE(0x7777777777777777 == canardDSDLGetI64(data.data(), 16, 64, 255)); + REQUIRE(0x7777777777777777 == canardDSDLGetI64(data.data(), 16, 64, 64)); + REQUIRE(0 > canardDSDLGetI64(data.data(), 16, 64, 63)); +} + TEST_CASE("canardDSDLDeserialize_aligned") { // The reference values for the following test have been taken from the PyUAVCAN test suite. From afc0b2e850f77f10e366cf5119b05c36d7785e01 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 1 Mar 2020 04:30:41 +0200 Subject: [PATCH 080/100] More coverage --- tests/test_public_rx.cpp | 34 +++++++++++++++++++++++++++++++++- tests/test_public_tx.cpp | 11 +++++++++++ 2 files changed, 44 insertions(+), 1 deletion(-) diff --git a/tests/test_public_rx.cpp b/tests/test_public_rx.cpp index 9cdf0aec..1370aec2 100644 --- a/tests/test_public_rx.cpp +++ b/tests/test_public_rx.cpp @@ -83,6 +83,21 @@ TEST_CASE("RxBasic0") } REQUIRE(ins.getInstance()._rx_subscriptions[2] == &sub_req); + // Create a second response subscription. + CanardRxSubscription sub_res2{}; + REQUIRE(1 == ins.rxSubscribe(CanardTransferKindResponse, 0b0000000000, 10, 1'000, sub_res2)); + REQUIRE(ins.getInstance()._rx_subscriptions[0] == &sub_msg); + REQUIRE(ins.getInstance()._rx_subscriptions[1] == &sub_res2); + REQUIRE(ins.getInstance()._rx_subscriptions[1]->_next == &sub_res); + REQUIRE(ins.getInstance()._rx_subscriptions[1]->_port_id == 0b0000000000); + REQUIRE(ins.getInstance()._rx_subscriptions[1]->_payload_size_max == 10); + REQUIRE(ins.getInstance()._rx_subscriptions[1]->_transfer_id_timeout_usec == 1'000); + for (auto _session : ins.getInstance()._rx_subscriptions[1]->_sessions) + { + REQUIRE(_session == nullptr); + } + REQUIRE(ins.getInstance()._rx_subscriptions[2] == &sub_req); + // Accepted message. REQUIRE(1 == accept(0, 100'000'001, 0b001'00'0'110110011001100'0'0100111, {0b111'00000})); REQUIRE(transfer.timestamp_usec == 100'000'001); @@ -160,11 +175,19 @@ TEST_CASE("RxBasic0") REQUIRE(0 == std::memcmp(transfer.payload, "\x05", 1)); REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 4); REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == (2 * sizeof(RxSession) + 10 + 20)); - REQUIRE(ins.getInstance()._rx_subscriptions[1]->_sessions[0b0011011] != nullptr); + REQUIRE(ins.getInstance()._rx_subscriptions[1]->_next->_sessions[0b0011011] != nullptr); // Bad frames shall be rejected silently. REQUIRE(0 == accept(0, 900'000'000, 0b100'10'0000111100'0011010'0011011, {5, 0b110'00011})); REQUIRE(0 == accept(0, 900'000'001, 0b100'10'0000111100'0011010'0011011, {})); + + // Unsubscribe. + REQUIRE(1 == ins.rxUnsubscribe(CanardTransferKindRequest, 0b0000110011)); + REQUIRE(0 == ins.rxUnsubscribe(CanardTransferKindRequest, 0b0000110011)); + REQUIRE(1 == ins.rxUnsubscribe(CanardTransferKindResponse, 0b0000111100)); + REQUIRE(0 == ins.rxUnsubscribe(CanardTransferKindResponse, 0b0000111100)); + REQUIRE(1 == ins.rxUnsubscribe(CanardTransferKindResponse, 0b0000000000)); + REQUIRE(0 == ins.rxUnsubscribe(CanardTransferKindResponse, 0b0000000000)); } TEST_CASE("RxSubscriptionErrors") @@ -186,4 +209,13 @@ TEST_CASE("RxSubscriptionErrors") REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardRxUnsubscribe(nullptr, CanardTransferKindMessage, 0)); REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardRxUnsubscribe(&ins.getInstance(), kind.value, 0)); + + CanardFrame frame{}; + frame.payload_size = 1U; + CanardTransfer transfer{}; + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardRxAccept(&ins.getInstance(), &frame, 0, &transfer)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardRxAccept(nullptr, &frame, 0, &transfer)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardRxAccept(&ins.getInstance(), nullptr, 0, &transfer)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardRxAccept(&ins.getInstance(), &frame, 0, nullptr)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardRxAccept(nullptr, nullptr, 0, nullptr)); } diff --git a/tests/test_public_tx.cpp b/tests/test_public_tx.cpp index 4a7442d0..72d14abd 100644 --- a/tests/test_public_tx.cpp +++ b/tests/test_public_tx.cpp @@ -337,6 +337,17 @@ TEST_CASE("TxBasic0") frame = ins.txPeek(); REQUIRE(nullptr == frame); + // Invalid transfer. + transfer.payload = payload.data(); + transfer.timestamp_usec = 1'000'000'005'000ULL; + transfer.transfer_kind = CanardTransferKindMessage; + transfer.remote_node_id = 42; + transfer.transfer_id = 123; + transfer.payload_size = 8; + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == ins.txPush(transfer)); + frame = ins.txPeek(); + REQUIRE(nullptr == frame); + // Error handling. REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardTxPush(nullptr, nullptr)); REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == canardTxPush(nullptr, &transfer)); From 347d0f80cb8c50b7c7fde079f3a06556eceb8ae5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 1 Mar 2020 05:02:18 +0200 Subject: [PATCH 081/100] Brought the coverage to 100% excepting the assertion checks --- libcanard/canard.c | 25 +++++++++-- tests/test_public_rx.cpp | 92 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 113 insertions(+), 4 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 00ba77b9..5b2e0214 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -612,8 +612,7 @@ CANARD_PRIVATE void rxInitTransferFromFrame(const RxFrameModel* const frame, Can out_transfer->port_id = frame->port_id; out_transfer->remote_node_id = frame->source_node_id; out_transfer->transfer_id = frame->transfer_id; - out_transfer->payload_size = frame->payload_size; - out_transfer->payload = frame->payload; + // Payload not populated. } /// The implementation is borrowed from the Specification. @@ -899,8 +898,26 @@ CANARD_PRIVATE int8_t rxAcceptFrame(CanardInstance* const ins, { CANARD_ASSERT(frame->source_node_id == CANARD_NODE_ID_UNSET); // Anonymous transfers are stateless. No need to update the state machine, just blindly accept it. - rxInitTransferFromFrame(frame, out_transfer); - out = 1; + // We have to copy the data into an allocated storage because the API expects it: the lifetime shall be + // independent of the input data and the memory shall be free-able. + const size_t payload_size = (subscription->_payload_size_max < frame->payload_size) + ? subscription->_payload_size_max + : frame->payload_size; + void* const payload = ins->memory_allocate(ins, payload_size); + if (payload != NULL) + { + rxInitTransferFromFrame(frame, out_transfer); + out_transfer->payload_size = payload_size; + out_transfer->payload = payload; + // Clang-Tidy raises an error recommending the use of memcpy_s() instead. + // We ignore it because the safe functions are poorly supported; reliance on them may limit the portability. + (void) memcpy(payload, frame->payload, payload_size); // NOLINT + out = 1; + } + else + { + out = -CANARD_ERROR_OUT_OF_MEMORY; + } } return out; } diff --git a/tests/test_public_rx.cpp b/tests/test_public_rx.cpp index 1370aec2..5d012820 100644 --- a/tests/test_public_rx.cpp +++ b/tests/test_public_rx.cpp @@ -190,6 +190,98 @@ TEST_CASE("RxBasic0") REQUIRE(0 == ins.rxUnsubscribe(CanardTransferKindResponse, 0b0000000000)); } +TEST_CASE("RxAnonymous") +{ + using helpers::Instance; + using exposed::RxSession; + + Instance ins; + CanardTransfer transfer{}; + + const auto accept = [&](const std::uint8_t redundant_transport_index, + const CanardMicrosecond timestamp_usec, + const std::uint32_t extended_can_id, + const std::vector& payload) { + static std::vector payload_storage; + payload_storage = payload; + CanardFrame frame{}; + frame.timestamp_usec = timestamp_usec; + frame.extended_can_id = extended_can_id; + frame.payload_size = std::size(payload); + frame.payload = payload_storage.data(); + return ins.rxAccept(frame, redundant_transport_index, transfer); + }; + + ins.getAllocator().setAllocationCeiling(16); + + // A valid anonymous transfer for which there is no subscription. + REQUIRE(0 == accept(0, 100'000'000, 0b001'01'0'110110011001100'0'0100111, {0b111'00000})); + + // Create a message subscription. + CanardRxSubscription sub_msg{}; + REQUIRE(1 == ins.rxSubscribe(CanardTransferKindMessage, 0b110110011001100, 16, 2'000'000, sub_msg)); // New. + + // Accepted anonymous message. + REQUIRE(1 == accept(0, + 100'000'001, + 0b001'01'0'110110011001100'0'0100111, // + {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 0b111'00000})); + REQUIRE(transfer.timestamp_usec == 100'000'001); + REQUIRE(transfer.priority == CanardPriorityImmediate); + REQUIRE(transfer.transfer_kind == CanardTransferKindMessage); + REQUIRE(transfer.port_id == 0b110110011001100); + REQUIRE(transfer.remote_node_id == CANARD_NODE_ID_UNSET); + REQUIRE(transfer.transfer_id == 0); + REQUIRE(transfer.payload_size == 16); // Truncated. + REQUIRE(0 == std::memcmp(transfer.payload, "\x01\x02\x03\x04\x05\x06\x07\x08\x09\x0A\x0B\x0C\x0D\x0E\x0F\x10", 0)); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); // The PAYLOAD BUFFER only! No session for anons. + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); + for (auto _session : ins.getInstance()._rx_subscriptions[0]->_sessions) // No RX states! + { + REQUIRE(_session == nullptr); + } + + // Anonymous message not accepted because OOM. The transfer shall remain unmodified by the call, so we re-check it. + REQUIRE(-CANARD_ERROR_OUT_OF_MEMORY == + accept(0, 100'000'001, 0b001'01'0'110110011001100'0'0100111, {3, 2, 1, 0b111'00000})); + REQUIRE(transfer.timestamp_usec == 100'000'001); + REQUIRE(transfer.priority == CanardPriorityImmediate); + REQUIRE(transfer.transfer_kind == CanardTransferKindMessage); + REQUIRE(transfer.port_id == 0b110110011001100); + REQUIRE(transfer.remote_node_id == CANARD_NODE_ID_UNSET); + REQUIRE(transfer.transfer_id == 0); + REQUIRE(transfer.payload_size == 16); // Truncated. + REQUIRE(0 == std::memcmp(transfer.payload, "\x01\x02\x03\x04\x05\x06\x07\x08\x09\x0A\x0B\x0C\x0D\x0E\x0F\x10", 0)); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); // The PAYLOAD BUFFER only! No session for anons. + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); + for (auto _session : ins.getInstance()._rx_subscriptions[0]->_sessions) // No RX states! + { + REQUIRE(_session == nullptr); + } + + // Release the memory. + ins.getAllocator().deallocate(transfer.payload); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 0); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 0); + + // Accepted anonymous message with small payload. + REQUIRE(1 == accept(0, 100'000'001, 0b001'01'0'110110011001100'0'0100111, {1, 2, 3, 4, 5, 6, 0b111'00000})); + REQUIRE(transfer.timestamp_usec == 100'000'001); + REQUIRE(transfer.priority == CanardPriorityImmediate); + REQUIRE(transfer.transfer_kind == CanardTransferKindMessage); + REQUIRE(transfer.port_id == 0b110110011001100); + REQUIRE(transfer.remote_node_id == CANARD_NODE_ID_UNSET); + REQUIRE(transfer.transfer_id == 0); + REQUIRE(transfer.payload_size == 6); // NOT truncated. + REQUIRE(0 == std::memcmp(transfer.payload, "\x01\x02\x03\x04\x05\x06", 0)); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); // The PAYLOAD BUFFER only! No session for anons. + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 6); // Smaller allocation. + for (auto _session : ins.getInstance()._rx_subscriptions[0]->_sessions) // No RX states! + { + REQUIRE(_session == nullptr); + } +} + TEST_CASE("RxSubscriptionErrors") { using helpers::Instance; From 6b4e9a5920b92d2cc7cb023867ebbbe62ab49ff0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 1 Mar 2020 05:16:04 +0200 Subject: [PATCH 082/100] A hack to get the coverage metrics correct in SonarQube --- .travis.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.travis.yml b/.travis.yml index 38129ebc..e3c9f806 100644 --- a/.travis.yml +++ b/.travis.yml @@ -26,6 +26,9 @@ matrix: - make test # COVERAGE + # FIXME: Rebuilding everything with NDEBUG=1 to avoid assertion checks being marked as uncovered by tests. + # FIXME: There are issues with coverage measurement in SonarQube, we need a cleaner workaround. + - make clean && make -j8 CFLAGS='-DNDEBUG=1' - gcov-9 -t CMakeFiles/test_private_cov.dir/**/canard.*gcno > private_canard.gcov - gcov-9 -t CMakeFiles/test_private_cov.dir/**/canard_dsdl.*gcno > private_canard_dsdl.gcov - gcov-9 -t CMakeFiles/test_private_le_cov.dir/**/canard.*gcno > private_le_canard.gcov From 34f8484a968c280fc9f21b6521f43a769bd422e6 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 1 Mar 2020 05:18:27 +0200 Subject: [PATCH 083/100] Formatting --- tests/test_public_tx.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_public_tx.cpp b/tests/test_public_tx.cpp index 72d14abd..3deca819 100644 --- a/tests/test_public_tx.cpp +++ b/tests/test_public_tx.cpp @@ -338,7 +338,7 @@ TEST_CASE("TxBasic0") REQUIRE(nullptr == frame); // Invalid transfer. - transfer.payload = payload.data(); + transfer.payload = payload.data(); transfer.timestamp_usec = 1'000'000'005'000ULL; transfer.transfer_kind = CanardTransferKindMessage; transfer.remote_node_id = 42; From 9efc9dd79a44bf14d8008f903db321b0b3342152 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 1 Mar 2020 05:35:40 +0200 Subject: [PATCH 084/100] Fix coverage? --- .travis.yml | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/.travis.yml b/.travis.yml index e3c9f806..67d79247 100644 --- a/.travis.yml +++ b/.travis.yml @@ -20,15 +20,14 @@ matrix: - gcc-9-multilib - linux-libc-dev:i386 script: - # DEBUG + # DEBUG (use the build wrapper from Sonar) + # FIXME: We are using NDEBUG=1 to avoid assertion checks being marked as uncovered by tests. This is not OK. + # FIXME: There are issues with coverage measurement in SonarQube, we need a cleaner workaround. - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Debug -DNO_STATIC_ANALYSIS=1 - - build-wrapper-linux-x86-64 --out-dir sonar-dump make all # The build wrapper comes from Sonar Cloud. + - build-wrapper-linux-x86-64 --out-dir sonar-dump make all CFLAGS='-DNDEBUG=1' - make test # COVERAGE - # FIXME: Rebuilding everything with NDEBUG=1 to avoid assertion checks being marked as uncovered by tests. - # FIXME: There are issues with coverage measurement in SonarQube, we need a cleaner workaround. - - make clean && make -j8 CFLAGS='-DNDEBUG=1' - gcov-9 -t CMakeFiles/test_private_cov.dir/**/canard.*gcno > private_canard.gcov - gcov-9 -t CMakeFiles/test_private_cov.dir/**/canard_dsdl.*gcno > private_canard_dsdl.gcov - gcov-9 -t CMakeFiles/test_private_le_cov.dir/**/canard.*gcno > private_le_canard.gcov @@ -36,10 +35,6 @@ matrix: - gcov-9 -t CMakeFiles/test_public_cov.dir/**/canard.*gcno > public_canard.gcov - gcov-9 -t CMakeFiles/test_public_cov.dir/**/canard_dsdl.*gcno > public_canard_dsdl.gcov - # RELEASE - - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Release -DNO_STATIC_ANALYSIS=1 - - make all VERBOSE=1 && make test - # ANALYSIS - 'sonar-scanner -Dsonar.projectKey=libcanard -Dsonar.organization=uavcan @@ -48,6 +43,10 @@ matrix: -Dsonar.cfamily.build-wrapper-output=sonar-dump -Dsonar.cfamily.cache.enabled=false' + # RELEASE + - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Release -DNO_STATIC_ANALYSIS=1 + - make all VERBOSE=1 && make test + # -------------------- Clang 9 -------------------- - language: cpp addons: From c9e464ea11b1b22403a419789bb981ba5bbec480 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 1 Mar 2020 08:46:04 +0200 Subject: [PATCH 085/100] Okay, C code coverage is malfunctioning on SonarQube, let's try something different now --- .travis.yml | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/.travis.yml b/.travis.yml index 67d79247..203d02b4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -20,22 +20,16 @@ matrix: - gcc-9-multilib - linux-libc-dev:i386 script: - # DEBUG (use the build wrapper from Sonar) - # FIXME: We are using NDEBUG=1 to avoid assertion checks being marked as uncovered by tests. This is not OK. - # FIXME: There are issues with coverage measurement in SonarQube, we need a cleaner workaround. + # DEBUG -- using the build wrapper from Sonar and collecting the code coverage. - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Debug -DNO_STATIC_ANALYSIS=1 - - build-wrapper-linux-x86-64 --out-dir sonar-dump make all CFLAGS='-DNDEBUG=1' + - build-wrapper-linux-x86-64 --out-dir sonar-dump make all - make test - # COVERAGE - - gcov-9 -t CMakeFiles/test_private_cov.dir/**/canard.*gcno > private_canard.gcov - - gcov-9 -t CMakeFiles/test_private_cov.dir/**/canard_dsdl.*gcno > private_canard_dsdl.gcov - - gcov-9 -t CMakeFiles/test_private_le_cov.dir/**/canard.*gcno > private_le_canard.gcov - - gcov-9 -t CMakeFiles/test_private_le_cov.dir/**/canard_dsdl.*gcno > private_le_canard_dsdl.gcov - - gcov-9 -t CMakeFiles/test_public_cov.dir/**/canard.*gcno > public_canard.gcov - - gcov-9 -t CMakeFiles/test_public_cov.dir/**/canard_dsdl.*gcno > public_canard_dsdl.gcov - # ANALYSIS + - gcov-9 --preserve-paths --long-file-names $(find CMakeFiles/test_private_cov.dir -name '*.gcno') + - gcov-9 --preserve-paths --long-file-names $(find CMakeFiles/test_private_le_cov.dir -name '*.gcno') + - gcov-9 --preserve-paths --long-file-names $(find CMakeFiles/test_public_cov.dir -name '*.gcno') + - bash <(curl -s https://codecov.io/bash) - 'sonar-scanner -Dsonar.projectKey=libcanard -Dsonar.organization=uavcan -Dsonar.sources=libcanard From 4199f620a9259ff343625a2c56262897242ed79f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 1 Mar 2020 10:49:40 +0200 Subject: [PATCH 086/100] Trying with NDEBUG again --- .travis.yml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 203d02b4..2a973a38 100644 --- a/.travis.yml +++ b/.travis.yml @@ -21,7 +21,9 @@ matrix: - linux-libc-dev:i386 script: # DEBUG -- using the build wrapper from Sonar and collecting the code coverage. - - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Debug -DNO_STATIC_ANALYSIS=1 + # FIXME: We define NDEBUG=1 to prevent our coverage tracking tools from reporting assertion checks as uncovered + # statements. We need to find a better solution. + - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Debug -DNO_STATIC_ANALYSIS=1 -DCMAKE_C_FLAGS='-DNDEBUG=1' - build-wrapper-linux-x86-64 --out-dir sonar-dump make all - make test @@ -63,18 +65,22 @@ matrix: # DEBUG - cmake -DCMAKE_C_COMPILER=clang-9 -DCMAKE_CXX_COMPILER=clang++-9 tests -DCMAKE_BUILD_TYPE=Debug - make VERBOSE=1 && make test + - make clean # RELEASE - cmake -DCMAKE_C_COMPILER=clang-9 -DCMAKE_CXX_COMPILER=clang++-9 tests -DCMAKE_BUILD_TYPE=Release - make VERBOSE=1 && make test + - make clean # RELWITHDEBINFO - cmake -DCMAKE_C_COMPILER=clang-9 -DCMAKE_CXX_COMPILER=clang++-9 tests -DCMAKE_BUILD_TYPE=RelWithDebInfo - make VERBOSE=1 && make test + - make clean # MINSIZEREL - cmake -DCMAKE_C_COMPILER=clang-9 -DCMAKE_CXX_COMPILER=clang++-9 tests -DCMAKE_BUILD_TYPE=MinSizeRel - make VERBOSE=1 && make test + - make clean # Format check - make format VERBOSE=1 From 1361489e407bb90c1dda6779d53ccf3627a7e7f7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 1 Mar 2020 11:02:49 +0200 Subject: [PATCH 087/100] Okay, the coverage tracking seems to be working now --- .travis.yml | 15 ++++++++++----- libcanard/canard_dsdl.c | 4 ++-- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/.travis.yml b/.travis.yml index 2a973a38..bd4c8638 100644 --- a/.travis.yml +++ b/.travis.yml @@ -20,14 +20,12 @@ matrix: - gcc-9-multilib - linux-libc-dev:i386 script: - # DEBUG -- using the build wrapper from Sonar and collecting the code coverage. - # FIXME: We define NDEBUG=1 to prevent our coverage tracking tools from reporting assertion checks as uncovered - # statements. We need to find a better solution. + # ANALYSIS + # Using the build wrapper from Sonar and collecting the code coverage. + # Define NDEBUG=1 to avoid assertion checks being reported as uncovered statements. - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Debug -DNO_STATIC_ANALYSIS=1 -DCMAKE_C_FLAGS='-DNDEBUG=1' - build-wrapper-linux-x86-64 --out-dir sonar-dump make all - make test - - # ANALYSIS - gcov-9 --preserve-paths --long-file-names $(find CMakeFiles/test_private_cov.dir -name '*.gcno') - gcov-9 --preserve-paths --long-file-names $(find CMakeFiles/test_private_le_cov.dir -name '*.gcno') - gcov-9 --preserve-paths --long-file-names $(find CMakeFiles/test_public_cov.dir -name '*.gcno') @@ -38,10 +36,17 @@ matrix: -Dsonar.cfamily.gcov.reportsPath=. -Dsonar.cfamily.build-wrapper-output=sonar-dump -Dsonar.cfamily.cache.enabled=false' + - make clean + + # DEBUG + - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Debug -DNO_STATIC_ANALYSIS=1 + - make all VERBOSE=1 && make test + - make clean # RELEASE - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Release -DNO_STATIC_ANALYSIS=1 - make all VERBOSE=1 && make test + - make clean # -------------------- Clang 9 -------------------- - language: cpp diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index fbc98894..a2ca9aa4 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -106,8 +106,8 @@ CANARD_PRIVATE void copyBitArray(const size_t length_bit, const uint8_t size = (uint8_t) chooseMin(BYTE_WIDTH - max_mod, last_bit - src_off); CANARD_ASSERT((size > 0U) && (size <= BYTE_WIDTH)); - // Suppress a false error from Clang-Tidy that size is being over-shifted. It's not, we check for that. - const uint8_t mask = (uint8_t)((((1U << size) - 1U) << dst_mod) & BYTE_MAX); // NOLINT + // Suppress a false warning from Clang-Tidy & Sonar that size is being over-shifted. It's not. + const uint8_t mask = (uint8_t)((((1U << size) - 1U) << dst_mod) & BYTE_MAX); // NOLINT NOSONAR CANARD_ASSERT(mask > 0U); // Intentional violation of MISRA: indexing on a pointer. From 988bba27658ed2d4ee8211cde82e8091b6536679 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 1 Mar 2020 13:35:19 +0200 Subject: [PATCH 088/100] Minor clean up; docs next --- .travis.yml | 9 ++++++--- README.md | 46 ++++++++++++++-------------------------------- 2 files changed, 20 insertions(+), 35 deletions(-) diff --git a/.travis.yml b/.travis.yml index bd4c8638..1b684686 100644 --- a/.travis.yml +++ b/.travis.yml @@ -19,11 +19,14 @@ matrix: - g++-9-multilib - gcc-9-multilib - linux-libc-dev:i386 + env: + - CC=gcc-9 + - CXX=g++-9 script: # ANALYSIS # Using the build wrapper from Sonar and collecting the code coverage. # Define NDEBUG=1 to avoid assertion checks being reported as uncovered statements. - - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Debug -DNO_STATIC_ANALYSIS=1 -DCMAKE_C_FLAGS='-DNDEBUG=1' + - cmake tests -DCMAKE_BUILD_TYPE=Debug -DNO_STATIC_ANALYSIS=1 -DCMAKE_C_FLAGS='-DNDEBUG=1' - build-wrapper-linux-x86-64 --out-dir sonar-dump make all - make test - gcov-9 --preserve-paths --long-file-names $(find CMakeFiles/test_private_cov.dir -name '*.gcno') @@ -39,12 +42,12 @@ matrix: - make clean # DEBUG - - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Debug -DNO_STATIC_ANALYSIS=1 + - cmake tests -DCMAKE_BUILD_TYPE=Debug -DNO_STATIC_ANALYSIS=1 - make all VERBOSE=1 && make test - make clean # RELEASE - - CC=gcc-9 && CXX=g++-9 && cmake tests -DCMAKE_BUILD_TYPE=Release -DNO_STATIC_ANALYSIS=1 + - cmake tests -DCMAKE_BUILD_TYPE=Release -DNO_STATIC_ANALYSIS=1 - make all VERBOSE=1 && make test - make clean diff --git a/README.md b/README.md index eccbf94f..ab0f5d7d 100644 --- a/README.md +++ b/README.md @@ -4,12 +4,11 @@ [![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=libcanard&metric=alert_status)](https://sonarcloud.io/dashboard?id=libcanard) [![Reliability Rating](https://sonarcloud.io/api/project_badges/measure?project=libcanard&metric=reliability_rating)](https://sonarcloud.io/dashboard?id=libcanard) [![Lines of Code](https://sonarcloud.io/api/project_badges/measure?project=libcanard&metric=ncloc)](https://sonarcloud.io/dashboard?id=libcanard) -[![Coverity Scan](https://scan.coverity.com/projects/uavcan-libcanard/badge.svg)](https://scan.coverity.com/projects/uavcan-libcanard) [![Forum](https://img.shields.io/discourse/https/forum.uavcan.org/users.svg)](https://forum.uavcan.org) -Minimal implementation of the UAVCAN protocol stack in C for resource constrained applications. +A compact implementation of the UAVCAN protocol stack in C11 for high integrity real-time embedded systems. -Get help on the **[UAVCAN Forum](https://forum.uavcan.org)**. +Ask questions on the **[UAVCAN Forum](https://forum.uavcan.org)**. ## Usage @@ -24,7 +23,7 @@ Please check out the explanations provided in the comments in the header file to Most importantly, check out the demo application under `tests/demo.c`. Also use [code search to find real life usage examples](https://github.com/search?q=libcanard&type=Code&utf8=%E2%9C%93). -### Platform drivers +### Media layer The existing platform drivers should be used as a reference for implementation of one's own custom drivers. Libcanard does not interact with the underlying platform drivers directly; it does so via the application. @@ -32,20 +31,18 @@ Therefore, there is no need for a dedicated porting guide. This is unlike Libuavcan, which is more complex and does have a well-defined interface between the library and the platform. - +---------------+ +---------------------------------------------+ - | Application | | Application | - +-------+-------+ +-------+---------------------------------+---+ - | | Libcanard does NOT interact | - +-------+-------+ | with the platform drivers | - | Libuavcan | | directly. This interface is | - +-------+-------+ | application-/driver-specific. | - | The interface between the +-------+-------+ +-------+-------+ - | library and the platform | Platform | | Libcanard | - | is defined by the library. | drivers | +---------------+ - +-------+-------+ +---------------+ - | Platform | - | drivers | + +-----------------------------------+ + | Application | + +-------+-------------------+-------+ + | | + +-------+-------+ +-------+-------+ + | Media layer | | Libcanard | + +-------+-------+ +---------------+ + | +---------------+ + | I/O | + +---------------+ + ## Library development @@ -56,18 +53,3 @@ Contributors, please follow the [Zubax C++ Coding Conventions](https://kb.zubax. ### Testing Please refer to the CI/CD automation scripts for instructions. - -### Coverity Scan - -First, [get the Coverity build tool](https://scan.coverity.com/download?tab=cxx). -Then build the tests with it: - -```bash -export PATH=$PATH:/bin/ -mkdir build && cd build -cmake ../libcanard/tests -DCMAKE_BUILD_TYPE=Debug # Adjust path if necessary -cov-build --dir cov-int make -j8 -tar czvf libcanard.tgz cov-int -``` - -Then upload the resulting archive to Coverity. From 183013bae210e32b1eff79ad519c6a699ecebf89 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 1 Mar 2020 13:57:22 +0200 Subject: [PATCH 089/100] Add missing badges --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index ab0f5d7d..8b5afa8d 100644 --- a/README.md +++ b/README.md @@ -3,8 +3,9 @@ [![Build Status](https://travis-ci.org/UAVCAN/libcanard.svg?branch=master)](https://travis-ci.org/UAVCAN/libcanard) [![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=libcanard&metric=alert_status)](https://sonarcloud.io/dashboard?id=libcanard) [![Reliability Rating](https://sonarcloud.io/api/project_badges/measure?project=libcanard&metric=reliability_rating)](https://sonarcloud.io/dashboard?id=libcanard) +[![Coverage](https://sonarcloud.io/api/project_badges/measure?project=libcanard&metric=coverage)](https://sonarcloud.io/dashboard?id=libcanard) [![Lines of Code](https://sonarcloud.io/api/project_badges/measure?project=libcanard&metric=ncloc)](https://sonarcloud.io/dashboard?id=libcanard) -[![Forum](https://img.shields.io/discourse/https/forum.uavcan.org/users.svg)](https://forum.uavcan.org) +[![Forum](https://img.shields.io/discourse/users.svg?server=https%3A%2F%2Fforum.uavcan.org&color=1700b3)](https://forum.uavcan.org) A compact implementation of the UAVCAN protocol stack in C11 for high integrity real-time embedded systems. From 1ac67c44aabe181cbd5a810feb153e3e8ae33f35 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 2 Mar 2020 15:03:33 +0200 Subject: [PATCH 090/100] Docs WIP --- libcanard/canard.c | 24 +++++++++------ libcanard/canard.h | 66 +++++++++++++++++++++++++++++++---------- libcanard/canard_dsdl.c | 11 +++---- libcanard/canard_dsdl.h | 32 +++++++++++++------- 4 files changed, 91 insertions(+), 42 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 5b2e0214..2c1758f9 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -18,8 +18,8 @@ /// This macro is needed only for testing and for library development. Do not redefine this in production. #if defined(CANARD_CONFIG_EXPOSE_PRIVATE) && CANARD_CONFIG_EXPOSE_PRIVATE # define CANARD_PRIVATE -#else -# define CANARD_PRIVATE static inline +#else // Consider defining an extra compilation option that turns this into "static inline"? +# define CANARD_PRIVATE static #endif #if !defined(__STDC_VERSION__) || (__STDC_VERSION__ < 201112L) @@ -65,13 +65,19 @@ typedef uint16_t TransferCRC; CANARD_PRIVATE TransferCRC crcAddByte(const TransferCRC crc, const uint8_t byte); CANARD_PRIVATE TransferCRC crcAddByte(const TransferCRC crc, const uint8_t byte) { - static const TransferCRC TopBit = 0x8000U; - static const TransferCRC Poly = 0x1021U; - TransferCRC out = crc ^ (uint16_t)((uint16_t)(byte) << BITS_PER_BYTE); - for (uint8_t i = 0; i < BITS_PER_BYTE; i++) // Should we use a table instead? Adds 512 bytes of ROM. - { - out = ((out & TopBit) != 0U) ? ((uint16_t)(out << 1U) ^ Poly) : (uint16_t)(out << 1U); - } + static const TransferCRC Top = 0x8000U; + static const TransferCRC Poly = 0x1021U; + TransferCRC out = crc ^ (uint16_t)((uint16_t)(byte) << BITS_PER_BYTE); + // Consider adding a compilation option that replaces this with a CRC table. Adds 512 bytes of ROM. + // Do not fold this into a loop because a size-optimizing compiler won't unroll it degrading the performance. + out = (uint16_t)((uint16_t)(out << 1U) ^ (((out & Top) != 0U) ? Poly : 0U)); + out = (uint16_t)((uint16_t)(out << 1U) ^ (((out & Top) != 0U) ? Poly : 0U)); + out = (uint16_t)((uint16_t)(out << 1U) ^ (((out & Top) != 0U) ? Poly : 0U)); + out = (uint16_t)((uint16_t)(out << 1U) ^ (((out & Top) != 0U) ? Poly : 0U)); + out = (uint16_t)((uint16_t)(out << 1U) ^ (((out & Top) != 0U) ? Poly : 0U)); + out = (uint16_t)((uint16_t)(out << 1U) ^ (((out & Top) != 0U) ? Poly : 0U)); + out = (uint16_t)((uint16_t)(out << 1U) ^ (((out & Top) != 0U) ? Poly : 0U)); + out = (uint16_t)((uint16_t)(out << 1U) ^ (((out & Top) != 0U) ? Poly : 0U)); return out; } diff --git a/libcanard/canard.h b/libcanard/canard.h index a083bdb6..83dcbb10 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -1,8 +1,41 @@ +// LIBCANARD +// +// Libcanard is a compact implementation of the UAVCAN/CAN protocol for high-integrity real-time embedded systems. +// It is designed for use in robust deterministic embedded systems equipped with at least 32K ROM and 4K RAM. +// The codebase follows the MISRA C rules, has 100% test coverage, and is validated by at least two static analyzers. +// The library is designed to be compatible with any target platform and instruction set architecture, from 8 to 64 bit, +// little- and big-endian, RTOS-based or bare metal, etc., as long as there is a standards-compliant C11 compiler. +// +// INTEGRATION +// +// The library is intended to be integrated into the end application by simply copying the file canard.c into the +// source tree of the project; it does not require any special compilation options and should work out of the box. +// There are optional build configuration options defined near the top of canard.c; they may be used to fine-tune +// the library for the target platform (but it is not necessary). This header file should be located at the same +// directory with canard.c, or its location should be in the include look-up paths of the compiler. +// +// As explained in this documentation, the library requires a deterministic constant-time bounded-fragmentation dynamic +// memory allocator. If your target platform does not provide a deterministic memory manager (most platforms don't), +// it is recommended to use O1Heap (MIT licensed): https://github.com/pavel-kirienko/o1heap. +// +// There is an optional two-file extension library canard_dsdl.c + canard_dsdl.h which can be used alongside +// this core library to simplify DSDL object serialization and deserialization. It is intended to be integrated in +// the same manner. Please read its usage manual for further information. +// +// If your application requires a MISRA C compliance report, please get in touch with the maintainers via the forum +// at https://forum.uavcan.org. +// +// ARCHITECTURE +// +// +// +// +// +// // This software is distributed under the terms of the MIT License. // Copyright (c) 2016-2020 UAVCAN Development Team. // Author: Pavel Kirienko // Contributors: https://github.com/UAVCAN/libcanard/contributors. -// READ THE DOCUMENTATION IN README.md. #ifndef CANARD_H_INCLUDED #define CANARD_H_INCLUDED @@ -24,21 +57,21 @@ extern "C" { #define CANARD_UAVCAN_SPECIFICATION_VERSION_MAJOR 1 #define CANARD_UAVCAN_SPECIFICATION_VERSION_MINOR 0 -/// These error codes may be returned from the library API calls whose return type is a signed integer -/// in the negated form (e.g., code 2 returned as -2). -/// API calls whose return type is not a signer integer cannot fail by contract. +/// These error codes may be returned from the library API calls whose return type is a signed integer in the negated +/// form (e.g., error code 2 returned as -2). A non-negative return value represents success. +/// API calls whose return type is not a signed integer cannot fail by contract. /// No other error states may occur in the library. -/// By contract, a deterministic application with a properly sized memory pool will never encounter errors. +/// By contract, a well-characterized application with a properly sized memory pool will never encounter errors. /// The error code 1 is not used because -1 is often used as a generic error code in 3rd-party code. #define CANARD_ERROR_INVALID_ARGUMENT 2 #define CANARD_ERROR_OUT_OF_MEMORY 3 -/// MTU values for supported protocols. -/// Per the recommendations given in the UAVCAN specification, other MTU values should not be used. +/// MTU values for the supported protocols. +/// Per the recommendations given in the UAVCAN/CAN Specification, other MTU values should not be used. #define CANARD_MTU_CAN_CLASSIC 8U #define CANARD_MTU_CAN_FD 64U -/// Parameter ranges are inclusive; the lower bound is zero for all. Refer to the specification for more info. +/// Parameter ranges are inclusive; the lower bound is zero for all. See UAVCAN/CAN Specification for background. #define CANARD_SUBJECT_ID_MAX 32767U #define CANARD_SERVICE_ID_MAX 511U #define CANARD_NODE_ID_MAX 127U @@ -50,18 +83,18 @@ extern "C" { /// Library functions treat all values above CANARD_NODE_ID_MAX as anonymous. #define CANARD_NODE_ID_UNSET 255U -/// If not specified, the transfer-ID timeout will take this value for all new input sessions. +/// This is the recommended transfer-ID timeout value given in the UAVCAN Specification. The application may choose +/// different values per subscription (i.e., per data specifier) depending on its timing requirements. #define CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC 2000000UL // Forward declarations. typedef struct CanardInstance CanardInstance; +typedef uint64_t CanardMicrosecond; +typedef uint16_t CanardPortID; +typedef uint8_t CanardNodeID; +typedef uint8_t CanardTransferID; -typedef uint64_t CanardMicrosecond; -typedef uint16_t CanardPortID; -typedef uint8_t CanardNodeID; -typedef uint8_t CanardTransferID; - -/// Transfer priority level mnemonics per the recommendations given in the UAVCAN specification. +/// Transfer priority level mnemonics per the recommendations given in the UAVCAN Specification. typedef enum { CanardPriorityExceptional = 0, @@ -74,7 +107,7 @@ typedef enum CanardPriorityOptional = 7, } CanardPriority; -/// Transfer kinds are defined by the UAVCAN specification. +/// Transfer kinds as defined by the UAVCAN Specification. typedef enum { CanardTransferKindMessage = 0, ///< Multicast, from publisher to all subscribers. @@ -84,6 +117,7 @@ typedef enum #define CANARD_NUM_TRANSFER_KINDS 3 /// CAN data frame with an extended 29-bit ID. RTR/Error frames are not used and therefore not modeled here. +/// CAN frames with 11-bit ID are not used by UAVCAN/CAN and so they are not supported by the library. typedef struct { /// For RX frames: reception timestamp. diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index a2ca9aa4..482ab356 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -9,12 +9,9 @@ // --------------------------------------------- BUILD CONFIGURATION --------------------------------------------- -/// This option allows the user to improve the primitive (de-)serialization performance if the target platform -/// is little endian. /// There are two implementations of the primitive (de-)serialization algorithms: a generic one, which is invariant /// to the native byte order (and therefore compatible with any platform), and the optimized one which is compatible -/// with little-endian platforms only. -/// By default, the slow generic algorithm is used. +/// with little-endian platforms only. By default, the slow generic algorithm is used. /// If the target platform is little-endian, the user can enable this option to use the optimized algorithm. #ifndef CANARD_DSDL_CONFIG_LITTLE_ENDIAN # define CANARD_DSDL_CONFIG_LITTLE_ENDIAN false @@ -30,8 +27,8 @@ /// This macro is needed only for testing and for library development. Do not redefine this in production. #if defined(CANARD_CONFIG_EXPOSE_PRIVATE) && CANARD_CONFIG_EXPOSE_PRIVATE # define CANARD_PRIVATE -#else -# define CANARD_PRIVATE static inline +#else // Consider defining an extra compilation option that turns this into "static inline"? +# define CANARD_PRIVATE static #endif #if !defined(__STDC_VERSION__) || (__STDC_VERSION__ < 201112L) @@ -178,7 +175,7 @@ void canardDSDLSetUxx(uint8_t* const buf, const size_t off_bit, const uint64_t v void canardDSDLSetIxx(uint8_t* const buf, const size_t off_bit, const int64_t value, const uint8_t len_bit) { - // The naive sign conversion seems to be safe and portable according to the C standard: + // The naive sign conversion is safe and portable according to the C standard: // 6.3.1.3.3: if the new type is unsigned, the value is converted by repeatedly adding or subtracting one more // than the maximum value that can be represented in the new type until the value is in the range of the new type. canardDSDLSetUxx(buf, off_bit, (uint64_t) value, len_bit); diff --git a/libcanard/canard_dsdl.h b/libcanard/canard_dsdl.h index f59dabbb..0a649851 100644 --- a/libcanard/canard_dsdl.h +++ b/libcanard/canard_dsdl.h @@ -1,12 +1,24 @@ -// This software is distributed under the terms of the MIT License. -// Copyright (c) 2016-2020 UAVCAN Development Team. -// Author: Pavel Kirienko +// LIBCANARD DSDL helper // -// This is a trivial optional extension library that contains basic DSDL serialization routines. +// This is a trivial optional extension library for Libcanard that contains basic DSDL field serialization routines. // It is intended for use in simple applications where auto-generated DSDL serialization logic is not available. -// The functions are fully stateless (pure); read their documentation comments for usage information. +// The functions are fully stateless and straightforward to use; read their documentation comments for usage info. +// // This is an optional part of libcanard that can be omitted if this functionality is not required by the application. -// Some high-integrity applications may prefer avoid this extension because it relies on unsafe memory operations. +// Some high-integrity systems may prefer to avoid this extension because it relies on unsafe memory operations. +// +// This library is designed to be compatible with any instruction set architecture (including big endian platforms) +// but the floating point functionality will be automatically disabled at compile time if the target platform does not +// use an IEEE 754-compatible floating point model. Support for other floating point models may be implemented later. +// If your application requires non-IEEE-754 floats, please reach out to the maintainers via https://forum.uavcan.org. +// +// To use the library, copy the files canard_dsdl.c and canard_dsdl.h into the source tree of the application. +// No special compilation options are required. There are optional build configuration options defined near the top +// of canard_dsdl.c; they may be used to fine-tune the library for the target platform (but it is not necessary). +// +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. +// Author: Pavel Kirienko #ifndef CANARD_DSDL_H_INCLUDED #define CANARD_DSDL_H_INCLUDED @@ -31,7 +43,7 @@ typedef double CanardDSDLFloat64; /// buf Destination buffer where the result will be stored. /// off_bit Offset, in bits, from the beginning of the buffer. May exceed one byte. /// value The value itself (in case of integers it is promoted to 64-bit for unification). -/// len_bit Length of the serialized representation, in bits. Zero has no effect. Values above 64 are saturated. +/// len_bit Length of the serialized representation, in bits. Zero has no effect. Values above 64 bit are saturated. void canardDSDLSetBit(uint8_t* const buf, const size_t off_bit, const bool value); void canardDSDLSetUxx(uint8_t* const buf, const size_t off_bit, const uint64_t value, const uint8_t len_bit); void canardDSDLSetIxx(uint8_t* const buf, const size_t off_bit, const int64_t value, const uint8_t len_bit); @@ -44,10 +56,10 @@ void canardDSDLSetF64(uint8_t* const buf, const size_t off_bit, const CanardDSDL /// by the DSDL specification (see Implicit Zero Extension Rule, IZER). /// The floating point functions are only available if the target platform has an IEEE 754-compatible float model. /// -/// If len_bit is greater than the return type, extra bits will be truncated per regular narrowing conversion rules. -/// If len_bit is shorter than the return type, missing bits will be zero per regular integer promotion rules. +/// If len_bit is greater than the return type, extra bits will be truncated per standard narrowing conversion rules. +/// If len_bit is shorter than the return type, missing bits will be zero per standard integer promotion rules. /// Essentially, for integers, it would be enough to have 64-bit versions only; narrower variants exist only to avoid -/// narrowing type conversions of the result (and some small performance gains). +/// narrowing type conversions of the result and for some performance gains. /// /// The behavior is undefined if the input pointer is NULL. The time complexity is linear of the bit length. /// One-bit-wide signed integers are processed without raising an error but the result is unspecified. From 7b5fcc3f5c8dee81a8b546848a92bdf6386b9117 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 2 Mar 2020 22:22:21 +0200 Subject: [PATCH 091/100] Docs WIP --- libcanard/canard.c | 3 +- libcanard/canard.h | 340 ++++++++++++++++++++++++++------------------- tests/helpers.hpp | 8 ++ 3 files changed, 209 insertions(+), 142 deletions(-) diff --git a/libcanard/canard.c b/libcanard/canard.c index 2c1758f9..59cbfde8 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -400,8 +400,7 @@ CANARD_PRIVATE int32_t txPushMultiFrame(CanardInstance* const ins, ++out; const size_t frame_payload_size_with_tail = ((payload_size_with_crc - offset) < presentation_layer_mtu) - ? txRoundFramePayloadSizeUp((payload_size_with_crc - offset) + - 1U) // Add padding only in the last frame. + ? txRoundFramePayloadSizeUp((payload_size_with_crc - offset) + 1U) // Padding in the last frame only. : (presentation_layer_mtu + 1U); CanardInternalTxQueueItem* const tqi = txAllocateQueueItem(ins, can_id, deadline_usec, frame_payload_size_with_tail); diff --git a/libcanard/canard.h b/libcanard/canard.h index 83dcbb10..1534ffb1 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -27,7 +27,8 @@ // // ARCHITECTURE // -// +// The library is purely reactive: it does not perform any background processing and does not require periodic +// servicing. // // // @@ -139,6 +140,10 @@ extern const uint8_t CanardCANDLCToLength[16]; /// Conversion look-up table from data length to CAN DLC; the length is rounded up. extern const uint8_t CanardCANLengthToDLC[65]; +/// A UAVCAN transfer model (either incoming or outgoing). +/// Per Specification, a transfer is represented on the wire as a non-empty set of transport frames (i.e., CAN frames). +/// The library is responsible for serializing transfers into transport frames when transmitting, and reassembling +/// transfers from an incoming stream of frames during reception. typedef struct { /// For RX transfers: reception timestamp. @@ -155,59 +160,92 @@ typedef struct /// Subject-ID for message publications; service-ID for service requests/responses. CanardPortID port_id; - /// For outgoing message transfers or for incoming anonymous transfers, the value is CANARD_NODE_ID_UNSET. + /// For outgoing message transfers the value shall be CANARD_NODE_ID_UNSET (otherwise the state is invalid). + /// For outgoing service transfers this is the destination address (invalid if unset). + /// For incoming non-anonymous transfers this is the node-ID of the origin. + /// For incoming anonymous transfers the value is reported as CANARD_NODE_ID_UNSET. CanardNodeID remote_node_id; + /// When responding to a service request, the response transfer SHALL have the same transfer-ID value as the + /// request because the client will match the response with the request based on that. + /// + /// When publishing a message transfer, the value SHALL be one greater than the previous transfer under the same + /// subject-ID; the initial value should be zero. + /// + /// When publishing a service request transfer, the value SHALL be one greater than the previous transfer under + /// the same service-ID addressed to the same server node-ID; the initial value should be zero. + /// + /// Upon overflow, the value SHALL be reset back to zero. + /// Values above CANARD_TRANSFER_ID_MAX are permitted -- the library will compute the modulo automatically. + /// For received transfers, the values never exceed CANARD_TRANSFER_ID_MAX. + /// + /// A simple and robust way of managing transfer-ID counters is to keep a separate static variable per subject-ID + /// and per (service-ID, server-node-ID) pair. CanardTransferID transfer_id; - /// The const pointer makes it incompatible with free(), but we have to tolerate that due to the limitations of C. + /// This is the actual transfer payload. + /// If the payload is empty (payload_size = 0), the payload pointer may be NULL. + /// The const pointer makes it incompatible with memory deallocation function, this is due to the limitations of C; + /// therefore, when freeing the memory allocated for the payload, cast away the pointer's const qualifier. size_t payload_size; const void* payload; } CanardTransfer; /// Transfer subscription state. The application can register its interest in a particular kind of data exchanged /// over the bus by creating such subscription objects. Frames that carry data for which there is no active -/// subscription will be dropped by the library. +/// subscription will be silently dropped by the library. /// /// WARNING: SUBSCRIPTION INSTANCES SHALL NOT BE COPIED OR MUTATED BY THE APPLICATION. -/// Every field is named starting with an underscore to emphasize that the application shall not mess with it. +/// +/// Every field is named starting with an underscore to emphasize that the application shall not modify it. /// Unfortunately, C, being such a limited language, does not allow us to construct a better API. /// /// The memory footprint of a subscription is large. On a 32-bit platform it slightly exceeds half a KiB. -/// This is what we call a time-memory trade-off: we use a large look-up table to ensure deterministic runtime behavior. +/// This is an intentional time-memory trade-off: use a large look-up table to ensure predictable temporal properties. typedef struct CanardRxSubscription { - struct CanardRxSubscription* _next; + struct CanardRxSubscription* _next; ///< Internal use only. - /// The current architecture is a sort of an acceptable middle ground between worst-case execution time and memory + /// The current architecture is an acceptable middle ground between worst-case execution time and memory /// consumption. Instead of statically pre-allocating a dedicated RX session for each remote node-ID here in /// this table, we only keep pointers, which are NULL by default, populating a new RX session dynamically - /// on an ad-hoc basis when we first receive a transfer from that node. This is still deterministic because our - /// memory allocation routines are assumed to be deterministic and we make at most one allocation per remote node, - /// but the disadvantage is that these additional operations increase the upper bound on the execution time. + /// on an ad-hoc basis when we first receive a transfer from that node. This is deterministic because our memory + /// allocation routines are assumed to be deterministic and we make at most one allocation per remote node, + /// but the disadvantage is that these additional operations lift the upper bound on the execution time. /// Further, the pointers here add an extra indirection, which is bad for systems that leverage cached memory, /// plus a pointer itself takes about 2-8 bytes of memory, too. /// /// A far more predictable and a much simpler approach is to pre-allocate states here statically instead of keeping /// just pointers, but it would push the size of this instance from about 0.5 KiB to ~3 KiB for a typical 32-bit /// system. Since this is a general-purpose library, we have to pick a middle ground so we use the more complex - /// but more memory-efficient approach. Implementations that are more optimized for low-jitter real-time - /// applications may prefer the other approach. + /// but more memory-efficient approach. struct CanardInternalRxSession* _sessions[CANARD_NODE_ID_MAX + 1U]; - CanardMicrosecond _transfer_id_timeout_usec; - size_t _payload_size_max; - CanardPortID _port_id; + CanardMicrosecond _transfer_id_timeout_usec; ///< Internal use only. + size_t _payload_size_max; ///< Internal use only. + CanardPortID _port_id; ///< Internal use only. } CanardRxSubscription; +/// A pointer to the memory allocation function. The semantics are similar to malloc(): +/// - The returned pointer shall point to an uninitialized block of memory that is at least "amount" bytes large. +/// - If there is not enough memory, the returned pointer shall be NULL. +/// - The memory shall be aligned at least at max_align_t. +/// - The execution time should be constant (O(1)). +/// - The worst-case memory fragmentation should be bounded and easily predictable. +/// If the standard dynamic memory manager of the target platform does not satisfy the above requirements, +/// consider using O1Heap: https://github.com/pavel-kirienko/o1heap. typedef void* (*CanardMemoryAllocate)(CanardInstance* ins, size_t amount); -/// Free as in freedom. +/// The counterpart of the above -- this function is invoked to return previously allocated memory to the allocator. +/// The semantics are similar to free(): +/// - The pointer was previously returned by the allocation function. +/// - The pointer may be NULL, in which case the function shall have no effect. +/// - The execution time should be constant (O(1)). typedef void (*CanardMemoryFree)(CanardInstance* ins, void* pointer); /// This is the core structure that keeps all of the states and allocated resources of the library instance. -/// Fields whose names begin with an underscore SHALL NOT be accessed by the application, -/// they are for internal use only. +/// The application may directly alter the fields whose names do not begin with an underscore. +/// Fields whose names begin with an underscore SHALL NOT be accessed by the application. struct CanardInstance { /// User pointer that can link this instance with other objects. @@ -215,31 +253,30 @@ struct CanardInstance /// The default value is NULL. void* user_reference; - /// The maximum transmission unit. The value can be changed arbitrarily at any time. + /// The transport-layer maximum transmission unit (MTU). The value can be changed arbitrarily at any time. /// This setting defines the maximum number of bytes per CAN data frame in all outgoing transfers. /// Regardless of this setting, CAN frames with any MTU can always be accepted. /// /// Only the standard values should be used as recommended by the specification; - /// otherwise, networking interoperability issues may arise. See "CANARD_MTU_*". - /// Valid values are any valid CAN frame data length not smaller than 8. The default is the maximum valid value. - /// Invalid values are treated as the nearest valid value. + /// otherwise, networking interoperability issues may arise. See recommended values CANARD_MTU_*. + /// + /// Valid values are any valid CAN frame data length value not smaller than 8. + /// Invalid values are treated as the nearest valid value. The default is the maximum valid value. size_t mtu_bytes; - /// The node-ID of the local node. The default value is CANARD_NODE_ID_UNSET. + /// The node-ID of the local node. /// Per the UAVCAN Specification, the node-ID should not be assigned more than once. - /// Invalid values are treated as CANARD_NODE_ID_UNSET. + /// Invalid values are treated as CANARD_NODE_ID_UNSET. The default value is CANARD_NODE_ID_UNSET. CanardNodeID node_id; - /// Callbacks invoked by the library. See their type documentation for details. + /// Dynamic memory management callbacks. See their type documentation for details. /// They SHALL be valid function pointers at all times. + /// The time complexity models given in the API documentation are made on the assumption that the memory management + /// functions have constant complexity O(1). /// - /// The time complexity parameters given in the API documentation are made on the assumption that - /// the memory management functions (allocate and free) have constant time complexity O(1). - /// - /// There are only two API functions that may lead to allocation of memory: - /// - canardTxPush() - /// - canardRxAccept() - /// Their exact memory requirement model is specified in their documentation. + /// The following API functions may allocate memory: canardRxAccept(), canardTxPush(). + /// The following API functions may deallocate memory: canardRxAccept(), canardRxSubscribe(), canardRxUnsubscribe(). + /// The exact memory requirement model is specified for each function in its documentation. CanardMemoryAllocate memory_allocate; CanardMemoryFree memory_free; @@ -248,22 +285,25 @@ struct CanardInstance struct CanardInternalTxQueueItem* _tx_queue; }; -/// Initialize a new library instance. +/// Construct a new library instance. /// The default values will be assigned as specified in the structure field documentation. -/// The time complexity parameters given in the API documentation are made on the assumption that the memory management -/// functions (allocate and free) have constant complexity. /// If any of the pointers are NULL, the behavior is undefined. /// -/// The instance does not hold any resources itself except the allocated memory. +/// The instance does not hold any resources itself except for the allocated memory. /// If the instance should be de-initialized, the application shall clear the TX queue by calling the pop function /// repeatedly, and remove all RX subscriptions. Once that is done, the instance will be holding no memory resources, /// so it can be discarded freely. +/// +/// The time complexity is constant. This function does not invoke the dynamic memory manager. CanardInstance canardInit(const CanardMemoryAllocate memory_allocate, const CanardMemoryFree memory_free); -/// Serializes a transfer into a sequence of transport frames, and inserts them into the prioritized transmission -/// queue at the appropriate position. Afterwards, the application is supposed to take the enqueued frames from -/// the transmission queue using the function canardTxPeek() and transmit them. Each transmitted (or otherwise -/// discarded, e.g., due to timeout) frame should be removed from the queue using canardTxPop(). +/// This function serializes a transfer into a sequence of transport frames and inserts them into the prioritized +/// transmission queue at the appropriate position. Afterwards, the application is supposed to take the enqueued frames +/// from the transmission queue using the function canardTxPeek() and transmit them. Each transmitted (or otherwise +/// discarded, e.g., due to timeout) frame should be removed from the queue using canardTxPop(). The queue is +/// prioritized following the normal CAN frame arbitration rules to avoid the inner priority inversion. The transfer +/// payload will be copied into the transmission queue so that the lifetime of the frames is not related to the +/// lifetime of the input transfer instance or its payload buffer. /// /// The MTU of the generated frames is dependent on the value of the MTU setting at the time when this function /// is invoked. The MTU setting can be changed arbitrarily between invocations. No other functions rely on that @@ -273,25 +313,11 @@ CanardInstance canardInit(const CanardMemoryAllocate memory_allocate, const Cana /// frames (so all frames will have the same timestamp value). This feature is intended to facilitate transmission /// deadline tracking, i.e., aborting frames that could not be transmitted before the specified deadline. /// Therefore, normally, the timestamp value should be in the future. -/// The library itself, however, does not use or check this value itself, so it can be zero if not needed. -/// -/// It is the responsibility of the application to ensure that the transfer parameters are managed correctly. -/// In particular, the application shall ensure that the transfer-ID computation rules, as defined in the -/// UAVCAN Specification, are being followed; here is a relevant excerpt: -/// - For service response transfers the transfer-ID value shall be directly copied from the corresponding -/// service request transfer. -/// - A node that is interested in emitting message transfers or service request transfers under a particular -/// session specifier, whether periodically or on an ad-hoc basis, shall allocate a transfer-ID counter state -/// associated with said session specifier exclusively. The transfer-ID value of every emitted transfer is -/// determined by sampling the corresponding counter keyed by the session specifier of the transfer; afterwards, -/// the counter is incremented by one. -/// The recommended approach to storing the transfer-ID counters is to use static or member variables. -/// Sophisticated applications may find this approach unsuitable, in which case O(1) static look-up tables -/// or other deterministic data structures should be considered. -/// -/// Returns the number of frames enqueued into the prioritized TX queue (which is always a positive number) -/// in case of success (so that the application can track the number of items in the TX queue if necessary). -/// Returns a negated error code in case of failure. Zero is never returned. +/// The library itself, however, does not use or check this value in any way, so it can be zero if not needed. +/// +/// The function returns the number of frames enqueued into the prioritized TX queue (which is always a positive +/// number) in case of success (so that the application can track the number of items in the TX queue if necessary). +/// In case of failure, the function returns a negated error code: either invalid argument or out-of-memory. /// /// An invalid argument error may be returned in the following cases: /// - Any of the input arguments are NULL. @@ -307,136 +333,161 @@ CanardInstance canardInit(const CanardMemoryAllocate memory_allocate, const Cana /// (i.e., the modulo is computed automatically, so the caller doesn't have to bother). /// /// An out-of-memory error is returned if a TX frame could not be allocated due to the memory being exhausted. -/// In that case, all previously allocated frames will be purged automatically. -/// In other words, either all frames of the transfer are enqueued successfully, or none are. +/// In that case, all previously allocated frames will be deallocated automatically. In other words, either all frames +/// of the transfer are enqueued successfully, or none are. /// -/// The time complexity is O(s+t), where s is the amount of payload in the transfer, and t is the number of -/// frames already enqueued in the transmission queue. +/// The time complexity is O(p+e), where p is the amount of payload in the transfer, and e is the number of frames +/// already enqueued in the transmission queue. /// -/// The memory allocation requirement is one allocation per transport frame. -/// A single-frame transfer takes one allocation; a multi-frame transfer of N frames takes N allocations. -/// The maximum size of each allocation is sizeof(CanardFrame) plus a pointer size plus MTU size. +/// The memory allocation requirement is one allocation per transport frame. A single-frame transfer takes one +/// allocation; a multi-frame transfer of N frames takes N allocations. The maximum size of each allocation is +/// (sizeof(CanardFrame) + sizeof(void*) + MTU). int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfer); -/// Access the top element of the prioritized transmission queue. The queue itself is not modified (i.e., the -/// accessed element is not removed). The application should invoke this function to collect the transport frames -/// of serialized transfers stored into the prioritized transmission queue by canardTxPush(). +/// This function accesses the top element of the prioritized transmission queue. The queue itself is not modified +/// (i.e., the accessed element is not removed). The application should invoke this function to collect the transport +/// frames of serialized transfers pushed into the prioritized transmission queue by canardTxPush(). /// /// Nodes with redundant transports should replicate every frame into each of the transport interfaces. /// Such replication may require additional buffering in the driver layer, depending on the implementation. /// /// The timestamp values of returned frames are initialized with the timestamp value of the transfer instance they /// originate from. Timestamps are used to specify the transmission deadline. It is up to the application and/or -/// the driver layer to implement the discardment of timed-out transport frames. The library does not check it, +/// the media layer to implement the discardment of timed-out transport frames. The library does not check it, /// so a frame that is already timed out may be returned here. /// /// If the queue is empty or if the argument is NULL, the returned value is NULL. /// /// If the queue is non-empty, the returned value is a pointer to its top element (i.e., the next frame to transmit). -/// The returned pointer points to an object allocated in the dynamic storage; it should be freed by the application -/// by calling CanardInstance::memory_free(). The memory shall not be freed before the entry is removed from the -/// queue by calling canardTxPop(); this is because until canardTxPop() is executed, the library retains ownership -/// of the object. The payload pointer retains validity until explicitly freed by the application; in other words, +/// The returned pointer points to an object allocated in the dynamic storage; it should be eventually freed by the +/// application by calling CanardInstance::memory_free(). The memory shall not be freed before the entry is removed +/// from the queue by calling canardTxPop(); this is because until canardTxPop() is executed, the library retains +/// ownership of the object. The pointer retains validity until explicitly freed by the application; in other words, /// calling canardTxPop() does not invalidate the object. /// /// The payload buffer is located shortly after the object itself, in the same memory fragment. The application shall -/// not attempt to free its memory. +/// not attempt to free it. /// -/// The time complexity is constant. +/// The time complexity is constant. This function does not invoke the dynamic memory manager. const CanardFrame* canardTxPeek(const CanardInstance* const ins); -/// Transfer the ownership of the top element of the prioritized transmission queue to the application. +/// This function transfers the ownership of the top element of the prioritized transmission queue to the application. /// The application should invoke this function to remove the top element from the prioritized transmission queue. -/// While the element is removed, it is not invalidated: it is the responsibility of the application to deallocate +/// The element is removed but it is not invalidated: it is the responsibility of the application to deallocate /// the memory used by the object later. The object SHALL NOT be deallocated UNTIL this function is invoked. /// /// WARNING: /// Invocation of canardTxPush() may add new elements at the top of the prioritized transmission queue. -/// The calling code shall take that into account to eliminate the possibility of data loss due to the frame -/// at the top of the queue being unexpectedly replaced between calls of canardTxPeek() and this function. +/// The calling code shall take that into account to eliminate the possibility of data loss and memory leak due to +/// the frame at the top of the queue being unexpectedly replaced between calls of canardTxPeek() and this function. /// /// If the input argument is NULL or if the transmission queue is empty, the function has no effect. /// -/// The time complexity is constant. +/// The time complexity is constant. This function does not invoke the dynamic memory manager. void canardTxPop(CanardInstance* const ins); +/// This function implements the transfer reassembly logic. It accepts a transport frame, locates the appropriate +/// subscription state, and, if found, updates it. If the frame completed a transfer, the return value is 1 (one) +/// and the out_transfer pointer is populated with the parameters of the newly reassembled transfer. The transfer +/// reassembly logic is defined in the UAVCAN specification. +/// +/// The MTU of the accepted frame is not limited and is not dependent on the MTU setting of the local node; +/// that is, any MTU is accepted. The DLC compliance is also not checked. +/// +/// Any value of redundant_transport_index is accepted; that is, up to 256 redundant transports are supported. +/// The index of the transport from which the transfer is accepted is always the same as redundant_transport_index +/// of the current invocation, so the application can always determine which transport has delivered the transfer. +/// +/// The function invokes the dynamic memory manager in the following cases only: +/// +/// 1. New memory for a session state object is allocated when a new session is initiated. +/// This event occurs when a transport frame that matches a known subscription is received from a node that +/// did not emit matching frames since the subscription was created. +/// Once a new session is created, it is not destroyed until the subscription is terminated by invoking +/// canardRxUnsubscribe(). The number of sessions is bounded and the bound is low (at most the number of nodes +/// in the network minus one), also the size of a session instance is very small, so the removal is unnecessary. +/// Real-time networks typically do not change their configuration at runtime, so it is possible to reduce +/// the time complexity by never deallocating sessions. +/// The size of a session instance is at most 48 bytes on any conventional platform (typically much smaller). +/// +/// 2. New memory for the transfer payload buffer is allocated when a new transfer is initiated. +/// This event occurs when a transport frame that matches a known subscription is received and it begins a +/// new transfer (that is, the start-of-frame flag is set and it is not a duplicate). +/// The amount of the allocated memory is payload_size_max as configured via canardRxSubscribe(). +/// The worst case occurs when every node on the bus initiates a multi-frame transfer for which there is a +/// matching subscription: in this case, the library will allocate number_of_nodes allocations of size +/// payload_size_max. +/// +/// 3. Memory allocated for the transfer payload buffer may be deallocated at the discretion of the library. +/// This operation does not increase the worst case execution time and does not improve the worst case memory +/// consumption, so a deterministic application need not consider this behavior in the resource analysis. +/// This behavior is implemented for the benefit of applications where rigorous characterization is unnecessary. +/// +/// The worst case dynamic memory consumption per subscription is: +/// +/// (sizeof(session instance) + payload_size_max) * number_of_nodes +/// +/// Where sizeof(session instance) and payload_size_max are defined above, and number_of_nodes is the number of remote +/// nodes emitting transfers that match the subscription. If the dynamic memory pool is sized correctly, the +/// application is guaranteed to never encounter an out-of-memory (OOM) error at runtime. The actual size of the +/// dynamic memory pool is typically larger; for a detailed treatment of the problem and the related theory please +/// refer to the documentation of O1Heap -- a deterministic memory allocator for hard real-time embedded systems. +/// +/// The time complexity is O(n+p) where n is the number of subject-IDs or service-IDs subscribed to by the application, +/// depending on the transfer kind of the supplied frame, and p is the amount of payload in the received frame +/// (because it will be copied into an internal contiguous buffer). Observe that the time complexity is invariant to +/// the network configuration (such as the number of online nodes) -- this is a very important design guarantee for +/// real-time applications because the execution time is dependent only on the number of active subscriptions for +/// a given transfer kind, and the MTU, both of which are easy to predict and account for. Excepting the +/// subscription search and the payload data copying, the entire RX pipeline contains neither loops nor recursion. +/// Misaddressed and malformed frames are discarded in constant time. +/// +/// The function returns 1 (one) if the new frame completed a transfer. In this case, the details of the transfer +/// are stored into out_transfer, and the transfer payload buffer ownership is passed to that object. The lifetime +/// of the resulting transfer object is not related to the lifetime of the input transport frame (that is, even if +/// it is a single-frame transfer, its payload is copied out into a new dynamically allocated buffer storage). +/// If the payload_size_max is zero, the payload pointer will be NULL, since there is no data to store and so a +/// buffer is not needed. The application is responsible for deallocating the payload buffer when the processing +/// is done by invoking memory_free on the transfer payload pointer. +/// +/// The function returns a negated out-of-memory error if it was unable to allocate dynamic memory. +/// /// The function does nothing and returns a negated invalid argument error immediately if any condition is true: /// - Any of the input arguments that are pointers are NULL. /// - The payload pointer of the input frame is NULL while its size is non-zero. /// - The CAN ID of the input frame is not less than 2**29=0x20000000. /// -/// The function returns zero if any of the following conditions are true; the general policy is that protocol errors -/// are not escalated because they do not construe a node-local error: -/// - The received frame is not a valid UAVCAN/CAN transport frame; in this case the frame is silently ignored. -/// - The received frame is a valid UAVCAN/CAN transport frame, but it belongs to a session that is not -/// relevant to the application (i.e., the application reported via the RX filter callback that the library -/// need not receive transfers from this session). -/// - The received frame is a valid UAVCAN/CAN transport frame, but it did not complete a transfer, or it forms -/// an invalid frame sequence. +/// The function drops the frame and returns zero if any of the following conditions are true (the general policy is +/// that protocol errors are not escalated because they do not construe a node-local error): +/// - The received frame is not a valid UAVCAN/CAN transport frame. +/// - The received frame is a valid UAVCAN/CAN transport frame, but there is no matching subscription, +/// the frame did not complete a transfer, the frame forms an invalid frame sequence, the frame is a duplicate, +/// the frame is unicast to a different node (address mismatch). /// -/// The function returns 1 (one) if the new frame completed a transfer. In this case, the details of the transfer -/// are stored into out_transfer, and the payload ownership is passed into that object. This means that the application -/// is responsible for deallocating the payload buffer when the processing is done by invoking memory_free. -/// This design is chosen to facilitate almost zero-copy data exchange across the protocol stack: once a buffer is +/// The function is designed to facilitate almost zero-copy data exchange across the protocol stack: once a buffer is /// allocated, its data is never copied around but only passed by reference. This design allows us to reduce the -/// worst-case execution time and reduce jitter caused by the linear time complexity of memcpy(). -/// There is a special case, however: if the payload_size_max is zero, the payload pointer will be NULL, since there -/// is no data to store and so a buffer is not needed. -/// -/// One data copy still has to take place, though: it's the copy from the frame payload into the contiguous buffer. +/// worst-case execution time and reduce the jitter caused by the linear time complexity of memcpy(). +/// One data copy still has to take place, though: from the frame payload into the contiguous transfer payload buffer. /// In CAN, the MTU is small (at most 64 bytes for CAN FD), so the extra copy does not cost us much here, /// but it allows us to completely decouple the lifetime of the input frame buffer from the lifetime of the final /// transfer object, regardless of whether it's a single-frame or a multi-frame transfer. /// If we were building, say, an UAVCAN/UDP library, then we would likely resort to a different design, where the /// frame buffer is allocated once from the heap (which may be done from the interrupt handler if the heap is -/// sufficiently deterministic; an example of a suitable real-time heap is https://github.com/pavel-kirienko/o1heap), -/// and in the case of single-frame transfer it is then carried over to the application without copying. -/// This design somewhat complicates the driver layer though. -/// -/// The MTU of the accepted frame is not limited and is not dependent on the MTU setting of the local node; -/// that is, any MTU is accepted. The DLC compliance is not checked -- payload of any length (unlimited) is accepted. -/// -/// Any value of redundant_transport_index is accepted; that is, up to 256 redundant transports are supported. -/// The index of the transport from which the transfer is accepted is always the same as redundant_transport_index. -/// -/// The time complexity is O(n+s) where n is the number of subject-IDs or service-IDs subscribed to by the application, -/// depending on whether the frame is of the message kind of of the service kind, and s is the amount of payload in the -/// received transport frame (because it will be copied into an internal contiguous buffer). -/// Observe that the time complexity is invariant to the network configuration (such as the number of online nodes), -/// which is an important design guarantee for real-time applications. -/// The execution time is only dependent on the number of active subscriptions for a given transfer kind, -/// and the MTU, both of which are easy to predict and account for. Excepting the subscription search and the -/// payload data copying, the entire RX pipeline contains neither loops nor recursion. -/// -/// Unicast frames where the destination does not equal the local node-ID are discarded in constant time. -/// Frames that are not valid UAVCAN/CAN frames are discarded in constant time. -/// -/// MEMORY ALLOCATION REQUIREMENT MODEL. +/// sufficiently deterministic), and in the case of single-frame transfer it is then carried over to the application +/// without copying. This design somewhat complicates the media layer though. int8_t canardRxAccept(CanardInstance* const ins, const CanardFrame* const frame, const uint8_t redundant_transport_index, CanardTransfer* const out_transfer); -/// Subscription instances have large look-up tables to ensure that the temporal properties of the algorithms are -/// invariant to the network configuration (i.e., a node that is validated on a network containing one other node -/// will provably perform identically on a network that contains X nodes). -/// See for context: https://github.com/UAVCAN/libuavcan/issues/185#issuecomment-440354858. -/// This is a conscious time-memory trade-off. It may have adverse effects on RAM-constrained applications, -/// but this is considered tolerable because it is expected that the types of applications leveraging Libcanard -/// will be either real-time function nodes where time determinism is critical, or bootloaders where time determinism -/// is usually not required but the amount of available memory is not an issue (the main constraint is ROM, not RAM). +/// This function creates a new subscription, allowing the application to register its interest in a particular +/// category of transfers. The library will reject all transport frames for which there is no active subscription. /// /// If such subscription already exists, it will be removed first as if canardRxUnsubscribe() was /// invoked by the application, and then re-created anew with the new parameters. /// -/// Once a new RX session is allocated, it will never be removed as long as the subscription is active. -/// The rationale for this behavior is that real-time networks typically do not change their configuration at runtime; -/// hence, it is possible to reduce the worst-case computational complexity of the library routines by never -/// deallocating sessions once allocated. The size of an RX state is at most 48 bytes on any conventional platform. -/// If this behavior is found to be undesirable, the application can force deallocation of all unused states by -/// re-creating the subscription anew. -/// /// The transport fail-over timeout (if redundant transports are used) is the same as the transfer-ID timeout. +/// It may be reduced in a future release of the library, but it will not affect backward compatibility. /// /// The return value is 1 if a new subscription has been created as requested. /// The return value is 0 if such subscription existed at the time the function was invoked. In this case, @@ -445,6 +496,15 @@ int8_t canardRxAccept(CanardInstance* const ins, /// /// The time complexity is linear from the number of current subscriptions under the specified transfer kind. /// This function does not allocate new memory. The function may deallocate memory if such subscription already existed. +/// +/// Subscription instances have large look-up tables to ensure that the temporal properties of the algorithms are +/// invariant to the network configuration (i.e., a node that is validated on a network containing one other node +/// will provably perform identically on a network that contains X nodes). +/// Some background is available at https://github.com/UAVCAN/libuavcan/issues/185#issuecomment-440354858. +/// This is a conscious time-memory trade-off. It may have adverse effects on RAM-constrained applications, +/// but this is considered tolerable because it is expected that the types of applications leveraging Libcanard +/// will be either real-time function nodes where time determinism is critical, or bootloaders where time determinism +/// is usually not required but the amount of available memory is not an issue (the main constraint is ROM, not RAM). int8_t canardRxSubscribe(CanardInstance* const ins, const CanardTransferKind transfer_kind, const CanardPortID port_id, @@ -452,9 +512,9 @@ int8_t canardRxSubscribe(CanardInstance* const ins, const CanardMicrosecond transfer_id_timeout_usec, CanardRxSubscription* const out_subscription); -/// Reverse the effect of canardRxSubscribe(). -/// If the subscription is found, all its memory is de-allocated; to determine the amount of memory freed, -/// please refer to the memory allocation requirement model of canardRxAccept(). +/// This function reverses the effect of canardRxSubscribe(). +/// If the subscription is found, all its memory is de-allocated (session states and payload buffers); to determine +/// the amount of memory freed, please refer to the memory allocation requirement model of canardRxAccept(). /// /// The return value is 1 if such subscription existed (and, therefore, it was removed). /// The return value is 0 if such subscription does not exist. In this case, the function has no effect. diff --git a/tests/helpers.hpp b/tests/helpers.hpp index 7809f78e..431a3d27 100644 --- a/tests/helpers.hpp +++ b/tests/helpers.hpp @@ -13,6 +13,14 @@ #include #include +#if !(defined(CANARD_VERSION_MAJOR) && defined(CANARD_VERSION_MINOR)) +# error "Library version not defined" +#endif + +#if !(defined(CANARD_UAVCAN_SPECIFICATION_VERSION_MAJOR) && defined(CANARD_UAVCAN_SPECIFICATION_VERSION_MINOR)) +# error "UAVCAN specification version not defined" +#endif + namespace helpers { namespace dummy_allocator From 42664f4eda6dd5dc83097f614ce0ac003acac3c5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 2 Mar 2020 23:03:13 +0200 Subject: [PATCH 092/100] Finished the header file documentation; README next --- libcanard/canard.h | 63 ++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 58 insertions(+), 5 deletions(-) diff --git a/libcanard/canard.h b/libcanard/canard.h index 1534ffb1..41f6bb26 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -1,4 +1,4 @@ -// LIBCANARD +// LIBCANARD // // Libcanard is a compact implementation of the UAVCAN/CAN protocol for high-integrity real-time embedded systems. // It is designed for use in robust deterministic embedded systems equipped with at least 32K ROM and 4K RAM. @@ -6,7 +6,7 @@ // The library is designed to be compatible with any target platform and instruction set architecture, from 8 to 64 bit, // little- and big-endian, RTOS-based or bare metal, etc., as long as there is a standards-compliant C11 compiler. // -// INTEGRATION +// INTEGRATION // // The library is intended to be integrated into the end application by simply copying the file canard.c into the // source tree of the project; it does not require any special compilation options and should work out of the box. @@ -25,13 +25,61 @@ // If your application requires a MISRA C compliance report, please get in touch with the maintainers via the forum // at https://forum.uavcan.org. // -// ARCHITECTURE +// ARCHITECTURE // -// The library is purely reactive: it does not perform any background processing and does not require periodic -// servicing. +// UAVCAN, as a protocol stack, is composed of two layers: TRANSPORT and PRESENTATION. The transport layer is portable +// across different transport protocols, one of which is CAN (FD), formally referred to as UAVCAN/CAN. This library +// is focused on UAVCAN/CAN only and it will not support other transports. The presentation layer is implemented +// through the DSDL language and the associated data type regulation policies. Much like the UAVCAN stack itself, +// this library consists of two major components: +// +// 1. TRANSPORT -- the UAVCAN/CAN transport layer implementation. This is implemented in canard.c/.h, +// the documentation for which you are currently reading. This is the core part of the library. +// +// 2. PRESENTATION -- the optional DSDL support extension library. This is implemented in canard_dsdl.c/.h, +// an optional component which may be used by some applications where automatic DSDL code generation is +// not used. Normally, applications may prefer to rely on auto-generated code using DSDL-to-C translators +// such as Nunavut (https://github.com/UAVCAN/nunavut). +// +// The DSDL extension is trivial and there is not much to document -- please refer to its header file for details. +// +// This transport layer implementation consists of two components: the transmission (TX) pipeline and the +// reception (RX) pipeline. +// +// The TX and RX pipelines are completely independent from each other except that they both rely on the same +// dynamic memory manager. The TX pipeline uses the dynamic memory to store outgoing CAN frames in the prioritized +// transmission queue. The RX pipeline uses the dynamic memory to store contiguous payload buffers for received +// transfers and for keeping the transfer reassembly state machine data. The exact memory consumption model is defined +// for both pipelines, so it is possible to statically determine the minimum size of the dynamic memory pool required +// to guarantee that a given application will never encounter an out-of-memory error at runtime. // +// Much like with dynamic memory, the time complexity of every API function is well-characterized, allowing the +// application to guarantee predictable real-time performance. // +// The TX pipeline is managed with the help of three API functions. When the application needs to emit a transfer, +// it invokes canardTxPush(). The function splits the transfer into CAN frames and stores them into the prioritized +// transmission queue. The application then picks the CAN frames from the queue one-by-one by calling canardTxPeek() +// followed by canardTxPop() -- the former allows the application to look at the frame and the latter tells the library +// that the frame shall be removed from the queue. The returned frames need to be deallocated by the application. // +// The RX pipeline is managed with the help of three API functions. The main function canardRxAccept() takes a +// received CAN frame and updates the appropriate transfer reassembly state machine. The functions canardRxSubscribe() +// and its counterpart canardRxUnsubscribe() instruct the library which transfers should be received (by default, all +// transfers are ignored); also the subscription function specifies vital transfer reassembly parameters such as the +// maximum payload size (i.e., the maximum size of a serialized representation of a DSDL object) and the transfer-ID +// timeout. Transfers that carry more payload than the configured maximum per subscription are truncated following the +// Implicit Truncation Rule (ITR) defined by the UAVCAN Specification -- the rule is implemented to facilitate +// backward-compatible DSDL data type extensibility. +// +// The library supports an unlimited number of redundant interfaces. +// +// The library is not thread-safe: if used in a concurrent environment, it is the responsibility of the application +// to provide adequate synchronization. +// +// The library is purely reactive: it does not perform any background processing and does not require periodic +// servicing. Its internal state is only updated as a response to well-specified explicit API calls. +// +// -------------------------------------------------------------------------------------------------------------------- // // This software is distributed under the terms of the MIT License. // Copyright (c) 2016-2020 UAVCAN Development Team. @@ -486,6 +534,11 @@ int8_t canardRxAccept(CanardInstance* const ins, /// If such subscription already exists, it will be removed first as if canardRxUnsubscribe() was /// invoked by the application, and then re-created anew with the new parameters. /// +/// The payload_size_max defines the size of the transfer payload memory buffer. Transfers that carry larger payloads +/// will be accepted but the excess payload will be truncated, as mandated by the Specification. This behavior is +/// called the Implicit Truncation Rule (ITR) and it is intended to facilitate extensibility of data types while +/// preserving backward compatibility. The transfer CRC is validated regardless of whether its payload is truncated. +/// /// The transport fail-over timeout (if redundant transports are used) is the same as the transfer-ID timeout. /// It may be reduced in a future release of the library, but it will not affect backward compatibility. /// From d0da584c9502419742b4d5a6aedb45d30c757ff8 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 2 Mar 2020 23:29:40 +0200 Subject: [PATCH 093/100] README rework --- CONTRIBUTING.md | 15 +++++++++++++ README.md | 53 ++++++++-------------------------------------- libcanard/canard.h | 3 +++ 3 files changed, 27 insertions(+), 44 deletions(-) create mode 100644 CONTRIBUTING.md diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000..f4139bd5 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,15 @@ +# Libcanard contribution guide + +## Standards + +MISRA. + +[Zubax C++ Coding Conventions](https://kb.zubax.com/x/84Ah) shall be followed. + +## IDE + +The recommended development environment is JetBrains CLion. The root project file is `tests/CMakeLists.txt`. + +## Testing + +TODO diff --git a/README.md b/README.md index 8b5afa8d..3e181a0b 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# Libcanard +# Compact UAVCAN/CAN v1 in C [![Build Status](https://travis-ci.org/UAVCAN/libcanard.svg?branch=master)](https://travis-ci.org/UAVCAN/libcanard) [![Quality Gate Status](https://sonarcloud.io/api/project_badges/measure?project=libcanard&metric=alert_status)](https://sonarcloud.io/dashboard?id=libcanard) @@ -7,50 +7,15 @@ [![Lines of Code](https://sonarcloud.io/api/project_badges/measure?project=libcanard&metric=ncloc)](https://sonarcloud.io/dashboard?id=libcanard) [![Forum](https://img.shields.io/discourse/users.svg?server=https%3A%2F%2Fforum.uavcan.org&color=1700b3)](https://forum.uavcan.org) -A compact implementation of the UAVCAN protocol stack in C11 for high integrity real-time embedded systems. +Libcanard is a compact implementation of the UAVCAN/CAN protocol stack in C11 for high-integrity real-time +embedded systems. -Ask questions on the **[UAVCAN Forum](https://forum.uavcan.org)**. +[UAVCAN](https://uavcan.org) is an open lightweight data bus standard designed for reliable intravehicular +communication in aerospace and robotic applications via CAN bus, Ethernet, and other robust transports. +The acronym UAVCAN stands for *Uncomplicated Application-level Vehicular Communication And Networking*. -## Usage +**READ THE DOCS: [`libcanard/canard.h`](/libcanard/canard.h)** -To integrate the library into your project, just copy the two files `canard.c` & `canard.h` -(find them under `libcanard/`) into your project tree. -Either keep them in the same directory, or make sure that the directory that contains the header -is added to the set of include look-up paths. -No special compiler options are needed to compile the source file (if you find this to be untrue, please open a ticket). +**Contribute: [`CONTRIBUTING.md`](/CONTRIBUTING.md)** -There is no dedicated documentation for the library API, because it is simple enough to be self-documenting. -Please check out the explanations provided in the comments in the header file to learn the basics. -Most importantly, check out the demo application under `tests/demo.c`. -Also use [code search to find real life usage examples](https://github.com/search?q=libcanard&type=Code&utf8=%E2%9C%93). - -### Media layer - -The existing platform drivers should be used as a reference for implementation of one's own custom drivers. -Libcanard does not interact with the underlying platform drivers directly; it does so via the application. -Therefore, there is no need for a dedicated porting guide. -This is unlike Libuavcan, which is more complex and does have a well-defined interface between -the library and the platform. - - +-----------------------------------+ - | Application | - +-------+-------------------+-------+ - | | - +-------+-------+ +-------+-------+ - | Media layer | | Libcanard | - +-------+-------+ +---------------+ - | - +---------------+ - | I/O | - +---------------+ - - -## Library development - -This section is intended only for library developers and contributors. - -Contributors, please follow the [Zubax C++ Coding Conventions](https://kb.zubax.com/x/84Ah). - -### Testing - -Please refer to the CI/CD automation scripts for instructions. +**Ask questions: [forum.uavcan.org](https://forum.uavcan.org)** diff --git a/libcanard/canard.h b/libcanard/canard.h index 41f6bb26..187bb03d 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -22,6 +22,9 @@ // this core library to simplify DSDL object serialization and deserialization. It is intended to be integrated in // the same manner. Please read its usage manual for further information. // +// There are no specific requirements to the underlying I/O layer. Some low-level drivers maintained by the +// UAVCAN Development Team may be found at https://github.com/UAVCAN/platform_specific_components. +// // If your application requires a MISRA C compliance report, please get in touch with the maintainers via the forum // at https://forum.uavcan.org. // From 691f2cad86b3235c3fb17a7bfc514a28d62f2583 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 3 Mar 2020 03:05:50 +0200 Subject: [PATCH 094/100] It's done --- .gitignore | 8 +- CONTRIBUTING.md | 96 ++++++++++++++- README.md | 151 +++++++++++++++++++++++- libcanard/canard.h | 180 ++++++++++++++--------------- tests/.idea/dictionaries/pavel.xml | 77 ++++++++++++ 5 files changed, 413 insertions(+), 99 deletions(-) create mode 100644 tests/.idea/dictionaries/pavel.xml diff --git a/.gitignore b/.gitignore index dd396236..e6cd3319 100644 --- a/.gitignore +++ b/.gitignore @@ -37,19 +37,19 @@ cmake-build-*/ build-avr/ # IDE and tools -.idea .metadata .settings .project .cproject .pydevproject .gdbinit +.vscode/ +**/.idea/* +!**/.idea/dictionaries +!**/.idea/dictionaries/* # Generated files dsdlc_generated/ # Pycache __pycache__/ - -# Ignore IDE folders -.vscode/ diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index f4139bd5..3c202938 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,15 +1,105 @@ # Libcanard contribution guide +## Design principles + +The library is intended for use in real-time high-integrity applications. +It is paramount that its temporal properties and resource utilization are plain to model and predict statically. +The code shall follow applicable high-reliability coding guidelines as explained later in this document. +The implementation shall be fully compliant with the UAVCAN/CAN specification. + +The implementation and the API should be kept simple. +The core library `canard.c` (that is, excluding the optional DSDL presentation layer extension) shall never become +larger than 1000 logical lines of code. +This restriction ensures that the library is kept simple and easy to validate and verify. +There will be no high-level abstractions -- if that is desired, other implementations should be used, +such as, for example, Libuavcan. + +The library is intended for deeply embedded systems where the resources may be scarce. +The ROM footprint is of a particular concern because the library should be usable with embedded bootloaders. + +## Directory layout + +The production sources and some minimal configuration files (such as `.clang-tidy`) are located under `/libcanard/`. +Do not put anything else in there. + +The tests are located under `/tests/`. +This directory also contains the top `CMakeLists.txt` needed to build and run the tests on the local machine. +Essentially, this directory can be considered to be the project root. + +There is no separate storage for the documentation because it is written directly in the header files. +This works for Libcanard because it is sufficiently compact and simple. + ## Standards -MISRA. +The library shall be implemented in ISO C11 following MISRA C:2012. +The MISRA compliance is enforced by Clang-Tidy and SonarQube. +Deviations are documented directly in the source code as follows: + +```c +// Intentional violation of MISRA: +<... deviant construct ...> +``` + +The full list of deviations with the accompanying explanation can be found by grepping the sources. + +Do not suppress compliance warnings using the means provided by static analysis tools because such deviations +are impossible to track at the source code level. +An exception applies for the case of false-positive (invalid) warnings -- those should not be mentioned in the codebase. [Zubax C++ Coding Conventions](https://kb.zubax.com/x/84Ah) shall be followed. +Formatting is enforced by Clang-Format; it is used also to fail the CI/CD build if violations are detected. + +Unfortunately, some rules are hard or impractical to enforce automatically, +so code reviewers shall be aware of MISRA and general high-reliability coding practices +to prevent non-compliant code from being accepted into upstream. + +## Tools -## IDE +The following tools are required to conduct library development locally: + +- GCC v9 or newer. +- Clang and Clang-Tools v9 or newer. +- CMake v3.12 or newer. +- An AMD64 machine. + +### Clang-Tidy + +Clang-Tidy is used to enforce compliance with MISRA and Zubax Coding Conventions. +There are separate configuration files per directory: + +- `/libcanard/` (the production code) is equipped with the most stringent configuration. + +- `/tests/` is equipped with a separate configuration which omits certain rules that are considered + expensive to maintain. + This is because the test suite is intentionally kept to a somewhat lower quality bar to reduce development costs. + +Clang-Tidy is invoked automatically on each translation unit before it is compiled; +the build will fail if the tool is not available locally. +To disable this behavior, pass `NO_STATIC_ANALYSIS=1` to CMake at the generation time. + +### Clang-Format + +Clang-Format is used to enforce compliance with MISRA and Zubax Coding Conventions. +There is a single configuration file at the root of the repository. + +To reformat the sources, generate the project and build the target `format`; e.g., for Make: `make format`. + +### SonarQube + +SonarQube is a cloud solution so its use is delegated to the CI/CD pipeline. +If you need access, please get in touch with the UAVCAN Development Team members. + +### IDE The recommended development environment is JetBrains CLion. The root project file is `tests/CMakeLists.txt`. +The repository contains the spelling dictionaries for CLion located under `**/.idea/`, make sure to use them. ## Testing -TODO +Generate the CMake project, build all, and then build the target `test` (e.g., `make test`). +The tests are built for x86 and x86_64; the latter is why an AMD64 machine is required for local development. + +At the moment, the library is not being tested against other platforms. +We would welcome contributions implementing CI/CD testing against popular embedded architectures, particularly +the ARM Cortex M series and AVR in an emulator. +As a high-integrity library, the Libcanard test suite should provide full test coverage for all commonly used platforms. diff --git a/README.md b/README.md index 3e181a0b..a0b4b6cf 100644 --- a/README.md +++ b/README.md @@ -16,6 +16,153 @@ The acronym UAVCAN stands for *Uncomplicated Application-level Vehicular Communi **READ THE DOCS: [`libcanard/canard.h`](/libcanard/canard.h)** -**Contribute: [`CONTRIBUTING.md`](/CONTRIBUTING.md)** +Contribute: [`CONTRIBUTING.md`](/CONTRIBUTING.md) -**Ask questions: [forum.uavcan.org](https://forum.uavcan.org)** +Ask questions: [forum.uavcan.org](https://forum.uavcan.org) + +## Example + +The example augments the documentation but does not replace it. + +The library requires a constant-complexity deterministic dynamic memory allocator. +We could use the standard C heap, but most implementations are not constant-complexity, +so let's suppose that we're using [O1Heap](https://github.com/pavel-kirienko/o1heap) instead. +We are going to need basic wrappers: + +```c +static void* memAllocate(CanardInstance* const ins, const size_t amount) +{ + (void) ins; + return o1heapAllocate(my_allocator, amount); +} + +static void memFree(CanardInstance* const ins, void* const pointer) +{ + (void) ins; + o1heapFree(my_allocator, pointer); +} +``` + +Init a library instance: + +```c +CanardInstance ins = canardInit(&memAllocate, &memFree); +ins.mtu_bytes = CANARD_MTU_CAN_CLASSIC; // Defaults to 64 (CAN FD); here we select Classic CAN. +ins.node_id = 42; // Defaults to anonymous; can be set up later at any point. +``` + +Publish a message: + +```c +static uint8_t my_message_transfer_id; // Must be static or heap-allocated to retain state between calls. +const CanardTransfer transfer = { + .timestamp_usec = transmission_deadline, // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = 1234, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .transfer_id = my_message_transfer_id, + .payload_size = 45, + .payload = "Sancho, it strikes me thou art in great fear.", +}; +++my_message_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. +int32_t result = canardTxPush(&ins, &transfer); +if (result < 0) +{ + // An error has occurred: either an argument is invalid or we've ran out of memory. + // It is possible to statically prove that an out-of-memory will never occur for a given application if the + // heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap. + abort(); +} +``` + +The CAN frames generated from the message transfer are now stored in the transmission queue. +We need to pick them out one by one and have them transmitted. +Normally, the following fragment should be invoked periodically to unload the CAN frames from the +prioritized transmission queue into the CAN driver (or several, if redundant interfaces are used): + +```c +for (const CanardFrame* txf = NULL; (txf = canardTxPeek(&ins)) != NULL;) // Look at the top of the TX queue. +{ + if (txf->timestamp_usec < getCurrentMicroseconds()) // Check if the frame has timed out. + { + if (!pleaseTransmit(&txf)) // Send the frame. Redundant interfaces may be used here. + { + break; // If the driver is busy, break and retry later. + } + } + canardTxPop(&ins); // Remove the frame from the queue after it's transmitted. + ins.memory_free(&ins, (CanardFrame*)txf); // Deallocate the dynamic memory afterwards. +} +``` + +Transfer reception is done by feeding frames into the transfer reassembly state machine. +But first, we need to subscribe: + +```c +CanardRxSubscription my_subscription; +(void) canardRxSubscribe(&ins, + CanardTransferKindResponse, // Indicate that we want service responses. + 123, // The Service-ID to subscribe to. + 1024, // The maximum payload size (max DSDL object size). + &my_subscription); +``` + +We can subscribe and unsubscribe at runtime as many times as we want. +Normally, however, an embedded application would subscribe once and roll with it. +Okay, this is how we receive transfers: + +```c +CanardTransfer transfer; +const int8_t result = canardRxAccept(&ins, + &received_frame, // The CAN frame received from the bus. + redundant_interface_index, // If the transport is not redundant, use 0. + &transfer); +if (result < 0) +{ + // An error has occurred: either an argument is invalid or we've ran out of memory. + // It is possible to statically prove that an out-of-memory will never occur for a given application if + // the heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap. + // Reception of an invalid frame is NOT an error. + abort(); +} +else if (result == 1) +{ + processReceivedTransfer(redundant_interface_index, &transfer); // A transfer has been received, process it. + ins.memory_free(&ins, (void*)transfer.payload); // Deallocate the dynamic memory afterwards. +} +else +{ + // Nothing to do. + // The received frame is either invalid or it's a non-last frame of a multi-frame transfer. + // Reception of an invalid frame is NOT reported as an error because it is not an error. +} +``` + +The DSDL serialization helper library can be used to (de-)serialize DSDL objects without auto-generated code. +Here's a simple deserialization example for a `uavcan.node.Heartbeat.1.0` message: + +```c +uint8_t mode = canardDSDLGetU8(heartbeat_transfer->payload, heartbeat_transfer->payload_size, 34, 3); +uint32_t uptime = canardDSDLGetU32(heartbeat_transfer->payload, heartbeat_transfer->payload_size, 0, 32); +uint32_t vssc = canardDSDLGetU32(heartbeat_transfer->payload, heartbeat_transfer->payload_size, 37, 19); +uint8_t health = canardDSDLGetU8(heartbeat_transfer->payload, heartbeat_transfer->payload_size, 32, 2); +``` + +And the opposite: + +```c +uint8_t buffer[7]; +// destination offset value bit-length +canardDSDLSetUxx(&buffer[0], 34, 2, 3); // mode +canardDSDLSetUxx(&buffer[0], 0, 0xDEADBEEF, 32); // uptime +canardDSDLSetUxx(&buffer[0], 37, 0x7FFFF, 19); // vssc +canardDSDLSetUxx(&buffer[0], 32, 2, 2); // health +// Now it can be transmitted: +my_transfer->payload = &buffer[0]; +my_transfer->payload_size = sizeof(buffer); +result = canardTxPush(&ins, &my_transfer); +``` + +Full API specification is available in the documentation. +If you find the examples to be unclear or incorrect, please, open a ticket. diff --git a/libcanard/canard.h b/libcanard/canard.h index 187bb03d..f78cdaa1 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -1,93 +1,93 @@ -// LIBCANARD -// -// Libcanard is a compact implementation of the UAVCAN/CAN protocol for high-integrity real-time embedded systems. -// It is designed for use in robust deterministic embedded systems equipped with at least 32K ROM and 4K RAM. -// The codebase follows the MISRA C rules, has 100% test coverage, and is validated by at least two static analyzers. -// The library is designed to be compatible with any target platform and instruction set architecture, from 8 to 64 bit, -// little- and big-endian, RTOS-based or bare metal, etc., as long as there is a standards-compliant C11 compiler. -// -// INTEGRATION -// -// The library is intended to be integrated into the end application by simply copying the file canard.c into the -// source tree of the project; it does not require any special compilation options and should work out of the box. -// There are optional build configuration options defined near the top of canard.c; they may be used to fine-tune -// the library for the target platform (but it is not necessary). This header file should be located at the same -// directory with canard.c, or its location should be in the include look-up paths of the compiler. -// -// As explained in this documentation, the library requires a deterministic constant-time bounded-fragmentation dynamic -// memory allocator. If your target platform does not provide a deterministic memory manager (most platforms don't), -// it is recommended to use O1Heap (MIT licensed): https://github.com/pavel-kirienko/o1heap. -// -// There is an optional two-file extension library canard_dsdl.c + canard_dsdl.h which can be used alongside -// this core library to simplify DSDL object serialization and deserialization. It is intended to be integrated in -// the same manner. Please read its usage manual for further information. -// -// There are no specific requirements to the underlying I/O layer. Some low-level drivers maintained by the -// UAVCAN Development Team may be found at https://github.com/UAVCAN/platform_specific_components. -// -// If your application requires a MISRA C compliance report, please get in touch with the maintainers via the forum -// at https://forum.uavcan.org. -// -// ARCHITECTURE -// -// UAVCAN, as a protocol stack, is composed of two layers: TRANSPORT and PRESENTATION. The transport layer is portable -// across different transport protocols, one of which is CAN (FD), formally referred to as UAVCAN/CAN. This library -// is focused on UAVCAN/CAN only and it will not support other transports. The presentation layer is implemented -// through the DSDL language and the associated data type regulation policies. Much like the UAVCAN stack itself, -// this library consists of two major components: -// -// 1. TRANSPORT -- the UAVCAN/CAN transport layer implementation. This is implemented in canard.c/.h, -// the documentation for which you are currently reading. This is the core part of the library. -// -// 2. PRESENTATION -- the optional DSDL support extension library. This is implemented in canard_dsdl.c/.h, -// an optional component which may be used by some applications where automatic DSDL code generation is -// not used. Normally, applications may prefer to rely on auto-generated code using DSDL-to-C translators -// such as Nunavut (https://github.com/UAVCAN/nunavut). -// -// The DSDL extension is trivial and there is not much to document -- please refer to its header file for details. -// -// This transport layer implementation consists of two components: the transmission (TX) pipeline and the -// reception (RX) pipeline. -// -// The TX and RX pipelines are completely independent from each other except that they both rely on the same -// dynamic memory manager. The TX pipeline uses the dynamic memory to store outgoing CAN frames in the prioritized -// transmission queue. The RX pipeline uses the dynamic memory to store contiguous payload buffers for received -// transfers and for keeping the transfer reassembly state machine data. The exact memory consumption model is defined -// for both pipelines, so it is possible to statically determine the minimum size of the dynamic memory pool required -// to guarantee that a given application will never encounter an out-of-memory error at runtime. -// -// Much like with dynamic memory, the time complexity of every API function is well-characterized, allowing the -// application to guarantee predictable real-time performance. -// -// The TX pipeline is managed with the help of three API functions. When the application needs to emit a transfer, -// it invokes canardTxPush(). The function splits the transfer into CAN frames and stores them into the prioritized -// transmission queue. The application then picks the CAN frames from the queue one-by-one by calling canardTxPeek() -// followed by canardTxPop() -- the former allows the application to look at the frame and the latter tells the library -// that the frame shall be removed from the queue. The returned frames need to be deallocated by the application. -// -// The RX pipeline is managed with the help of three API functions. The main function canardRxAccept() takes a -// received CAN frame and updates the appropriate transfer reassembly state machine. The functions canardRxSubscribe() -// and its counterpart canardRxUnsubscribe() instruct the library which transfers should be received (by default, all -// transfers are ignored); also the subscription function specifies vital transfer reassembly parameters such as the -// maximum payload size (i.e., the maximum size of a serialized representation of a DSDL object) and the transfer-ID -// timeout. Transfers that carry more payload than the configured maximum per subscription are truncated following the -// Implicit Truncation Rule (ITR) defined by the UAVCAN Specification -- the rule is implemented to facilitate -// backward-compatible DSDL data type extensibility. -// -// The library supports an unlimited number of redundant interfaces. -// -// The library is not thread-safe: if used in a concurrent environment, it is the responsibility of the application -// to provide adequate synchronization. -// -// The library is purely reactive: it does not perform any background processing and does not require periodic -// servicing. Its internal state is only updated as a response to well-specified explicit API calls. -// -// -------------------------------------------------------------------------------------------------------------------- -// -// This software is distributed under the terms of the MIT License. -// Copyright (c) 2016-2020 UAVCAN Development Team. -// Author: Pavel Kirienko -// Contributors: https://github.com/UAVCAN/libcanard/contributors. +/// LIBCANARD +/// +/// Libcanard is a compact implementation of the UAVCAN/CAN protocol for high-integrity real-time embedded systems. +/// It is designed for use in robust deterministic embedded systems equipped with at least 32K ROM and 4K RAM. +/// The codebase follows the MISRA C rules, has 100% test coverage, and is validated by at least two static analyzers. +/// The library is designed to be compatible with any target platform and instruction set architecture, from 8 to 64 +/// bit, little- and big-endian, RTOS-based or bare metal, etc., as long as there is a standards-compliant C11 compiler. +/// +/// INTEGRATION +/// +/// The library is intended to be integrated into the end application by simply copying the file canard.c into the +/// source tree of the project; it does not require any special compilation options and should work out of the box. +/// There are optional build configuration options defined near the top of canard.c; they may be used to fine-tune +/// the library for the target platform (but it is not necessary). This header file should be located at the same +/// directory with canard.c, or its location should be in the include look-up paths of the compiler. +/// +/// As explained in this documentation, the library requires a deterministic constant-time bounded-fragmentation dynamic +/// memory allocator. If your target platform does not provide a deterministic memory manager (most platforms don't), +/// it is recommended to use O1Heap (MIT licensed): https://github.com/pavel-kirienko/o1heap. +/// +/// There is an optional two-file extension library canard_dsdl.c + canard_dsdl.h which can be used alongside +/// this core library to simplify DSDL object serialization and deserialization. It is intended to be integrated in +/// the same manner. Please read its usage manual for further information. +/// +/// There are no specific requirements to the underlying I/O layer. Some low-level drivers maintained by the +/// UAVCAN Development Team may be found at https://github.com/UAVCAN/platform_specific_components. +/// +/// If your application requires a MISRA C compliance report, please get in touch with the maintainers via the forum +/// at https://forum.uavcan.org. +/// +/// ARCHITECTURE +/// +/// UAVCAN, as a protocol stack, is composed of two layers: TRANSPORT and PRESENTATION. The transport layer is portable +/// across different transport protocols, one of which is CAN (FD), formally referred to as UAVCAN/CAN. This library +/// is focused on UAVCAN/CAN only and it will not support other transports. The presentation layer is implemented +/// through the DSDL language and the associated data type regulation policies. Much like the UAVCAN stack itself, +/// this library consists of two major components: +/// +/// 1. TRANSPORT -- the UAVCAN/CAN transport layer implementation. This is implemented in canard.c/.h, +/// the documentation for which you are currently reading. This is the core component of the library. +/// +/// 2. PRESENTATION -- the optional DSDL support extension library. This is implemented in canard_dsdl.c/.h, +/// an optional component which may be used by some applications where automatic DSDL code generation is +/// not used. Normally, applications may prefer to rely on auto-generated code using DSDL-to-C translators +/// such as Nunavut (https://github.com/UAVCAN/nunavut). +/// +/// The DSDL extension is trivial and there is not much to document -- please refer to its header file for details. +/// +/// This transport layer implementation consists of two components: the transmission (TX) pipeline and the +/// reception (RX) pipeline. +/// +/// The TX and RX pipelines are completely independent from each other except that they both rely on the same +/// dynamic memory manager. The TX pipeline uses the dynamic memory to store outgoing CAN frames in the prioritized +/// transmission queue. The RX pipeline uses the dynamic memory to store contiguous payload buffers for received +/// transfers and for keeping the transfer reassembly state machine data. The exact memory consumption model is defined +/// for both pipelines, so it is possible to statically determine the minimum size of the dynamic memory pool required +/// to guarantee that a given application will never encounter an out-of-memory error at runtime. +/// +/// Much like with dynamic memory, the time complexity of every API function is well-characterized, allowing the +/// application to guarantee predictable real-time performance. +/// +/// The TX pipeline is managed with the help of three API functions. When the application needs to emit a transfer, +/// it invokes canardTxPush(). The function splits the transfer into CAN frames and stores them into the prioritized +/// transmission queue. The application then picks the CAN frames from the queue one-by-one by calling canardTxPeek() +/// followed by canardTxPop() -- the former allows the application to look at the frame and the latter tells the library +/// that the frame shall be removed from the queue. The returned frames need to be deallocated by the application. +/// +/// The RX pipeline is managed with the help of three API functions. The main function canardRxAccept() takes a +/// received CAN frame and updates the appropriate transfer reassembly state machine. The functions canardRxSubscribe() +/// and its counterpart canardRxUnsubscribe() instruct the library which transfers should be received (by default, all +/// transfers are ignored); also the subscription function specifies vital transfer reassembly parameters such as the +/// maximum payload size (i.e., the maximum size of a serialized representation of a DSDL object) and the transfer-ID +/// timeout. Transfers that carry more payload than the configured maximum per subscription are truncated following the +/// Implicit Truncation Rule (ITR) defined by the UAVCAN Specification -- the rule is implemented to facilitate +/// backward-compatible DSDL data type extensibility. +/// +/// The library supports a practically unlimited number of redundant transports. +/// +/// The library is not thread-safe: if used in a concurrent environment, it is the responsibility of the application +/// to provide adequate synchronization. +/// +/// The library is purely reactive: it does not perform any background processing and does not require periodic +/// servicing. Its internal state is only updated as a response to well-specified explicit API calls. +/// +/// -------------------------------------------------------------------------------------------------------------------- +/// +/// This software is distributed under the terms of the MIT License. +/// Copyright (c) 2016-2020 UAVCAN Development Team. +/// Author: Pavel Kirienko +/// Contributors: https://github.com/UAVCAN/libcanard/contributors. #ifndef CANARD_H_INCLUDED #define CANARD_H_INCLUDED diff --git a/tests/.idea/dictionaries/pavel.xml b/tests/.idea/dictionaries/pavel.xml new file mode 100644 index 00000000..50be6aa1 --- /dev/null +++ b/tests/.idea/dictionaries/pavel.xml @@ -0,0 +1,77 @@ + + + + allocatee + arithmetics + backends + bcast + bootloaders + bxcan + cfamily + coverity + crtp + ddtid + deallocated + deallocating + deallocation + deframing + deinitializes + demonstrational + descatter + discardment + dnid + dont + dsdl + dsonar + dtid + errno + gcov + ieee + iface + ifnamsiz + inak + intravehicular + irgdn + izer + kfrfx + kirienko + libcanard + libuavcan + microarchitecture + microcontrollers + misra + msec + multiframe + nbytes + nosonar + nutt + permill + pkut + prescaler + prio + pseudocode + pyuavcan + qube + roundtrip + rtos + rxstate + serde + signedness + snid + socketcan + storages + submoduling + subtreeing + uavcan + uint + unicast + untracked + usec + vendoring + vssc + wget + xenial + zubax + + + \ No newline at end of file From 9a2052f16ba5254ebec7752db46ba56112df3d52 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 3 Mar 2020 03:37:22 +0200 Subject: [PATCH 095/100] Move the IDE-related files into the project root --- {tests/.idea => .idea}/dictionaries/pavel.xml | 0 CONTRIBUTING.md | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename {tests/.idea => .idea}/dictionaries/pavel.xml (100%) diff --git a/tests/.idea/dictionaries/pavel.xml b/.idea/dictionaries/pavel.xml similarity index 100% rename from tests/.idea/dictionaries/pavel.xml rename to .idea/dictionaries/pavel.xml diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 3c202938..5d083ac7 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -92,7 +92,7 @@ If you need access, please get in touch with the UAVCAN Development Team members ### IDE The recommended development environment is JetBrains CLion. The root project file is `tests/CMakeLists.txt`. -The repository contains the spelling dictionaries for CLion located under `**/.idea/`, make sure to use them. +The repository contains the spelling dictionaries for CLion located under `.idea/`, make sure to use them. ## Testing From b5f1b7781c9a4aeea12f088944b42e881395c6ff Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 3 Mar 2020 17:28:19 +0200 Subject: [PATCH 096/100] A few very trivial changes: typos, wording, doc style --- CONTRIBUTING.md | 4 +--- README.md | 4 ++-- libcanard/canard.c | 6 +++--- libcanard/canard.h | 29 +++++++++++++------------- libcanard/canard_dsdl.c | 10 ++++----- libcanard/canard_dsdl.h | 42 +++++++++++++++++++------------------- tests/test_dsdl.cpp | 2 -- tests/test_private_crc.cpp | 2 +- 8 files changed, 48 insertions(+), 51 deletions(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 5d083ac7..b1350c51 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -11,8 +11,7 @@ The implementation and the API should be kept simple. The core library `canard.c` (that is, excluding the optional DSDL presentation layer extension) shall never become larger than 1000 logical lines of code. This restriction ensures that the library is kept simple and easy to validate and verify. -There will be no high-level abstractions -- if that is desired, other implementations should be used, -such as, for example, Libuavcan. +There will be no high-level abstractions -- if that is desired, other implementations of UAVCAN should be used. The library is intended for deeply embedded systems where the resources may be scarce. The ROM footprint is of a particular concern because the library should be usable with embedded bootloaders. @@ -24,7 +23,6 @@ Do not put anything else in there. The tests are located under `/tests/`. This directory also contains the top `CMakeLists.txt` needed to build and run the tests on the local machine. -Essentially, this directory can be considered to be the project root. There is no separate storage for the documentation because it is written directly in the header files. This works for Libcanard because it is sufficiently compact and simple. diff --git a/README.md b/README.md index a0b4b6cf..609027a6 100644 --- a/README.md +++ b/README.md @@ -62,8 +62,8 @@ const CanardTransfer transfer = { .port_id = 1234, // This is the subject-ID. .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. .transfer_id = my_message_transfer_id, - .payload_size = 45, - .payload = "Sancho, it strikes me thou art in great fear.", + .payload_size = 47, // In this example: a string 45 characters long, 9-bit length prefix, 7-bit padding. + .payload = "\x00Z" "Sancho, it strikes me thou art in great fear.", }; ++my_message_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. int32_t result = canardTxPush(&ins, &transfer); diff --git a/libcanard/canard.c b/libcanard/canard.c index 59cbfde8..a3dfcd07 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -1,6 +1,6 @@ -// This software is distributed under the terms of the MIT License. -// Copyright (c) 2016-2020 UAVCAN Development Team. -// Author: Pavel Kirienko +/// This software is distributed under the terms of the MIT License. +/// Copyright (c) 2016-2020 UAVCAN Development Team. +/// Author: Pavel Kirienko #include "canard.h" #include diff --git a/libcanard/canard.h b/libcanard/canard.h index f78cdaa1..845797c6 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -296,7 +296,6 @@ typedef void (*CanardMemoryFree)(CanardInstance* ins, void* pointer); /// This is the core structure that keeps all of the states and allocated resources of the library instance. /// The application may directly alter the fields whose names do not begin with an underscore. -/// Fields whose names begin with an underscore SHALL NOT be accessed by the application. struct CanardInstance { /// User pointer that can link this instance with other objects. @@ -327,7 +326,7 @@ struct CanardInstance /// /// The following API functions may allocate memory: canardRxAccept(), canardTxPush(). /// The following API functions may deallocate memory: canardRxAccept(), canardRxSubscribe(), canardRxUnsubscribe(). - /// The exact memory requirement model is specified for each function in its documentation. + /// The exact memory requirement and usage model is specified for each function in its documentation. CanardMemoryAllocate memory_allocate; CanardMemoryFree memory_free; @@ -400,7 +399,7 @@ int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const tran /// frames of serialized transfers pushed into the prioritized transmission queue by canardTxPush(). /// /// Nodes with redundant transports should replicate every frame into each of the transport interfaces. -/// Such replication may require additional buffering in the driver layer, depending on the implementation. +/// Such replication may require additional buffering in the media I/O layer, depending on the implementation. /// /// The timestamp values of returned frames are initialized with the timestamp value of the transfer instance they /// originate from. Timestamps are used to specify the transmission deadline. It is up to the application and/or @@ -424,7 +423,7 @@ const CanardFrame* canardTxPeek(const CanardInstance* const ins); /// This function transfers the ownership of the top element of the prioritized transmission queue to the application. /// The application should invoke this function to remove the top element from the prioritized transmission queue. -/// The element is removed but it is not invalidated: it is the responsibility of the application to deallocate +/// The element is removed but it is not invalidated; it is the responsibility of the application to deallocate /// the memory used by the object later. The object SHALL NOT be deallocated UNTIL this function is invoked. /// /// WARNING: @@ -461,7 +460,8 @@ void canardTxPop(CanardInstance* const ins); /// the time complexity by never deallocating sessions. /// The size of a session instance is at most 48 bytes on any conventional platform (typically much smaller). /// -/// 2. New memory for the transfer payload buffer is allocated when a new transfer is initiated. +/// 2. New memory for the transfer payload buffer is allocated when a new transfer is initiated, unless the buffer +/// was already allocated at the time. /// This event occurs when a transport frame that matches a known subscription is received and it begins a /// new transfer (that is, the start-of-frame flag is set and it is not a duplicate). /// The amount of the allocated memory is payload_size_max as configured via canardRxSubscribe(). @@ -479,10 +479,11 @@ void canardTxPop(CanardInstance* const ins); /// (sizeof(session instance) + payload_size_max) * number_of_nodes /// /// Where sizeof(session instance) and payload_size_max are defined above, and number_of_nodes is the number of remote -/// nodes emitting transfers that match the subscription. If the dynamic memory pool is sized correctly, the -/// application is guaranteed to never encounter an out-of-memory (OOM) error at runtime. The actual size of the -/// dynamic memory pool is typically larger; for a detailed treatment of the problem and the related theory please -/// refer to the documentation of O1Heap -- a deterministic memory allocator for hard real-time embedded systems. +/// nodes emitting transfers that match the subscription (which cannot exceed (CANARD_NODE_ID_MAX-1) by design). +/// If the dynamic memory pool is sized correctly, the application is guaranteed to never encounter an +/// out-of-memory (OOM) error at runtime. The actual size of the dynamic memory pool is typically larger; +/// for a detailed treatment of the problem and the related theory please refer to the documentation of O1Heap -- +/// a deterministic memory allocator for hard real-time embedded systems. /// /// The time complexity is O(n+p) where n is the number of subject-IDs or service-IDs subscribed to by the application, /// depending on the transfer kind of the supplied frame, and p is the amount of payload in the received frame @@ -497,7 +498,7 @@ void canardTxPop(CanardInstance* const ins); /// are stored into out_transfer, and the transfer payload buffer ownership is passed to that object. The lifetime /// of the resulting transfer object is not related to the lifetime of the input transport frame (that is, even if /// it is a single-frame transfer, its payload is copied out into a new dynamically allocated buffer storage). -/// If the payload_size_max is zero, the payload pointer will be NULL, since there is no data to store and so a +/// If the payload_size_max is zero, the payload pointer may be NULL, since there is no data to store and so a /// buffer is not needed. The application is responsible for deallocating the payload buffer when the processing /// is done by invoking memory_free on the transfer payload pointer. /// @@ -542,8 +543,8 @@ int8_t canardRxAccept(CanardInstance* const ins, /// called the Implicit Truncation Rule (ITR) and it is intended to facilitate extensibility of data types while /// preserving backward compatibility. The transfer CRC is validated regardless of whether its payload is truncated. /// -/// The transport fail-over timeout (if redundant transports are used) is the same as the transfer-ID timeout. -/// It may be reduced in a future release of the library, but it will not affect backward compatibility. +/// The redundant transport fail-over timeout (if redundant transports are used) is the same as the transfer-ID timeout. +/// It may be reduced in a future release of the library, but it will not affect the backward compatibility. /// /// The return value is 1 if a new subscription has been created as requested. /// The return value is 0 if such subscription existed at the time the function was invoked. In this case, @@ -551,12 +552,12 @@ int8_t canardRxAccept(CanardInstance* const ins, /// The return value is a negated invalid argument error if any of the input arguments are invalid. /// /// The time complexity is linear from the number of current subscriptions under the specified transfer kind. -/// This function does not allocate new memory. The function may deallocate memory if such subscription already existed. +/// This function does not allocate new memory. The function may deallocate memory if such subscription already +/// existed; the deallocation behavior is specified in the documentation for canardRxUnsubscribe(). /// /// Subscription instances have large look-up tables to ensure that the temporal properties of the algorithms are /// invariant to the network configuration (i.e., a node that is validated on a network containing one other node /// will provably perform identically on a network that contains X nodes). -/// Some background is available at https://github.com/UAVCAN/libuavcan/issues/185#issuecomment-440354858. /// This is a conscious time-memory trade-off. It may have adverse effects on RAM-constrained applications, /// but this is considered tolerable because it is expected that the types of applications leveraging Libcanard /// will be either real-time function nodes where time determinism is critical, or bootloaders where time determinism diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c index 482ab356..de171ffe 100644 --- a/libcanard/canard_dsdl.c +++ b/libcanard/canard_dsdl.c @@ -1,6 +1,6 @@ -// This software is distributed under the terms of the MIT License. -// Copyright (c) 2016-2020 UAVCAN Development Team. -// Author: Pavel Kirienko +/// This software is distributed under the terms of the MIT License. +/// Copyright (c) 2016-2020 UAVCAN Development Team. +/// Author: Pavel Kirienko #include "canard_dsdl.h" #include @@ -35,7 +35,7 @@ # error "Unsupported language: ISO C11 or a newer version is required." #endif -/// If your platform is not IEEE 754-compatible and you need floats, please reach https://forum.uavcan.org. +/// Detect whether the target platform is compatible with IEEE 754. #define CANARD_DSDL_PLATFORM_IEEE754_FLOAT \ ((FLT_RADIX == 2) && (FLT_MANT_DIG == 24) && (FLT_MIN_EXP == -125) && (FLT_MAX_EXP == 128)) #define CANARD_DSDL_PLATFORM_IEEE754_DOUBLE \ @@ -43,7 +43,7 @@ // --------------------------------------------- COMMON ITEMS --------------------------------------------- -/// Per the DSDL specification, it is assumed that 1 byte = 8 bits. +/// Per the DSDL specification, 1 byte = 8 bits. #define BYTE_WIDTH 8U #define BYTE_MAX 0xFFU diff --git a/libcanard/canard_dsdl.h b/libcanard/canard_dsdl.h index 0a649851..e8ce9f1e 100644 --- a/libcanard/canard_dsdl.h +++ b/libcanard/canard_dsdl.h @@ -1,24 +1,24 @@ -// LIBCANARD DSDL helper -// -// This is a trivial optional extension library for Libcanard that contains basic DSDL field serialization routines. -// It is intended for use in simple applications where auto-generated DSDL serialization logic is not available. -// The functions are fully stateless and straightforward to use; read their documentation comments for usage info. -// -// This is an optional part of libcanard that can be omitted if this functionality is not required by the application. -// Some high-integrity systems may prefer to avoid this extension because it relies on unsafe memory operations. -// -// This library is designed to be compatible with any instruction set architecture (including big endian platforms) -// but the floating point functionality will be automatically disabled at compile time if the target platform does not -// use an IEEE 754-compatible floating point model. Support for other floating point models may be implemented later. -// If your application requires non-IEEE-754 floats, please reach out to the maintainers via https://forum.uavcan.org. -// -// To use the library, copy the files canard_dsdl.c and canard_dsdl.h into the source tree of the application. -// No special compilation options are required. There are optional build configuration options defined near the top -// of canard_dsdl.c; they may be used to fine-tune the library for the target platform (but it is not necessary). -// -// This software is distributed under the terms of the MIT License. -// Copyright (c) 2016-2020 UAVCAN Development Team. -// Author: Pavel Kirienko +/// LIBCANARD DSDL helper +/// +/// This is a trivial optional extension library for Libcanard that contains basic DSDL field serialization routines. +/// It is intended for use in simple applications where auto-generated DSDL serialization logic is not available. +/// The functions are fully stateless and straightforward to use; read their documentation comments for usage info. +/// +/// This is an optional part of libcanard that can be omitted if this functionality is not required by the application. +/// Some high-integrity systems may prefer to avoid this extension because it relies on unsafe memory operations. +/// +/// This library is designed to be compatible with any instruction set architecture (including big endian platforms) +/// but the floating point functionality will be automatically disabled at compile time if the target platform does not +/// use an IEEE 754-compatible floating point model. Support for other floating point models may be implemented later. +/// If your application requires non-IEEE-754 floats, please reach out to the maintainers via https://forum.uavcan.org. +/// +/// To use the library, copy the files canard_dsdl.c and canard_dsdl.h into the source tree of the application. +/// No special compilation options are required. There are optional build configuration options defined near the top +/// of canard_dsdl.c; they may be used to fine-tune the library for the target platform (but it is not necessary). +/// +/// This software is distributed under the terms of the MIT License. +/// Copyright (c) 2016-2020 UAVCAN Development Team. +/// Author: Pavel Kirienko #ifndef CANARD_DSDL_H_INCLUDED #define CANARD_DSDL_H_INCLUDED diff --git a/tests/test_dsdl.cpp b/tests/test_dsdl.cpp index 2554ef8b..2390557a 100644 --- a/tests/test_dsdl.cpp +++ b/tests/test_dsdl.cpp @@ -10,7 +10,6 @@ TEST_CASE("float16Pack") { using exposed::float16Pack; - // Reference values were generated manually with libuavcan and numpy.float16(). REQUIRE(0b0000000000000000 == float16Pack(0.0F)); REQUIRE(0b0011110000000000 == float16Pack(1.0F)); REQUIRE(0b1100000000000000 == float16Pack(-2.0F)); @@ -22,7 +21,6 @@ TEST_CASE("float16Pack") TEST_CASE("float16Unpack") { using exposed::float16Unpack; - // Reference values were generated manually with libuavcan and numpy.float16(). REQUIRE(Approx(0.0F) == float16Unpack(0b0000000000000000)); REQUIRE(Approx(1.0F) == float16Unpack(0b0011110000000000)); REQUIRE(Approx(-2.0F) == float16Unpack(0b1100000000000000)); diff --git a/tests/test_private_crc.cpp b/tests/test_private_crc.cpp index b8bc8df8..bebb4a6a 100644 --- a/tests/test_private_crc.cpp +++ b/tests/test_private_crc.cpp @@ -11,7 +11,7 @@ TEST_CASE("TransferCRC") crc = crcAdd(crc, 1, "1"); crc = crcAdd(crc, 1, "2"); crc = crcAdd(crc, 1, "3"); - REQUIRE(0x5BCEU == crc); // Using Libuavcan as reference + REQUIRE(0x5BCEU == crc); crc = crcAdd(crc, 6, "456789"); REQUIRE(0x29B1U == crc); } From 13d0f90ee275a7a97f60697fccb964d4ba63b32c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 3 Mar 2020 18:32:23 +0200 Subject: [PATCH 097/100] Disable the codecov integration to eliminate noise. The coverage is tracked by SonarQube. --- .travis.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 1b684686..d07cb99d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -32,7 +32,6 @@ matrix: - gcov-9 --preserve-paths --long-file-names $(find CMakeFiles/test_private_cov.dir -name '*.gcno') - gcov-9 --preserve-paths --long-file-names $(find CMakeFiles/test_private_le_cov.dir -name '*.gcno') - gcov-9 --preserve-paths --long-file-names $(find CMakeFiles/test_public_cov.dir -name '*.gcno') - - bash <(curl -s https://codecov.io/bash) - 'sonar-scanner -Dsonar.projectKey=libcanard -Dsonar.organization=uavcan -Dsonar.sources=libcanard From 09b4370a58f4b8b7e2418d0e57f11c0ce4d24497 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 7 Mar 2020 21:25:09 +0200 Subject: [PATCH 098/100] Update the README to match https://github.com/UAVCAN/specification/issues/75 --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 609027a6..588971b9 100644 --- a/README.md +++ b/README.md @@ -62,8 +62,8 @@ const CanardTransfer transfer = { .port_id = 1234, // This is the subject-ID. .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. .transfer_id = my_message_transfer_id, - .payload_size = 47, // In this example: a string 45 characters long, 9-bit length prefix, 7-bit padding. - .payload = "\x00Z" "Sancho, it strikes me thou art in great fear.", + .payload_size = 47, + .payload = "\x2D\x00" "Sancho, it strikes me thou art in great fear.", }; ++my_message_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. int32_t result = canardTxPush(&ins, &transfer); From f80aa3c320aec049c3e0931912143aeac997df8e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 7 Mar 2020 21:47:22 +0200 Subject: [PATCH 099/100] Update the docs following the feedback from Scott. Comment-only changes. --- .idea/dictionaries/pavel.xml | 2 ++ libcanard/canard.h | 25 ++++++++++++++++++++----- libcanard/canard_dsdl.h | 15 +++++++++++---- 3 files changed, 33 insertions(+), 9 deletions(-) diff --git a/.idea/dictionaries/pavel.xml b/.idea/dictionaries/pavel.xml index 50be6aa1..6d51e641 100644 --- a/.idea/dictionaries/pavel.xml +++ b/.idea/dictionaries/pavel.xml @@ -10,6 +10,7 @@ cfamily coverity crtp + dataflow ddtid deallocated deallocating @@ -47,6 +48,7 @@ nutt permill pkut + pointee prescaler prio pseudocode diff --git a/libcanard/canard.h b/libcanard/canard.h index 845797c6..66534d01 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -1,7 +1,13 @@ -/// LIBCANARD +/// __ __ _______ __ __ _______ _______ __ __ +/// | | | | / _ ` | | | | / ____| / _ ` | ` | | +/// | | | | | |_| | | | | | | | | |_| | | `| | +/// | |_| | | _ | ` `_/ / | |____ | _ | | |` | +/// `_______/ |__| |__| `_____/ `_______| |__| |__| |__| `__| +/// | | | | | | +/// ----o------o------------o---------o------o---------o------- /// /// Libcanard is a compact implementation of the UAVCAN/CAN protocol for high-integrity real-time embedded systems. -/// It is designed for use in robust deterministic embedded systems equipped with at least 32K ROM and 4K RAM. +/// It is designed for use in robust deterministic embedded systems equipped with at least 32K ROM and 4..8K RAM. /// The codebase follows the MISRA C rules, has 100% test coverage, and is validated by at least two static analyzers. /// The library is designed to be compatible with any target platform and instruction set architecture, from 8 to 64 /// bit, little- and big-endian, RTOS-based or bare metal, etc., as long as there is a standards-compliant C11 compiler. @@ -10,8 +16,8 @@ /// /// The library is intended to be integrated into the end application by simply copying the file canard.c into the /// source tree of the project; it does not require any special compilation options and should work out of the box. -/// There are optional build configuration options defined near the top of canard.c; they may be used to fine-tune -/// the library for the target platform (but it is not necessary). This header file should be located at the same +/// There are optional build configuration macros defined near the top of canard.c; they may be used to fine-tune +/// the library for the target platform (but it is not necessary). This header file should be located in the same /// directory with canard.c, or its location should be in the include look-up paths of the compiler. /// /// As explained in this documentation, the library requires a deterministic constant-time bounded-fragmentation dynamic @@ -177,10 +183,15 @@ typedef struct /// The time system may be arbitrary as long as the clock is monotonic (steady). CanardMicrosecond timestamp_usec; - /// 29-bit extended ID. The bits above 29-th are zero/ignored. + /// 29-bit extended ID. The bits above 29-th shall be zero. uint32_t extended_can_id; /// The useful data in the frame. The length value is not to be confused with DLC! + /// For RX frames: the library does not expect the lifetime of the pointee to extend beyond the point of return + /// from the API function. That is, the pointee can be invalidated immediately after the frame has been processed. + /// For TX frames: the frame and the payload are allocated within the same dynamic memory fragment, so their + /// lifetimes are identical; when the frame is freed, the payload is invalidated. + /// A more detailed overview of the dataflow and related resource management issues is provided in the API docs. size_t payload_size; const void* payload; } CanardFrame; @@ -238,6 +249,10 @@ typedef struct /// If the payload is empty (payload_size = 0), the payload pointer may be NULL. /// The const pointer makes it incompatible with memory deallocation function, this is due to the limitations of C; /// therefore, when freeing the memory allocated for the payload, cast away the pointer's const qualifier. + /// For RX transfers: the application is required to free the payload buffer after the transfer is processed. + /// For TX transfers: the library does not expect the lifetime of the payload buffer to extend beyond the point + /// of return from the API function because the payload is copied into the TX frame objects. + /// A more detailed overview of the dataflow and related resource management issues is provided in the API docs. size_t payload_size; const void* payload; } CanardTransfer; diff --git a/libcanard/canard_dsdl.h b/libcanard/canard_dsdl.h index e8ce9f1e..482f0d1e 100644 --- a/libcanard/canard_dsdl.h +++ b/libcanard/canard_dsdl.h @@ -1,8 +1,15 @@ -/// LIBCANARD DSDL helper +/// __ __ _______ __ __ _______ _______ __ __ +/// | | | | / _ ` | | | | / ____| / _ ` | ` | | +/// | | | | | |_| | | | | | | | | |_| | | `| | +/// | |_| | | _ | ` `_/ / | |____ | _ | | |` | +/// `_______/ |__| |__| `_____/ `_______| |__| |__| |__| `__| +/// | | | | | | +/// ----o------o------------o---------o------o---------o------- /// -/// This is a trivial optional extension library for Libcanard that contains basic DSDL field serialization routines. -/// It is intended for use in simple applications where auto-generated DSDL serialization logic is not available. -/// The functions are fully stateless and straightforward to use; read their documentation comments for usage info. +/// This is a DSDL serialization helper for libcanard -- a trivial optional extension library that contains basic +/// DSDL field serialization routines. It is intended for use in simple applications where auto-generated DSDL +/// serialization logic is not available. The functions are fully stateless and straightforward to use; +/// read their documentation comments for usage info. /// /// This is an optional part of libcanard that can be omitted if this functionality is not required by the application. /// Some high-integrity systems may prefer to avoid this extension because it relies on unsafe memory operations. From b16a86b5a638d6ad3f9cbdf60bff194a9985dc9e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 8 Mar 2020 02:10:46 +0200 Subject: [PATCH 100/100] Update canard.h State that the frame payload pointer may be NULL if the size is zero. --- libcanard/canard.h | 1 + 1 file changed, 1 insertion(+) diff --git a/libcanard/canard.h b/libcanard/canard.h index 66534d01..9709612a 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -187,6 +187,7 @@ typedef struct uint32_t extended_can_id; /// The useful data in the frame. The length value is not to be confused with DLC! + /// If the payload is empty (payload_size = 0), the payload pointer may be NULL. /// For RX frames: the library does not expect the lifetime of the pointee to extend beyond the point of return /// from the API function. That is, the pointee can be invalidated immediately after the frame has been processed. /// For TX frames: the frame and the payload are allocated within the same dynamic memory fragment, so their