diff --git a/components/dataLink/DataLinkFrames.cpp b/components/dataLink/DataLinkFrames.cpp index 5f38b85..e6d88cd 100644 --- a/components/dataLink/DataLinkFrames.cpp +++ b/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 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>(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(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>(); 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); diff --git a/components/dataLink/DataLinkManager.cpp b/components/dataLink/DataLinkManager.cpp index d163fa7..4fc17d8 100644 --- a/components/dataLink/DataLinkManager.cpp +++ b/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>(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& 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++] = (data_len >> 8) & 0xFF; + send_data[offset++] = wire_data_len & 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 += control_frame.data_len; + offset += 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){ - ESP_LOGE(DEBUG_LINK_TAG, "Data len 0"); + if (wire_data_len <= CONTROL_FRAME_HASH_SIZE){ + 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)){ - ESP_LOGE(DEBUG_LINK_TAG, "Invalid payload length: %u", *message_size); + if (payload_len > MAX_CONTROL_DATA_LEN || (8 + wire_data_len + 2 > data_len)){ + 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 + header->data_len] | ((uint16_t)data[9 + header->data_len] << 8)); + uint16_t crc_calc = ((uint16_t)data[8 + wire_data_len] | ((uint16_t)data[9 + wire_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; diff --git a/components/dataLink/include/DataLinkManager.h b/components/dataLink/include/DataLinkManager.h index 0829efd..d763468 100644 --- a/components/dataLink/include/DataLinkManager.h +++ b/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 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& 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 { diff --git a/components/dataLink/include/Frames.h b/components/dataLink/include/Frames.h index 362f34d..ad6d453 100644 --- a/components/dataLink/include/Frames.h +++ b/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 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> data; uint16_t data_len; diff --git a/components/rpc/MessagingInterface.cpp b/components/rpc/MessagingInterface.cpp index 17b5564..6289240 100644 --- a/components/rpc/MessagingInterface.cpp +++ b/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(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(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;