diff --git a/main/network/udp.cpp b/main/network/udp.cpp index 571344c71a08a92f440a74cba675e6cb967987af..a84a1d4f3f6185d866b90a26e490fab1bbd46af4 100644 --- a/main/network/udp.cpp +++ b/main/network/udp.cpp @@ -13,32 +13,20 @@ static const char *TAG = "udp-server"; static const in_port_t PORT = 4213; -static std::map<uint32_t, qthing::udpPacketCallback> packetCallbackMap; +static std::map<std::string, 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) { +void qthing::addUDPPacketCallback(std::string magicString, qthing::udpPacketCallback callback) { + if (packetCallbackMap.count(magicString) > 0) { + qthing::udpPacketCallback old_callback = packetCallbackMap.at(magicString); + packetCallbackMap[magicString] = [old_callback, callback](udpPacket packet) { old_callback(packet); callback(packet); }; } else { - packetCallbackMap[magicNumber] = callback; + packetCallbackMap[magicString] = 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)); + ESP_LOGI(TAG, "Registered callback for packets with magic string %s", magicString.c_str()); } static char rx_buffer[2048]; // TODO: get MTU @@ -76,6 +64,7 @@ static void udp_server_task(void *pvParameters) struct sockaddr_in6 sourceAddr; // Large enough for both IPv4 or IPv6 socklen_t socklen = sizeof(sourceAddr); + memset(rx_buffer, 0, sizeof(rx_buffer)); int len = recvfrom(sock, rx_buffer, sizeof(rx_buffer) - 1, 0, (struct sockaddr *)&sourceAddr, &socklen); // Error occured during receiving @@ -85,30 +74,28 @@ static void udp_server_task(void *pvParameters) } // Data received else { - ESP_LOGI(TAG, "Received %d bytes from %s:", len, to_string(sourceAddr).c_str()); - - if (len >= sizeof(uint32_t)) { + ESP_LOGI(TAG, "Received %d bytes from %s", len, to_string(sourceAddr).c_str()); + + std::string payload_str(rx_buffer); + int16_t lenBestMatch = -1; + qthing::udpPacketCallback callback = NULL; + for(auto it = packetCallbackMap.begin(); it != packetCallbackMap.end(); ++it) { + if (len >= it->first.length() && payload_str.find(it->first) == 0) { + lenBestMatch = len; + callback = it->second; + } + } - uint32_t magicNumber = rx_buffer[0] << 24 | - rx_buffer[1] << 16 | - rx_buffer[2] << 8 | - rx_buffer[3]; - + if (lenBestMatch > -1) { qthing::udpPacket packet = { .sourceAddr = sourceAddr, - .payload = rx_buffer + 4, - .length = (uint16_t)(len - 4) + .payload = rx_buffer, + .length = (uint16_t)len }; - 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); - } + callback(packet); } else { - ESP_LOGW(TAG, "Packet is too small, cannot determine magic number"); + ESP_LOGW(TAG, "Packet has unknown magic string"); } } } diff --git a/main/qthing.h b/main/qthing.h index b0a4a4ff04d41b733076cdeef49a68c1e4cff0ce..9562bf326f0183c9aa98f9e8d69f9a6cd21c9e1d 100644 --- a/main/qthing.h +++ b/main/qthing.h @@ -130,7 +130,7 @@ namespace qthing { } udpPacket; typedef std::function<void(udpPacket)> udpPacketCallback; - void addUDPPacketCallback(uint32_t magicNumber, udpPacketCallback callback); + void addUDPPacketCallback(std::string magicString, udpPacketCallback callback); void start_udp_server(); }