mirror of
https://github.com/BotChain-Robots/firmware.git
synced 2026-07-08 17:47:21 +02:00
486 lines
17 KiB
C++
486 lines
17 KiB
C++
#include "DataLinkManager.h"
|
|
#include "esp_log.h"
|
|
#include <cstring>
|
|
#include <type_traits>
|
|
|
|
/**
|
|
* @brief Creates a Control Frame from `FrameHeader`
|
|
*
|
|
* @param header
|
|
* @return ControlFrame
|
|
*/
|
|
ControlFrame make_control_frame_from_header(const FrameHeader& header) {
|
|
ControlFrame frame{};
|
|
frame.preamble = header.preamble;
|
|
frame.sender_id = header.sender_id;
|
|
frame.receiver_id = header.receiver_id;
|
|
frame.seq_num = header.seq_num;
|
|
frame.type_flag = header.type_flag;
|
|
frame.data_len = header.data_len;
|
|
frame.crc_16 = header.crc_16;
|
|
return frame;
|
|
}
|
|
|
|
/**
|
|
* @brief Creates a Generic Frame from `FrameHeader`
|
|
*
|
|
* @param header
|
|
* @return GenericFrame
|
|
*/
|
|
GenericFrame make_generic_frame_from_header(const FrameHeader& header) {
|
|
GenericFrame frame{};
|
|
frame.preamble = header.preamble;
|
|
frame.sender_id = header.sender_id;
|
|
frame.receiver_id = header.receiver_id;
|
|
frame.seq_num = header.seq_num;
|
|
frame.type_flag = header.type_flag;
|
|
frame.total_frag = (header.frag_info >> 16) & 0xFFFF;
|
|
frame.frag_num = (header.frag_info) & 0xFFFF;
|
|
frame.data_len = header.data_len;
|
|
frame.crc_16 = header.crc_16;
|
|
return frame;
|
|
}
|
|
|
|
/**
|
|
* @brief Store a fragment that has been received
|
|
*
|
|
* @param fragment
|
|
* @param channel
|
|
* @return esp_err_t
|
|
*/
|
|
esp_err_t DataLinkManager::store_fragment(GenericFrame* fragment, uint8_t channel){
|
|
if (fragment == nullptr){
|
|
return ESP_ERR_INVALID_ARG;
|
|
}
|
|
|
|
if (fragment->data_len == 0){
|
|
return ESP_ERR_INVALID_ARG;
|
|
}
|
|
|
|
if (fragment->receiver_id != this_board_id){
|
|
return ESP_ERR_INVALID_ARG;
|
|
}
|
|
|
|
// ESP_LOGI(DEBUG_LINK_TAG, "got frame %d, fragment %d of %d", fragment->seq_num, fragment->frag_num, fragment->total_frag);
|
|
|
|
if (rx_fragment_mutex[channel] == NULL){
|
|
return ESP_FAIL;
|
|
}
|
|
|
|
if (xSemaphoreTake(rx_fragment_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){
|
|
return ESP_ERR_TIMEOUT;
|
|
}
|
|
|
|
if (fragment_map[channel][fragment->receiver_id].find(fragment->seq_num) == fragment_map[channel][fragment->receiver_id].end()){
|
|
FragmentMetadata& metadata = fragment_map[channel][fragment->receiver_id][fragment->seq_num];
|
|
metadata.num_fragments_rx = 0;
|
|
|
|
metadata.fragments.reserve(fragment->total_frag);
|
|
for (uint16_t i = 0; i < fragment->total_frag; i++){
|
|
metadata.fragments.push_back(GenericFrame{});
|
|
}
|
|
}
|
|
|
|
FragmentMetadata& metadata = fragment_map[channel][fragment->receiver_id][fragment->seq_num];
|
|
if ((fragment->frag_num-1) > metadata.fragments.size()){
|
|
xSemaphoreGive(rx_fragment_mutex[channel]);
|
|
return ESP_ERR_INVALID_STATE;
|
|
}
|
|
|
|
if (metadata.fragments[fragment->frag_num-1].data_len == 0){
|
|
metadata.fragments[fragment->frag_num-1] = *fragment;
|
|
metadata.num_fragments_rx++;
|
|
// ESP_LOGI(DEBUG_LINK_TAG, "store frame %d fragment %d success; got %d out of %d", fragment->seq_num, fragment->frag_num, metadata.num_fragments_rx, metadata.fragments.size());
|
|
}
|
|
|
|
uint16_t last_consec_rx_frag = 0;
|
|
if (static_cast<FrameType>(GET_TYPE(fragment->type_flag)) != FrameType::MISC_UDP_GENERIC_TYPE){
|
|
for (; last_consec_rx_frag < fragment->total_frag; last_consec_rx_frag++){
|
|
if (metadata.fragments[last_consec_rx_frag].data_len == 0){
|
|
//found missing fragment
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
size_t metadata_fragment_size = metadata.fragments.size();
|
|
xSemaphoreGive(rx_fragment_mutex[channel]);
|
|
|
|
if (static_cast<FrameType>(GET_TYPE(fragment->type_flag)) != FrameType::MISC_UDP_GENERIC_TYPE){
|
|
SendAckMetaData data = {
|
|
.data = {GENERIC_FRAG_ACK_PREAMBLE, static_cast<uint8_t>((last_consec_rx_frag & 0xFF00) >> 8), static_cast<uint8_t>(last_consec_rx_frag & 0xFF),
|
|
static_cast<uint8_t>((fragment->total_frag & 0xFF00) >> 8), static_cast<uint8_t>(fragment->total_frag & 0xFF),
|
|
static_cast<uint8_t>((fragment->seq_num & 0xFF00) >> 8), static_cast<uint8_t>(fragment->seq_num & 0xFF)},
|
|
.sender_id = fragment->sender_id,
|
|
};
|
|
if (xSemaphoreTake(send_ack_queue_mutex[channel], pdMS_TO_TICKS(SEND_ACK_MUTEX_WAIT)) != pdTRUE){
|
|
return ESP_FAIL;
|
|
}
|
|
|
|
send_ack_queue[channel].push(data);
|
|
xSemaphoreGive(send_ack_queue_mutex[channel]);
|
|
}
|
|
|
|
if (metadata.num_fragments_rx == metadata_fragment_size){
|
|
//all fragments received
|
|
return complete_fragment(fragment->receiver_id, fragment->seq_num, channel);
|
|
}
|
|
|
|
return ESP_OK;
|
|
}
|
|
|
|
/**
|
|
* @brief Removes the corresponding entry from `fragment_map` and pushes the data onto `async_receive_queue`
|
|
*
|
|
* @param board_id
|
|
* @param sequence_num
|
|
* @return esp_err_t
|
|
*/
|
|
esp_err_t DataLinkManager::complete_fragment(uint16_t board_id, uint16_t sequence_num, uint8_t channel){
|
|
Rx_Metadata rx;
|
|
|
|
if (xSemaphoreTake(rx_fragment_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){
|
|
return ESP_ERR_TIMEOUT;
|
|
}
|
|
|
|
if (fragment_map[channel][board_id].find(sequence_num) == fragment_map[channel][board_id].end()){
|
|
return ESP_ERR_NOT_FOUND;
|
|
}
|
|
|
|
FragmentMetadata& metadata = fragment_map[channel][board_id][sequence_num];
|
|
if (metadata.num_fragments_rx != metadata.fragments.size()){
|
|
return ESP_ERR_INVALID_STATE;
|
|
}
|
|
uint16_t total_data_len = metadata.num_fragments_rx*MAX_FRAME_SIZE; //max data size with n fragments
|
|
xSemaphoreGive(rx_fragment_mutex[channel]);
|
|
|
|
auto combined_data = std::make_unique<std::vector<uint8_t>>();
|
|
combined_data->resize(total_data_len);
|
|
rx.data_len = total_data_len;
|
|
if (combined_data == nullptr){
|
|
return ESP_ERR_NO_MEM;
|
|
}
|
|
|
|
if (rx_fragment_mutex[channel] == NULL){
|
|
return ESP_FAIL;
|
|
}
|
|
|
|
// ESP_LOGI(DEBUG_LINK_TAG, "completing %d fragments for frame %d", metadata.num_fragments_rx, sequence_num);
|
|
|
|
if (xSemaphoreTake(rx_fragment_mutex[channel], pdMS_TO_TICKS(ASYNC_QUEUE_WAIT_TICKS)) != pdTRUE){
|
|
return ESP_ERR_TIMEOUT;
|
|
}
|
|
|
|
if (fragment_map[channel][board_id].find(sequence_num) == fragment_map[channel][board_id].end()){
|
|
xSemaphoreGive(rx_fragment_mutex[channel]);
|
|
return ESP_ERR_NOT_FOUND;
|
|
}
|
|
|
|
uint16_t prev_index = 0;
|
|
for (size_t i = 0; i < metadata.num_fragments_rx; i++){
|
|
memcpy(&combined_data->data()[prev_index], metadata.fragments[i].data, metadata.fragments[i].data_len);
|
|
prev_index += metadata.fragments[i].data_len;
|
|
}
|
|
|
|
xSemaphoreGive(rx_fragment_mutex[channel]);
|
|
|
|
rx.data = std::move(combined_data);
|
|
rx.data_len = prev_index;
|
|
|
|
if (async_rx_queue_mutex[channel] == nullptr){
|
|
return ESP_FAIL;
|
|
}
|
|
|
|
GenericFrame frame = metadata.fragments[0];
|
|
|
|
rx.header = {
|
|
.preamble = START_OF_FRAME,
|
|
.sender_id = frame.sender_id,
|
|
.receiver_id = frame.receiver_id,
|
|
.seq_num = frame.seq_num,
|
|
.type_flag = frame.type_flag,
|
|
.frag_info = (uint32_t)((frame.total_frag << 16) | frame.frag_num),
|
|
.data_len = prev_index,
|
|
.crc_16 = 0
|
|
};
|
|
|
|
// ESP_LOGI(DEBUG_LINK_TAG, "pushing frame %d onto async rx queue", sequence_num);
|
|
|
|
if (!async_receive_queue->enqueue(std::move(rx), std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS))) {
|
|
return ESP_ERR_TIMEOUT;
|
|
}
|
|
|
|
fragment_map[channel][board_id].erase(sequence_num);
|
|
|
|
if (fragment_map[channel][board_id].empty()) {
|
|
fragment_map[channel].erase(board_id);
|
|
}
|
|
|
|
// ESP_LOGI(DEBUG_LINK_TAG, "frame %d pushed success", sequence_num);
|
|
|
|
return ESP_OK;
|
|
}
|
|
|
|
/**
|
|
* @brief Sends an ACK
|
|
*
|
|
* @param sender_id This is the board id that is receiving the ACK (the original sender board id)
|
|
* @param data
|
|
* @param data_len
|
|
* @return esp_err_t
|
|
*
|
|
* @note This may be moved to a private function - Unsure if users should be able to manually send ACKs
|
|
*/
|
|
esp_err_t DataLinkManager::send_ack(uint8_t sender_id, uint8_t* data, uint16_t data_len){
|
|
// todo: change this to take in a unique_ptr
|
|
auto buffer = std::make_unique<std::vector<uint8_t>>();
|
|
buffer->resize(data_len);
|
|
memcpy(buffer->data(), data, data_len);
|
|
return send(sender_id, std::move(buffer), FrameType::ACK_TYPE, 0x0);
|
|
}
|
|
|
|
/**
|
|
* @brief Get the first frame's data
|
|
*
|
|
* @param data Char array of the actual combined data
|
|
* @param data_len Combined data length
|
|
* @param header Header information of returning frame
|
|
*
|
|
*/
|
|
std::optional<std::unique_ptr<std::vector<uint8_t>>> DataLinkManager::async_receive(){
|
|
auto maybe_top = async_receive_queue->dequeue(std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS));
|
|
if (!maybe_top) {
|
|
return std::nullopt;
|
|
}
|
|
Rx_Metadata top = std::move(*maybe_top);
|
|
|
|
return std::make_optional<std::unique_ptr<std::vector<uint8_t>>>(std::move(top.data));
|
|
// ESP_LOGI(DEBUG_LINK_TAG, "pushed frame %d onto async queue", header->seq_num);
|
|
}
|
|
|
|
esp_err_t DataLinkManager::receive_rmt(uint8_t channel){
|
|
uint16_t data_len = MAX_FRAME_SIZE; //max possible data len
|
|
uint8_t data[data_len];
|
|
|
|
size_t recv_len = 0;
|
|
|
|
esp_err_t res = phys_comms->receive(data, data_len, &recv_len, channel);
|
|
|
|
if (res != ESP_OK){
|
|
// ESP_LOGE(DEBUG_LINK_TAG, "RMT Failed to receive - recieve_rmt");
|
|
return ESP_ERR_TIMEOUT;
|
|
}
|
|
|
|
if (recv_len > MAX_FRAME_SIZE){
|
|
ESP_LOGE(DEBUG_LINK_TAG, "Received frame is too large to be control or generic");
|
|
return ESP_ERR_INVALID_RESPONSE;
|
|
}
|
|
|
|
if (recv_len < CONTROL_FRAME_OVERHEAD) {
|
|
//Frame is too small
|
|
return ESP_ERR_INVALID_RESPONSE;
|
|
}
|
|
|
|
auto message = std::make_unique<std::vector<uint8_t>>();
|
|
message->resize(MAX_FRAME_SIZE);
|
|
|
|
size_t message_size = 0;
|
|
FrameHeader header;
|
|
|
|
res = get_data_from_frame(data, recv_len, message->data(), &message_size, &header);
|
|
if (res != ESP_OK){
|
|
// print_buffer_binary(message, message_size);
|
|
return res;
|
|
}
|
|
message->resize(message_size);
|
|
|
|
// print_buffer_binary(message, message_size);
|
|
|
|
//push control frame onto async_receive_queue
|
|
if (static_cast<FrameType>(GET_TYPE(header.type_flag)) == FrameType::ACK_TYPE){
|
|
if (message_size != GENERIC_FRAG_ACK_DATA_SIZE || message_size == 0){
|
|
return ESP_OK;
|
|
}
|
|
|
|
if (message->data()[0] != GENERIC_FRAG_ACK_PREAMBLE){
|
|
return ESP_OK;
|
|
}
|
|
|
|
FrameAckRecord record = {
|
|
.last_ack = static_cast<uint16_t>((message->data()[1] << 8) | (message->data()[2])),
|
|
.total_frags = static_cast<uint16_t>((message->data()[3] << 8) | (message->data()[4])),
|
|
.seq_num = static_cast<uint16_t>((message->data()[5] << 8) | (message->data()[6]))
|
|
};
|
|
|
|
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;
|
|
}
|
|
|
|
if (!IS_CONTROL_FRAME(header.type_flag)){
|
|
//Handle generic frame fragment
|
|
GenericFrame frame = make_generic_frame_from_header(header);
|
|
if (message_size > MAX_FRAME_SIZE){
|
|
return ESP_FAIL;
|
|
}
|
|
|
|
memcpy(frame.data, message->data(), message_size);
|
|
esp_err_t res = store_fragment(&frame, channel);
|
|
return res;
|
|
}
|
|
|
|
//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);
|
|
|
|
//check for a rip frame
|
|
if (static_cast<FrameType>(GET_TYPE(header.type_flag)) == FrameType::RIP_TABLE_CONTROL){
|
|
ESP_LOGI(DEBUG_LINK_TAG, "Got a RIP frame");
|
|
|
|
for (size_t i = 0; i < message_size-1; i+=2){
|
|
uint8_t board_id = message->data()[i];
|
|
uint8_t hops = message->data()[i+1];
|
|
// ESP_LOGI(DEBUG_LINK_TAG, "Received: board_id %d and number of hops %d on channel %d", board_id, hops, channel);
|
|
|
|
RIPRow* entry = nullptr;
|
|
|
|
res = rip_find_entry(board_id, &entry, true);
|
|
if (res != ESP_OK){
|
|
return ESP_FAIL;
|
|
}
|
|
|
|
if (entry == nullptr){
|
|
printf("rip pointer\n");
|
|
return ESP_FAIL; //no room for more entries in the table
|
|
}
|
|
|
|
if (entry->valid == RIP_NEW_ROW){
|
|
//adding a new entry
|
|
rip_add_entry(board_id, hops + 1, channel, &entry);
|
|
} else {
|
|
//updating an entry
|
|
rip_update_entry(hops + 1, channel, &entry);
|
|
}
|
|
|
|
if (GET_FLAG(header.type_flag) == FLAG_DISCOVERY){
|
|
//discovery -> send routing table
|
|
// ESP_LOGI(DEBUG_LINK_TAG, "got discovery reply");
|
|
RIPRow_public row_queue = {
|
|
.info = entry->info,
|
|
.channel = entry->channel
|
|
};
|
|
|
|
xQueueSendToBack(discovery_tables, &row_queue, (TickType_t)10);
|
|
}
|
|
|
|
}
|
|
if (message_size == RIP_DISCOVERY_MESSAGE_SIZE){
|
|
res = send_rip_frame(false, header.sender_id);
|
|
if (res != ESP_OK){
|
|
ESP_LOGE(DEBUG_LINK_TAG, "Failed to send back rip table to board %d", header.sender_id);
|
|
return res;
|
|
}
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
uint16_t seq_num = 0;
|
|
|
|
res = get_sequence_num(header.receiver_id, &seq_num);
|
|
|
|
if (res != ESP_OK){
|
|
return res;
|
|
}
|
|
|
|
//got frame but not destined for this board
|
|
if (header.receiver_id != this_board_id && header.receiver_id != BROADCAST_ADDR && header.seq_num > seq_num){
|
|
// ESP_LOGI(DEBUG_LINK_TAG, "Sending message to board %d with message %s", header.receiver_id, message);
|
|
res = send(header.receiver_id, std::move(message), FrameType::MISC_CONTROL_TYPE, 0);
|
|
return res;
|
|
}
|
|
|
|
Rx_Metadata metadata = {
|
|
.data = std::move(message),
|
|
.data_len = (uint16_t)message_size,
|
|
.header = header
|
|
};
|
|
|
|
if (!async_receive_queue->enqueue(std::move(metadata), std::chrono::milliseconds(ASYNC_QUEUE_WAIT_TICKS))){
|
|
return ESP_ERR_TIMEOUT;
|
|
}
|
|
return ESP_OK;
|
|
}
|
|
|
|
[[noreturn]] void DataLinkManager::receive_thread_main(void* args){
|
|
DataLinkManager* link_layer_obj = static_cast<DataLinkManager*>(args);
|
|
if (link_layer_obj == nullptr || link_layer_obj->manual_broadcasts == nullptr){
|
|
ESP_LOGE(DEBUG_LINK_TAG, "Receive thread failed to start due to invalid pointer");
|
|
vTaskDelete(nullptr);
|
|
}
|
|
|
|
ESP_LOGI(DEBUG_LINK_TAG, "Starting Receive thread task");
|
|
|
|
esp_err_t res;
|
|
|
|
for (uint8_t i = 0; i < link_layer_obj->num_channels; i++){
|
|
res = link_layer_obj->start_receive_frames_rmt(i);
|
|
}
|
|
while(!link_layer_obj->stop_tasks){
|
|
for (uint8_t i = 0; i < link_layer_obj->num_channels; i++){
|
|
res = link_layer_obj->receive_rmt(i);
|
|
res = link_layer_obj->start_receive_frames_rmt(i);
|
|
}
|
|
|
|
vTaskDelay(pdMS_TO_TICKS(RECEIVE_TASK_PERIOD_MS));
|
|
}
|
|
|
|
vTaskDelete(nullptr);
|
|
}
|
|
|
|
[[noreturn]] void DataLinkManager::send_ack_thread_main(void* args){
|
|
DataLinkManager* link_layer_obj = static_cast<DataLinkManager*>(args);
|
|
if (link_layer_obj == nullptr || link_layer_obj->manual_broadcasts == nullptr){
|
|
ESP_LOGE(DEBUG_LINK_TAG, "Send Ack thread failed to start due to invalid pointer");
|
|
vTaskDelete(nullptr);
|
|
}
|
|
|
|
for (uint8_t i = 0; i < link_layer_obj->num_channels; i++){
|
|
if (link_layer_obj->send_ack_queue_mutex[i] == NULL){
|
|
ESP_LOGE(DEBUG_LINK_TAG, "%d send ack queue mutex is null!", i);
|
|
vTaskDelete(nullptr);
|
|
}
|
|
}
|
|
|
|
ESP_LOGI(DEBUG_LINK_TAG, "Starting Send ACK task");
|
|
|
|
esp_err_t res;
|
|
|
|
while(!link_layer_obj->stop_tasks){
|
|
for (uint8_t channel = 0; channel < link_layer_obj->num_channels; channel++){
|
|
vTaskDelay(pdMS_TO_TICKS(SEND_ACK_PERIOD_MS));
|
|
|
|
if (xSemaphoreTake(link_layer_obj->send_ack_queue_mutex[channel], pdMS_TO_TICKS(SEND_ACK_MUTEX_WAIT)) != pdTRUE){
|
|
continue;
|
|
}
|
|
|
|
if (link_layer_obj->send_ack_queue[channel].empty()){
|
|
xSemaphoreGive(link_layer_obj->send_ack_queue_mutex[channel]);
|
|
continue;
|
|
}
|
|
|
|
SendAckMetaData data = link_layer_obj->send_ack_queue[channel].front();
|
|
link_layer_obj->send_ack_queue[channel].pop();
|
|
link_layer_obj->send_ack(data.sender_id, data.data, GENERIC_FRAG_ACK_DATA_SIZE);
|
|
xSemaphoreGive(link_layer_obj->send_ack_queue_mutex[channel]);
|
|
}
|
|
}
|
|
|
|
vTaskDelete(nullptr);
|
|
}
|