botchain/firmware
1
0
Fork 0
mirror of https://github.com/BotChain-Robots/firmware.git synced 2026-07-08 17:47: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

84
components/dataLink/DataLinkFrames.cpp
View File

@@ -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);

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;

22
components/dataLink/include/DataLinkManager.h
View File

@@ -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 {

16
components/dataLink/include/Frames.h
View File

@@ -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;

3
components/rpc/MessagingInterface.cpp
View File

@@ -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;