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

Author: hungatamazon

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 void* const header)
+{
+    UDPARD_ASSERT(header != NULL);
+    CyphalHeaderCRC    out = crc;
+    const uint8_t* p   = (const uint8_t*) header;
+    size_t cyphal_header_size_without_crc = sizeof(UdpardFrameHeader) - CYPHAL_HEADER_CRC_SIZE_BYTES;
+    for (size_t i = 0; i < cyphal_header_size_without_crc; 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.
@@ -329,6 +368,7 @@ UDPARD_PRIVATE void txMakeFrameHeader(UdpardFrameHeader* const header,
     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, 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 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,13 @@ TEST_CASE("TransferCRC")
     crc = crcAdd(crc, 6, "456789");
     REQUIRE(0xE3069283U == crcValue(crc));
 }
+
+TEST_CASE("CyphalHeaderCRC")
+{
+    using exposed::CyphalHeaderCrcAdd;
+
+    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, header);
+    REQUIRE(0xB731 == crc);
+}