From 0da9cf70eaff120882e7afc6a47c5d2e913fb8f6 Mon Sep 17 00:00:00 2001
From: goldsimon <goldsimon@gmx.de>
Date: Fri, 24 Feb 2017 21:36:49 +0100
Subject: [PATCH] Added pbuf_get_contiguous() to get data in one piece (either
 zero copy from pbuf or memcpied into a supplied buffer)

---
 src/core/pbuf.c         | 41 +++++++++++++++++++++++++++++++++++++++++
 src/include/lwip/pbuf.h |  1 +
 2 files changed, 42 insertions(+)

diff --git a/src/core/pbuf.c b/src/core/pbuf.c
index 3c515b7c..60df50e7 100644
--- a/src/core/pbuf.c
+++ b/src/core/pbuf.c
@@ -1056,6 +1056,47 @@ pbuf_copy_partial(const struct pbuf *buf, void *dataptr, u16_t len, u16_t offset
   return copied_total;
 }
 
+/** Get part of a pbuf's payload as contiguous memory. The returned memory is
+ * either a pointer into the pbuf's payload or, if split over multiple pbufs,
+ * a copy into the user-supplied buffer.
+ *
+ * @param p the pbuf from which to copy data
+ * @param dataptr the application supplied buffer
+ * @param len length of data to copy (dataptr must be big enough). No more
+ * than buf->tot_len will be copied, irrespective of len
+ * @param offset offset into the packet buffer from where to begin copying len bytes
+ * @return the number of bytes copied, or 0 on failure
+ */
+void *
+pbuf_get_contiguous(const struct pbuf *p, void *buffer, size_t bufsize, u16_t len, u16_t offset)
+{
+  const struct pbuf *q;
+
+  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;);
+
+  for (q = p; q != NULL; q = q->next) {
+    if ((offset != 0) && (offset >= q->len)) {
+      /* don't copy from this buffer -> on to the next */
+     offset -= q->len;
+    } else {
+      if (q->len >= (offset + len)) {
+        /* all data in this pbuf, return zero-copy */
+        return (u8_t*)q->payload + offset;
+      }
+      /* need to copy */
+      if (pbuf_copy_partial(q, buffer, len, offset) != len) {
+        /* copying failed: pbuf is too short */
+        return NULL;
+      }
+      return buffer;
+    }
+  }
+  /* pbuf is too short (offset does not fit in) */
+  return NULL;
+}
+
 #if LWIP_TCP && TCP_QUEUE_OOSEQ && LWIP_WND_SCALE
 /**
  * This method modifies a 'pbuf chain', so that its total length is
diff --git a/src/include/lwip/pbuf.h b/src/include/lwip/pbuf.h
index a0004bf8..3ce8449d 100644
--- a/src/include/lwip/pbuf.h
+++ b/src/include/lwip/pbuf.h
@@ -240,6 +240,7 @@ void pbuf_chain(struct pbuf *head, struct pbuf *tail);
 struct pbuf *pbuf_dechain(struct pbuf *p);
 err_t pbuf_copy(struct pbuf *p_to, const struct pbuf *p_from);
 u16_t pbuf_copy_partial(const struct pbuf *p, void *dataptr, u16_t len, u16_t offset);
+void *pbuf_get_contiguous(const struct pbuf *p, void *buffer, size_t bufsize, u16_t len, u16_t offset);
 err_t pbuf_take(struct pbuf *buf, const void *dataptr, u16_t len);
 err_t pbuf_take_at(struct pbuf *buf, const void *dataptr, u16_t len, u16_t offset);
 struct pbuf *pbuf_skip(struct pbuf* in, u16_t in_offset, u16_t* out_offset);