Adding callbacks to UDP server

main/network/udp.cpp

 #include <qthing.h>
 
+#include <map>
+
 #include "esp_log.h"
 
 #include "lwip/err.h"
@@ -11,9 +13,38 @@ static const char *TAG = "udp-server";
 
 static const in_port_t PORT = 4213;
 
+static std::map<uint32_t, qthing::udpPacketCallback> packetCallbackMap;
+
+void qthing::addUDPPacketCallback(uint32_t magicNumber, qthing::udpPacketCallback callback) {
+    if (packetCallbackMap.count(magicNumber) > 0) {
+        qthing::udpPacketCallback old_callback = packetCallbackMap.at(magicNumber);
+        packetCallbackMap[magicNumber] = [old_callback, callback](udpPacket packet) {
+            old_callback(packet);
+            callback(packet);
+        };
+    } else {
+        packetCallbackMap[magicNumber] = callback;
+    }
+
+    ESP_LOGI(TAG, "Registered callback for packets with magic number 0x%08X", magicNumber);
+}
+
+std::string to_string(struct sockaddr_in6 sourceAddr) {
+    char addr_str[128];
+
+    if (sourceAddr.sin6_family == PF_INET) {
+        inet_ntoa_r(((struct sockaddr_in *)&sourceAddr)->sin_addr.s_addr, addr_str, sizeof(addr_str) - 1);
+    } else if (sourceAddr.sin6_family == PF_INET6) {
+        inet6_ntoa_r(sourceAddr.sin6_addr, addr_str, sizeof(addr_str) - 1);
+    }
+
+    return(std::string(addr_str));
+}
+
+static char rx_buffer[2048];  // TODO: get MTU
+
 static void udp_server_task(void *pvParameters)
 {
-    char rx_buffer[128];
     char addr_str[128];
     int addr_family;
     int ip_protocol;
@@ -39,10 +70,9 @@ static void udp_server_task(void *pvParameters)
             ESP_LOGE(TAG, "Socket unable to bind: errno %d", errno);
         }
         ESP_LOGI(TAG, "Socket bound");
+        ESP_LOGI(TAG, "Waiting for data");
 
         while (true) {
-
-            ESP_LOGI(TAG, "Waiting for data");
             struct sockaddr_in6 sourceAddr; // Large enough for both IPv4 or IPv6
             socklen_t socklen = sizeof(sourceAddr);
 
@@ -55,21 +85,30 @@ static void udp_server_task(void *pvParameters)
             }
             // Data received
             else {
-                // Get the sender's ip address as string
-                if (sourceAddr.sin6_family == PF_INET) {
-                    inet_ntoa_r(((struct sockaddr_in *)&sourceAddr)->sin_addr.s_addr, addr_str, sizeof(addr_str) - 1);
-                } else if (sourceAddr.sin6_family == PF_INET6) {
-                    inet6_ntoa_r(sourceAddr.sin6_addr, addr_str, sizeof(addr_str) - 1);
-                }
-
-                rx_buffer[len] = 0; // Null-terminate whatever we received and treat like a string...
-                ESP_LOGI(TAG, "Received %d bytes from %s:", len, addr_str);
-                ESP_LOGI(TAG, "%s", rx_buffer);
-
-                int err = sendto(sock, rx_buffer, len, 0, (struct sockaddr *)&sourceAddr, sizeof(sourceAddr));
-                if (err < 0) {
-                    ESP_LOGE(TAG, "Error occured during sending: errno %d", errno);
-                    break;
+                ESP_LOGI(TAG, "Received %d bytes from %s:", len, to_string(sourceAddr).c_str());
+
+                if (len >= sizeof(uint32_t)) {
+
+                    uint32_t magicNumber = rx_buffer[0] << 24 |
+                                           rx_buffer[1] << 16 |
+                                           rx_buffer[2] << 8  |
+                                           rx_buffer[3];
+                    
+                    qthing::udpPacket packet = {
+                        .sourceAddr = sourceAddr,
+                        .payload = rx_buffer + 4,
+                        .length = (uint16_t)(len - 4)
+                    };
+
+                    auto iterator = packetCallbackMap.find(magicNumber);
+                    if (iterator != packetCallbackMap.end()) {
+                        auto callback = iterator->second;
+                        callback(packet);
+                    } else {
+                        ESP_LOGW(TAG, "Packet has unknown magic number 0x%08X", magicNumber);
+                    }
+                } else {
+                    ESP_LOGW(TAG, "Packet is too small, cannot determine magic number");
                 }
             }
         }
@@ -85,4 +124,4 @@ static void udp_server_task(void *pvParameters)
 
 void start_udp_server() {
     xTaskCreate(udp_server_task, "udp_server", 4096, NULL, 5, NULL);
-}
\ No newline at end of file
+}