Merge pull request #8 from hungatamazon/main

Implement crc-16-ccitt-false for cyphal header checksum

Author: SchoberMJ

libudpard/udpard.c

@@ -182,6 +182,45 @@ UDPARD_PRIVATE TransferCRC crcValue(const TransferCRC crc)
     return (uint32_t) (crc ^ CRC_XOR);
 }
 
+/// --------------------------------------------- CYPHAL HEADER CRC ---------------------------------------------
+
+typedef uint16_t CyphalHeaderCRC;
+
+// Based on CRC-16-CCITT-FALSE Function
+#define CYPHAL_HEADER_CRC_INITIAL 0xFFFFU
+#define CYPHAL_HEADER_CRC_SIZE_BYTES 2U
+
+UDPARD_PRIVATE CyphalHeaderCRC cyphalHeaderCrcAddByte(const CyphalHeaderCRC crc, const uint8_t byte)
+{
+    // Based on CRC-16-CCITT-FALSE Function
+    static const CyphalHeaderCRC Top  = 0x8000U;
+    static const CyphalHeaderCRC Poly = 0x1021U;
+    CyphalHeaderCRC              out  = crc ^ (uint16_t) ((uint16_t) (byte) << BITS_PER_BYTE);
+    // 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;
+}
+
+UDPARD_PRIVATE CyphalHeaderCRC cyphalHeaderCrcAdd(const CyphalHeaderCRC crc, const size_t size, const void* const header)
+{
+    UDPARD_ASSERT(header != NULL);
+    CyphalHeaderCRC    out = crc;
+    const uint8_t* p   = (const uint8_t*) header;
+    for (size_t i = 0; i < size; i++)
+    {
+        out = cyphalHeaderCrcAddByte(out, *p);
+        ++p;
+    }
+    return out;
+}
+
 // --------------------------------------------- TRANSMISSION ---------------------------------------------
 
 /// This is a subclass of UdpardTxQueueItem. A pointer to this type can be cast to UdpardTxQueueItem safely.
@@ -324,11 +363,13 @@ UDPARD_PRIVATE void txMakeFrameHeader(UdpardFrameHeader* const header,
 {
     UDPARD_ASSERT(frame_index <= UDPARD_MAX_FRAME_INDEX);
     uint32_t end_of_transfer_mask = (uint32_t) (end_of_transfer ? 1 : 0) << (uint32_t) UDPARD_END_OF_TRANSFER_OFFSET;
+    size_t cyphal_header_size_without_crc = sizeof(UdpardFrameHeader) - CYPHAL_HEADER_CRC_SIZE_BYTES;
     header->transfer_id           = transfer_id;
     header->priority              = (uint8_t) priority;
     header->frame_index_eot       = end_of_transfer_mask | frame_index;
     header->source_node_id        = src_node_id;
     header->destination_node_id   = dst_node_id;
+    header->cyphal_header_checksum = cyphalHeaderCrcAdd(CYPHAL_HEADER_CRC_INITIAL, cyphal_header_size_without_crc, header);
     if (transfer_kind == UdpardTransferKindMessage)
     {
         header->data_specifier = (uint16_t) UPDARD_DATA_SPECIFIER_MESSAGE & port_id;  // SNM (0) + Subject ID

tests/exposed.hpp

@@ -79,6 +79,8 @@ extern "C" {
 
 auto crcAdd(const std::uint32_t crc, const std::size_t size, const void* const bytes) -> std::uint32_t;
 
+auto cyphalHeaderCrcAdd(const std::uint16_t crc, const std::size_t size, const void* const bytes) -> std::uint16_t;
+
 auto crcValue(const std::uint32_t crc) -> std::uint32_t;
 
 auto txMakeMessageSessionSpecifier(const UdpardPortID            subject_id,

tests/test_private_crc.cpp

@@ -21,3 +21,33 @@ TEST_CASE("TransferCRC")
     crc = crcAdd(crc, 6, "456789");
     REQUIRE(0xE3069283U == crcValue(crc));
 }
+
+TEST_CASE("CyphalHeaderCRC")
+{
+    using exposed::cyphalHeaderCrcAdd;
+
+    // Standard Cyphal header size = 24. The last 2 bytes are for the CRC.
+
+    // Standard use case. Header size = 24; CRC is calculated from the first 22 bytes.
+    // The last two bytes (CRC) are ignored in the calculation.
+    std::uint16_t crc = 0xFFFFU;
+    const uint8_t* header = reinterpret_cast<const uint8_t*>("\x01\x02\x03\x04\x05\x06\x07\x08\x09\x10\x11\x12\x13\x14\x15\x16\x17\x18\x19\x20\x21\x22\x23\x24");
+    crc = cyphalHeaderCrcAdd(crc, 22, header);
+    REQUIRE(0xB731 == crc);
+
+    // Verify CRC when the size field is equal to the size of the header.
+    crc = 0xFFFFU;
+    header = reinterpret_cast<const uint8_t*>("\x01\x02\x03\x04\x05\x06\x07\x08\x09\x10\x11\x12\x13\x14\x15\x16\x17\x18\x19\x20\x21\x22");
+    crc = cyphalHeaderCrcAdd(crc, 22, header);
+    REQUIRE(0xB731 == crc);
+    crc = 0xFFFFU;
+    header = reinterpret_cast<const uint8_t*>("\x01\x02\x03\x04\x05\x06\x07\x08\x09\x10\x11\x12\x13\x14\x15\x16\x17\x18\x19\x20\x21\x22\x23\x24");
+    crc = cyphalHeaderCrcAdd(crc, 24, header);
+    REQUIRE(0x96BB == crc);
+
+    // Verify CRC when the size field is less than the size of the header. Extra data is ignored.
+    crc = 0xFFFFU;
+    header = reinterpret_cast<const uint8_t*>("\x01\x02\x03\x04\x05\x06\x07\x08\x09\x10\x11\x12\x13\x14\x15\x16\x17\x18\x19\x20\x21\x22\x23\x24");
+    crc = cyphalHeaderCrcAdd(crc, 10, header);
+    REQUIRE(0x9F09 == crc);
+}