From 7f717f56ba2f4c08574de84b6c8c6982ef67f25f Mon Sep 17 00:00:00 2001 From: Johnathon Slightham Date: Tue, 6 Jan 2026 17:16:50 -0500 Subject: [PATCH] Fix shutdown functions --- components/rpc/CommunicationRouter.cpp | 11 +- components/rpc/wireless/TCPServer.cpp | 432 +++++++++++++------------ components/rpc/wireless/UDPServer.cpp | 338 +++++++++---------- 3 files changed, 402 insertions(+), 379 deletions(-) diff --git a/components/rpc/CommunicationRouter.cpp b/components/rpc/CommunicationRouter.cpp index eb061d3..e602f74 100644 --- a/components/rpc/CommunicationRouter.cpp +++ b/components/rpc/CommunicationRouter.cpp @@ -34,13 +34,6 @@ CommunicationRouter::~CommunicationRouter() { vTaskDelete(m_router_thread); } const auto that = static_cast(args); while (true) { - - if (std::chrono::system_clock::now() - that->m_last_leader_updated > std::chrono::seconds(15)) { - that->m_last_leader_updated = std::chrono::system_clock::now(); - ESP_LOGI(TAG, "Updating leader"); - that->update_leader(); - } - for (uint8_t channel = 0; channel < MODULE_TO_NUM_CHANNELS_MAP[that->m_config_manager.get_module_type()]; @@ -57,7 +50,7 @@ CommunicationRouter::~CommunicationRouter() { vTaskDelete(m_router_thread); } data.resize(frame_size); that->m_data_link_manager->async_receive(data.data(), frame_size, &frame_header, channel); - that->route(data.data(), frame_size); + that->route(data.data(), frame_size); } vTaskDelay(pdMS_TO_TICKS(50)); @@ -95,7 +88,7 @@ void CommunicationRouter::update_leader() { } else if (this->m_leader == m_module_id) { m_pc_connection->disconnect(); m_lossless_server->shutdown(); - m_lossless_server->shutdown(); + m_lossy_server->shutdown(); } } diff --git a/components/rpc/wireless/TCPServer.cpp b/components/rpc/wireless/TCPServer.cpp index 4a61ffb..6cf3ed5 100644 --- a/components/rpc/wireless/TCPServer.cpp +++ b/components/rpc/wireless/TCPServer.cpp @@ -1,19 +1,19 @@ #include -#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>>& rx_queue) { - this->m_port = port; - this->m_mutex = xSemaphoreCreateMutex(); - this->m_clients = std::unordered_set(); - 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>> &rx_queue) { + this->m_port = port; + this->m_mutex = xSemaphoreCreateMutex(); + this->m_clients = std::unordered_set(); + 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(args); + const auto that = static_cast(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(&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(&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(&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(&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(args); + const auto that = static_cast(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 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>(); - 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 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>(); + 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; } diff --git a/components/rpc/wireless/UDPServer.cpp b/components/rpc/wireless/UDPServer.cpp index 90da67b..a178772 100644 --- a/components/rpc/wireless/UDPServer.cpp +++ b/components/rpc/wireless/UDPServer.cpp @@ -1,21 +1,20 @@ -#include #include +#include -#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>>& 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>> &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(args); + const auto that = static_cast(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>(); - 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(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>(); + 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(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; }