Fix/enable UDP packet reassembly

libraries/ESP8266WiFi/src/include/UdpContext.h

@@ -187,7 +187,7 @@ class UdpContext
         if (!_rx_buf)
             return 0;
 
-        return _rx_buf->len - _rx_buf_offset;
+        return _rx_buf->tot_len - _rx_buf_offset;
     }
 
     size_t tell() const
@@ -202,7 +202,7 @@ class UdpContext
     }
 
     bool isValidOffset(const size_t pos) const {
-        return (pos <= _rx_buf->len);
+        return (pos <= _rx_buf->tot_len);
     }
 
     CONST IPAddress& getRemoteAddress() CONST
@@ -238,6 +238,8 @@ class UdpContext
         }
 
         auto deleteme = _rx_buf;
+
+        while(_rx_buf->len != _rx_buf->tot_len) _rx_buf = _rx_buf->next;
         _rx_buf = _rx_buf->next;
 
         if (_rx_buf)
@@ -274,10 +276,10 @@ class UdpContext
 
     int read()
     {
-        if (!_rx_buf || _rx_buf_offset >= _rx_buf->len)
+        if (!_rx_buf || _rx_buf_offset >= _rx_buf->tot_len)
             return -1;
 
-        char c = reinterpret_cast<char*>(_rx_buf->payload)[_rx_buf_offset];
+        char c = pbuf_get_at(_rx_buf, _rx_buf_offset);
         _consume(1);
         return c;
     }
@@ -287,22 +289,26 @@ class UdpContext
         if (!_rx_buf)
             return 0;
 
-        size_t max_size = _rx_buf->len - _rx_buf_offset;
+        size_t max_size = _rx_buf->tot_len - _rx_buf_offset;
         size = (size < max_size) ? size : max_size;
-        DEBUGV(":urd %d, %d, %d\r\n", size, _rx_buf->len, _rx_buf_offset);
+        DEBUGV(":urd %d, %d, %d\r\n", size, _rx_buf->tot_len, _rx_buf_offset);
+
+        void* buf = pbuf_get_contiguous(_rx_buf, dst, size, size, _rx_buf_offset);
+        if(!buf) return 0;
+
+        if(buf != dst) memcpy(dst, buf, size);
 
-        memcpy(dst, reinterpret_cast<char*>(_rx_buf->payload) + _rx_buf_offset, size);
         _consume(size);
 
         return size;
     }
 
     int peek() const
     {
-        if (!_rx_buf || _rx_buf_offset == _rx_buf->len)
+        if (!_rx_buf || _rx_buf_offset == _rx_buf->tot_len)
             return -1;
 
-        return reinterpret_cast<char*>(_rx_buf->payload)[_rx_buf_offset];
+        return pbuf_get_at(_rx_buf, _rx_buf_offset);
     }
 
     void flush()
@@ -311,7 +317,7 @@ class UdpContext
         if (!_rx_buf)
             return;
 
-        _consume(_rx_buf->len - _rx_buf_offset);
+        _consume(_rx_buf->tot_len - _rx_buf_offset);
     }
 
     size_t append(const char* data, size_t size)
@@ -432,8 +438,8 @@ class UdpContext
     void _consume(size_t size)
     {
         _rx_buf_offset += size;
-        if (_rx_buf_offset > _rx_buf->len) {
-            _rx_buf_offset = _rx_buf->len;
+        if (_rx_buf_offset > _rx_buf->tot_len) {
+            _rx_buf_offset = _rx_buf->tot_len;
         }
     }
 
@@ -522,6 +528,90 @@ class UdpContext
         reinterpret_cast<UdpContext*>(arg)->_recv(upcb, p, srcaddr, srcport);
     }
 
+#if LWIP_VERSION_MAJOR == 1
+    /*
+     * Code in this conditional block is copied/backported verbatim from
+     * LwIP 2.1.2 to provide pbuf_get_contiguous.
+     */
+
+    static const struct pbuf *
+    pbuf_skip_const(const struct pbuf *in, u16_t in_offset, u16_t *out_offset)
+    {
+      u16_t offset_left = in_offset;
+      const struct pbuf *q = in;
+
+      /* get the correct pbuf */
+      while ((q != NULL) && (q->len <= offset_left)) {
+        offset_left = (u16_t)(offset_left - q->len);
+        q = q->next;
+      }
+      if (out_offset != NULL) {
+        *out_offset = offset_left;
+      }
+      return q;
+    }
+
+    u16_t
+    pbuf_copy_partial(const struct pbuf *buf, void *dataptr, u16_t len, u16_t offset)
+    {
+      const struct pbuf *p;
+      u16_t left = 0;
+      u16_t buf_copy_len;
+      u16_t copied_total = 0;
+
+      LWIP_ERROR("pbuf_copy_partial: invalid buf", (buf != NULL), return 0;);
+      LWIP_ERROR("pbuf_copy_partial: invalid dataptr", (dataptr != NULL), return 0;);
+
+      /* Note some systems use byte copy if dataptr or one of the pbuf payload pointers are unaligned. */
+      for (p = buf; len != 0 && p != NULL; p = p->next) {
+        if ((offset != 0) && (offset >= p->len)) {
+          /* don't copy from this buffer -> on to the next */
+          offset = (u16_t)(offset - p->len);
+        } else {
+          /* copy from this buffer. maybe only partially. */
+          buf_copy_len = (u16_t)(p->len - offset);
+          if (buf_copy_len > len) {
+            buf_copy_len = len;
+          }
+          /* copy the necessary parts of the buffer */
+          MEMCPY(&((char *)dataptr)[left], &((char *)p->payload)[offset], buf_copy_len);
+          copied_total = (u16_t)(copied_total + buf_copy_len);
+          left = (u16_t)(left + buf_copy_len);
+          len = (u16_t)(len - buf_copy_len);
+          offset = 0;
+        }
+      }
+      return copied_total;
+    }
+
+    void *
+    pbuf_get_contiguous(const struct pbuf *p, void *buffer, size_t bufsize, u16_t len, u16_t offset)
+    {
+      const struct pbuf *q;
+      u16_t out_offset;
+
+      LWIP_ERROR("pbuf_get_contiguous: invalid buf", (p != NULL), return NULL;);
+      LWIP_ERROR("pbuf_get_contiguous: invalid dataptr", (buffer != NULL), return NULL;);
+      LWIP_ERROR("pbuf_get_contiguous: invalid dataptr", (bufsize >= len), return NULL;);
+
+      q = pbuf_skip_const(p, offset, &out_offset);
+      if (q != NULL) {
+        if (q->len >= (out_offset + len)) {
+          /* all data in this pbuf, return zero-copy */
+          return (u8_t *)q->payload + out_offset;
+        }
+        /* need to copy */
+        if (pbuf_copy_partial(q, buffer, len, out_offset) != len) {
+          /* copying failed: pbuf is too short */
+          return NULL;
+        }
+        return buffer;
+      }
+      /* pbuf is too short (offset does not fit in) */
+      return NULL;
+    }
+#endif
+
 private:
     udp_pcb* _pcb;
     pbuf* _rx_buf;