udp: restore correct address/port when parsing packet

libraries/ESP8266WiFi/examples/Udp/Udp.ino

@@ -26,7 +26,7 @@
 unsigned int localPort = 8888;      // local port to listen on
 
 // buffers for receiving and sending data
-char packetBuffer[UDP_TX_PACKET_MAX_SIZE]; //buffer to hold incoming packet,
+char packetBuffer[UDP_TX_PACKET_MAX_SIZE + 1]; //buffer to hold incoming packet,
 char  ReplyBuffer[] = "acknowledged\r\n";       // a string to send back
 
 WiFiUDP Udp;
@@ -49,21 +49,15 @@ void loop() {
   // if there's data available, read a packet
   int packetSize = Udp.parsePacket();
   if (packetSize) {
-    Serial.print("Received packet of size ");
-    Serial.println(packetSize);
-    Serial.print("From ");
-    IPAddress remote = Udp.remoteIP();
-    for (int i = 0; i < 4; i++) {
-      Serial.print(remote[i], DEC);
-      if (i < 3) {
-        Serial.print(".");
-      }
-    }
-    Serial.print(", port ");
-    Serial.println(Udp.remotePort());
+    Serial.printf("Received packet of size %d from %s:%d\n    (to %s:%d, free heap = %d B)\n",
+                  packetSize,
+                  Udp.remoteIP().toString().c_str(), Udp.remotePort(),
+                  Udp.destinationIP().toString().c_str(), Udp.localPort(),
+                  ESP.getFreeHeap());
 
     // read the packet into packetBufffer
-    Udp.read(packetBuffer, UDP_TX_PACKET_MAX_SIZE);
+    int n = Udp.read(packetBuffer, UDP_TX_PACKET_MAX_SIZE);
+    packetBuffer[n] = 0;
     Serial.println("Contents:");
     Serial.println(packetBuffer);
 
@@ -72,7 +66,7 @@ void loop() {
     Udp.write(ReplyBuffer);
     Udp.endPacket();
   }
-  delay(10);
+
 }
 
 /*

libraries/ESP8266WiFi/src/include/UdpContext.h

@@ -32,7 +32,7 @@ void esp_schedule();
 
 #include <AddrList.h>
 
-#define GET_UDP_HDR(pb) (reinterpret_cast<udp_hdr*>(((uint8_t*)((pb)->payload)) - UDP_HLEN))
+#define ALIGNER(x) ((void*)((((intptr_t)(x))+3)&~3))
 
 class UdpContext
 {
@@ -207,21 +207,17 @@ class UdpContext
 
     CONST IPAddress& getRemoteAddress() CONST
     {
-        return _src_addr;
+        return current_addr.srcaddr;
     }
 
     uint16_t getRemotePort() const
     {
-        if (!_rx_buf)
-            return 0;
-
-        udp_hdr* udphdr = GET_UDP_HDR(_rx_buf);
-        return lwip_ntohs(udphdr->src);
+        return current_addr.srcport;
     }
 
     const IPAddress& getDestAddress() const
     {
-        return _dst_addr;
+        return current_addr.dstaddr;
     }
 
     uint16_t getLocalPort() const
@@ -249,6 +245,16 @@ class UdpContext
         if (_rx_buf)
         {
             pbuf_ref(_rx_buf);
+
+            // _rx_buf is an address helper
+            current_addr = *((addrhelper_s*)ALIGNER(_rx_buf->payload));
+
+            // swallow helper pbuf
+            auto head = _rx_buf;
+            _rx_buf = _rx_buf->next;
+            pbuf_free(head);
+
+            pbuf_ref(_rx_buf);
         }
         pbuf_free(head);
         return _rx_buf != 0;
@@ -425,37 +431,64 @@ class UdpContext
         (void) upcb;
         (void) addr;
         (void) port;
+
+        addrhelper_s* helper;
+
+        // chain this helper pbuf first
         if (_rx_buf)
         {
             // there is some unread data
+            // chain pbuf
+
+            // Addresses/ports are stored from this callback because lwIP's
+            // macro are valid only now.
+            //
+            // When peeking data from before payload start (like it was done
+            // before IPv6), there's no easy way to safely guess whether
+            // packet is from v4 or v6.
+            //
+            // Now storing data in an intermediate chained pbuf containing
+            // addrhelper_s
+
+            // allocate new pbuf to store addresses/ports
+            pbuf* pb_helper = pbuf_alloc(PBUF_RAW, sizeof(addrhelper_s) + /*alignment shift*/4, PBUF_RAM);
+            if (!pb_helper)
+            {
+                // memory issue - discard received data
+                pbuf_free(pb);
+                return;
+            }
+
+            helper = (addrhelper_s*)ALIGNER(pb_helper->payload);
+            // construct in place
+            new(&helper->srcaddr) IPAddress();
+            new(&helper->dstaddr) IPAddress();
+            pbuf_cat(_rx_buf, pb_helper);
+
+            // now chain effective data
             // chain the new pbuf to the existing one
             DEBUGV(":urch %d, %d\r\n", _rx_buf->tot_len, pb->tot_len);
             pbuf_cat(_rx_buf, pb);
         }
         else
         {
+            helper = &current_addr;
+
             DEBUGV(":urn %d\r\n", pb->tot_len);
             _first_buf_taken = false;
             _rx_buf = pb;
             _rx_buf_offset = 0;
         }
 
-        // --> Arduino's UDP is a stream but UDP is not <--
-        // When IPv6 is enabled, we store addresses from here
-        // because lwIP's macro are valid only in this callback
-        // (there's no easy way to safely guess whether packet
-        //  is from v4 or v6 when we have only access to payload)
-        // Because of this stream-ed way this is inacurate when
-        // user does not swallow data quickly enough (the former
-        // IPv4-only way suffers from the exact same issue.
-
+        // fill addresses and port
 #if LWIP_VERSION_MAJOR == 1
-        _src_addr = current_iphdr_src;
-        _dst_addr = current_iphdr_dest;
+        helper->srcaddr = current_iphdr_src;
+        helper->dstaddr = current_iphdr_dest;
 #else
-        _src_addr = ip_data.current_iphdr_src;
-        _dst_addr = ip_data.current_iphdr_dest;
+        helper->srcaddr = *ip_current_src_addr();
+        helper->dstaddr = *ip_current_dest_addr();
 #endif
+        helper->srcport = port;
 
         if (_on_rx) {
             _on_rx();
@@ -483,7 +516,11 @@ class UdpContext
 #ifdef LWIP_MAYBE_XCC
     uint16_t _mcast_ttl;
 #endif
-    IPAddress _src_addr, _dst_addr;
+    struct addrhelper_s
+    {
+        IPAddress srcaddr, dstaddr;
+        int16_t srcport;
+    } current_addr;
 };