Added AsyncUdpListener and taskutils

CMakeLists.txt

@@ -1,16 +1,23 @@
 set(headers
-    include/lockhelper.h
-    include/lockingqueue.h
-    include/recursivelockhelper.h
-    include/esprandom.h
-    include/wrappers/binary_semaphore.h
-    include/wrappers/counting_semaphore.h
-    include/wrappers/event_group.h
-    include/wrappers/http_client.h
-    include/wrappers/mutex_semaphore.h
-    include/wrappers/queue.h
-    include/wrappers/recursive_mutex_semaphore.h
-    include/wrappers/websocket_client.h
+    src/asyncudplistener.h
+    src/esprandom.h
+    src/lockhelper.h
+    src/lockingqueue.h
+    src/recursivelockhelper.h
+    src/taskutils.h
+    src/wrappers/binary_semaphore.h
+    src/wrappers/counting_semaphore.h
+    src/wrappers/event_group.h
+    src/wrappers/http_client.h
+    src/wrappers/mutex_semaphore.h
+    src/wrappers/queue.h
+    src/wrappers/recursive_mutex_semaphore.h
+    src/wrappers/websocket_client.h
 )
 
-idf_component_register(INCLUDE_DIRS include SRCS ${headers} REQUIRES freertos esp_system esp_http_client esp_websocket_client cpputils)
+set(sources
+    src/asyncudplistener.cpp
+    src/taskutils.cpp
+)
+
+idf_component_register(INCLUDE_DIRS src SRCS ${headers} ${sources} REQUIRES freertos esp_system esp_http_client esp_websocket_client cpputils lwip)

src/asyncudplistener.cpp

@@ -0,0 +1,260 @@
+#include "asyncudplistener.h"
+
+// system includes
+#include <cassert>
+
+// esp-idf includes
+#include <lwip/priv/tcpip_priv.h>
+#include <lwip/prot/ethernet.h>
+#include <esp_log.h>
+
+// local utils
+#include "taskutils.h"
+
+namespace espcpputils {
+namespace {
+typedef struct
+{
+    void *arg;
+    udp_pcb *pcb;
+    pbuf *pb;
+    const ip_addr_t *addr;
+    uint16_t port;
+    struct netif *netif;
+} lwip_event_packet_t;
+
+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)
+{
+    while (pb != NULL)
+    {
+        pbuf *this_pb = pb;
+        pb = pb->next;
+        this_pb->next = NULL;
+
+        if (!AsyncUdpListener::_udp_task_post(arg, pcb, this_pb, addr, port, ip_current_input_netif()))
+            pbuf_free(this_pb);
+    }
+}
+} // namespace
+
+UdpPacketWrapper::UdpPacketWrapper(pbuf *pb, const ip_addr_t *raddr, uint16_t rport, struct netif *ntif)
+{
+    _pb = pb;
+    _if = TCPIP_ADAPTER_IF_MAX;
+    _data = (uint8_t*)(pb->payload);
+    _len = pb->len;
+    _index = 0;
+
+    //memcpy(&_remoteIp, raddr, sizeof(ip_addr_t));
+    _remoteIp.type = raddr->type;
+    _localIp.type = _remoteIp.type;
+
+    eth_hdr* eth = NULL;
+    const udp_hdr* udphdr = reinterpret_cast<const udp_hdr*>(_data - UDP_HLEN);
+    _localPort = ntohs(udphdr->dest);
+    _remotePort = ntohs(udphdr->src);
+
+    if (_remoteIp.type == IPADDR_TYPE_V4)
+    {
+        eth = (eth_hdr *)(((uint8_t *)(pb->payload)) - UDP_HLEN - IP_HLEN - SIZEOF_ETH_HDR);
+        struct ip_hdr * iphdr = (struct ip_hdr *)(((uint8_t *)(pb->payload)) - UDP_HLEN - IP_HLEN);
+        _localIp.u_addr.ip4.addr = iphdr->dest.addr;
+        _remoteIp.u_addr.ip4.addr = iphdr->src.addr;
+    }
+    else
+    {
+        eth = (eth_hdr *)(((uint8_t *)(pb->payload)) - UDP_HLEN - IP6_HLEN - SIZEOF_ETH_HDR);
+        struct ip6_hdr * ip6hdr = (struct ip6_hdr *)(((uint8_t *)(pb->payload)) - UDP_HLEN - IP6_HLEN);
+        std::memcpy(&_localIp.u_addr.ip6.addr, (uint8_t *)ip6hdr->dest.addr, 16);
+        std::memcpy(&_remoteIp.u_addr.ip6.addr, (uint8_t *)ip6hdr->src.addr, 16);
+    }
+    std::memcpy(_remoteMac, eth->src.addr, 6);
+
+    struct netif *netif{NULL};
+    void *nif{NULL};
+    for (int i=0; i<TCPIP_ADAPTER_IF_MAX; i++)
+    {
+        tcpip_adapter_get_netif((tcpip_adapter_if_t)i, &nif);
+        netif = (struct netif *)nif;
+        if (netif && netif == ntif)
+        {
+            _if = (tcpip_adapter_if_t)i;
+            break;
+        }
+    }
+}
+
+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("AsyncUdpListener", "xQueueCreate failed");
+            return false;
+        }
+    }
+
+    if (!_init())
+    {
+        ESP_LOGE("AsyncUdpListener", "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("AsyncUdpListener", "failed to bind");
+        return false;
+    }
+
+    _connected = true;
+
+    return true;
+}
+
+void AsyncUdpListener::poll(TickType_t xTicksToWait)
+{
+    if (!_udp_queue.constructed())
+        return;
+
+    lwip_event_packet_t *e{NULL};
+    while (_udp_queue->receive(&e, 0) == pdTRUE)
+    {
+        if (!e->pb)
+        {
+            free((void*)(e));
+            continue;
+        }
+
+        _recv(e->pcb, e->pb, e->addr, e->port, e->netif);
+
+        free((void*)(e));
+    }
+}
+
+bool AsyncUdpListener::_udp_task_post(void *arg, udp_pcb *pcb, pbuf *pb, const ip_addr_t *addr, uint16_t port, struct netif *netif)
+{
+    if (!arg)
+        return false;
+
+    auto &queue = reinterpret_cast<AsyncUdpListener*>(arg)->_udp_queue;
+    if (!queue.constructed())
+        return false;
+
+    lwip_event_packet_t *e = (lwip_event_packet_t *)malloc(sizeof(lwip_event_packet_t));
+    if (!e)
+        return false;
+
+    e->arg = arg;
+    e->pcb = pcb;
+    e->pb = pb;
+    e->addr = addr;
+    e->port = port;
+    e->netif = netif;
+
+    if (queue->send(&e, portMAX_DELAY) != pdPASS)
+    {
+        free((void*)(e));
+        return false;
+    }
+
+    return true;
+}
+
+bool AsyncUdpListener::_init()
+{
+    if (_pcb)
+        return true;
+
+    _pcb = udp_new();
+    if (!_pcb)
+    {
+        ESP_LOGE("AsyncUdpListener", "udp_new() failed");
+        return false;
+    }
+
+    udp_recv(_pcb, &_udp_recv, (void *)this);
+
+    return true;
+}
+
+void AsyncUdpListener::_recv(udp_pcb *upcb, pbuf *pb, const ip_addr_t *addr, uint16_t port, struct netif *netif)
+{
+    while (pb != NULL)
+    {
+        pbuf *this_pb = pb;
+        pb = pb->next;
+        this_pb->next = NULL;
+
+        if (_handler)
+            _handler(UdpPacketWrapper{this_pb, addr, port, netif});
+
+        pbuf_free(this_pb);
+    }
+}
+
+void AsyncUdpListener::close()
+{
+    if (_pcb)
+    {
+        if (_connected)
+            _udp_disconnect(_pcb);
+
+        _connected = false;
+    }
+}
+
+} // namespace espcpputils

src/asyncudplistener.h

@@ -0,0 +1,136 @@
+#pragma once
+
+// system includes
+#include <functional>
+#include <cstring>
+#include <optional>
+#include <array>
+
+// esp-idf includes
+#include <lwip/ip_addr.h>
+#include <lwip/udp.h>
+#include <lwip/pbuf.h>
+#include <esp_netif.h>
+
+// local includes
+#include "cppmacros.h"
+#include "delayedconstruction.h"
+#include "wrappers/queue.h"
+
+namespace espcpputils {
+class UdpPacketWrapper
+{
+    CPP_DISABLE_COPY_MOVE(UdpPacketWrapper)
+
+public:
+    UdpPacketWrapper(pbuf *pb, const ip_addr_t *addr, uint16_t port, struct netif * netif);
+
+    const uint8_t * data() const { return _data; }
+    size_t length() const { return _len; }
+    bool isBroadcast() const
+    {
+        if (_localIp.type == IPADDR_TYPE_V6)
+            return false;
+        uint32_t ip = _localIp.u_addr.ip4.addr;
+        return ip == 0xFFFFFFFF || ip == 0 || (ip & 0xFF000000) == 0xFF000000;
+    }
+    bool isMulticast() const { return ip_addr_ismulticast(&(_localIp)); }
+    bool isIPv6() const { return _localIp.type == IPADDR_TYPE_V6; }
+
+    tcpip_adapter_if_t interface() const { return _if; }
+
+    std::optional<u32_t> localIP() const
+    {
+        if (_localIp.type != IPADDR_TYPE_V4)
+            return std::nullopt;
+        return _localIp.u_addr.ip4.addr;
+    }
+
+    std::optional<std::array<u32_t, 4>> localIPv6() const
+    {
+        if (_localIp.type != IPADDR_TYPE_V6)
+            return std::nullopt;
+        return *reinterpret_cast<const std::array<u32_t, 4>*>(_localIp.u_addr.ip6.addr);
+    }
+
+    uint16_t localPort() const { return _localPort; }
+
+    std::optional<u32_t> remoteIP() const
+    {
+        if (_remoteIp.type != IPADDR_TYPE_V4)
+            return std::nullopt;
+        return _remoteIp.u_addr.ip4.addr;
+    }
+
+    std::optional<std::array<u32_t, 4>> remoteIPv6() const
+    {
+        if (_remoteIp.type != IPADDR_TYPE_V6)
+            return std::nullopt;
+        return *reinterpret_cast<const std::array<u32_t, 4>*>(_remoteIp.u_addr.ip6.addr);
+    }
+
+    uint16_t remotePort() const { return _remotePort; }
+
+    void remoteMac(uint8_t * mac) const { std::memcpy(mac, _remoteMac, 6); }
+
+private:
+    const pbuf *_pb;
+    tcpip_adapter_if_t _if;
+    ip_addr_t _localIp;
+    uint16_t _localPort;
+    ip_addr_t _remoteIp;
+    uint16_t _remotePort;
+    uint8_t _remoteMac[6];
+    const uint8_t *_data;
+    size_t _len;
+    size_t _index;
+};
+
+class AsyncUdpListener
+{
+    CPP_DISABLE_COPY_MOVE(AsyncUdpListener)
+
+public:
+    AsyncUdpListener() = default;
+
+    void onPacket(std::function<void(const UdpPacketWrapper &packet)> cb) { _handler = cb; }
+
+    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);
+    }
+
+    void poll(TickType_t xTicksToWait = 0);
+
+    static bool _udp_task_post(void *arg, udp_pcb *pcb, pbuf *pb, const ip_addr_t *addr, uint16_t port, struct netif *netif);
+
+private:
+    bool _init();
+    void _recv(udp_pcb *upcb, pbuf *pb, const ip_addr_t *addr, uint16_t port, struct netif *netif);
+    void close();
+
+private:
+    cpputils::DelayedConstruction<espcpputils::queue> _udp_queue;
+    udp_pcb *_pcb{};
+    bool _connected{};
+    std::function<void(const UdpPacketWrapper &packet)> _handler;
+};
+} // namespace espcpputils

src/taskutils.cpp

@@ -0,0 +1,5 @@
+#include "taskutils.h"
+
+namespace espcpputils {
+IMPLEMENT_TYPESAFE_ENUM(CoreAffinity, : uint8_t, CoreAffinityValues)
+} // namespace espcpputils

src/taskutils.h

@@ -0,0 +1,38 @@
+#pragma once
+
+// espressif includes
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+
+// local includes
+#include "cpptypesafeenum.h"
+
+namespace espcpputils {
+#define CoreAffinityValues(x) \
+    x(Core0) \
+    x(Core1) \
+    x(Both)
+DECLARE_TYPESAFE_ENUM(CoreAffinity, : uint8_t, CoreAffinityValues)
+
+namespace {
+BaseType_t createTask(TaskFunction_t pvTaskCode,
+                      const char * const pcName,
+                      const uint32_t usStackDepth,
+                      void * const pvParameters,
+                      UBaseType_t uxPriority,
+                      TaskHandle_t * const pvCreatedTask,
+                      CoreAffinity coreAffinity)
+{
+    switch (coreAffinity)
+    {
+    case CoreAffinity::Core0:
+    case CoreAffinity::Core1:
+        return xTaskCreatePinnedToCore(pvTaskCode, pcName, usStackDepth, pvParameters, uxPriority, pvCreatedTask, int(coreAffinity));
+    case CoreAffinity::Both:
+        return xTaskCreate(pvTaskCode, pcName, usStackDepth, pvParameters, uxPriority, pvCreatedTask);
+    default:
+        __builtin_unreachable();
+    }
+}
+} // namespace
+} // namespace espcpputils