Add a cache to data link layer for control frames with hash

components/dataLink/DataLinkFrames.cpp

@@ -281,6 +281,74 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
         return ESP_ERR_INVALID_RESPONSE;
     }
 
+    // ---- LUT fast-path for control frames ----
+    // Raw wire layout for control frames:
+    //   [0]    preamble
+    //   [1]    sender_id
+    //   [2]    receiver_id
+    //   [3..4] seq_num (LE)
+    //   [5]    type_flag
+    //   [6..7] wire_data_len (LE)  -- includes CONTROL_FRAME_HASH_SIZE
+    //   [8..11] 4-byte FNV-1a hash (LE)
+    //   [12..] actual payload
+    //   [-2..-1] CRC-16
+    bool is_control = IS_CONTROL_FRAME(data[5]);
+    if (is_control && recv_len >= (size_t)(CONTROL_FRAME_OVERHEAD + CONTROL_FRAME_HASH_SIZE)){
+        uint32_t peeked_hash = ((uint32_t)data[8]      ) |
+                               ((uint32_t)data[9]  <<  8) |
+                               ((uint32_t)data[10] << 16) |
+                               ((uint32_t)data[11] << 24);
+
+        std::vector<uint8_t> cached_message;
+        FrameHeader cached_header;
+
+        if (lut_lookup(peeked_hash, cached_message, cached_header)){
+            // Cache hit: replay the previously validated payload directly onto the receive queue.
+            // Skip CRC checking, hash verification, data-length checks, and full parsing.
+
+            auto message = std::make_unique<std::vector<uint8_t>>(cached_message);
+
+            // Update per-frame dynamic fields from the raw bytes (sender/receiver/seq differ per transmission)
+            cached_header.sender_id   = data[1];
+            cached_header.receiver_id = data[2];
+            cached_header.seq_num     = (uint16_t)data[3] | ((uint16_t)data[4] << 8);
+
+            // RIP control frames are handled internally – replay them too
+            if (static_cast<FrameType>(GET_TYPE(cached_header.type_flag)) == FrameType::RIP_TABLE_CONTROL){
+                // Re-run the RIP update using the cached message
+                for (size_t i = 0; i < cached_message.size() - 1; i += 2){
+                    uint8_t board_id = cached_message[i];
+                    uint8_t hops     = cached_message[i + 1];
+                    RIPRow* entry    = nullptr;
+                    if (rip_find_entry(board_id, &entry, true) != ESP_OK || entry == nullptr){
+                        continue;
+                    }
+                    if (entry->valid == RIP_NEW_ROW){
+                        rip_add_entry(board_id, hops + 1, channel, &entry);
+                    } else {
+                        rip_update_entry(hops + 1, channel, &entry);
+                    }
+                }
+                return ESP_OK;
+            }
+
+            // Data is already validated – enqueue directly, no further checks needed.
+            Rx_Metadata metadata = {
+                .data     = std::move(message),
+                .data_len = (uint16_t)cached_message.size(),
+                .header   = cached_header
+            };
+
+            if (!async_receive_queue->enqueue(std::move(metadata), std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS))){
+                return ESP_ERR_TIMEOUT;
+            }
+            return ESP_OK;
+        }
+        // Cache miss – fall through to full validation below
+        // ESP_LOGI("TMP", "Control frame LUT cache MISS - hash=0x%08lX", peeked_hash);
+    }
+    // ---- end LUT fast-path ----
+
     auto message = std::make_unique<std::vector<uint8_t>>();
     message->resize(MAX_FRAME_SIZE);
 
@@ -314,12 +382,6 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
 
         res = inc_head_sliding_window(channel, header.sender_id, record.seq_num, &record);
 
-        // if (res == ESP_OK){
-        //     ESP_LOGI(DEBUG_LINK_TAG, "Got ACK for seq number %d from board %d! Highest Conseq ACK: 0x%X%X Total Frag: 0x%X%X", record.seq_num, header.sender_id, message[1], message[2], message[3], message[4]);
-        // } else {
-        //     ESP_LOGI(DEBUG_LINK_TAG, "Got ACK for seq number %d from board %d but got a lower conseq ack 0x%x%X Total Frag: 0x%X%X", record.seq_num, header.sender_id, message[1], message[2], message[3], message[4]);
-        // }
-
         return ESP_OK;
     }
 
@@ -335,6 +397,16 @@ esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
         return res;
     }
 
+    // Control frame fully validated – store in LUT for future replays
+    // Extract the hash that was embedded at data[8..11] (already validated in get_data_from_frame)
+    {
+        uint32_t validated_hash = ((uint32_t)data[8]      ) |
+                                  ((uint32_t)data[9]  <<  8) |
+                                  ((uint32_t)data[10] << 16) |
+                                  ((uint32_t)data[11] << 24);
+        lut_insert(validated_hash, message->data(), message_size, header);
+    }
+
     //control frame handling: - TODO: clean up :)
     // ESP_LOGI(DEBUG_LINK_TAG, "Received frame of type 0x%X destined for board %d", GET_TYPE(header.type_flag), header.receiver_id);
 

components/dataLink/DataLinkManager.cpp

@@ -39,6 +39,14 @@ DataLinkManager::DataLinkManager(uint8_t board_id, uint8_t num_channels = MAX_CH
 
     async_receive_queue = std::make_unique<BlockingQueue<Rx_Metadata>>(MAX_RX_QUEUE_SIZE);
 
+    // Initialise receiver-side control frame LFU LUT
+    control_frame_lut_mutex = xSemaphoreCreateMutex();
+    for (int i = 0; i < CONTROL_FRAME_LUT_SIZE; i++) {
+        control_frame_lut[i] = ControlFrameLutEntry{};
+        control_frame_lut[i].valid = false;
+        control_frame_lut[i].frequency = 0;
+    }
+
     init_rip();
     init_scheduler();
 }
@@ -185,6 +193,117 @@ esp_err_t DataLinkManager::get_board_id(uint8_t& board_id){
     return ESP_OK;
 }
 
+/**
+ * @brief Compute a 32-bit FNV-1a hash over a byte buffer
+ *
+ * @param data  Input bytes
+ * @param len   Number of bytes
+ * @return uint32_t hash value
+ */
+uint32_t DataLinkManager::compute_fnv1a_hash(const uint8_t* data, size_t len){
+    constexpr uint32_t FNV_PRIME  = 0x01000193U;
+    constexpr uint32_t FNV_OFFSET = 0x811C9DC5U;
+    uint32_t hash = FNV_OFFSET;
+    for (size_t i = 0; i < len; i++){
+        hash ^= data[i];
+        hash *= FNV_PRIME;
+    }
+    return hash;
+}
+
+/**
+ * @brief Look up a hash in the receiver-side control-frame LFU LUT.
+ *
+ * On a hit the cached message and header are copied out, the frequency counter is
+ * incremented, and `true` is returned.  On a miss `false` is returned and the
+ * output parameters are left untouched.
+ *
+ * @param hash         32-bit FNV-1a hash to search for
+ * @param out_message  Destination vector – filled with the cached payload on hit
+ * @param out_header   Destination header – filled with the cached header on hit
+ * @return true  Cache hit
+ * @return false Cache miss
+ */
+bool DataLinkManager::lut_lookup(uint32_t hash, std::vector<uint8_t>& out_message, FrameHeader& out_header){
+    if (xSemaphoreTake(control_frame_lut_mutex, pdMS_TO_TICKS(SEQUENCE_NUM_MAP_MUTEX_MAX_WAIT_MS)) != pdTRUE){
+        return false;
+    }
+
+    bool found = false;
+    for (int i = 0; i < CONTROL_FRAME_LUT_SIZE; i++){
+        if (control_frame_lut[i].valid && control_frame_lut[i].hash == hash){
+            control_frame_lut[i].frequency++;
+            out_message = control_frame_lut[i].message;
+            out_header  = control_frame_lut[i].header;
+            found = true;
+            // ESP_LOGI("TMP", "Control frame LUT cache HIT  - hash=0x%08lX freq=%lu", hash, control_frame_lut[i].frequency);
+            break;
+        }
+    }
+
+    xSemaphoreGive(control_frame_lut_mutex);
+    return found;
+}
+
+/**
+ * @brief Insert an entry into the control-frame LFU LUT.
+ *
+ * If the hash already exists its frequency is incremented and the cached data
+ * updated.  If the table is full the entry with the lowest frequency count is
+ * evicted (ties broken by lowest index).
+ *
+ * @param hash        32-bit FNV-1a hash (key)
+ * @param message     Decoded payload bytes (without the 4-byte hash prefix)
+ * @param message_len Payload length
+ * @param header      Parsed frame header to cache alongside the payload
+ */
+void DataLinkManager::lut_insert(uint32_t hash, const uint8_t* message, size_t message_len, const FrameHeader& header){
+    if (xSemaphoreTake(control_frame_lut_mutex, pdMS_TO_TICKS(SEQUENCE_NUM_MAP_MUTEX_MAX_WAIT_MS)) != pdTRUE){
+        return;
+    }
+
+    // Check if the hash is already present – if so update it
+    for (int i = 0; i < CONTROL_FRAME_LUT_SIZE; i++){
+        if (control_frame_lut[i].valid && control_frame_lut[i].hash == hash){
+            control_frame_lut[i].frequency++;
+            control_frame_lut[i].message.assign(message, message + message_len);
+            control_frame_lut[i].header = header;
+            xSemaphoreGive(control_frame_lut_mutex);
+            return;
+        }
+    }
+
+    // Find an empty slot first
+    int target = -1;
+    for (int i = 0; i < CONTROL_FRAME_LUT_SIZE; i++){
+        if (!control_frame_lut[i].valid){
+            target = i;
+            break;
+        }
+    }
+
+    // No empty slot – evict the least-frequently-used entry
+    if (target == -1){
+        uint32_t min_freq = control_frame_lut[0].frequency;
+        target = 0;
+        for (int i = 1; i < CONTROL_FRAME_LUT_SIZE; i++){
+            if (control_frame_lut[i].frequency < min_freq){
+                min_freq = control_frame_lut[i].frequency;
+                target = i;
+            }
+        }
+        ESP_LOGD(DEBUG_LINK_TAG, "LUT evicting entry with hash 0x%08lX (freq=%lu)", control_frame_lut[target].hash, control_frame_lut[target].frequency);
+    }
+
+    control_frame_lut[target].hash      = hash;
+    control_frame_lut[target].message.assign(message, message + message_len);
+    control_frame_lut[target].header    = header;
+    control_frame_lut[target].frequency = 1;
+    control_frame_lut[target].valid     = true;
+
+    xSemaphoreGive(control_frame_lut_mutex);
+}
+
 /**
  * @brief Helper function to create a control frame
  *
@@ -231,6 +350,11 @@ esp_err_t DataLinkManager::create_control_frame(uint8_t* data, uint16_t data_len
         return ESP_ERR_INVALID_ARG;
     }
 
+    // Compute FNV-1a hash of the original payload and build the wire payload:
+    // [ 4-byte hash (LE) | original data ]
+    uint32_t payload_hash = compute_fnv1a_hash(data, data_len);
+    uint16_t wire_data_len = (uint16_t)(CONTROL_FRAME_HASH_SIZE + data_len);
+
     size_t offset = 0;
     send_data[offset++] = control_frame.preamble;
     send_data[offset++] = control_frame.sender_id;
@@ -238,12 +362,17 @@ esp_err_t DataLinkManager::create_control_frame(uint8_t* data, uint16_t data_len
     send_data[offset++] = control_frame.seq_num & 0xFF;
     send_data[offset++] = (control_frame.seq_num >> 8) & 0xFF;
     send_data[offset++] = control_frame.type_flag;
-    send_data[offset++] = data_len;
+    send_data[offset++] = wire_data_len & 0xFF;
-    send_data[offset++] = (data_len >> 8) & 0xFF;
+    send_data[offset++] = (wire_data_len >> 8) & 0xFF;
+
+    // Prepend hash (little-endian)
+    send_data[offset++] = (payload_hash      ) & 0xFF;
+    send_data[offset++] = (payload_hash >>  8) & 0xFF;
+    send_data[offset++] = (payload_hash >> 16) & 0xFF;
+    send_data[offset++] = (payload_hash >> 24) & 0xFF;
 
     memcpy(&send_data[offset], data, data_len);
-
+    offset += data_len;
-    offset += control_frame.data_len;
 
     geneate_crc_16(send_data, offset, &control_frame.crc_16);
 
@@ -524,30 +653,30 @@ esp_err_t DataLinkManager::get_data_from_frame(uint8_t* data, size_t data_len, u
             return ESP_ERR_INVALID_SIZE;
         }
 
-        header->data_len = (uint16_t)data[6] | ((uint16_t)data[7] << 8);
+        // data_len field on the wire = CONTROL_FRAME_HASH_SIZE + actual payload length
+        uint16_t wire_data_len = (uint16_t)data[6] | ((uint16_t)data[7] << 8);
 
-        if (header->data_len > data_len){
+        if (wire_data_len > data_len){
             ESP_LOGE(DEBUG_LINK_TAG, "Mismatch data length in control frame");
             return ESP_ERR_INVALID_RESPONSE;
         }
 
-        if (header->data_len == 0){
+        if (wire_data_len <= CONTROL_FRAME_HASH_SIZE){
-            ESP_LOGE(DEBUG_LINK_TAG, "Data len 0");
+            ESP_LOGE(DEBUG_LINK_TAG, "Wire data len too small to contain hash");
             return ESP_ERR_INVALID_SIZE;
         }
 
-        *message_size = header->data_len;
+        uint16_t payload_len = wire_data_len - CONTROL_FRAME_HASH_SIZE;
 
-        if (*message_size > MAX_CONTROL_DATA_LEN || (10 + *message_size > data_len)){
+        if (payload_len > MAX_CONTROL_DATA_LEN || (8 + wire_data_len + 2 > data_len)){
-            ESP_LOGE(DEBUG_LINK_TAG, "Invalid payload length: %u", *message_size);
+            ESP_LOGE(DEBUG_LINK_TAG, "Invalid payload length: %u", payload_len);
             return ESP_ERR_INVALID_SIZE;
         }
 
-        memcpy(message, &data[8], header->data_len);
+        // CRC covers: header (8 bytes) + wire payload (hash + data)
+        geneate_crc_16(data, 8 + wire_data_len, &header->crc_16);
 
-        geneate_crc_16(data, 8*sizeof(uint8_t) + header->data_len, &header->crc_16);
+        uint16_t crc_calc = ((uint16_t)data[8 + wire_data_len] | ((uint16_t)data[9 + wire_data_len] << 8));
-
-        uint16_t crc_calc = ((uint16_t)data[8 + header->data_len] | ((uint16_t)data[9 + header->data_len] << 8));
 
         if (crc_calc != header->crc_16){
             //CRC mismatch
@@ -556,8 +685,17 @@ esp_err_t DataLinkManager::get_data_from_frame(uint8_t* data, size_t data_len, u
             return ESP_ERR_INVALID_CRC;
         }
 
+        // Strip the 4-byte hash prefix – it is used only as a LUT key in receive_rmt,
+        // not as an additional integrity check here.
+        const uint8_t* payload_ptr = &data[8 + CONTROL_FRAME_HASH_SIZE];
+
+        // Return the actual payload (without the hash prefix)
+        header->data_len = payload_len;
+        *message_size = payload_len;
+        memcpy(message, payload_ptr, payload_len);
+
     } else {
-        //generic frame
+        // Generic frame
 
         if (data_len < 13){
             return ESP_ERR_INVALID_SIZE;

components/dataLink/include/DataLinkManager.h

@@ -46,6 +46,7 @@ static const uint16_t crc16_table[256] = {
 #define ASYNC_QUEUE_WAIT_TICKS 100
 #define SEQUENCE_NUM_MAP_MUTEX_MAX_WAIT_MS 50
 #define MAX_RX_QUEUE_SIZE 100
+#define CONTROL_FRAME_LUT_SIZE 5  // Maximum number of control frames cached in the receiver-side LFU LUT
 
 /**
  * @brief Class to represent the Data Link Layer
@@ -203,6 +204,27 @@ class DataLinkManager{
 
         SemaphoreHandle_t send_ack_queue_mutex[MAX_CHANNELS];
         std::queue<SendAckMetaData> send_ack_queue[MAX_CHANNELS];
+
+        // ==== Control Frame Hash / LFU LUT (receiver side) ====
+
+        /**
+         * @brief Compute a 32-bit FNV-1a hash over a byte buffer
+         */
+        static uint32_t compute_fnv1a_hash(const uint8_t* data, size_t len);
+
+        /**
+         * @brief Look up a hash in the control-frame LUT.
+         *        Returns true and populates @p out_message / @p out_header on a hit.
+         */
+        bool lut_lookup(uint32_t hash, std::vector<uint8_t>& out_message, FrameHeader& out_header);
+
+        /**
+         * @brief Insert (or update) an entry in the control-frame LUT using LFU eviction.
+         */
+        void lut_insert(uint32_t hash, const uint8_t* message, size_t message_len, const FrameHeader& header);
+
+        ControlFrameLutEntry control_frame_lut[CONTROL_FRAME_LUT_SIZE];
+        SemaphoreHandle_t control_frame_lut_mutex;
 };
 
 struct frame_scheduler_args {

components/dataLink/include/Frames.h

@@ -30,8 +30,11 @@
 #define CONTROL_FRAME_OVERHEAD 9
 #define GENERIC_FRAME_OVERHEAD 14
 
+#define CONTROL_FRAME_HASH_SIZE 4   // 4-byte FNV-1a hash prepended to control frame payload on the wire
+
 #define MAX_GENERIC_DATA_LEN (MAX_FRAME_SIZE - GENERIC_FRAME_OVERHEAD)
-#define MAX_CONTROL_DATA_LEN (MAX_FRAME_SIZE - CONTROL_FRAME_OVERHEAD)
+// Control data max accounts for the 4-byte hash prefix that is prepended on transmit and stripped on receive
+#define MAX_CONTROL_DATA_LEN (MAX_FRAME_SIZE - CONTROL_FRAME_OVERHEAD - CONTROL_FRAME_HASH_SIZE)
 
 //Generic Frame Fragment ACK
 #define GENERIC_FRAG_ACK_DATA_SIZE 7
@@ -107,6 +110,17 @@ typedef struct _fragment_metadata {
     uint16_t num_fragments_rx;
 } FragmentMetadata;
 
+/**
+ * @brief Entry in the receiver-side control-frame LFU lookup table
+ */
+typedef struct _control_frame_lut_entry {
+    uint32_t hash;                        // FNV-1a hash of the original payload (key)
+    std::vector<uint8_t> message;         // Cached decoded payload (sans hash prefix)
+    FrameHeader header;                   // Cached frame header
+    uint32_t frequency;                   // Hit count – used for LFU eviction
+    bool valid;                           // Is this slot populated?
+} ControlFrameLutEntry;
+
 typedef struct _receive_metadata{
     std::unique_ptr<std::vector<uint8_t>> data;
     uint16_t data_len;

components/rpc/MessagingInterface.cpp

@@ -41,7 +41,8 @@ MessagingInterface::~MessagingInterface() {
 
 int MessagingInterface::send(uint8_t* buffer, const size_t size, const uint8_t destination, const uint8_t tag, const bool durable) {
     Flatbuffers::MPIMessageBuilder builder;
-    const auto [mpi_buffer, mpi_size] = builder.build_mpi_message(Messaging::MessageType_PTP, m_config_manager.get_module_id(), destination, m_sequence_number++, durable, tag, std::vector<uint8_t>(buffer, buffer + size));
+    const auto [mpi_buffer, mpi_size] = builder.build_mpi_message(Messaging::MessageType_PTP, m_config_manager.get_module_id(), destination, 0, durable, tag, std::vector<uint8_t>(buffer, buffer + size));
+    // Intentionally set sequence_number = 0 so that the messages can get cached
 
     m_router->send_msg((uint8_t *)mpi_buffer, mpi_size);
     return 0;