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

Fix shutdown functions

Browse Source
This commit is contained in:
Johnathon Slightham
2026-01-06 17:16:50 -05:00
parent 9f34ab14ab
commit 7f717f56ba
3 changed files with 402 additions and 379 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,19 +1,19 @@
#include <memory>
#include "esp_log.h"
#include "sys/param.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_event.h"
#include "esp_netif.h"
#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include "lwip/netdb.h"
#include "wireless/TCPServer.h"
#include "bits/shared_ptr_base.h"
#include "constants/app_comms.h"
#include "constants/tcp.h"
#include "esp_event.h"
#include "esp_log.h"
#include "esp_netif.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "lwip/err.h"
#include "lwip/netdb.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include "sys/param.h"
#include "wireless/TCPServer.h"
#define TAG "TCPServer"
@@ -23,241 +23,263 @@
// - authenticate (don't just return true from the auth function)
// - tx from board
TCPServer::TCPServer(const int port, const std::shared_ptr<PtrQueue<std::vector<uint8_t>>>& rx_queue) {
this->m_port = port;
this->m_mutex = xSemaphoreCreateMutex();
this->m_clients = std::unordered_set<int>();
this->m_task = nullptr;
this->m_rx_task = nullptr;
this->m_rx_queue = rx_queue;
this->m_server_sock = 0;
TCPServer::TCPServer(
const int port,
const std::shared_ptr<PtrQueue<std::vector<uint8_t>>> &rx_queue) {
this->m_port = port;
this->m_mutex = xSemaphoreCreateMutex();
this->m_clients = std::unordered_set<int>();
this->m_task = nullptr;
this->m_rx_task = nullptr;
this->m_rx_queue = rx_queue;
this->m_server_sock = 0;
}
TCPServer::~TCPServer() {
this->shutdown();
vSemaphoreDelete(this->m_mutex);
this->shutdown();
vSemaphoreDelete(this->m_mutex);
}
void TCPServer::startup() {
ESP_LOGI(TAG, "Starting TCP server on port %d", this->m_port);
if (nullptr != this->m_task || nullptr != this->m_rx_task) {
ESP_LOGW(TAG, "Attempted to start TCP server when already started, ignoring start request");
return;
}
ESP_LOGI(TAG, "Starting TCP server on port %d", this->m_port);
if (nullptr != this->m_task || nullptr != this->m_rx_task) {
ESP_LOGW(TAG, "Attempted to start TCP server when already started, "
"ignoring start request");
return;
}
xTaskCreate(tcp_server_task, "tcp_accept_server", 3072, this, 5, &this->m_task);
xTaskCreate(socket_monitor_thread, "tcp_rx", 4096, this, 5, &this->m_rx_task);
xTaskCreate(tcp_server_task, "tcp_accept_server", 3072, this, 5,
&this->m_task);
xTaskCreate(socket_monitor_thread, "tcp_rx", 4096, this, 5, &this->m_rx_task);
}
void TCPServer::shutdown() {
ESP_LOGI(TAG, "Shutting down TCP server");
if (nullptr != this->m_task) {
vTaskDelete(this->m_task);
close(this->m_server_sock);
}
ESP_LOGI(TAG, "Shutting down TCP server");
if (nullptr != this->m_task) {
vTaskDelete(this->m_task);
close(this->m_server_sock);
this->m_task = nullptr;
this->m_server_sock = -1;
}
if (nullptr != this->m_rx_task) {
vTaskDelete(this->m_rx_task);
for (const auto sock : this->m_clients) {
close(sock);
}
if (nullptr != this->m_rx_task) {
vTaskDelete(this->m_rx_task);
for (const auto sock : this->m_clients) {
close(sock);
}
this->m_rx_task = nullptr;
this->m_clients.clear();
}
}
[[noreturn]] void TCPServer::tcp_server_task(void *args) {
constexpr int keepAlive = 1;
constexpr int keepIdle = KEEPALIVE_IDLE;
constexpr int keepInterval = KEEPALIVE_INTERVAL;
constexpr int keepCount = KEEPALIVE_COUNT;
constexpr int keepAlive = 1;
constexpr int keepIdle = KEEPALIVE_IDLE;
constexpr int keepInterval = KEEPALIVE_INTERVAL;
constexpr int keepCount = KEEPALIVE_COUNT;
const auto that = static_cast<TCPServer*>(args);
const auto that = static_cast<TCPServer *>(args);
while (true) {
ESP_LOGI(TAG, "Attempting to start TCP Server on port %d", that->m_port);
while (true) {
ESP_LOGI(TAG, "Attempting to start TCP Server on port %d", that->m_port);
that->m_server_sock = socket(AF_INET, SOCK_STREAM, IPPROTO_IP);
if (that->m_server_sock < 0) {
ESP_LOGE(TAG, "Unable to create TCP socket: errno %d\n", errno);
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
constexpr int opt = 1;
setsockopt(that->m_server_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
ESP_LOGI(TAG, "Socket created");
sockaddr_in server_addr = {
.sin_family = AF_INET,
.sin_port = htons(that->m_port),
.sin_addr = {
.s_addr = htonl(INADDR_ANY),
},
};
int err = bind(that->m_server_sock, reinterpret_cast<struct sockaddr *>(&server_addr), sizeof(server_addr));
if (0 != err) {
ESP_LOGE(TAG, "Socket unable to bind: errno %d\n", errno);
close(that->m_server_sock);
that->m_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
ESP_LOGI(TAG, "Socket bound to port %d\n", that->m_port);
err = listen(that->m_server_sock, TCP_DEFAULT_LISTEN_BACKLOG);
if (0 != err) {
ESP_LOGE(TAG, "Error occurred during TCP listen: errno %d\n", errno);
close(that->m_server_sock);
that->m_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
while (is_network_connected()) {
sockaddr_in client_addr{};
socklen_t addr_len = sizeof(client_addr);
int client_sock = accept(that->m_server_sock, reinterpret_cast<struct sockaddr *>(&client_addr), &addr_len);
if (client_sock < 0) {
ESP_LOGE(TAG, "Unable to accept TCP connection: errno %d\n", errno);
continue;
}
err = that->authenticate_client(client_sock);
if (0 != err) {
ESP_LOGE(TAG, "Client failed authentication\n");
}
setsockopt(client_sock, SOL_SOCKET, SO_KEEPALIVE, &keepAlive, sizeof(int));
setsockopt(client_sock, IPPROTO_TCP, TCP_KEEPIDLE, &keepIdle, sizeof(int));
setsockopt(client_sock, IPPROTO_TCP, TCP_KEEPINTVL, &keepInterval, sizeof(int));
setsockopt(client_sock, IPPROTO_TCP, TCP_KEEPCNT, &keepCount, sizeof(int));
xSemaphoreTake(that->m_mutex, portMAX_DELAY);
that->m_clients.emplace(client_sock);
xSemaphoreGive(that->m_mutex);
}
close(that->m_server_sock);
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
that->m_server_sock = socket(AF_INET, SOCK_STREAM, IPPROTO_IP);
if (that->m_server_sock < 0) {
ESP_LOGE(TAG, "Unable to create TCP socket: errno %d\n", errno);
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
constexpr int opt = 1;
setsockopt(that->m_server_sock, SOL_SOCKET, SO_REUSEADDR, &opt,
sizeof(opt));
ESP_LOGI(TAG, "Socket created");
sockaddr_in server_addr = {
.sin_family = AF_INET,
.sin_port = htons(that->m_port),
.sin_addr =
{
.s_addr = htonl(INADDR_ANY),
},
};
int err = bind(that->m_server_sock,
reinterpret_cast<struct sockaddr *>(&server_addr),
sizeof(server_addr));
if (0 != err) {
ESP_LOGE(TAG, "Socket unable to bind: errno %d\n", errno);
close(that->m_server_sock);
that->m_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
ESP_LOGI(TAG, "Socket bound to port %d\n", that->m_port);
err = listen(that->m_server_sock, TCP_DEFAULT_LISTEN_BACKLOG);
if (0 != err) {
ESP_LOGE(TAG, "Error occurred during TCP listen: errno %d\n", errno);
close(that->m_server_sock);
that->m_server_sock = -1;
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
continue;
}
while (is_network_connected()) {
sockaddr_in client_addr{};
socklen_t addr_len = sizeof(client_addr);
int client_sock =
accept(that->m_server_sock,
reinterpret_cast<struct sockaddr *>(&client_addr), &addr_len);
if (client_sock < 0) {
ESP_LOGE(TAG, "Unable to accept TCP connection: errno %d\n", errno);
continue;
}
err = that->authenticate_client(client_sock);
if (0 != err) {
ESP_LOGE(TAG, "Client failed authentication\n");
}
setsockopt(client_sock, SOL_SOCKET, SO_KEEPALIVE, &keepAlive,
sizeof(int));
setsockopt(client_sock, IPPROTO_TCP, TCP_KEEPIDLE, &keepIdle,
sizeof(int));
setsockopt(client_sock, IPPROTO_TCP, TCP_KEEPINTVL, &keepInterval,
sizeof(int));
setsockopt(client_sock, IPPROTO_TCP, TCP_KEEPCNT, &keepCount,
sizeof(int));
xSemaphoreTake(that->m_mutex, portMAX_DELAY);
that->m_clients.emplace(client_sock);
xSemaphoreGive(that->m_mutex);
}
close(that->m_server_sock);
vTaskDelay(SLEEP_AFTER_FAIL_MS / portTICK_PERIOD_MS);
}
}
[[noreturn]] void TCPServer::socket_monitor_thread(void *args) {
const auto that = static_cast<TCPServer *>(args);
const auto that = static_cast<TCPServer *>(args);
while (true) {
vTaskDelay(0); // Avoid starving other threads
while (true) {
vTaskDelay(0); // Avoid starving other threads
fd_set readfds;
FD_ZERO(&readfds);
int max_fd = -1;
fd_set readfds;
FD_ZERO(&readfds);
int max_fd = -1;
xSemaphoreTake(that->m_mutex, portMAX_DELAY);
if (that->m_clients.size() < 1) {
vTaskDelay(NO_CLIENT_SLEEP_MS / portTICK_PERIOD_MS);
}
for (const auto sock : that->m_clients) {
FD_SET(sock, &readfds);
if (sock > max_fd) max_fd = sock;
}
xSemaphoreGive(that->m_mutex);
timeval timeout = {.tv_sec = 1, .tv_usec = 0}; // 1s timeout
int ret = select(max_fd + 1, &readfds, nullptr, nullptr, &timeout);
vTaskDelay(0); // Avoid starving other threads
if (ret > 0) {
xSemaphoreTake(that->m_mutex, portMAX_DELAY);
std::vector<int> to_remove;
for (int sock : that->m_clients) {
vTaskDelay(0); // Avoid starving other threads
if (FD_ISSET(sock, &readfds)) {
uint32_t msg_size = 0;
if (int len = recv(sock, &msg_size, 4, MSG_WAITALL); len < 0) {
ESP_LOGE(TAG, "Error occurred during receiving msg length: errno %d\n", errno);
to_remove.emplace_back(sock);
continue;
} else if (0 == len) {
ESP_LOGI(TAG, "TCP Connection closed when receiving msg length\n");
close(sock);
to_remove.emplace_back(sock);
continue;
}
if (msg_size < 1 || msg_size > MAX_RX_BUFFER_SIZE) {
continue;
}
auto buffer = std::make_unique<std::vector<uint8_t>>();
buffer->resize(MIN(MAX_RX_BUFFER_SIZE, msg_size));
if (int len = recv(sock, buffer->data(), msg_size, MSG_WAITALL); len < 0) {
ESP_LOGE(TAG, "Error occurred during receiving: errno %d\n", errno);
to_remove.emplace_back(sock);
} else if (0 == len) {
ESP_LOGI(TAG, "TCP Connection closed\n");
close(sock);
to_remove.emplace_back(sock);
} else {
ESP_LOGD(TAG, "TCP Server Received %d bytes\n", len);
buffer->resize(len);
that->m_rx_queue->enqueue(std::move(buffer));
}
}
}
for (const auto r : to_remove) {
that->m_clients.erase(r);
close(r);
}
xSemaphoreGive(that->m_mutex);
}
xSemaphoreTake(that->m_mutex, portMAX_DELAY);
if (that->m_clients.size() < 1) {
vTaskDelay(NO_CLIENT_SLEEP_MS / portTICK_PERIOD_MS);
}
for (const auto sock : that->m_clients) {
FD_SET(sock, &readfds);
if (sock > max_fd)
max_fd = sock;
}
xSemaphoreGive(that->m_mutex);
timeval timeout = {.tv_sec = 1, .tv_usec = 0}; // 1s timeout
int ret = select(max_fd + 1, &readfds, nullptr, nullptr, &timeout);
vTaskDelay(0); // Avoid starving other threads
if (ret > 0) {
xSemaphoreTake(that->m_mutex, portMAX_DELAY);
std::vector<int> to_remove;
for (int sock : that->m_clients) {
vTaskDelay(0); // Avoid starving other threads
if (FD_ISSET(sock, &readfds)) {
uint32_t msg_size = 0;
if (int len = recv(sock, &msg_size, 4, MSG_WAITALL); len < 0) {
ESP_LOGE(TAG,
"Error occurred during receiving msg length: errno %d\n",
errno);
to_remove.emplace_back(sock);
continue;
} else if (0 == len) {
ESP_LOGI(TAG, "TCP Connection closed when receiving msg length\n");
close(sock);
to_remove.emplace_back(sock);
continue;
}
if (msg_size < 1 || msg_size > MAX_RX_BUFFER_SIZE) {
continue;
}
auto buffer = std::make_unique<std::vector<uint8_t>>();
buffer->resize(MIN(MAX_RX_BUFFER_SIZE, msg_size));
if (int len = recv(sock, buffer->data(), msg_size, MSG_WAITALL);
len < 0) {
ESP_LOGE(TAG, "Error occurred during receiving: errno %d\n", errno);
to_remove.emplace_back(sock);
} else if (0 == len) {
ESP_LOGI(TAG, "TCP Connection closed\n");
close(sock);
to_remove.emplace_back(sock);
} else {
ESP_LOGD(TAG, "TCP Server Received %d bytes\n", len);
buffer->resize(len);
that->m_rx_queue->enqueue(std::move(buffer));
}
}
}
for (const auto r : to_remove) {
that->m_clients.erase(r);
close(r);
}
xSemaphoreGive(that->m_mutex);
}
}
}
bool TCPServer::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 TCPServer::authenticate_client(int sock) {
// todo: authentication (key?)
return 0;
// todo: authentication (key?)
return 0;
}
int TCPServer::send_msg(char *buffer, const uint32_t length) const {
if (!is_network_connected()) {
return -1;
}
if (!is_network_connected()) {
return -1;
}
for (const auto client_sock : m_clients) {
send(client_sock, &length, 4, 0);
send(client_sock, buffer, length, 0);
}
for (const auto client_sock : m_clients) {
send(client_sock, &length, 4, 0);
send(client_sock, buffer, length, 0);
}
return 0;
return 0;
}

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

@@ -1,21 +1,20 @@
#include <memory>
#include <cstring>
#include <memory>
#include "esp_log.h"
#include "sys/param.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_event.h"
#include "esp_netif.h"
#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include "lwip/netdb.h"
#include "wireless/UDPServer.h"
#include "bits/shared_ptr_base.h"
#include "constants/app_comms.h"
#include "constants/udp.h"
#include "esp_event.h"
#include "esp_log.h"
#include "esp_netif.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "lwip/err.h"
#include "lwip/netdb.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include "sys/param.h"
#include "wireless/UDPServer.h"
#define TAG "UDPServer"
@@ -23,192 +22,201 @@
// 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<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() {
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);
}
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);
ESP_LOGI(TAG, "Before rx");
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));
}
}
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);
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);
ESP_LOGI(TAG, "Before rx");
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));
}
}
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;
}
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 = length;
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 = length;
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;
}