diff --git a/CMakeLists.txt b/CMakeLists.txt index c9240ee..37576f3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,6 @@ set(headers + src/asyncudplistener.h + src/udpsender.h src/espwifistack.h src/espwifistackconfig.h src/espwifistackenums.h @@ -6,6 +8,8 @@ set(headers ) set(sources + src/asyncudplistener.cpp + src/udpsender.cpp src/espwifistack.cpp src/espwifistackenums.cpp src/espwifiutils.cpp diff --git a/Kconfig.projbuild b/Kconfig.projbuild index 8904432..b15dcef 100644 --- a/Kconfig.projbuild +++ b/Kconfig.projbuild @@ -1,4 +1,40 @@ -menu "Simple WiFi Stack settings" +menu "ESP WiFi Stack settings" + +choice LOG_LOCAL_LEVEL_ASYNC_UDP_LISTENER + bool "ASYNC_UDP_LISTENER log verbosity" + default LOG_LOCAL_LEVEL_ASYNC_UDP_LISTENER_INFO + help + Specify how much output to compile into the binary. + You can set lower verbosity level at runtime using + esp_log_level_set function. + + Note that this setting limits which log statements + are compiled into the program. So setting this to, + say, "Warning" would mean that changing log level + to "Debug" at runtime will not be possible. + + config LOG_LOCAL_LEVEL_ASYNC_UDP_LISTENER_NONE + bool "No output" + config LOG_LOCAL_LEVEL_ASYNC_UDP_LISTENER_ERROR + bool "Error" + config LOG_LOCAL_LEVEL_ASYNC_UDP_LISTENER_WARN + bool "Warning" + config LOG_LOCAL_LEVEL_ASYNC_UDP_LISTENER_INFO + bool "Info" + config LOG_LOCAL_LEVEL_ASYNC_UDP_LISTENER_DEBUG + bool "Debug" + config LOG_LOCAL_LEVEL_ASYNC_UDP_LISTENER_VERBOSE + bool "Verbose" +endchoice + +config LOG_LOCAL_LEVEL_ASYNC_UDP_LISTENER + int + default 0 if LOG_LOCAL_LEVEL_ASYNC_UDP_LISTENER_NONE + default 1 if LOG_LOCAL_LEVEL_ASYNC_UDP_LISTENER_ERROR + default 2 if LOG_LOCAL_LEVEL_ASYNC_UDP_LISTENER_WARN + default 3 if LOG_LOCAL_LEVEL_ASYNC_UDP_LISTENER_INFO + default 4 if LOG_LOCAL_LEVEL_ASYNC_UDP_LISTENER_DEBUG + default 5 if LOG_LOCAL_LEVEL_ASYNC_UDP_LISTENER_VERBOSE choice LOG_LOCAL_LEVEL_WIFI_STACK bool "WIFI_STACK log verbosity" diff --git a/src/asyncudplistener.cpp b/src/asyncudplistener.cpp new file mode 100644 index 0000000..0c91bb6 --- /dev/null +++ b/src/asyncudplistener.cpp @@ -0,0 +1,297 @@ +#include "asyncudplistener.h" + +#include "sdkconfig.h" +#define LOG_LOCAL_LEVEL CONFIG_LOG_LOCAL_LEVEL_ASYNC_UDP_LISTENER + +// system includes +#include + +// esp-idf includes +#include +#include +#include + +namespace wifi_stack { +namespace { +constexpr const char * const TAG = "ASYNC_UDP_LISTENER"; + +struct lwip_event_packet_t +{ + udp_pcb *pcb; + pbufUniquePtr pb; + const ip_addr_t *addr; + uint16_t port; + struct netif *netif; +}; + +typedef struct +{ + struct tcpip_api_call_data call; + udp_pcb * pcb; + const ip_addr_t *addr; + uint16_t port; + struct pbuf *pb; + struct netif *netif; + err_t err; +} udp_api_call_t; + +err_t _udp_bind_api(struct tcpip_api_call_data *api_call_msg) +{ + udp_api_call_t *msg = (udp_api_call_t *)api_call_msg; + msg->err = udp_bind(msg->pcb, msg->addr, msg->port); + return msg->err; +} + +err_t _udp_bind(struct udp_pcb *pcb, const ip_addr_t *addr, u16_t port) +{ + udp_api_call_t msg; + msg.pcb = pcb; + msg.addr = addr; + msg.port = port; + tcpip_api_call(_udp_bind_api, (struct tcpip_api_call_data*)&msg); + return msg.err; +} + +err_t _udp_disconnect_api(struct tcpip_api_call_data *api_call_msg) +{ + udp_api_call_t *msg = (udp_api_call_t *)api_call_msg; + msg->err = 0; + udp_disconnect(msg->pcb); + return msg->err; +} + +void _udp_disconnect(struct udp_pcb *pcb) +{ + udp_api_call_t msg; + msg.pcb = pcb; + tcpip_api_call(_udp_disconnect_api, (struct tcpip_api_call_data*)&msg); +} + +void _udp_recv(void *arg, udp_pcb *pcb, pbuf *pb, const ip_addr_t *addr, uint16_t port) +{ + if (!arg) + { + ESP_LOGW(TAG, "called without arg"); + return; + } + + auto *_this = reinterpret_cast(arg); + _this->_udp_task_post(pcb, pb, addr, port, ip_current_input_netif()); +} + +UdpPacketWrapper makeUdpPacketWrapper(pbufUniquePtr &&_pb, const ip_addr_t *raddr, uint16_t rport, struct netif *_ntif) +{ + assert(_pb); + + auto payload = reinterpret_cast(_pb->payload); + + uint16_t _localPort; + uint16_t _remotePort; + + { + const udp_hdr *udphdr = reinterpret_cast(payload - UDP_HLEN); + _localPort = ntohs(udphdr->dest); + _remotePort = ntohs(udphdr->src); + } + + const eth_hdr *ethHdr{}; + //memcpy(&_remoteIp, raddr, sizeof(ip_addr_t)); + ip_addr_t _localAddr { .type = raddr->type }; + ip_addr_t _remoteAddr { .type = raddr->type }; + switch (_remoteAddr.type) + { + case IPADDR_TYPE_V4: + { + ethHdr = reinterpret_cast(payload - UDP_HLEN - IP_HLEN - SIZEOF_ETH_HDR); + + const ip_hdr *iphdr = reinterpret_cast(payload - UDP_HLEN - IP_HLEN); + _localAddr.u_addr.ip4.addr = iphdr->dest.addr; + _remoteAddr.u_addr.ip4.addr = iphdr->src.addr; + + break; + } + case IPADDR_TYPE_V6: + { + ethHdr = reinterpret_cast(payload - UDP_HLEN - IP6_HLEN - SIZEOF_ETH_HDR); + + const ip6_hdr *ip6hdr = reinterpret_cast(payload - UDP_HLEN - IP6_HLEN); + std::copy(std::cbegin(ip6hdr->dest.addr), std::cend(ip6hdr->dest.addr), std::begin(_localAddr.u_addr.ip6.addr)); + std::copy(std::cbegin(ip6hdr->src.addr), std::cend(ip6hdr->src.addr), std::begin(_remoteAddr.u_addr.ip6.addr)); + + break; + } + default: + ESP_LOGW(TAG, "unknown ip type %i", _remoteAddr.type); + } + + std::string_view _data{payload, _pb->len}; + return UdpPacketWrapper{ + ._pb = std::move(_pb), + ._data = _data, + ._ntif = _ntif, + ._local = { .addr = _localAddr, .port = _localPort }, + ._remote = { .addr = _remoteAddr, .port = _remotePort }, + ._remoteMac = ethHdr ? wifi_stack::mac_t{ethHdr->src.addr} : wifi_stack::mac_t{} + }; +} +} // namespace + +tcpip_adapter_if_t UdpPacketWrapper::tcpIpAdapter() const +{ + for (int i = 0; i < TCPIP_ADAPTER_IF_MAX; i++) + { + tcpip_adapter_if_t tcpip_if = tcpip_adapter_if_t(i); + struct netif *nif{}; + if (const auto result = tcpip_adapter_get_netif(tcpip_if, &nif); result != ESP_OK) + { + ESP_LOGW(TAG, "tcpip_adapter_get_netif() failed with %s", esp_err_to_name(result)); + continue; + } + + if (nif && nif == _ntif) + return tcpip_if; + } + + return TCPIP_ADAPTER_IF_MAX; +} + +bool AsyncUdpListener::listen(const ip_addr_t *addr, uint16_t port) +{ + if (!_udp_queue.constructed()) + { + _udp_queue.construct(UBaseType_t{32}, sizeof(lwip_event_packet_t *)); + if (!_udp_queue->handle) + { + _udp_queue.destruct(); + ESP_LOGE(TAG, "xQueueCreate failed"); + return false; + } + } + + if (!_init()) + { + ESP_LOGE(TAG, "failed to init"); + return false; + } + + close(); + + if (addr) + { + IP_SET_TYPE_VAL(_pcb->local_ip, addr->type); + IP_SET_TYPE_VAL(_pcb->remote_ip, addr->type); + } + + if (_udp_bind(_pcb, addr, port) != ERR_OK) + { + ESP_LOGE(TAG, "failed to bind"); + return false; + } + + _connected = true; + + return true; +} + +std::optional AsyncUdpListener::poll(TickType_t xTicksToWait) +{ + if (!_udp_queue.constructed()) + { + ESP_LOGW(TAG, "queue not constructed"); + return std::nullopt; + } + + lwip_event_packet_t *ePtr{}; + if (const auto result = _udp_queue->receive(&ePtr, xTicksToWait); result != pdTRUE) + { + //ESP_LOGE(TAG, "_udp_queue->receive() failed with %i", result); + return std::nullopt; + } + + if (!ePtr) + { + ESP_LOGE(TAG, "invalid ptr from queue received"); + return std::nullopt; + } + + std::unique_ptr e{ePtr}; + if (!e->pb) + { + ESP_LOGE(TAG, "invalid pb"); + return std::nullopt; + } + + // we can only return 1, so no linked lists please + assert(!e->pb->next); + + //udp_pcb *upcb = e->pcb; + const ip_addr_t *addr = e->addr; + uint16_t port = e->port; + struct netif *netif = e->netif; + + return makeUdpPacketWrapper(std::move(e->pb), addr, port, netif); +} + +void AsyncUdpListener::_udp_task_post(udp_pcb *_pcb, pbuf *pb, const ip_addr_t *_addr, uint16_t _port, struct netif *_netif) +{ + if (!_udp_queue.constructed()) + { + ESP_LOGW(TAG, "queue not constructed"); + return; + } + + while (pb) + { + pbufUniquePtr this_pb{pb, pbuf_free}; + pb = pb->next; + this_pb->next = nullptr; + + auto e = std::unique_ptr{new lwip_event_packet_t{ + .pcb = _pcb, + .pb = std::move(this_pb), + .addr = _addr, + .port = _port, + .netif = _netif + }}; + + auto ptr = e.get(); + if (const auto result = _udp_queue->send(&ptr, portMAX_DELAY); result != pdPASS) + { + ESP_LOGE(TAG, "_udp_queue->send failed with %i", result); + continue; + } + + // queue takes ownership + e.release(); + } +} + +bool AsyncUdpListener::_init() +{ + if (_pcb) + return true; + + _pcb = udp_new(); + if (!_pcb) + { + ESP_LOGE(TAG, "udp_new() failed"); + return false; + } + + udp_recv(_pcb, &_udp_recv, (void *)this); + + return true; +} + +void AsyncUdpListener::close() +{ + if (_pcb) + { + if (_connected) + _udp_disconnect(_pcb); + + _connected = false; + } +} + +} // namespace wifi_stack diff --git a/src/asyncudplistener.h b/src/asyncudplistener.h new file mode 100644 index 0000000..3d97ee9 --- /dev/null +++ b/src/asyncudplistener.h @@ -0,0 +1,109 @@ +#pragma once + +// system includes +#include +#include +#include +#include +#include + +// esp-idf includes +#include +#include +#include +#include + +// local includes +#include "cppmacros.h" +#include "delayedconstruction.h" +#include "espwifiutils.h" +#include "wrappers/queue.h" + +namespace wifi_stack { +using pbufUniquePtr = std::unique_ptr; + +struct UdpPacketWrapper +{ + //UdpPacketWrapper(pbufUniquePtr &&pb, const ip_addr_t *addr, uint16_t port, struct netif * netif); + ~UdpPacketWrapper() = default; + + UdpPacketWrapper(UdpPacketWrapper &&other) = default; + UdpPacketWrapper(const UdpPacketWrapper &other) = delete; + + UdpPacketWrapper &operator=(UdpPacketWrapper &&other) = default; + UdpPacketWrapper &operator=(const UdpPacketWrapper &other) = delete; + + auto data() const { return _data; } + + tcpip_adapter_if_t tcpIpAdapter() const; + + struct netif *ntif() const { return _ntif; } + + bool isBroadcast() const { return ip_addr_isbroadcast(&(_local.addr), _ntif); } + bool isMulticast() const { return ip_addr_ismulticast(&(_local.addr)); } + + ip_addr_t localAddr() const { return _local.addr; } + + uint16_t localPort() const { return _local.port; } + + ip_addr_t remoteAddr() const { return _remote.addr; } + + uint16_t remotePort() const { return _remote.port; } + + wifi_stack::mac_t remoteMac() const { return _remoteMac; } + + pbufUniquePtr _pb; + std::string_view _data; + struct netif *_ntif; + struct { + ip_addr_t addr; + uint16_t port; + } _local, _remote; + wifi_stack::mac_t _remoteMac; +}; + +class AsyncUdpListener +{ + CPP_DISABLE_COPY_MOVE(AsyncUdpListener) + +public: + AsyncUdpListener() = default; + + bool listen(const ip_addr_t *addr, uint16_t port); + +// bool listen(const IPAddress addr, uint16_t port) +// { +// ip_addr_t laddr; +// laddr.type = IPADDR_TYPE_V4; +// laddr.u_addr.ip4.addr = addr; +// return listen(&laddr, port); +// } + +// bool listen(const IPv6Address addr, uint16_t port) +// { +// ip_addr_t laddr; +// laddr.type = IPADDR_TYPE_V6; +// memcpy((uint8_t*)(laddr.u_addr.ip6.addr), (const uint8_t*)addr, 16); +// return listen(&laddr, port); +// } + + bool listen(uint16_t port) + { + return listen(IP_ANY_TYPE, port); + } + + std::optional poll(TickType_t xTicksToWait = 0); + + void _udp_task_post(udp_pcb *pcb, pbuf *pb, const ip_addr_t *addr, uint16_t port, struct netif *netif); + +private: + bool _init(); + void close(); + +private: + cpputils::DelayedConstruction _udp_queue; + udp_pcb *_pcb{}; + bool _connected{}; +}; + +} // namespace wifi_stack diff --git a/src/udpsender.cpp b/src/udpsender.cpp new file mode 100644 index 0000000..059cd87 --- /dev/null +++ b/src/udpsender.cpp @@ -0,0 +1,85 @@ +#include "udpsender.h" + +// esp-idf includes +#include +#include +#include +#include + +// 3rdparty lib includes +#include +#include + +// local includes +#include "espwifistack.h" + +namespace wifi_stack { +namespace { +constexpr const char * const TAG = "UDPSENDER"; +} // namespace + +UdpSender::UdpSender() : + m_udp_server{socket(AF_INET, SOCK_DGRAM, 0)} +{ + if (!ready()) + { + ESP_LOGE(TAG, "could not create socket: %d", errno); + return; + } + + fcntl(m_udp_server, F_SETFL, O_NONBLOCK); +} + +UdpSender::~UdpSender() +{ + if (ready()) + close(m_udp_server); +} + +tl::expected UdpSender::send(esp_interface_t interf, uint16_t port, std::string_view buf) +{ + const auto interfPtr = esp_netifs[interf]; + if (!interfPtr) + return tl::make_unexpected(fmt::format("esp_netifs[{}] is invalid", std::to_underlying(interf))); + + return send(interfPtr, port, buf); +} + +tl::expected UdpSender::send(esp_netif_t *interf, uint16_t port, std::string_view buf) +{ + if (!interf) + return tl::make_unexpected("invalid interf"); + + esp_netif_ip_info_t ip; + if (const auto result = esp_netif_get_ip_info(interf, &ip); result != ESP_OK) + return tl::make_unexpected(fmt::format("esp_netif_get_ip_info() failed with {}", esp_err_to_name(result))); + + return send(ip, port, buf); +} + +tl::expected UdpSender::send(const esp_netif_ip_info_t &ip, uint16_t port, std::string_view buf) +{ + struct sockaddr_in recipient; + + recipient.sin_addr.s_addr = wifi_calculate_broadcast(ip_address_t(ip.gw.addr), ip_address_t(ip.netmask.addr)).value(); + + recipient.sin_family = AF_INET; + recipient.sin_port = htons(port); + + return send(recipient, buf); +} + +tl::expected UdpSender::send(const struct sockaddr_in &recipient, std::string_view buf) +{ + if (!ready()) + return tl::make_unexpected("initializing failed, not ready to send"); + + if (const ssize_t sent = sendto(m_udp_server, buf.data(), buf.size(), 0, (const struct sockaddr*)&recipient, sizeof(recipient)); sent < 0) + return tl::make_unexpected(fmt::format("send failed with {} (errno={})", sent, errno)); + else if (sent != buf.size()) + return tl::make_unexpected(fmt::format("sent bytes does not match, expected={}, sent={}", buf.size(), sent)); + + return {}; +} + +} // namespace wifi_stack diff --git a/src/udpsender.h b/src/udpsender.h new file mode 100644 index 0000000..5cbcf46 --- /dev/null +++ b/src/udpsender.h @@ -0,0 +1,35 @@ +#pragma once + +// system includes +#include +#include + +// esp-idf includes +#include +#include +#include +#include + +// 3rdparty lib includes +#include + +namespace wifi_stack { + +class UdpSender +{ +public: + UdpSender(); + ~UdpSender(); + + bool ready() const { return m_udp_server != -1; } + + tl::expected send(esp_interface_t interf, uint16_t port, std::string_view buf); + tl::expected send(esp_netif_t *interf, uint16_t port, std::string_view buf); + tl::expected send(const esp_netif_ip_info_t &ip, uint16_t port, std::string_view buf); + tl::expected send(const struct sockaddr_in &recipient, std::string_view buf); + +private: + const int m_udp_server; +}; + +} // namespace wifi_stack