botchain/firmware
1
0
Fork 0
mirror of https://github.com/BotChain-Robots/firmware.git synced 2026-07-08 09:37:21 +02:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Johnathon Slightham
2026-03-02 01:46:24 -05:00
parent 1083d3e2c0
commit bb08a2f0ba
5 changed files with 271 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

170
components/dataLink/DataLinkManager.cpp
View File

@@ -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++] = (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;