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/.idea/dictionaries/pavel.xml b/.idea/dictionaries/pavel.xml new file mode 100644 index 00000000..6d51e641 --- /dev/null +++ b/.idea/dictionaries/pavel.xml @@ -0,0 +1,79 @@ + + + + allocatee + arithmetics + backends + bcast + bootloaders + bxcan + cfamily + coverity + crtp + dataflow + 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 + pointee + 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 diff --git a/.travis.yml b/.travis.yml index 09e851a3..d07cb99d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,4 +1,6 @@ dist: xenial +env: + - CTEST_OUTPUT_ON_FAILURE=1 matrix: include: @@ -17,11 +19,36 @@ matrix: - g++-9-multilib - gcc-9-multilib - linux-libc-dev:i386 + env: + - CC=gcc-9 + - CXX=g++-9 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 + # ANALYSIS + # Using the build wrapper from Sonar and collecting the code coverage. + # Define NDEBUG=1 to avoid assertion checks being reported as uncovered statements. + - 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') + - 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') + - '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' + - make clean + + # DEBUG + - cmake tests -DCMAKE_BUILD_TYPE=Debug -DNO_STATIC_ANALYSIS=1 + - make all VERBOSE=1 && make test + - make clean + + # RELEASE + - cmake tests -DCMAKE_BUILD_TYPE=Release -DNO_STATIC_ANALYSIS=1 + - make all VERBOSE=1 && make test + - make clean # -------------------- Clang 9 -------------------- - language: cpp @@ -29,32 +56,55 @@ matrix: apt: sources: - ubuntu-toolchain-r-test - packages: - - libstdc++-7-dev:i386 + 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: + # Set up the toolchain. - 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 + - sudo apt install clang-tidy-9 clang-format-9 + - clang++-9 -E -x c++ - -v < /dev/null # Print the Clang configuration for troubleshooting purposes. - # -------------------- Static analysis -------------------- - - language: cpp - script: - - sudo rm -rf /usr/local/clang* - - 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 + # 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 - # TODO: invoke clang-tidy. + # RELWITHDEBINFO + - cmake -DCMAKE_C_COMPILER=clang-9 -DCMAKE_CXX_COMPILER=clang++-9 tests -DCMAKE_BUILD_TYPE=RelWithDebInfo + - make VERBOSE=1 && make test + - make clean - # Code style enforcement. - - ./format.sh + # 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 - '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 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=c11 -mmcu=at90can64 -Wall -Wextra -Werror -pedantic -Wconversion -Wtype-limits git: depth: false # Disable shallow clone because it is incompatible with SonarCloud diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000..b1350c51 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,103 @@ +# 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 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. + +## 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. + +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 + +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 + +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 + +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/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/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/README.md b/README.md index 55f4c653..588971b9 100644 --- a/README.md +++ b/README.md @@ -1,114 +1,168 @@ -# 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) [![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) -[![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) +[![Forum](https://img.shields.io/discourse/users.svg?server=https%3A%2F%2Fforum.uavcan.org&color=1700b3)](https://forum.uavcan.org) -Minimal implementation of the UAVCAN protocol stack in C for resource constrained applications. +Libcanard is a compact implementation of the UAVCAN/CAN protocol stack in C11 for high-integrity real-time +embedded systems. -Get help 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)** -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. +Contribute: [`CONTRIBUTING.md`](/CONTRIBUTING.md) -The entire library is contained in three files under `libcanard/`: +Ask questions: [forum.uavcan.org](https://forum.uavcan.org) -- `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`. +## Example -Add `canard.c` to your application build, add `libcanard` directory to the include paths, -and you're ready to roll. +The example augments the documentation but does not replace it. -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. +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: -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`. +```c +static void* memAllocate(CanardInstance* const ins, const size_t amount) +{ + (void) ins; + return o1heapAllocate(my_allocator, amount); +} -Example for Make: +static void memFree(CanardInstance* const ins, void* const pointer) +{ + (void) ins; + o1heapFree(my_allocator, pointer); +} +``` + +Init a library instance: -```make -# Adding the library. -INCLUDE += libcanard/libcanard/ -CSRC += libcanard/libcanard/canard.c +```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. +``` -# 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 +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 = 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); +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(); +} ``` -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. -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 | | 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 | - +---------------+ - -## Library development - -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 +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. +} ``` -### Coverity Scan +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); +``` -First, [get the Coverity build tool](https://scan.coverity.com/download?tab=cxx). -Then build the tests with it: +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); +``` -```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 +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); ``` -Then upload the resulting archive to Coverity. +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/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 29643514..a3dfcd07 100644 --- a/libcanard/canard.c +++ b/libcanard/canard.c @@ -1,1421 +1,1149 @@ -/* - * 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 - */ - -#include "canard_internals.h" +/// 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 #include -#undef MIN -#undef MAX -#define MIN(a, b) (((a) < (b)) ? (a) : (b)) -#define MAX(a, b) (((a) > (b)) ? (a) : (b)) +// --------------------------------------------- BUILD CONFIGURATION --------------------------------------------- -#define TRANSFER_CRC_INITIAL 0xFFFFU +/// 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 -#define TRANSFER_TIMEOUT_USEC 2000000 +/// 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 // Consider defining an extra compilation option that turns this into "static inline"? +# define CANARD_PRIVATE static +#endif -#define TRANSFER_ID_BIT_LEN 5U +#if !defined(__STDC_VERSION__) || (__STDC_VERSION__ < 201112L) +# error "Unsupported language: ISO C11 or a newer version is required." +#endif -#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)) +// --------------------------------------------- COMMON CONSTANTS --------------------------------------------- -#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 BITS_PER_BYTE 8U +#define BYTE_MAX 0xFFU -#define TRANSFER_ID_FROM_TAIL_BYTE(x) ((uint8_t)((x) &0x1FU)) +#define CAN_EXT_ID_MASK ((UINT32_C(1) << 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)) +#define MFT_NON_LAST_FRAME_PAYLOAD_MIN 7U -struct CanardTxQueueItem -{ - CanardTxQueueItem* next; - CanardCANFrame frame; -}; +#define PADDING_BYTE_VALUE 0U -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); +#define OFFSET_PRIORITY 26U +#define OFFSET_SUBJECT_ID 8U +#define OFFSET_SERVICE_ID 14U +#define OFFSET_DST_NODE_ID 7U - memset(out_ins, 0, sizeof(*out_ins)); +#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) - 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; +#define TAIL_START_OF_TRANSFER 128U +#define TAIL_END_OF_TRANSFER 64U +#define TAIL_TOGGLE 32U - const uint16_t pool_capacity = (uint16_t) MIN(0xFFFFU, mem_arena_size / CANARD_MEM_BLOCK_SIZE); - initPoolAllocator(&out_ins->allocator, mem_arena, pool_capacity); -} +#define INITIAL_TOGGLE_STATE true -void* canardGetUserReference(CanardInstance* ins) +// --------------------------------------------- TRANSFER CRC --------------------------------------------- + +typedef uint16_t TransferCRC; + +#define CRC_INITIAL 0xFFFFU +#define CRC_RESIDUE 0x0000U +#define CRC_SIZE_BYTES 2U + +CANARD_PRIVATE TransferCRC crcAddByte(const TransferCRC crc, const uint8_t byte); +CANARD_PRIVATE TransferCRC crcAddByte(const TransferCRC crc, const uint8_t byte) { - CANARD_ASSERT(ins != NULL); - return ins->user_reference; + 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; } -int16_t canardSetLocalNodeID(CanardInstance* ins, uint8_t self_node_id) +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(ins != NULL); - - if ((ins->node_id == CANARD_BROADCAST_NODE_ID) && (self_node_id <= CANARD_MAX_NODE_ID)) + CANARD_ASSERT((data != NULL) || (size == 0U)); + TransferCRC out = crc; + const uint8_t* p = (const uint8_t*) data; + for (size_t i = 0; i < size; i++) { - ins->node_id = self_node_id; - } - else - { - return -CANARD_ERROR_INVALID_ARGUMENT; + out = crcAddByte(out, *p); + ++p; } + return out; +} + +// --------------------------------------------- TRANSMISSION --------------------------------------------- - return CANARD_OK; +/// 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 +{ + CanardFrame frame; + struct CanardInternalTxQueueItem* next; + + // 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[]; // 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); } -uint8_t canardGetLocalNodeID(const CanardInstance* ins) +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) { - return ins->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; } -int16_t canardPublishMessage(CanardInstance* ins, - uint16_t subject_id, - uint8_t* inout_transfer_id, - uint8_t priority, - const void* payload, - uint16_t payload_len) +/// This is the transport MTU rounded up to next full DLC minus the tail byte. +CANARD_PRIVATE size_t txGetPresentationLayerMTU(const CanardInstance* const ins); +CANARD_PRIVATE size_t txGetPresentationLayerMTU(const CanardInstance* const ins) { - if (payload == NULL && payload_len > 0) + const size_t max_index = (sizeof(CanardCANLengthToDLC) / sizeof(CanardCANLengthToDLC[0])) - 1U; + size_t mtu = 0U; + if (ins->mtu_bytes < CANARD_MTU_CAN_CLASSIC) { - return -CANARD_ERROR_INVALID_ARGUMENT; + mtu = CANARD_MTU_CAN_CLASSIC; } - if (priority > CANARD_TRANSFER_PRIORITY_OPTIONAL) + else if (ins->mtu_bytes <= max_index) { - 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); + mtu = CanardCANDLCToLength[CanardCANLengthToDLC[ins->mtu_bytes]]; // Round up to nearest valid length. } else { - if (payload_len > 7) - { - crc = crcAdd(crc, payload, payload_len); - } - - can_id |= ((uint32_t)(canardGetLocalNodeID(ins) & 0x7FU) << 1U); + mtu = CanardCANDLCToLength[CanardCANLengthToDLC[max_index]]; } - - const int16_t result = enqueueTxFrames(ins, can_id, inout_transfer_id, crc, payload, payload_len); - - incrementTransferID(inout_transfer_id); - - return result; + return mtu - 1U; } -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) +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) { - if (payload == NULL && payload_len > 0) + CANARD_ASSERT(tr != NULL); + CANARD_ASSERT(presentation_layer_mtu > 0); + int32_t out = -CANARD_ERROR_INVALID_ARGUMENT; + if ((tr->transfer_kind == CanardTransferKindMessage) && (CANARD_NODE_ID_UNSET == tr->remote_node_id) && + (tr->port_id <= CANARD_SUBJECT_ID_MAX)) { - return -CANARD_ERROR_INVALID_ARGUMENT; - } - if (priority > CANARD_TRANSFER_PRIORITY_OPTIONAL) - { - return -CANARD_ERROR_INVALID_ARGUMENT; + if (local_node_id <= CANARD_NODE_ID_MAX) + { + out = (int32_t) txMakeMessageSessionSpecifier(tr->port_id, local_node_id); + CANARD_ASSERT(out >= 0); + } + else if (tr->payload_size <= presentation_layer_mtu) + { + 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 = txMakeMessageSessionSpecifier(tr->port_id, c) | FLAG_ANONYMOUS_MESSAGE; + CANARD_ASSERT(spec <= CAN_EXT_ID_MASK); + out = (int32_t) spec; + } + else + { + out = -CANARD_ERROR_INVALID_ARGUMENT; // Anonymous multi-frame message trs are not allowed. + } } - if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) + 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)) { - return -CANARD_ERROR_NODE_ID_NOT_SET; // Service transfers not supported for anonymous nodes. + if (local_node_id <= CANARD_NODE_ID_MAX) + { + out = (int32_t) txMakeServiceSessionSpecifier(tr->port_id, + tr->transfer_kind == CanardTransferKindRequest, + local_node_id, + tr->remote_node_id); + CANARD_ASSERT(out >= 0); + } + else + { + out = -CANARD_ERROR_INVALID_ARGUMENT; // Anonymous service transfers are not allowed. + } } - - 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) + else { - crc = crcAdd(crc, payload, payload_len); + out = -CANARD_ERROR_INVALID_ARGUMENT; } - 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 + if (out >= 0) { - incrementTransferID(inout_transfer_id); + const uint32_t prio = (uint32_t) tr->priority; + if (prio <= CANARD_PRIORITY_MAX) + { + const uint32_t id = ((uint32_t) out) | (prio << OFFSET_PRIORITY); + out = (int32_t) id; + } + else + { + out = -CANARD_ERROR_INVALID_ARGUMENT; + } } - - return result; + return out; } -const CanardCANFrame* canardPeekTxQueue(const CanardInstance* ins) +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) { - if (ins->tx_queue == NULL) - { - return NULL; - } - return &ins->tx_queue->frame; + 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)); } -void canardPopTxQueue(CanardInstance* ins) +/// Takes a frame payload size, returns a new size that is >=x and is rounded up to the nearest valid DLC. +CANARD_PRIVATE size_t txRoundFramePayloadSizeUp(const size_t x); +CANARD_PRIVATE size_t txRoundFramePayloadSizeUp(const size_t x) { - CanardTxQueueItem* item = ins->tx_queue; - ins->tx_queue = item->next; - freeBlock(&ins->allocator, item); + 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. + const size_t y = CanardCANLengthToDLC[x]; // NOSONAR + CANARD_ASSERT(y < (sizeof(CanardCANDLCToLength) / sizeof(CanardCANDLCToLength[0]))); + return CanardCANDLCToLength[y]; } -int16_t canardHandleRxFrame(CanardInstance* ins, const CanardCANFrame* frame, uint64_t timestamp_usec) +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) { - 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)) + CANARD_ASSERT(ins != NULL); + CANARD_ASSERT(payload_size > 0U); + CanardInternalTxQueueItem* const out = + (CanardInternalTxQueueItem*) ins->memory_allocate(ins, sizeof(CanardInternalTxQueueItem) + payload_size); + if (out != NULL) { - return -CANARD_ERROR_RX_INCOMPATIBLE_PACKET; + 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; +} - if (transfer_kind != CanardTransferKindMessagePublication && destination_node_id != canardGetLocalNodeID(ins)) +/// 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_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); + CanardInternalTxQueueItem* out = ins->_tx_queue; + if ((NULL == out) || (out->frame.extended_can_id > can_id)) { - return -CANARD_ERROR_RX_WRONG_ADDRESS; + 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->frame.extended_can_id <= can_id)) + { + out = out->next; + } + } + CANARD_ASSERT((out == NULL) || (out->frame.extended_can_id <= can_id)); + return out; +} - 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); +/// Returns the number of frames enqueued or error (i.e., =1 or <0). +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)); - CanardRxState* rx_state = NULL; + 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; - if (IS_START_OF_TRANSFER(tail_byte)) + CanardInternalTxQueueItem* const tqi = txAllocateQueueItem(ins, can_id, deadline_usec, frame_payload_size); + if (tqi != NULL) { - if (ins->should_accept(ins, port_id, transfer_kind, source_node_id)) + if (payload_size > 0U) // The check is needed to avoid calling memcpy() with a NULL pointer, it's an UB. { - rx_state = traverseRxStates(ins, session_specifier); + CANARD_ASSERT(payload != NULL); + // 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(&tqi->payload_buffer[0], payload, payload_size); // NOLINT + } - if (rx_state == NULL) - { - return -CANARD_ERROR_OUT_OF_MEMORY; - } + // Clang-Tidy raises an error recommending the use of memset_s() instead. + // 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 - 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; + 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; + sup->next = tqi; } else { - return -CANARD_ERROR_RX_NOT_WANTED; + tqi->next = ins->_tx_queue; + ins->_tx_queue = tqi; } + out = 1; // One frame enqueued. } 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; - } + out = -CANARD_ERROR_OUT_OF_MEMORY; } + CANARD_ASSERT((out < 0) || (out == 1)); + return out; +} - 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) +/// Returns the number of frames enqueued or error. +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); + 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; + + const size_t payload_size_with_crc = payload_size + CRC_SIZE_BYTES; + size_t offset = 0U; + TransferCRC crc = crcAdd(CRC_INITIAL, payload_size, payload); + bool start_of_transfer = true; + bool toggle = INITIAL_TOGGLE_STATE; + const uint8_t* payload_ptr = (const uint8_t*) payload; + + while (offset < payload_size_with_crc) + { + ++out; + const size_t frame_payload_size_with_tail = + ((payload_size_with_crc - offset) < presentation_layer_mtu) + ? 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); + if (NULL == head) { - return -CANARD_ERROR_RX_SHORT_FRAME; + head = tqi; } - if (TOGGLE_BIT(tail_byte) != (bool) rx_state->next_toggle) + else { - return -CANARD_ERROR_RX_WRONG_TOGGLE; + tail->next = tqi; } - if (TRANSFER_ID_FROM_TAIL_BYTE(tail_byte) != (uint8_t) rx_state->transfer_id) + tail = tqi; + if (NULL == tail) { - return -CANARD_ERROR_RX_UNEXPECTED_TID; + break; } - if (!IS_END_OF_TRANSFER(tail_byte)) // Any multi frame that do not complete the transfer + + // Copy the payload into the frame. + const size_t frame_payload_size = frame_payload_size_with_tail - 1U; + size_t frame_offset = 0U; + if (offset < payload_size) { - const int16_t ret = - bufferBlockPushBytes(&ins->allocator, rx_state, frame->data, (uint8_t)(frame->data_len - 1)); - if (ret < 0) + size_t move_size = payload_size - offset; + if (move_size > frame_payload_size) { - releaseStatePayload(ins, rx_state); - prepareForNextTransfer(rx_state); - return -CANARD_ERROR_OUT_OF_MEMORY; + move_size = frame_payload_size; } - rx_state->calculated_crc = - crcAdd((uint16_t) rx_state->calculated_crc, frame->data, (uint8_t)(frame->data_len - 1)); + // 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(&tail->payload_buffer[0], payload_ptr, move_size); // NOLINT + frame_offset = frame_offset + move_size; + offset += move_size; + payload_ptr += move_size; } - 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 + // Handle the last frame of the transfer: it is special because it also contains padding and CRC. + 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) { - // 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]; - } - } + tail->payload_buffer[frame_offset] = PADDING_BYTE_VALUE; + ++frame_offset; + crc = crcAddByte(crc, PADDING_BYTE_VALUE); } - 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) + // Insert the CRC. + if ((frame_offset < frame_payload_size) && (offset == payload_size)) { - ins->on_reception(ins, &rx_transfer); + tail->payload_buffer[frame_offset] = (uint8_t)(crc >> BITS_PER_BYTE); + ++frame_offset; + ++offset; } - - // 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) + if ((frame_offset < frame_payload_size) && (offset > payload_size)) { - return -CANARD_ERROR_RX_BAD_CRC; + tail->payload_buffer[frame_offset] = (uint8_t)(crc & BYTE_MAX); + ++frame_offset; + ++offset; } } - rx_state->next_toggle = (bool) rx_state->next_toggle ? 0 : 1; + // Finalize the frame. + 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; } - 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) + if (tail != NULL) { - 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) + 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 = txFindQueueSupremum(ins, can_id); + if (NULL == sup) // Once the insertion point is located, we insert the entire frame sequence in constant time. { - 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)); - } + tail->next = ins->_tx_queue; + ins->_tx_queue = head; } 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; + tail->next = sup->next; + sup->next = head; } } - 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 + 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) { - CANARD_ASSERT(false); - return -CANARD_ERROR_INTERNAL; + CanardInternalTxQueueItem* const next = head->next; + ins->memory_free(ins, head); + head = next; } } - CANARD_ASSERT(result <= bit_length); - CANARD_ASSERT(result > 0); - return result; + CANARD_ASSERT((out < 0) || (out >= 2)); + return out; } -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; +// --------------------------------------------- RECEPTION --------------------------------------------- - // 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); +#define RX_SESSIONS_PER_SUBSCRIPTION (CANARD_NODE_ID_MAX + 1U) - 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) +/// The memory requirement model provided in the documentation assumes that the maximum size of this structure never +/// 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 { - 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) + 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 transfer_id; + uint8_t redundant_transport_index; ///< Arbitrary value in [0, 255]. + bool toggle; +} CanardInternalRxSession; + +/// High-level transport frame model. +typedef struct { - return ins->allocator.statistics; -} - -uint16_t canardConvertNativeFloatToFloat16(float value) + 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 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); +CANARD_PRIVATE bool rxTryParseFrame(const CanardFrame* const frame, RxFrameModel* const out) { - 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) + CANARD_ASSERT(frame != NULL); + CANARD_ASSERT(frame->extended_can_id <= CAN_EXT_ID_MASK); + CANARD_ASSERT(out != NULL); + bool valid = false; + if (frame->payload_size > 0) + { + CANARD_ASSERT(frame->payload != NULL); + out->timestamp_usec = frame->timestamp_usec; + + // CAN ID parsing. + 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)) { - in.u = f16inf.u; + 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->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 + { + 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); } - out = (uint16_t)(in.u >> 13U); - } - - out |= (uint16_t)(sign >> 16U); - return out; + // Payload parsing. + out->payload_size = frame->payload_size - 1U; // Cut off the tail byte. + out->payload = frame->payload; + + // Tail byte parsing. + // 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. + // Protocol version check: if SOT is set, then the toggle shall also be set. + 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)); + // 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; } -float canardConvertFloat16ToNativeFloat(uint16_t value) +CANARD_PRIVATE void rxInitTransferFromFrame(const RxFrameModel* const frame, CanardTransfer* const out_transfer); +CANARD_PRIVATE void rxInitTransferFromFrame(const RxFrameModel* const frame, CanardTransfer* const out_transfer) { - 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_ASSERT(frame != NULL); + CANARD_ASSERT(frame->payload != NULL); + CANARD_ASSERT(out_transfer != NULL); + 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; + // Payload not populated. } -CANARD_INTERNAL int16_t computeTransferIDForwardDistance(uint8_t a, uint8_t b) +/// The implementation is borrowed from the Specification. +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 d = (int16_t)(b - a); - if (d < 0) + 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) { - d = (int16_t)(d + (int16_t)(1U << TRANSFER_ID_BIT_LEN)); + const uint8_t modulo = 1U << CANARD_TRANSFER_ID_BIT_LENGTH; + diff = (int16_t)(diff + (int16_t) modulo); } - return d; + return (uint8_t) diff; } -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_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) { CANARD_ASSERT(ins != NULL); - CANARD_ASSERT((can_id & CANARD_CAN_EXT_ID_MASK) == can_id); // Flags must be cleared + 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); - if (transfer_id == NULL) - { - return -CANARD_ERROR_INVALID_ARGUMENT; - } + rxs->total_payload_size += payload_size; - if ((payload_len > 0) && (payload == NULL)) + // Allocate the payload lazily, as late as possible. + if ((NULL == rxs->payload) && (payload_size_max > 0U)) { - return -CANARD_ERROR_INVALID_ARGUMENT; + CANARD_ASSERT(rxs->payload_size == 0); + rxs->payload = ins->memory_allocate(ins, payload_size_max); } - int16_t result = 0; - - if (payload_len < CANARD_CAN_FRAME_MAX_DATA_LEN) // Single frame transfer + int8_t out = 0; + if (rxs->payload != NULL) { - CanardTxQueueItem* queue_item = createTxItem(&ins->allocator); - if (queue_item == NULL) + // 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_max) { - return -CANARD_ERROR_OUT_OF_MEMORY; + 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); } - - memcpy(queue_item->frame.data, payload, payload_len); - - 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++; + // 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. + // 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); } - else // Multi frame transfer + else { - 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; - } + CANARD_ASSERT(rxs->payload_size == 0); + out = (payload_size_max > 0U) ? -CANARD_ERROR_OUT_OF_MEMORY : 0; } - - return result; + CANARD_ASSERT(out <= 0); + return out; } -CANARD_INTERNAL void pushTxQueue(CanardInstance* ins, CanardTxQueueItem* item) +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(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_ASSERT(rxs != NULL); + ins->memory_free(ins, rxs->payload); // May be NULL, which is OK. + 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_INTERNAL bool isPriorityHigher(uint32_t rhs, uint32_t id) +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) { - 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; - } + 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); - /* - * 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) + if (frame->start_of_transfer) // The transfer timestamp is the timestamp of its first frame. { - return rhs_rtr; + rxs->transfer_timestamp_usec = frame->timestamp_usec; } - /* - * 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) + const bool single_frame = frame->start_of_transfer && frame->end_of_transfer; + if (!single_frame) { - return SUBJECT_TYPE_FROM_ID(id); + // 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); } - 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) + int8_t out = rxSessionWritePayload(ins, rxs, payload_size_max, frame->payload_size, frame->payload); + if (out < 0) { - return CanardTransferKindMessagePublication; + CANARD_ASSERT(-CANARD_ERROR_OUT_OF_MEMORY == out); + rxSessionRestart(ins, rxs); // Out-of-memory. } - if (REQUEST_NOT_RESPONSE_FROM_ID(id) == 1) + else if (frame->end_of_transfer) { - 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) + CANARD_ASSERT(0 == out); + if (single_frame || (CRC_RESIDUE == rxs->calculated_crc)) { - return NULL; - } - ins->rx_states = states; - return states; - } - - states = findRxState(states, session_specifier); - if (states != NULL) - { - return states; - } - return prependRxState(ins, session_specifier); -} + 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; + + // 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; + } -CANARD_INTERNAL CanardRxState* findRxState(CanardRxState* state, uint32_t session_specifier) -{ - while (state != NULL) - { - if (state->session_specifier == session_specifier) - { - return state; + rxs->payload = NULL; // Ownership passed over to the application, nullify to prevent freeing. } - state = state->next; + rxSessionRestart(ins, rxs); // Successful completion. } - return NULL; -} - -CANARD_INTERNAL CanardRxState* prependRxState(CanardInstance* ins, uint32_t session_specifier) -{ - CanardRxState* state = createRxState(&ins->allocator, session_specifier); - - if (state == NULL) + else { - return NULL; + rxs->toggle = !rxs->toggle; } - - state->next = ins->rx_states; - ins->rx_states = state; - return state; + return out; } -CANARD_INTERNAL CanardRxState* createRxState(CanardPoolAllocator* allocator, uint32_t session_specifier) +/// 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 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, + 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) { - 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)); + CANARD_ASSERT(ins != NULL); + 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); - return state; -} + const bool tid_timed_out = (frame->timestamp_usec > rxs->transfer_timestamp_usec) && + ((frame->timestamp_usec - rxs->transfer_timestamp_usec) > transfer_id_timeout_usec); -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; -} + const bool not_previous_tid = rxComputeTransferIDDifference(rxs->transfer_id, frame->transfer_id) > 1; -CANARD_INTERNAL int16_t bufferBlockPushBytes(CanardPoolAllocator* allocator, - CanardRxState* state, - const uint8_t* data, - uint8_t data_len) -{ - uint16_t data_index = 0; + const bool need_restart = tid_timed_out || ((rxs->redundant_transport_index == redundant_transport_index) && + frame->start_of_transfer && not_previous_tid); - // if head is not full, add data to head - if ((CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE - state->payload_len) > 0) + if (need_restart) { - 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; + rxs->total_payload_size = 0U; + rxs->payload_size = 0U; + rxs->calculated_crc = CRC_INITIAL; + rxs->transfer_id = frame->transfer_id; + rxs->toggle = INITIAL_TOGGLE_STATE; + rxs->redundant_transport_index = redundant_transport_index; + } - // buffer blocks uninitialized - if (state->buffer_blocks == NULL) + int8_t out = 0; + if (need_restart && (!frame->start_of_transfer)) { - 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; + rxSessionRestart(ins, rxs); // SOT-miss, no point going further. } else { - uint16_t nth_block = 1; - - // get to block - block = state->buffer_blocks; - while (block->next != NULL) + const bool correct_transport = (rxs->redundant_transport_index == redundant_transport_index); + 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) { - nth_block++; - block = block->next; + out = rxSessionAcceptFrame(ins, rxs, frame, payload_size_max, out_transfer); } + } + return out; +} - 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) +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); + 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); + + int8_t out = 0; + 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) { - block->next = createBufferBlock(allocator); - if (block->next == NULL) + CanardInternalRxSession* const rxs = + (CanardInternalRxSession*) ins->memory_allocate(ins, sizeof(CanardInternalRxSession)); + subscription->_sessions[frame->source_node_id] = rxs; + 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->transfer_id = frame->transfer_id; + rxs->redundant_transport_index = redundant_transport_index; + rxs->toggle = INITIAL_TOGGLE_STATE; + } + else { - return -CANARD_ERROR_OUT_OF_MEMORY; + out = -CANARD_ERROR_OUT_OF_MEMORY; } - block = block->next; + } + // 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(out == 0); + out = rxSessionUpdate(ins, + subscription->_sessions[frame->source_node_id], + frame, + redundant_transport_index, + subscription->_transfer_id_timeout_usec, + subscription->_payload_size_max, + out_transfer); } } - - // add data to current block until it becomes full, add new block if necessary - while (data_index < data_len) + else { - for (uint16_t i = index_at_nth_block; i < CANARD_BUFFER_BLOCK_DATA_SIZE && data_index < data_len; - i++, data_index++) + 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. + // 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) { - block->data[i] = data[data_index]; + 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; } - - if (data_index < data_len) + else { - block->next = createBufferBlock(allocator); - if (block->next == NULL) - { - return -CANARD_ERROR_OUT_OF_MEMORY; - } - block = block->next; - index_at_nth_block = 0; + out = -CANARD_ERROR_OUT_OF_MEMORY; } } - - state->payload_len = - (uint16_t)((size_t) state->payload_len + data_len) & ((1U << CANARD_TRANSFER_PAYLOAD_LEN_BITS) - 1U); - - return 1; + return out; } -CANARD_INTERNAL CanardBufferBlock* createBufferBlock(CanardPoolAllocator* allocator) -{ - CanardBufferBlock* block = (CanardBufferBlock*) allocateBlock(allocator); - if (block == NULL) - { - return NULL; - } - block->next = NULL; - return block; -} +// --------------------------------------------- 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-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 +}; -void copyBitArray(const uint8_t* src, uint32_t src_offset, uint32_t src_len, uint8_t* dst, uint32_t dst_offset) +CanardInstance canardInit(const CanardMemoryAllocate memory_allocate, const CanardMemoryFree memory_free) { - 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_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, + .memory_allocate = memory_allocate, + .memory_free = memory_free, + ._rx_subscriptions = {NULL, NULL, NULL}, + ._tx_queue = NULL, + }; + return out; } -CANARD_INTERNAL int16_t descatterTransferPayload(const CanardRxTransfer* transfer, - uint32_t bit_offset, - uint8_t bit_length, - void* output) +int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfer) { - 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) + int32_t out = -CANARD_ERROR_INVALID_ARGUMENT; + if ((ins != NULL) && (transfer != NULL) && ((transfer->payload != NULL) || (0U == transfer->payload_size))) { - 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)) + 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) { - 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) + if (transfer->payload_size <= pl_mtu) { - 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); + out = txPushSingleFrame(ins, + transfer->timestamp_usec, + (uint32_t) maybe_can_id, + transfer->transfer_id, + transfer->payload_size, + transfer->payload); + } + else + { + out = txPushMultiFrame(ins, + pl_mtu, + transfer->timestamp_usec, + (uint32_t) maybe_can_id, + transfer->transfer_id, + transfer->payload_size, + transfer->payload); } - - 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)) + else { - 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; + out = maybe_can_id; } - - 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; + return out; } -CANARD_INTERNAL bool isBigEndian(void) +const CanardFrame* canardTxPeek(const CanardInstance* const ins) { -#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 + const CanardFrame* out = NULL; + if ((ins != NULL) && (ins->_tx_queue != NULL)) + { + // 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; } -CANARD_INTERNAL void swapByteOrder(void* data, size_t size) +void canardTxPop(CanardInstance* const ins) { - CANARD_ASSERT(data != NULL); - uint8_t* const bytes = (uint8_t*) data; - size_t fwd = 0; - size_t rev = size - 1; - while (fwd < rev) + if ((ins != NULL) && (ins->_tx_queue != NULL)) { - const uint8_t x = bytes[fwd]; - bytes[fwd] = bytes[rev]; - bytes[rev] = x; - fwd++; - rev--; + // The memory is NOT deallocated. The application is responsible for that. + ins->_tx_queue = ins->_tx_queue->next; } } -CANARD_INTERNAL uint16_t crcAddByte(uint16_t crc_val, uint8_t byte) +int8_t canardRxAccept(CanardInstance* const ins, + const CanardFrame* const frame, + const uint8_t redundant_transport_index, + CanardTransfer* const out_transfer) { - crc_val ^= (uint16_t)((uint16_t)(byte) << 8U); - for (uint8_t j = 0; j < 8; j++) + 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) || (0 == frame->payload_size))) { - if (crc_val & 0x8000U) + RxFrameModel model = {0}; + if (rxTryParseFrame(frame, &model)) { - crc_val = (uint16_t)((uint16_t)(crc_val << 1U) ^ 0x1021U); + if ((CANARD_NODE_ID_UNSET == model.destination_node_id) || (ins->node_id == model.destination_node_id)) + { + // 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 = rxAcceptFrame(ins, sub, &model, redundant_transport_index, out_transfer); + } + else + { + out = 0; // No matching subscription. + } + } + else + { + out = 0; // Mis-addressed frame (normally it should be filtered out by the hardware). + } } else { - crc_val = (uint16_t)(crc_val << 1U); + out = 0; // A non-UAVCAN/CAN input frame. } } - return crc_val; + CANARD_ASSERT(out <= 1); + return out; } -CANARD_INTERNAL uint16_t crcAdd(uint16_t crc_val, const uint8_t* bytes, size_t len) +int8_t canardRxSubscribe(CanardInstance* const ins, + const CanardTransferKind transfer_kind, + const CanardPortID port_id, + const size_t payload_size_max, + const CanardMicrosecond transfer_id_timeout_usec, + CanardRxSubscription* const out_subscription) { - while (len--) - { - crc_val = crcAddByte(crc_val, *bytes++); + int8_t out = -CANARD_ERROR_INVALID_ARGUMENT; + const size_t tk = (size_t) transfer_kind; + if ((ins != NULL) && (out_subscription != NULL) && (tk < CANARD_NUM_TRANSFER_KINDS)) + { + // 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 < 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. + // 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; + 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; + out = (out > 0) ? 0 : 1; + } } - return crc_val; + return out; } -CANARD_INTERNAL void initPoolAllocator(CanardPoolAllocator* allocator, CanardPoolAllocatorBlock* buf, uint16_t buf_len) +int8_t canardRxUnsubscribe(CanardInstance* const ins, + const CanardTransferKind transfer_kind, + const CanardPortID port_id) { - size_t current_index = 0; - CanardPoolAllocatorBlock** current_block = &(allocator->free_list); - while (current_index < buf_len) + int8_t out = -CANARD_ERROR_INVALID_ARGUMENT; + const size_t tk = (size_t) transfer_kind; + if ((ins != NULL) && (tk < CANARD_NUM_TRANSFER_KINDS)) { - *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; -} + CanardRxSubscription* prv = NULL; + CanardRxSubscription* sub = ins->_rx_subscriptions[tk]; + while ((sub != NULL) && (sub->_port_id != port_id)) + { + prv = sub; + sub = sub->_next; + } -CANARD_INTERNAL void* allocateBlock(CanardPoolAllocator* allocator) -{ - // Check if there are any blocks available in the free list. - if (allocator->free_list == NULL) - { - return NULL; - } + if (sub != NULL) + { + CANARD_ASSERT(sub->_port_id == port_id); + out = 1; - // Take first available block and prepares next block for use. - void* result = allocator->free_list; - allocator->free_list = allocator->free_list->next; + if (prv != NULL) + { + prv->_next = sub->_next; + } + else + { + ins->_rx_subscriptions[tk] = sub->_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; + 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]); + sub->_sessions[i] = NULL; + } + } + else + { + out = 0; + } } - - return result; -} - -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--; + return out; } diff --git a/libcanard/canard.h b/libcanard/canard.h index 158337cf..9709612a 100644 --- a/libcanard/canard.h +++ b/libcanard/canard.h @@ -1,528 +1,603 @@ -/* - * 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 +/// __ __ _______ __ __ _______ _______ __ __ +/// | | | | / _ ` | | | | / ____| / _ ` | ` | | +/// | | | | | |_| | | | | | | | | |_| | | `| | +/// | |_| | | _ | ` `_/ / | |____ | _ | | |` | +/// `_______/ |__| |__| `_____/ `_______| |__| |__| |__| `__| +/// | | | | | | +/// ----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 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. +/// +/// 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 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 +/// 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 -#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 +#include +#include #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 +/// 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 100 /// 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 (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 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 -/// 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 +/// 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 -#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). -/// 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 +/// 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 +#define CANARD_PRIORITY_MAX 7U +#define CANARD_TRANSFER_ID_BIT_LENGTH 5U +#define CANARD_TRANSFER_ID_MAX ((1U << CANARD_TRANSFER_ID_BIT_LENGTH) - 1U) -/// 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 -#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. - */ -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; -} CanardCANFrame; - -/** - * Transfer types are defined by the UAVCAN specification. - */ -typedef enum -{ - CanardTransferKindServiceResponse = 0, - CanardTransferKindServiceRequest = 1, - CanardTransferKindMessagePublication = 2 -} CanardTransferKind; +/// This value represents an undefined node-ID: broadcast destination or anonymous source. +/// Library functions treat all values above CANARD_NODE_ID_MAX as anonymous. +#define CANARD_NODE_ID_UNSET 255U -/** - * Types of service transfers. These are not applicable to message transfers. - */ -typedef enum -{ - CanardResponse, - CanardRequest -} CanardRequestResponse; +/// 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 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. - */ -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 struct CanardInstance CanardInstance; +typedef uint64_t CanardMicrosecond; +typedef uint16_t CanardPortID; +typedef uint8_t CanardNodeID; +typedef uint8_t CanardTransferID; -typedef union CanardPoolAllocatorBlock_u +/// Transfer priority level mnemonics per the recommendations given in the UAVCAN Specification. +typedef enum { - char bytes[CANARD_MEM_BLOCK_SIZE]; - union CanardPoolAllocatorBlock_u* next; -} CanardPoolAllocatorBlock; + CanardPriorityExceptional = 0, + CanardPriorityImmediate = 1, + CanardPriorityFast = 2, + CanardPriorityHigh = 3, + CanardPriorityNominal = 4, ///< Nominal priority level should be the default. + CanardPriorityLow = 5, + CanardPrioritySlow = 6, + CanardPriorityOptional = 7, +} CanardPriority; + +/// Transfer kinds as defined by the UAVCAN Specification. +typedef enum +{ + 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; +#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 { - CanardPoolAllocatorBlock* free_list; - CanardPoolAllocatorStatistics statistics; -} CanardPoolAllocator; - -typedef struct CanardBufferBlock + /// For RX frames: reception timestamp. + /// For TX frames: transmission deadline. + /// 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 shall be zero. + 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 + /// 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; + +/// 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]; + +/// 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 { - struct CanardBufferBlock* next; - uint8_t data[]; -} CanardBufferBlock; - -struct CanardRxState + /// For RX transfers: reception timestamp. + /// For TX transfers: transmission deadline. + /// 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. + /// If this is not the case, then this field contains the priority level of the last frame to arrive. + CanardPriority priority; + + CanardTransferKind transfer_kind; + + /// Subject-ID for message publications; service-ID for service requests/responses. + CanardPortID port_id; + + /// 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; + + /// 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. + /// 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; + +/// 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 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 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 an intentional time-memory trade-off: use a large look-up table to ensure predictable temporal properties. +typedef struct CanardRxSubscription { - struct CanardRxState* next; - - CanardBufferBlock* buffer_blocks; - - uint64_t timestamp_usec; - - const uint32_t session_specifier; - - // 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. - - uint16_t payload_crc; - - uint8_t buffer_head[]; -}; -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 CanardRxSubscription* _next; ///< Internal use only. + + /// 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 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. + struct CanardInternalRxSession* _sessions[CANARD_NODE_ID_MAX + 1U]; + + 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); + +/// 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. +/// The application may directly alter the fields whose names do not begin with an underscore. struct CanardInstance { - uint8_t node_id; ///< Local node-ID; may be zero if the node is anonymous - - CanardShouldAcceptTransfer should_accept; ///< Function to decide whether the application wants this transfer - CanardOnTransferReception on_reception; ///< Function the library calls after RX transfer is complete - - CanardPoolAllocator allocator; ///< Pool allocator - - CanardRxState* rx_states; ///< RX transfer states - CanardTxQueueItem* tx_queue; ///< TX frames awaiting transmission - - void* user_reference; ///< User pointer that can link this instance with other objects -}; - -/** - * 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 -{ - /** - * 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 + /// 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 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 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. + /// Per the UAVCAN Specification, the node-ID should not be assigned more than once. + /// Invalid values are treated as CANARD_NODE_ID_UNSET. The default value is CANARD_NODE_ID_UNSET. + CanardNodeID node_id; + + /// 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 following API functions may allocate memory: canardRxAccept(), canardTxPush(). + /// The following API functions may deallocate memory: canardRxAccept(), canardRxSubscribe(), canardRxUnsubscribe(). + /// The exact memory requirement and usage model is specified for each function in its documentation. + CanardMemoryAllocate memory_allocate; + CanardMemoryFree memory_free; + + /// These fields are for internal use only. Do not access from the application. + CanardRxSubscription* _rx_subscriptions[CANARD_NUM_TRANSFER_KINDS]; + struct CanardInternalTxQueueItem* _tx_queue; }; -/** - * 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. - */ -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"); +/// Construct 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. +/// +/// 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); + +/// 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 +/// 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 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. +/// - 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. +/// - 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). +/// +/// 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 deallocated automatically. In other words, either all frames +/// of the transfer are enqueued successfully, or none are. +/// +/// 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) + sizeof(void*) + MTU). +int32_t canardTxPush(CanardInstance* const ins, const CanardTransfer* const transfer); + +/// 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 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 +/// 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 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 it. +/// +/// The time complexity is constant. This function does not invoke the dynamic memory manager. +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 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 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. 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, 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(). +/// 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 (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 +/// (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 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. +/// +/// 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 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 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 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), 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); + +/// 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. +/// +/// 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 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, +/// the existing subscription is terminated and then a new one is created in its place. Pending transfers may be lost. +/// 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; 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). +/// 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, + const size_t payload_size_max, + const CanardMicrosecond transfer_id_timeout_usec, + CanardRxSubscription* const out_subscription); + +/// 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. +/// 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); #ifdef __cplusplus } diff --git a/libcanard/canard_dsdl.c b/libcanard/canard_dsdl.c new file mode 100644 index 00000000..de171ffe --- /dev/null +++ b/libcanard/canard_dsdl.c @@ -0,0 +1,434 @@ +/// 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 --------------------------------------------- + +/// 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 +// 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_CONFIG_EXPOSE_PRIVATE) && CANARD_CONFIG_EXPOSE_PRIVATE +# define CANARD_PRIVATE +#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) +# error "Unsupported language: ISO C11 or a newer version is required." +#endif + +/// 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 \ + ((FLT_RADIX == 2) && (DBL_MANT_DIG == 53) && (DBL_MIN_EXP == -1021) && (DBL_MAX_EXP == 1024)) + +// --------------------------------------------- COMMON ITEMS --------------------------------------------- + +/// Per the DSDL specification, 1 byte = 8 bits. +#define BYTE_WIDTH 8U +#define BYTE_MAX 0xFFU + +#define WIDTH16 16U +#define WIDTH32 32U +#define WIDTH64 64U + +// --------------------------------------------- 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; +} + +/// 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 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, + const uint8_t* const src, + uint8_t* const dst) +{ + 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 ((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. + // 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 + { + 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_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 uint8_t size = (uint8_t) chooseMin(BYTE_WIDTH - max_mod, last_bit - src_off); + CANARD_ASSERT((size > 0U) && (size <= BYTE_WIDTH)); + + // 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. + // 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; + } + CANARD_ASSERT(last_bit == src_off); + } +} + +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, chooseMin(requested_length_bit, value_length_bit)); +} + +// --------------------------------------------- PUBLIC API - INTEGER --------------------------------------------- + +void canardDSDLSetBit(uint8_t* const buf, const size_t off_bit, const bool value) +{ + CANARD_ASSERT(buf != NULL); + 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) +{ + _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(saturated_len_bit, 0U, off_bit, (const uint8_t*) &value, buf); +#else + 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 +} + +void canardDSDLSetIxx(uint8_t* const buf, const size_t off_bit, const int64_t value, const uint8_t len_bit) +{ + // 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); +} + +bool canardDSDLGetBit(const uint8_t* const buf, const size_t buf_size, const size_t off_bit) +{ + 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) +{ + CANARD_ASSERT(buf != NULL); + 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, len_bit, WIDTH16); + CANARD_ASSERT(copy_size <= (sizeof(uint16_t) * BYTE_WIDTH)); +#if CANARD_DSDL_CONFIG_LITTLE_ENDIAN + 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]); + return (uint16_t)(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, len_bit, WIDTH32); + CANARD_ASSERT(copy_size <= (sizeof(uint32_t) * BYTE_WIDTH)); +#if CANARD_DSDL_CONFIG_LITTLE_ENDIAN + 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]); + 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 +} + +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, len_bit, WIDTH64); + CANARD_ASSERT(copy_size <= (sizeof(uint64_t) * BYTE_WIDTH)); +#if CANARD_DSDL_CONFIG_LITTLE_ENDIAN + 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]); + 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 +} + +int8_t canardDSDLGetI8(const uint8_t* const buf, const size_t buf_size, const size_t off_bit, const uint8_t len_bit) +{ + 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) +{ + 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) +{ + 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) +{ + 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; +} + +// --------------------------------------------- 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. + union // NOSONAR + { + uint32_t in; + CanardDSDLFloat32 fl; + } const tmp = {canardDSDLGetU32(buf, buf_size, off_bit, WIDTH32)}; // NOSONAR + 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. + union // NOSONAR + { + uint64_t in; + CanardDSDLFloat64 fl; + } const tmp = {canardDSDLGetU64(buf, buf_size, off_bit, WIDTH64)}; // NOSONAR + 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 new file mode 100644 index 00000000..482f0d1e --- /dev/null +++ b/libcanard/canard_dsdl.h @@ -0,0 +1,95 @@ +/// __ __ _______ __ __ _______ _______ __ __ +/// | | | | / _ ` | | | | / ____| / _ ` | ` | | +/// | | | | | |_| | | | | | | | | |_| | | `| | +/// | |_| | | _ | ` `_/ / | |____ | _ | | |` | +/// `_______/ |__| |__| `_____/ `_______| |__| |__| |__| `__| +/// | | | | | | +/// ----o------o------------o---------o------o---------o------- +/// +/// 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. +/// +/// 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 + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef float CanardDSDLFloat32; +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. +/// 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 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); +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); + +/// 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 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 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. +/// +/// 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. 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 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 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); +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 +} +#endif +#endif // CANARD_DSDL_H_INCLUDED 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 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/sonar-project.properties b/sonar-project.properties deleted file mode 100644 index fc1342bf..00000000 --- a/sonar-project.properties +++ /dev/null @@ -1,8 +0,0 @@ -sonar.projectKey=libcanard -sonar.organization=uavcan - -sonar.sources=libcanard -sonar.sourceEncoding=UTF-8 - -sonar.cfamily.build-wrapper-output=sonar-dump -sonar.cfamily.cache.enabled=false diff --git a/tests/.clang-tidy b/tests/.clang-tidy new file mode 100644 index 00000000..0c248721 --- /dev/null +++ b/tests/.clang-tidy @@ -0,0 +1,35 @@ +--- +# 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-*, + -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, + -cppcoreguidelines-avoid-magic-numbers, + -cppcoreguidelines-pro-type-union-access, + -cppcoreguidelines-pro-type-reinterpret-cast, +WarningsAsErrors: '*' +HeaderFilterRegex: '.*\.hpp' +AnalyzeTemporaryDtors: false +FormatStyle: file +... diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index caf963ec..094b7ff8 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,47 +1,89 @@ -# -# 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_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") 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}) +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_flags link_flags) + add_executable(${name} ${common_sources} ${files}) + target_compile_definitions(${name} PUBLIC ${compile_definitions}) + 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}_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}" "-g -O0 --coverage" "--coverage") + endif () +endfunction() + +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" + "") 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/exposed.hpp b/tests/exposed.hpp new file mode 100644 index 00000000..175c754f --- /dev/null +++ b/tests/exposed.hpp @@ -0,0 +1,125 @@ +// 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 +#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 exposed +{ +using TransferCRC = std::uint16_t; + +struct TxQueueItem final +{ + CanardFrame frame{}; + TxQueueItem* next = nullptr; + + [[nodiscard]] auto getPayloadByte(const std::size_t offset) const -> std::uint8_t + { + return reinterpret_cast(frame.payload)[offset]; + } + + [[nodiscard]] auto getTailByte() const + { + REQUIRE(frame.payload_size >= 1U); + return getPayloadByte(frame.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; } + + ~TxQueueItem() = default; + TxQueueItem(const TxQueueItem&) = delete; + TxQueueItem(const TxQueueItem&&) = delete; + auto operator=(const TxQueueItem&) -> TxQueueItem& = delete; + auto operator=(const TxQueueItem &&) -> TxQueueItem& = delete; +}; + +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 transfer_id = std::numeric_limits::max(); + std::uint8_t redundant_transport_index = std::numeric_limits::max(); + bool toggle = false; +}; + +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" { + +auto crcAdd(const std::uint16_t crc, const std::size_t size, const void* const bytes) -> std::uint16_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 txGetPresentationLayerMTU(const CanardInstance* const ins) -> std::size_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 txMakeTailByte(const bool start_of_transfer, + const bool end_of_transfer, + const bool toggle, + const std::uint8_t transfer_id) -> std::uint8_t; + +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, + 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); + +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; + +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); + +auto float16Pack(const float value) -> std::uint16_t; +auto float16Unpack(const std::uint16_t value) -> float; +} +} // namespace exposed diff --git a/tests/helpers.hpp b/tests/helpers.hpp new file mode 100644 index 00000000..431a3d27 --- /dev/null +++ b/tests/helpers.hpp @@ -0,0 +1,232 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. + +#pragma once + +#include "canard.h" +#include "exposed.hpp" +#include +#include +#include +#include +#include +#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 +{ +inline auto allocate(CanardInstance* const ins, const std::size_t amount) -> void* +{ + (void) ins; + (void) amount; + return nullptr; +} + +inline void free(CanardInstance* const ins, void* const pointer) +{ + (void) ins; + (void) 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::atomic 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() + { + std::unique_lock locker(lock_); + 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) + { + 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. + 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. + } + // Random-fill the memory to make sure no assumptions are made about its contents. + 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; + } + + /// 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) + { + std::unique_lock locker(lock_); + const auto it = allocated_.find(pointer); + 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, [&]() { + 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 + { + std::unique_lock locker(lock_); + return std::size(allocated_); + } + + [[nodiscard]] auto getTotalAllocatedAmount() const -> std::size_t + { + std::unique_lock locker(lock_); + std::size_t out = 0U; + for (auto [_, size] : allocated_) + { + out += size; + } + return out; + } + + [[nodiscard]] auto getAllocationCeiling() const { return static_cast(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); + 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); + } + +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() const { return canardTxPeek(&canard_); } + + void txPop() { canardTxPop(&canard_); } + + [[nodiscard]] auto rxAccept(const CanardFrame& frame, + const uint8_t redundant_transport_index, + CanardTransfer& 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; } + 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_; } + + [[nodiscard]] auto getInstance() -> CanardInstance& { return canard_; } + [[nodiscard]] auto getInstance() const -> const CanardInstance& { return canard_; } +}; + +} // namespace helpers 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_dsdl.cpp b/tests/test_dsdl.cpp new file mode 100644 index 00000000..2390557a --- /dev/null +++ b/tests/test_dsdl.cpp @@ -0,0 +1,518 @@ +// This software is distributed under the terms of the MIT License. +// Copyright (c) 2016-2020 UAVCAN Development Team. + +#include "canard_dsdl.h" +#include "exposed.hpp" +#include +#include +#include + +TEST_CASE("float16Pack") +{ + using exposed::float16Pack; + 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("float16Unpack") +{ + using exposed::float16Unpack; + 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(float16Unpack(0b0111111111111111)))); +} + +TEST_CASE("canardDSDLFloat16") +{ + using exposed::float16Pack; + using exposed::float16Unpack; + float x = -1000.0F; + while (x <= 1000.0F) + { + 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") +{ + using exposed::copyBitArray; + + { + 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, + 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(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})); +} + +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, + 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)); + + 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) { + set_b(offset, 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); + + set_u(357, 0, 3); + + REQUIRE(std::size(dest) == std::size(Reference)); + REQUIRE_THAT(dest, Catch::Matchers::Equals(Reference)); +} + +TEST_CASE("canardDSDLSerialize_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)); + + 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) { + set_b(offset, 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))); + + 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))); + + offset = 45; + push_bit(false); + push_bit(true); + push_bit(true); + + set_u(48, 0x12, 8); + set_u(56, 0x34, 8); + set_u(64, 0x56, 8); + + 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))); + + 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))); + + set_f64(99, 1.0); + set_f32(163, 1.0F); + set_f16(195, -99999.0F); + + 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("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)); + + 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); + }; + + 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)); +} + +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. + 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)); + 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 +} 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_private_crc.cpp b/tests/test_private_crc.cpp new file mode 100644 index 00000000..bebb4a6a --- /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); + crc = crcAdd(crc, 6, "456789"); + REQUIRE(0x29B1U == crc); +} diff --git a/tests/test_private_rx.cpp b/tests/test_private_rx.cpp new file mode 100644 index 00000000..da514914 --- /dev/null +++ b/tests/test_private_rx.cpp @@ -0,0 +1,685 @@ +// 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("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, 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 == 7U); + REQUIRE(!model.start_of_transfer); + REQUIRE(!model.end_of_transfer); + REQUIRE(!model.toggle); + 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, {0, 1, 2, 3, 4, 5, 6, 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 == 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 + // 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})); + 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 + + // 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 + 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})); + 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 + REQUIRE(!parse(888'888U, 0b100'10'0000110011'0011010'0011010U, {255, 0b010'00000U | 1U})); // SRC == DST +} + +TEST_CASE("rxSessionWritePayload") +{ + using helpers::Instance; + using exposed::RxSession; + using exposed::rxSessionWritePayload; + using exposed::rxSessionRestart; + + Instance ins; + RxSession rxs; + rxs.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.transfer_id == 1); + REQUIRE(rxs.toggle); + + // Double restart has no effect on memory. + 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.transfer_id == 24U); + REQUIRE(rxs.toggle); + + // Restart with a transfer-ID overflow. + 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.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")); + 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); +} + +TEST_CASE("rxSessionUpdate") +{ + using helpers::Instance; + using exposed::RxSession; + using exposed::RxFrameModel; + using exposed::rxSessionUpdate; + using exposed::crcAdd; + + Instance ins; + ins.getAllocator().setAllocationCeiling(16); + + 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.transfer_id = 31; + 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, 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. + REQUIRE(rxs.calculated_crc == 0xFFFF); + 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); + 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() == 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, 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. + REQUIRE(rxs.calculated_crc == 0xFFFF); + REQUIRE(rxs.transfer_id == 12U); // Incremented. + REQUIRE(rxs.toggle); + 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, 16)); + REQUIRE(rxs.transfer_timestamp_usec == 10'000'050); + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); + REQUIRE(rxs.calculated_crc == 0xFFFF); + 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); + 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() == 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, 16)); + REQUIRE(rxs.transfer_timestamp_usec == 10'000'050); + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); + REQUIRE(rxs.calculated_crc == 0xFFFF); + REQUIRE(rxs.transfer_id == 13U); + REQUIRE(rxs.toggle); + 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, 16)); + REQUIRE(rxs.transfer_timestamp_usec == 20'000'000); + REQUIRE(rxs.payload_size == 0); + REQUIRE(rxs.payload == nullptr); + REQUIRE(rxs.calculated_crc == 0xFFFF); + 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); + 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() == 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 = 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 == 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.transfer_id == 13U); + REQUIRE(!rxs.toggle); + REQUIRE(rxs.redundant_transport_index == 0); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + 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\x07\x07\x07"); + REQUIRE(0 == update(0, 1'000'000, 16)); + REQUIRE(rxs.transfer_timestamp_usec == 20'000'100); + 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.transfer_id == 13U); + REQUIRE(rxs.toggle); + REQUIRE(rxs.redundant_transport_index == 0); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); + + // 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, 16)); + REQUIRE(rxs.transfer_timestamp_usec == 20'000'100); + 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.transfer_id == 13U); + REQUIRE(rxs.toggle); + REQUIRE(rxs.redundant_transport_index == 0); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); + + // 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\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); + REQUIRE(rxs.calculated_crc == 0xFFFF); + 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); + 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 == 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() == 16); + 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 = 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); + REQUIRE(rxs.calculated_crc == 0xFFFF); + REQUIRE(rxs.transfer_id == 13U); + REQUIRE(rxs.toggle); + 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 = 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 == 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.transfer_id == 11U); + REQUIRE(!rxs.toggle); + REQUIRE(rxs.redundant_transport_index == 2); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); + + // 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 = 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 == 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.transfer_id == 11U); + REQUIRE(!rxs.toggle); + REQUIRE(rxs.redundant_transport_index == 2); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); + + // 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\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); + REQUIRE(rxs.calculated_crc == 0xFFFF); + 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); + 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 == 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() == 16); + 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 = 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 == 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.transfer_id == 0); + REQUIRE(!rxs.toggle); + REQUIRE(rxs.redundant_transport_index == 0); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); + + // 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.transfer_id == 1U); + REQUIRE(rxs.toggle); + 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.transfer_id == 31U); + REQUIRE(!rxs.toggle); + REQUIRE(rxs.redundant_transport_index == 0); + REQUIRE(ins.getAllocator().getNumAllocatedFragments() == 1); + REQUIRE(ins.getAllocator().getTotalAllocatedAmount() == 16); + + // 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.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); + + // 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.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_private_tx.cpp b/tests/test_private_tx.cpp new file mode 100644 index 00000000..983d7da0 --- /dev/null +++ b/tests/test_private_tx.cpp @@ -0,0 +1,221 @@ +// 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("SessionSpecifier") +{ + REQUIRE(0b000'00'0011001100110011'0'1010101 == + exposed::txMakeMessageSessionSpecifier(0b0011001100110011, 0b1010101)); + REQUIRE(0b000'11'0100110011'0101010'1010101 == + exposed::txMakeServiceSessionSpecifier(0b0100110011, true, 0b1010101, 0b0101010)); + REQUIRE(0b000'10'0100110011'1010101'0101010 == + exposed::txMakeServiceSessionSpecifier(0b0100110011, false, 0b0101010, 0b1010101)); +} + +TEST_CASE("txGetPresentationLayerMTU") +{ + auto ins = canardInit(&helpers::dummy_allocator::allocate, &helpers::dummy_allocator::free); + REQUIRE(63 == exposed::txGetPresentationLayerMTU(&ins)); // This is the default. + ins.mtu_bytes = 0; + REQUIRE(7 == exposed::txGetPresentationLayerMTU(&ins)); + ins.mtu_bytes = 255; + REQUIRE(63 == exposed::txGetPresentationLayerMTU(&ins)); + ins.mtu_bytes = 32; + REQUIRE(31 == exposed::txGetPresentationLayerMTU(&ins)); + ins.mtu_bytes = 30; // Round up. + REQUIRE(31 == exposed::txGetPresentationLayerMTU(&ins)); +} + +TEST_CASE("txMakeCANID") +{ + using exposed::txMakeCANID; + + 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 = exposed::crcAdd(0xFFFFU, 3, "\x01\x02\x03"); + + union PriorityAlias + { + std::uint8_t bits; + CanardPriority prio; + }; + + // MESSAGE TRANSFERS + REQUIRE(0b000'00'0011001100110011'0'1010101 == // Regular message. + txMakeCANID(mk_transfer(CanardPriorityExceptional, + CanardTransferKindMessage, + 0b0011001100110011, + CANARD_NODE_ID_UNSET), + 0b1010101, + 7U)); + REQUIRE(0b111'00'0011001100110011'0'1010101 == // Regular message. + 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. + 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. + 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. + txMakeCANID(mk_transfer(CanardPriorityHigh, CanardTransferKindMessage, 0b0011001100110011, 123U), 0U, 7U)); + REQUIRE( + -CANARD_ERROR_INVALID_ARGUMENT == // Bad subject-ID. + txMakeCANID(mk_transfer(CanardPriorityLow, CanardTransferKindMessage, 0xFFFFU, CANARD_NODE_ID_UNSET), 0U, 7U)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Bad priority. + txMakeCANID(mk_transfer(PriorityAlias{123}.prio, + CanardTransferKindMessage, + 0b0011001100110011, + CANARD_NODE_ID_UNSET), + 0b1010101, + 7U)); + + // SERVICE TRANSFERS + REQUIRE(0b000'11'0100110011'0101010'1010101 == // Request. + txMakeCANID(mk_transfer(CanardPriorityExceptional, CanardTransferKindRequest, 0b0100110011, 0b0101010), + 0b1010101, + 7U)); + REQUIRE(0b111'10'0100110011'0101010'1010101 == // Response. + txMakeCANID(mk_transfer(CanardPriorityOptional, CanardTransferKindResponse, 0b0100110011, 0b0101010), + 0b1010101, + 7U)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Anonymous service transfers not permitted. + txMakeCANID(mk_transfer(CanardPriorityExceptional, CanardTransferKindRequest, 0b0100110011, 0b0101010), + CANARD_NODE_ID_UNSET, + 7U)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Broadcast service transfers not permitted. + txMakeCANID(mk_transfer(CanardPrioritySlow, CanardTransferKindResponse, 0b0100110011, CANARD_NODE_ID_UNSET), + 0b1010101, + 7U)); + REQUIRE( + -CANARD_ERROR_INVALID_ARGUMENT == // Bad service-ID. + txMakeCANID(mk_transfer(CanardPriorityNominal, CanardTransferKindResponse, 0xFFFFU, 0b0101010), 0b1010101, 7U)); + REQUIRE(-CANARD_ERROR_INVALID_ARGUMENT == // Bad priority. + txMakeCANID(mk_transfer(PriorityAlias{123}.prio, CanardTransferKindResponse, 0b0100110011, 0b0101010), + 0b1010101, + 7U)); +} + +TEST_CASE("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)); + 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("txRoundFramePayloadSizeUp") +{ + using exposed::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("txFindQueueSupremum") +{ + using exposed::txFindQueueSupremum; + using TxQueueItem = exposed::TxQueueItem; + + auto ins = canardInit(&helpers::dummy_allocator::allocate, &helpers::dummy_allocator::free); + + const auto find = [&](std::uint32_t x) -> TxQueueItem* { return txFindQueueSupremum(&ins, x); }; + + REQUIRE(nullptr == find(0)); + REQUIRE(nullptr == find((1UL << 29U) - 1U)); + + TxQueueItem 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.frame.extended_can_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.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)); + 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)); +} diff --git a/tests/test_public_roundtrip.cpp b/tests/test_public_roundtrip.cpp new file mode 100644 index 00000000..efe51485 --- /dev/null +++ b/tests/test_public_roundtrip.cpp @@ -0,0 +1,241 @@ +// 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) + { + const CanardFrame* frame = nullptr; + { + std::lock_guard locker(lock); + 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 (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) // + << '\n'; + + CanardTransfer transfer{}; + 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. + { + 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 + { + if (!keep_going) + { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + ins_tx.getAllocator().deallocate(frame); + + 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)); +} diff --git a/tests/test_public_rx.cpp b/tests/test_public_rx.cpp new file mode 100644 index 00000000..5d012820 --- /dev/null +++ b/tests/test_public_rx.cpp @@ -0,0 +1,313 @@ +// 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); + + // 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); + 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); + 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]->_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("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; + 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)); + + 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 new file mode 100644 index 00000000..3deca819 --- /dev/null +++ b/tests/test_public_tx.cpp @@ -0,0 +1,362 @@ +// 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("TxBasic0") +{ + using exposed::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(nullptr == ins.getTxQueueRoot()); + REQUIRE(0 == ins.getTxQueueLength()); + REQUIRE(0 == alloc.getNumAllocatedFragments()); + + alloc.setAllocationCeiling(200); + + CanardTransfer transfer{}; + transfer.payload = payload.data(); + + // Single-frame with padding. + 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()->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); + 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. 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->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->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->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()); + 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()); + + // Pop the queue. + // hex(pyuavcan.transport.commons.crc.CRC16CCITT.new(list(range(8))).value) + 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); + REQUIRE(2 == ins.getTxQueueLength()); + REQUIRE(2 == alloc.getNumAllocatedFragments()); + 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); + REQUIRE(1 == ins.getTxQueueLength()); + REQUIRE(1 == alloc.getNumAllocatedFragments()); + 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); + REQUIRE(0 == ins.getTxQueueLength()); + REQUIRE(nullptr == ins.getTxQueueRoot()); + REQUIRE(0 == alloc.getNumAllocatedFragments()); + 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()); + frame = ins.txPeek(); + REQUIRE(nullptr == frame); + + alloc.setAllocationCeiling(1000); + + // 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; + 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()); + REQUIRE(40 < alloc.getTotalAllocatedAmount()); + REQUIRE(220 > alloc.getTotalAllocatedAmount()); + // Read the generated frames. + 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); + REQUIRE(2 == ins.getTxQueueLength()); + REQUIRE(2 == alloc.getNumAllocatedFragments()); + 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); + REQUIRE(1 == ins.getTxQueueLength()); + REQUIRE(1 == alloc.getNumAllocatedFragments()); + 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); + 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; + 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()); + REQUIRE(40 < alloc.getTotalAllocatedAmount()); + REQUIRE(220 > alloc.getTotalAllocatedAmount()); + // Read the generated frames. + 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); + REQUIRE(2 == ins.getTxQueueLength()); + REQUIRE(2 == alloc.getNumAllocatedFragments()); + 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); + REQUIRE(1 == ins.getTxQueueLength()); + REQUIRE(1 == alloc.getNumAllocatedFragments()); + 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); + 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. + 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); + REQUIRE(1 == ins.getTxQueueLength()); + REQUIRE(1 == alloc.getNumAllocatedFragments()); + 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); + ins.txPop(); + ins.getAllocator().deallocate(frame); + 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; + transfer.payload = nullptr; // Null is OK if size is 0. + REQUIRE(1 == ins.txPush(transfer)); + REQUIRE(1 == ins.getTxQueueLength()); + REQUIRE(1 == alloc.getNumAllocatedFragments()); + REQUIRE(60 > alloc.getTotalAllocatedAmount()); + 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()); + 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); + REQUIRE(0 == ins.getTxQueueLength()); + REQUIRE(0 == alloc.getNumAllocatedFragments()); + + // Nothing left to peek at. + 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)); + 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(nullptr == canardTxPeek(nullptr)); + + canardTxPop(&ins.getInstance()); // No effect. +} 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]); -} diff --git a/tests/test_self.cpp b/tests/test_self.cpp new file mode 100644 index 00000000..a83dde5b --- /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 "exposed.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()); +}