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

Reduce DataLink latency

Browse Source
This commit is contained in:
Johnathon Slightham
2026-01-27 22:16:24 -05:00
committed by Johnathon Slightham
parent f54063aa8c
commit c14c82ce9c
25 changed files with 801 additions and 757 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
components/rpc/CommunicationFactory.cpp
View File

@@ -30,7 +30,7 @@ std::unique_ptr<IDiscoveryService> CommunicationFactory::create_discovery_servic
}
}
std::unique_ptr<IRPCServer> CommunicationFactory::create_lossy_server(const CommunicationMethod type, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue) {
std::unique_ptr<IRPCServer> CommunicationFactory::create_lossy_server(const CommunicationMethod type, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>& rx_queue) {
switch (type) {
case Wireless:
return std::make_unique<UDPServer>(RECV_PORT, SEND_PORT, rx_queue);
@@ -39,7 +39,7 @@ std::unique_ptr<IRPCServer> CommunicationFactory::create_lossy_server(const Comm
}
}
std::unique_ptr<IRPCServer> CommunicationFactory::create_lossless_server(const CommunicationMethod type, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue) {
std::unique_ptr<IRPCServer> CommunicationFactory::create_lossless_server(const CommunicationMethod type, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>& rx_queue) {
switch (type) {
case Wireless:
return std::make_unique<TCPServer>(TCP_PORT, rx_queue);

251
components/rpc/CommunicationRouter.cpp
View File

@@ -1,3 +1,5 @@
#include <chrono>
#include <cstring>
#include <iostream>
#include "AngleControlMessageBuilder.h"
@@ -14,157 +16,172 @@
#include "include/wireless/mDNSDiscoveryService.h"
#define TAG "CommunicationRouter"
#define MAX_RX_BUFFER_SIZE 1024
#define WIRELESS_DEQUEUE_TIMEOUT_MS 3000
CommunicationRouter::~CommunicationRouter() { vTaskDelete(m_router_thread); }
// todo: we really need to change all char to uint8_t everywhere
// todo: get rid of copying going on, need to pass around sharedptrs/uniqueptrs
CommunicationRouter::~CommunicationRouter() {
vTaskDelete(m_router_thread);
}
[[noreturn]] void CommunicationRouter::router_thread(void *args) {
const auto that = static_cast<CommunicationRouter *>(args);
const auto that = static_cast<CommunicationRouter *>(args);
while (true) {
const auto buffer = that->m_tcp_rx_queue->dequeue();
ESP_LOGD(TAG, "Got message from TCP");
that->route(buffer->data(), buffer->size());
}
while (true) {
if( auto maybe_buffer = that->m_tcp_rx_queue->dequeue(std::chrono::milliseconds(WIRELESS_DEQUEUE_TIMEOUT_MS))) {
ESP_LOGD(TAG, "Got message from TCP");
that->route(std::move(*maybe_buffer));
}
}
}
[[noreturn]] void CommunicationRouter::link_layer_thread(void *args) {
const auto that = static_cast<CommunicationRouter *>(args);
const auto that = static_cast<CommunicationRouter *>(args);
while (true) {
if (std::chrono::system_clock::now() - that->m_last_leader_updated >
std::chrono::seconds(2)) {
that->m_last_leader_updated = std::chrono::system_clock::now();
that->update_leader();
while (true) {
if (std::chrono::system_clock::now() - that->m_last_leader_updated >
std::chrono::seconds(2)) {
that->m_last_leader_updated = std::chrono::system_clock::now();
that->update_leader();
}
if (auto ptr = that->m_data_link_manager->async_receive()) {
that->route(std::move(*ptr));
}
}
for (uint8_t channel = 0;
channel <
MODULE_TO_NUM_CHANNELS_MAP[that->m_config_manager.get_module_type()];
channel++) {
uint16_t frame_size = 0;
FrameHeader frame_header{};
if (ESP_OK != that->m_data_link_manager->async_receive_info(
&frame_size, &frame_header, channel) ||
0 == frame_size) {
continue;
}
std::vector<uint8_t> data{};
data.resize(frame_size);
that->m_data_link_manager->async_receive(data.data(), frame_size,
&frame_header, channel);
that->route(data.data(), frame_size);
}
vTaskDelay(pdMS_TO_TICKS(50));
}
}
int CommunicationRouter::send_msg(char *buffer, const size_t length) const {
ESP_LOGD(TAG, "Got message from application");
route(reinterpret_cast<uint8_t *>(buffer), length);
return 0;
route(reinterpret_cast<uint8_t *>(buffer), length);
return 0;
}
void CommunicationRouter::update_leader() {
RIPRow_public table[RIP_MAX_ROUTES];
size_t table_size = RIP_MAX_ROUTES;
this->m_data_link_manager->get_routing_table(table, &table_size);
RIPRow_public table[RIP_MAX_ROUTES];
size_t table_size = RIP_MAX_ROUTES;
this->m_data_link_manager->get_routing_table(table, &table_size);
// Leader election (just get the highest id in rip)
std::vector<int> connected_module_ids;
uint8_t max = m_module_id;
for (int i = 0; i < table_size; i++) {
const auto id = table[i].info.board_id;
connected_module_ids.emplace_back(id);
if (id > max) { // todo: change this to be correct
max = id;
// Leader election (just get the highest id in rip)
std::vector<int> connected_module_ids;
uint8_t max = m_module_id;
for (int i = 0; i < table_size; i++) {
const auto id = table[i].info.board_id;
connected_module_ids.emplace_back(id);
if (id > max) { // todo: change this to be correct
max = id;
}
}
}
// Leader has changed, we may need to change PC connection state
if (this->m_leader != max) {
ESP_LOGI(TAG, "Leader has changed from %d to %d", this->m_leader, max);
if (max == m_module_id) {
m_pc_connection->connect();
m_lossless_server->startup();
m_lossy_server->startup();
} else if (this->m_leader == m_module_id) {
m_pc_connection->disconnect();
m_lossless_server->shutdown();
m_lossy_server->shutdown();
// Leader has changed, we may need to change PC connection state
if (this->m_leader != max) {
ESP_LOGI(TAG, "Leader has changed from %d to %d", this->m_leader, max);
if (max == m_module_id) {
m_pc_connection->connect();
m_lossless_server->startup();
m_lossy_server->startup();
} else if (this->m_leader == m_module_id) {
m_pc_connection->disconnect();
m_lossless_server->shutdown();
m_lossy_server->shutdown();
}
}
}
this->m_leader = max;
this->m_leader = max;
if (this->m_leader == m_module_id) {
this->m_discovery_service->set_connected_boards(connected_module_ids);
}
if (this->m_leader == m_module_id) {
this->m_discovery_service->set_connected_boards(connected_module_ids);
}
}
void CommunicationRouter::route(uint8_t *buffer, const size_t length) const {
flatbuffers::Verifier verifier(buffer, length);
// This could be moved to just be called on wireline data to save cpu cycles.
if (bool ok = Messaging::VerifyMPIMessageBuffer(verifier); !ok) {
ESP_LOGW(TAG, "route: got an invalid MPI message, disregarding");
return;
}
if (const auto &mpi_message =
Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer);
mpi_message->destination() == m_module_id) {
this->m_rx_callback(reinterpret_cast<char *>(buffer), 512);
} else if (mpi_message->destination() == PC_ADDR &&
this->m_leader == m_module_id) {
if (mpi_message->is_durable()) {
this->m_lossless_server->send_msg(reinterpret_cast<char *>(buffer), 512);
} else {
this->m_lossy_server->send_msg(reinterpret_cast<char *>(buffer), 512);
// Route without trying to copy to heap. Only call if you do not have a unique_ptr.
// To handle the case of writing directly from control -> TCP/UDP, nothing has to touch the heap.
void CommunicationRouter::route(uint8_t *buffer, size_t size) const {
flatbuffers::Verifier verifier(buffer, size);
// This could be moved to just be called on wireline data to save cpu cycles.
if (bool ok = Messaging::VerifyMPIMessageBuffer(verifier); !ok) {
ESP_LOGW(TAG, "route: got an invalid MPI message, disregarding");
return;
}
if (const auto &mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer);
mpi_message->destination() == m_module_id) {
auto ubuffer = std::make_unique<std::vector<uint8_t>>();
ubuffer->resize(size);
memcpy(ubuffer->data(), buffer, size);
this->m_rx_callback(std::move(ubuffer));
} else if (mpi_message->destination() == PC_ADDR && this->m_leader == m_module_id) {
if (mpi_message->is_durable()) {
this->m_lossless_server->send_msg(buffer, size);
} else {
this->m_lossy_server->send_msg(buffer, size);
}
} else {
const auto dest = mpi_message->destination() == PC_ADDR ? this->m_leader : mpi_message->destination();
auto u_buffer = std::make_unique<std::vector<uint8_t>>();
u_buffer->resize(size);
memcpy(u_buffer->data(), buffer, size);
this->m_data_link_manager->send(dest, std::move(u_buffer), FrameType::MOTOR_TYPE, 0);
}
}
// Route heap messages
void CommunicationRouter::route(std::unique_ptr<std::vector<uint8_t>>&& buffer) const {
flatbuffers::Verifier verifier(buffer->data(), buffer->size());
// This could be moved to just be called on wireline data to save cpu cycles.
if (bool ok = Messaging::VerifyMPIMessageBuffer(verifier); !ok) {
ESP_LOGW(TAG, "route: got an invalid MPI message, disregarding");
return;
}
if (const auto &mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer->data());
mpi_message->destination() == m_module_id) {
this->m_rx_callback(std::move(buffer));
} else if (mpi_message->destination() == PC_ADDR && this->m_leader == m_module_id) {
if (mpi_message->is_durable()) {
this->m_lossless_server->send_msg(buffer->data(), buffer->size());
} else {
this->m_lossy_server->send_msg(buffer->data(), buffer->size());
}
} else if (mpi_message->destination() == PC_ADDR) {
this->m_data_link_manager->send(this->m_leader, std::move(buffer), FrameType::MOTOR_TYPE, 0);
} else {
this->m_data_link_manager->send(mpi_message->destination(), std::move(buffer), FrameType::MOTOR_TYPE,
0);
}
} else if (mpi_message->destination() == PC_ADDR) {
this->m_data_link_manager->send(this->m_leader, buffer, length,
FrameType::MOTOR_TYPE, 0);
} else {
this->m_data_link_manager->send(mpi_message->destination(), buffer, length,
FrameType::MOTOR_TYPE, 0);
}
}
std::pair<std::vector<uint8_t>, std::vector<Orientation>>
CommunicationRouter::get_physically_connected_modules() const {
std::vector<RIPRow_public> table;
table.resize(RIP_MAX_ROUTES);
size_t table_size = RIP_MAX_ROUTES * sizeof(RIPRow_public);
m_data_link_manager->get_routing_table(table.data(), &table_size);
std::vector<RIPRow_public> table;
table.resize(RIP_MAX_ROUTES);
size_t table_size = RIP_MAX_ROUTES * sizeof(RIPRow_public);
m_data_link_manager->get_routing_table(table.data(), &table_size);
std::vector<uint8_t> connected_module_ids;
std::vector<Orientation> connected_module_orientations;
connected_module_ids.resize(MAX_WIRED_CONNECTIONS);
connected_module_orientations.resize(MAX_WIRED_CONNECTIONS);
std::vector<uint8_t> connected_module_ids;
std::vector<Orientation> connected_module_orientations;
connected_module_ids.resize(MAX_WIRED_CONNECTIONS);
connected_module_orientations.resize(MAX_WIRED_CONNECTIONS);
for (int i = 0; i < MAX_WIRED_CONNECTIONS; i++) {
connected_module_ids[i] = 0; // this is not the PC ID here, marking as nc.
}
for (int i = 0; i < table_size; i++) {
if (table[i].info.hops == 1 && table[i].channel < MAX_WIRED_CONNECTIONS) {
connected_module_ids[table[i].channel] = table[i].info.board_id;
for (int i = 0; i < MAX_WIRED_CONNECTIONS; i++) {
connected_module_ids[i] = 0; // this is not the PC ID here, marking as nc.
}
}
if (const auto id = connected_module_ids[0]; 0 == id) {
connected_module_orientations[0] = Orientation_Deg0;
} else {
connected_module_orientations[0] = OrientationDetection::get_orientation(0);
}
for (int i = 0; i < table_size; i++) {
if (table[i].info.hops == 1 && table[i].channel < MAX_WIRED_CONNECTIONS) {
connected_module_ids[table[i].channel] = table[i].info.board_id;
}
}
return {connected_module_ids, connected_module_orientations};
if (const auto id = connected_module_ids[0]; 0 == id) {
connected_module_orientations[0] = Orientation_Deg0;
} else {
connected_module_orientations[0] = OrientationDetection::get_orientation(0);
}
return {connected_module_ids, connected_module_orientations};
}
[[nodiscard]] uint8_t CommunicationRouter::get_leader() const {
return this->m_leader;
return this->m_leader;
}

5
components/rpc/MessagingInterface.cpp
View File

@@ -13,7 +13,6 @@
#include "MPIMessageBuilder.h"
MessagingInterface::~MessagingInterface() {
vQueueDelete(m_mpi_rx_queue);
vSemaphoreDelete(m_map_semaphore);
for (const auto queue: m_tag_to_queue | std::views::values) {
@@ -52,8 +51,8 @@ int MessagingInterface::sendrecv(char* send_buffer, const int send_size, const i
}
// todo: when handleRecv returns, remove from queue (from router)
void MessagingInterface::handleRecv(const char* recv_buffer, int recv_size) {
const auto mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(reinterpret_cast<const uint8_t *>(recv_buffer));
void MessagingInterface::handleRecv(std::unique_ptr<std::vector<uint8_t>>&& buffer) {
const auto mpi_message = Flatbuffers::MPIMessageBuilder::parse_mpi_message(buffer->data());
checkOrInsertTag(mpi_message->tag());

4
components/rpc/OrientationDetection.cpp
View File

@@ -21,16 +21,12 @@ void OrientationDetection::init() {
Orientation OrientationDetection::get_orientation(const uint8_t channel) {
if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_90_DEG_MAP[channel]))) {
ESP_LOGD(TAG, "90deg");
return Orientation_Deg90;
} else if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_180_DEG_MAP[channel]))) {
ESP_LOGD(TAG, "180deg");
return Orientation_Deg180;
} else if (gpio_get_level(static_cast<gpio_num_t>(CHANNEL_TO_270_DEG_MAP[channel]))) {
ESP_LOGD(TAG, "270deg");
return Orientation_Deg270;
} else {
ESP_LOGD(TAG, "No orientation detected");
return Orientation_Deg0;
}
}

6
components/rpc/include/CommunicationFactory.h
View File

@@ -13,15 +13,15 @@
#include "IConnectionManager.h"
#include "IDiscoveryService.h"
#include "IRPCServer.h"
#include "PtrQueue.h"
#include "BlockingQueue.h"
#include "enums.h"
class CommunicationFactory {
public:
static std::unique_ptr<IConnectionManager> create_connection_manager(CommunicationMethod type);
static std::unique_ptr<IDiscoveryService> create_discovery_service(CommunicationMethod type);
static std::unique_ptr<IRPCServer> create_lossy_server(CommunicationMethod type, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>> &rx_queue);
static std::unique_ptr<IRPCServer> create_lossless_server(CommunicationMethod type, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue);
static std::unique_ptr<IRPCServer> create_lossy_server(CommunicationMethod type, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue);
static std::unique_ptr<IRPCServer> create_lossless_server(CommunicationMethod type, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>& rx_queue);
};
#endif //COMMUNICATIONFACTORY_H

16
components/rpc/include/CommunicationRouter.h
View File

@@ -21,13 +21,15 @@
#include "wireless/TCPServer.h"
#include "wireless/WifiManager.h"
#define MAX_NETWORK_QUEUE_SIZE 10
class CommunicationRouter {
public:
explicit CommunicationRouter(
const std::function<void(char *, int)> &rx_callback)
: m_tcp_rx_queue(std::make_shared<PtrQueue<std::vector<uint8_t>>>(10)),
m_rx_callback(rx_callback),
const std::function<void(std::unique_ptr<std::vector<uint8_t>>&&)> &rx_callback)
: m_tcp_rx_queue(std::make_shared<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>(MAX_NETWORK_QUEUE_SIZE)),
m_rx_callback(std::move(rx_callback)),
m_config_manager(ConfigManager::get_instance()),
m_pc_connection(CommunicationFactory::create_connection_manager(
m_config_manager.get_communication_method())),
@@ -56,15 +58,15 @@ public:
[[noreturn]] static void link_layer_thread(void *args);
int send_msg(char *buffer, size_t length) const;
void update_leader();
void route(uint8_t *buffer, size_t length) const;
void route(std::unique_ptr<std::vector<uint8_t>>&& buffer) const;
void route(uint8_t* buffer, size_t size) const;
[[nodiscard]] std::pair<std::vector<uint8_t>, std::vector<Orientation>>
get_physically_connected_modules() const;
[[nodiscard]] uint8_t get_leader() const;
// todo: does this really need to be here (so i can access from thread)?
std::shared_ptr<PtrQueue<std::vector<uint8_t>>>
m_tcp_rx_queue; // todo: this should probably be thread safe
std::function<void(char *, int)> m_rx_callback;
std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_tcp_rx_queue;
std::function<void(std::unique_ptr<std::vector<std::uint8_t>>)> m_rx_callback;
private:
TaskHandle_t m_router_thread = nullptr;

7
components/rpc/include/IRPCServer.h
View File

@@ -5,12 +5,15 @@
#ifndef IRPCSERVER_H
#define IRPCSERVER_H
#include <memory>
#include <vector>
class IRPCServer {
public:
public:
virtual ~IRPCServer() = default;
virtual void startup() = 0;
virtual void shutdown() = 0;
virtual int send_msg(char* buffer, uint32_t length) const = 0;
virtual int send_msg(uint8_t *buffer, size_t size) const = 0;
};
#endif //IRPCSERVER_H

9
components/rpc/include/MessagingInterface.h
View File

@@ -9,6 +9,7 @@
#include <unordered_map>
#include <flatbuffers_generated/TopologyMessage_generated.h>
#include "BlockingQueue.h"
#include "constants/app_comms.h"
#include "CommunicationRouter.h"
@@ -16,8 +17,8 @@ class MessagingInterface {
public:
explicit MessagingInterface()
: m_config_manager(ConfigManager::get_instance()),
m_mpi_rx_queue(xQueueCreate(MAX_RX_BUFFER_SIZE, RX_QUEUE_SIZE)),
m_router(std::make_unique<CommunicationRouter>([this](const char* buffer, const int size) { handleRecv(buffer, size); })),
m_mpi_rx_queue(std::make_unique<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>>(RX_QUEUE_SIZE)),
m_router(std::make_unique<CommunicationRouter>([this](std::unique_ptr<std::vector<uint8_t>>&& buffer) { handleRecv(std::move(buffer)); })),
m_map_semaphore(xSemaphoreCreateMutex()) {};
~MessagingInterface();
@@ -31,13 +32,13 @@ public:
uint8_t get_leader() const;
private:
void handleRecv(const char* recv_buffer, int recv_size);
void handleRecv(std::unique_ptr<std::vector<uint8_t>>&& buffer);
void checkOrInsertTag(uint8_t tag);
ConfigManager& m_config_manager;
uint16_t m_sequence_number = 0;
QueueHandle_t m_mpi_rx_queue; // todo: maybe move this down classes more
std::unique_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_mpi_rx_queue;
std::unique_ptr<CommunicationRouter> m_router;
SemaphoreHandle_t m_map_semaphore;
std::unordered_map<uint8_t, QueueHandle_t> m_tag_to_queue;

16
components/rpc/include/wireless/TCPServer.h
View File

@@ -6,22 +6,22 @@
#define TCPSERVER_H
#include <memory>
#include <vector>
#include <unordered_set>
#include <vector>
#include "freertos/FreeRTOS.h"
#include "IRPCServer.h"
#include "PtrQueue.h"
#include "BlockingQueue.h"
#include "freertos/FreeRTOS.h"
class TCPServer final : public IRPCServer {
public:
TCPServer(int port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue);
public:
TCPServer(int port, const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue);
~TCPServer() override;
void startup() override;
void shutdown() override;
int send_msg(char* buffer, uint32_t length) const override;
int send_msg(uint8_t* buffer, size_t size) const override;
private:
private:
bool authenticate_client(int client_sock);
static bool is_network_connected();
@@ -34,7 +34,7 @@ private:
TaskHandle_t m_task;
TaskHandle_t m_rx_task;
std::shared_ptr<PtrQueue<std::vector<uint8_t>>> m_rx_queue;
std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_rx_queue;
SemaphoreHandle_t m_mutex;
std::unordered_set<int> m_clients;

21
components/rpc/include/wireless/UDPServer.h
View File

@@ -6,36 +6,37 @@
#define UDPSERVER_H
#include <memory>
#include <vector>
#include <unordered_set>
#include <vector>
#include "freertos/FreeRTOS.h"
#include "IRPCServer.h"
#include "PtrQueue.h"
#include "BlockingQueue.h"
#include "freertos/FreeRTOS.h"
class UDPServer final : public IRPCServer {
public:
UDPServer(int rx_port, int tx_port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue);
public:
UDPServer(int rx_port, int tx_port,
const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue);
~UDPServer() override;
void startup() override;
void shutdown() override;
int send_msg(char* buffer, uint32_t length) const override;
int send_msg(uint8_t *buffer, size_t size) const override;
private:
private:
bool authenticate_client(int client_sock);
static bool is_network_connected();
[[noreturn]] static void socket_monitor_thread(void *args);
int m_tx_port;
int m_tx_port;
int m_rx_port;
int m_tx_server_sock;
int m_rx_server_sock;
int m_rx_server_sock;
TaskHandle_t m_rx_task;
std::shared_ptr<PtrQueue<std::vector<uint8_t>>> m_rx_queue;
std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> m_rx_queue;
};
#endif //UDPSERVER_H

11
components/rpc/wireless/TCPServer.cpp
View File

@@ -1,3 +1,4 @@
#include <chrono>
#include <memory>
#include "bits/shared_ptr_base.h"
@@ -15,6 +16,8 @@
#include "sys/param.h"
#include "wireless/TCPServer.h"
#define RX_QUEUE_ENQUEUE_TIMEOUT_MS 50 // must be small to ensure we drain TCP buffer
#define TAG "TCPServer"
#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
@@ -24,7 +27,7 @@
// - tx from board
TCPServer::TCPServer(const int port,
const std::shared_ptr<PtrQueue<std::vector<uint8_t>>> &rx_queue) {
const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue) {
this->m_port = port;
this->m_mutex = xSemaphoreCreateMutex();
this->m_clients = std::unordered_set<int>();
@@ -215,7 +218,7 @@ void TCPServer::shutdown() {
} else {
ESP_LOGD(TAG, "TCP Server Received %d bytes\n", len);
buffer->resize(len);
that->m_rx_queue->enqueue(std::move(buffer));
that->m_rx_queue->enqueue(std::move(buffer), std::chrono::milliseconds(RX_QUEUE_ENQUEUE_TIMEOUT_MS));
}
}
}
@@ -260,11 +263,13 @@ bool TCPServer::authenticate_client(int sock) {
return 0;
}
int TCPServer::send_msg(char *buffer, const uint32_t length) const {
int TCPServer::send_msg(uint8_t *buffer, size_t size) const {
if (!is_network_connected()) {
return -1;
}
const auto length = (uint32_t)size;
for (const auto client_sock : m_clients) {
send(client_sock, &length, 4, 0);
send(client_sock, buffer, length, 0);

325
components/rpc/wireless/UDPServer.cpp
View File

@@ -1,3 +1,4 @@
#include <chrono>
#include <cstring>
#include <memory>
@@ -17,205 +18,207 @@
#include "wireless/UDPServer.h"
#define TAG "UDPServer"
#define MAX_RX_QUEUE_ENQUEUE_TIMEOUT_MS 50
#define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
// todo: - authenticate
UDPServer::UDPServer(
const int rx_port, const int tx_port,
const std::shared_ptr<PtrQueue<std::vector<uint8_t>>> &rx_queue) {
this->m_rx_port = rx_port;
this->m_tx_port = tx_port;
this->m_rx_task = nullptr;
this->m_rx_queue = rx_queue;
this->m_rx_server_sock = 0;
this->m_tx_server_sock = 0;
UDPServer::UDPServer(const int rx_port, const int tx_port,
const std::shared_ptr<BlockingQueue<std::unique_ptr<std::vector<uint8_t>>>> &rx_queue) {
this->m_rx_port = rx_port;
this->m_tx_port = tx_port;
this->m_rx_task = nullptr;
this->m_rx_queue = rx_queue;
this->m_rx_server_sock = 0;
this->m_tx_server_sock = 0;
}
UDPServer::~UDPServer() { this->shutdown(); }
UDPServer::~UDPServer() {
this->shutdown();
}
void UDPServer::startup() {
ESP_LOGI(TAG, "Starting UDP server on port %d", this->m_rx_port);
if (nullptr != this->m_rx_task) {
ESP_LOGW(TAG, "Attempted to start UDP server when already started, "
"ignoring start request");
return;
}
ESP_LOGI(TAG, "Starting UDP server on port %d", this->m_rx_port);
if (nullptr != this->m_rx_task) {
ESP_LOGW(TAG, "Attempted to start UDP server when already started, "
"ignoring start request");
return;
}
xTaskCreate(socket_monitor_thread, "udp_rx", 4096, this, 5, &this->m_rx_task);
xTaskCreate(socket_monitor_thread, "udp_rx", 4096, this, 5, &this->m_rx_task);
}
void UDPServer::shutdown() {
ESP_LOGI(TAG, "Shutting down UDP server");
if (nullptr != this->m_rx_task) {
vTaskDelete(this->m_rx_task);
close(this->m_rx_server_sock);
close(this->m_tx_server_sock);
this->m_rx_task = nullptr;
this->m_rx_server_sock = -1;
this->m_tx_server_sock = -1;
}
ESP_LOGI(TAG, "Shutting down UDP server");
if (nullptr != this->m_rx_task) {
vTaskDelete(this->m_rx_task);
close(this->m_rx_server_sock);
close(this->m_tx_server_sock);
this->m_rx_task = nullptr;
this->m_rx_server_sock = -1;
this->m_tx_server_sock = -1;
}
}
[[noreturn]] void UDPServer::socket_monitor_thread(void *args) {
const auto that = static_cast<UDPServer *>(args);
const auto that = static_cast<UDPServer *>(args);
while (true) {
ESP_LOGI(TAG, "Attempting to start UDP Server on %d", that->m_rx_port);
while (true) {
ESP_LOGI(TAG, "Attempting to start UDP Server on %d", that->m_rx_port);
if (!is_network_connected()) {
ESP_LOGW(TAG, "Network is disconnected");
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
sockaddr_in saddr = {0};
sockaddr_in from_addr = {0};
that->m_rx_server_sock = socket(AF_INET, SOCK_DGRAM, 0);
if (that->m_rx_server_sock == -1) {
ESP_LOGE(TAG, "Create UDP socket fail");
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
that->m_tx_server_sock = socket(AF_INET, SOCK_DGRAM, 0);
if (that->m_tx_server_sock < 0) {
ESP_LOGE(TAG, "Unable to create UDP tx socket: errno %d", errno);
close(that->m_rx_server_sock);
that->m_rx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
int reuse = 1;
if (setsockopt(that->m_rx_server_sock, SOL_SOCKET, SO_REUSEADDR, &reuse,
sizeof(reuse)) < 0) {
ESP_LOGE(TAG, "Failed to set SO_REUSEADDR. Error %d", errno);
close(that->m_rx_server_sock);
close(that->m_tx_server_sock);
that->m_rx_server_sock = -1;
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
saddr.sin_family = AF_INET;
saddr.sin_port = htons(that->m_rx_port);
saddr.sin_addr.s_addr = htonl(INADDR_ANY);
int ret = bind(that->m_rx_server_sock, (struct sockaddr *)&saddr,
sizeof(struct sockaddr_in));
if (ret < 0) {
ESP_LOGE(TAG, "Failed to bind socket. Error %d", errno);
close(that->m_rx_server_sock);
close(that->m_tx_server_sock);
that->m_rx_server_sock = -1;
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
struct ip_mreq imreq = {};
imreq.imr_multiaddr.s_addr = inet_addr(RECV_MCAST);
imreq.imr_interface.s_addr = htonl(INADDR_ANY);
if (setsockopt(that->m_rx_server_sock, IPPROTO_IP, IP_ADD_MEMBERSHIP,
&imreq, sizeof(struct ip_mreq)) < 0) {
ESP_LOGE(TAG, "Failed to set IP_ADD_MEMBERSHIP. Error %d", errno);
close(that->m_rx_server_sock);
close(that->m_tx_server_sock);
that->m_rx_server_sock = -1;
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
uint32_t msg_size;
while (is_network_connected()) {
auto buffer = std::make_unique<std::vector<uint8_t>>();
buffer->resize(MAX_RX_BUFFER_SIZE + 4);
if (int len = recvfrom(that->m_rx_server_sock, buffer->data(),
MAX_RX_BUFFER_SIZE, 0, nullptr, nullptr);
len < 0) {
ESP_LOGE(TAG, "Error occurred during receiving: errno %d", errno);
} else if (len < 4 || len > MAX_RX_BUFFER_SIZE) {
ESP_LOGE(TAG, "Got illegal message size");
} else {
msg_size = *reinterpret_cast<uint32_t *>(buffer->data());
if (msg_size > len - 4) {
ESP_LOGW(TAG, "Message size incorrect");
continue;
if (!is_network_connected()) {
ESP_LOGW(TAG, "Network is disconnected");
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
buffer->erase(buffer->begin(), buffer->begin() + 4); // todo: copying
buffer->resize(msg_size);
that->m_rx_queue->enqueue(std::move(buffer));
}
}
ESP_LOGW(TAG, "Network disconnected");
close(that->m_tx_server_sock);
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
}
sockaddr_in saddr = {0};
sockaddr_in from_addr = {0};
that->m_rx_server_sock = socket(AF_INET, SOCK_DGRAM, 0);
if (that->m_rx_server_sock == -1) {
ESP_LOGE(TAG, "Create UDP socket fail");
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
that->m_tx_server_sock = socket(AF_INET, SOCK_DGRAM, 0);
if (that->m_tx_server_sock < 0) {
ESP_LOGE(TAG, "Unable to create UDP tx socket: errno %d", errno);
close(that->m_rx_server_sock);
that->m_rx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
int reuse = 1;
if (setsockopt(that->m_rx_server_sock, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)) <
0) {
ESP_LOGE(TAG, "Failed to set SO_REUSEADDR. Error %d", errno);
close(that->m_rx_server_sock);
close(that->m_tx_server_sock);
that->m_rx_server_sock = -1;
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
saddr.sin_family = AF_INET;
saddr.sin_port = htons(that->m_rx_port);
saddr.sin_addr.s_addr = htonl(INADDR_ANY);
int ret =
bind(that->m_rx_server_sock, (struct sockaddr *)&saddr, sizeof(struct sockaddr_in));
if (ret < 0) {
ESP_LOGE(TAG, "Failed to bind socket. Error %d", errno);
close(that->m_rx_server_sock);
close(that->m_tx_server_sock);
that->m_rx_server_sock = -1;
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
struct ip_mreq imreq = {};
imreq.imr_multiaddr.s_addr = inet_addr(RECV_MCAST);
imreq.imr_interface.s_addr = htonl(INADDR_ANY);
if (setsockopt(that->m_rx_server_sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &imreq,
sizeof(struct ip_mreq)) < 0) {
ESP_LOGE(TAG, "Failed to set IP_ADD_MEMBERSHIP. Error %d", errno);
close(that->m_rx_server_sock);
close(that->m_tx_server_sock);
that->m_rx_server_sock = -1;
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
uint32_t msg_size;
while (is_network_connected()) {
auto buffer = std::make_unique<std::vector<uint8_t>>();
buffer->resize(MAX_RX_BUFFER_SIZE + 4);
if (int len = recvfrom(that->m_rx_server_sock, buffer->data(), MAX_RX_BUFFER_SIZE, 0,
nullptr, nullptr);
len < 0) {
ESP_LOGE(TAG, "Error occurred during receiving: errno %d", errno);
} else if (len < 4 || len > MAX_RX_BUFFER_SIZE) {
ESP_LOGE(TAG, "Got illegal message size");
} else {
msg_size = *reinterpret_cast<uint32_t *>(buffer->data());
if (msg_size > len - 4) {
ESP_LOGW(TAG, "Message size incorrect");
continue;
}
buffer->erase(buffer->begin(), buffer->begin() + 4); // todo: copying
buffer->resize(msg_size);
that->m_rx_queue->enqueue(std::move(buffer), std::chrono::milliseconds(MAX_RX_QUEUE_ENQUEUE_TIMEOUT_MS));
}
}
ESP_LOGW(TAG, "Network disconnected");
close(that->m_tx_server_sock);
that->m_tx_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
}
}
bool UDPServer::is_network_connected() {
esp_netif_ip_info_t ip_info;
esp_netif_t *netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
esp_netif_ip_info_t ip_info;
esp_netif_t *netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
if (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) {
return true;
}
if (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) {
return true;
}
if (0 != ip_info.ip.addr) {
return true;
}
if (0 != ip_info.ip.addr) {
return true;
}
netif = esp_netif_get_handle_from_ifkey("WIFI_AP_DEF");
netif = esp_netif_get_handle_from_ifkey("WIFI_AP_DEF");
if (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) {
return true;
}
if (netif != nullptr && esp_netif_get_ip_info(netif, &ip_info) == ESP_OK) {
return true;
}
if (0 != ip_info.ip.addr) {
return true;
}
if (0 != ip_info.ip.addr) {
return true;
}
return false;
return false;
}
bool UDPServer::authenticate_client(int sock) {
// todo: authentication (key?)
return 0;
// todo: authentication (key?)
return 0;
}
int UDPServer::send_msg(char *buffer, const uint32_t length) const {
if (!is_network_connected() || m_tx_server_sock == -1) {
return -1;
}
int UDPServer::send_msg(uint8_t *buffer, size_t len) const {
if (!is_network_connected() || m_tx_server_sock == -1) {
return -1;
}
sockaddr_in mcast_dest = {
.sin_family = AF_INET,
.sin_port = htons(m_tx_port),
.sin_addr = {.s_addr = inet_addr(SEND_MCAST)},
};
sockaddr_in mcast_dest = {
.sin_family = AF_INET,
.sin_port = htons(m_tx_port),
.sin_addr = {.s_addr = inet_addr(SEND_MCAST)},
};
uint32_t size = length;
uint32_t size = (uint32_t)len;
iovec iov[2];
iov[0].iov_base = &size;
iov[0].iov_len = 4;
iov[1].iov_base = buffer;
iov[1].iov_len = length;
iovec iov[2];
iov[0].iov_base = &size;
iov[0].iov_len = 4;
iov[1].iov_base = buffer;
iov[1].iov_len = size;
msghdr msg = {};
msg.msg_iov = iov;
msg.msg_iovlen = 2;
msg.msg_name = &mcast_dest;
msg.msg_namelen = sizeof(mcast_dest);
msghdr msg = {};
msg.msg_iov = iov;
msg.msg_iovlen = 2;
msg.msg_name = &mcast_dest;
msg.msg_namelen = sizeof(mcast_dest);
sendmsg(this->m_tx_server_sock, &msg, 0);
sendmsg(this->m_tx_server_sock, &msg, 0);
return 0;
return 0;
}