pbuf: Add pbuf_copy_partial_pbuf library function

Like pbuf_copy, but can copy part of a pbuf to an offset in another.
pbuf_copy now uses this function internally.

Replace pbuf_take_at loop in icmp6 with pbuf_copy_partial_pbuf().

src/core/ipv6/icmp6.c

@@ -388,7 +388,6 @@ icmp6_send_response_with_addrs_and_netif(struct pbuf *p, u8_t code, u32_t data,
   struct pbuf *q;
   struct icmp6_hdr *icmp6hdr;
   u16_t datalen = LWIP_MIN(p->tot_len, LWIP_ICMP6_DATASIZE);
-  u16_t offset;
 
   /* ICMPv6 header + datalen (as much of the offending packet as possible) */
   q = pbuf_alloc(PBUF_IP, sizeof(struct icmp6_hdr) + datalen,
@@ -406,16 +405,8 @@ icmp6_send_response_with_addrs_and_netif(struct pbuf *p, u8_t code, u32_t data,
   icmp6hdr->code = code;
   icmp6hdr->data = lwip_htonl(data);
 
-  /* copy fields from original packet (which may be a chain of pbufs) */
+  /* copy fields from original packet */
-  offset = sizeof(struct icmp6_hdr);
+  pbuf_copy_partial_pbuf(q, p, datalen, sizeof(struct icmp6_hdr));
-  while (p && datalen) {
-    u16_t len = LWIP_MIN(datalen, p->len);
-    err_t res = pbuf_take_at(q, p->payload, len, offset);
-    if (res != ERR_OK) break;
-    datalen -= len;
-    offset += len;
-    p = p->next;
-  }
 
   /* calculate checksum */
   icmp6hdr->chksum = 0;

src/core/pbuf.c

@@ -956,14 +956,44 @@ pbuf_dechain(struct pbuf *p)
 err_t
 pbuf_copy(struct pbuf *p_to, const struct pbuf *p_from)
 {
-  size_t offset_to = 0, offset_from = 0, len;
-
   LWIP_DEBUGF(PBUF_DEBUG | LWIP_DBG_TRACE, ("pbuf_copy(%p, %p)\n",
               (const void *)p_to, (const void *)p_from));
 
+  LWIP_ERROR("pbuf_copy: invalid source", p_from != NULL, return ERR_ARG;);
+  return pbuf_copy_partial_pbuf(p_to, p_from, p_from->tot_len, 0);
+}
+
+/**
+ * @ingroup pbuf
+ * Copy part or all of one packet buffer into another, to a specified offset.
+ *
+ * @note Only data in one packet is copied, no packet queue!
+ * @note Argument order is shared with pbuf_copy, but different than pbuf_copy_partial.
+ *
+ * @param p_to pbuf destination of the copy
+ * @param p_from pbuf source of the copy
+ * @param copy_len number of bytes to copy
+ * @param offset offset in destination pbuf where to copy to
+ *
+ * @return ERR_OK if copy_len bytes were copied
+ *         ERR_ARG if one of the pbufs is NULL or p_from is shorter than copy_len
+ *                 or p_to is not big enough to hold copy_len at offset
+ *         ERR_VAL if any of the pbufs are part of a queue
+ */
+err_t
+pbuf_copy_partial_pbuf(struct pbuf *p_to, const struct pbuf *p_from, u16_t copy_len, u16_t offset)
+{
+  size_t offset_to = offset, offset_from = 0, len;
+
+  LWIP_DEBUGF(PBUF_DEBUG | LWIP_DBG_TRACE, ("pbuf_copy_partial_pbuf(%p, %p, %"U16_F", %"U16_F")\n",
+              (const void *)p_to, (const void *)p_from, copy_len, offset));
+
+  /* is the copy_len in range? */
+  LWIP_ERROR("pbuf_copy_partial_pbuf: copy_len bigger than source", ((p_from != NULL) &&
+             (p_from->tot_len >= copy_len)), return ERR_ARG;);
   /* is the target big enough to hold the source? */
-  LWIP_ERROR("pbuf_copy: target not big enough to hold source", ((p_to != NULL) &&
+  LWIP_ERROR("pbuf_copy_partial_pbuf: target not big enough", ((p_to != NULL) &&
-             (p_from != NULL) && (p_to->tot_len >= p_from->tot_len)), return ERR_ARG;);
+             (p_to->tot_len >= (offset + copy_len))), return ERR_ARG;);
 
   /* iterate through pbuf chain */
   do {
@@ -975,35 +1005,38 @@ pbuf_copy(struct pbuf *p_to, const struct pbuf *p_from)
       /* current p_from does not fit into current p_to */
       len = p_to->len - offset_to;
     }
+    len = LWIP_MIN(copy_len, len);
     MEMCPY((u8_t *)p_to->payload + offset_to, (u8_t *)p_from->payload + offset_from, len);
     offset_to += len;
     offset_from += len;
+    copy_len -= len;
     LWIP_ASSERT("offset_to <= p_to->len", offset_to <= p_to->len);
     LWIP_ASSERT("offset_from <= p_from->len", offset_from <= p_from->len);
     if (offset_from >= p_from->len) {
       /* on to next p_from (if any) */
       offset_from = 0;
       p_from = p_from->next;
+      LWIP_ERROR("p_from != NULL", (p_from != NULL) || (copy_len == 0), return ERR_ARG;);
     }
     if (offset_to == p_to->len) {
       /* on to next p_to (if any) */
       offset_to = 0;
       p_to = p_to->next;
-      LWIP_ERROR("p_to != NULL", (p_to != NULL) || (p_from == NULL), return ERR_ARG;);
+      LWIP_ERROR("p_to != NULL", (p_to != NULL) || (copy_len == 0), return ERR_ARG;);
     }
 
     if ((p_from != NULL) && (p_from->len == p_from->tot_len)) {
       /* don't copy more than one packet! */
-      LWIP_ERROR("pbuf_copy() does not allow packet queues!",
+      LWIP_ERROR("pbuf_copy_partial_pbuf() does not allow packet queues!",
                  (p_from->next == NULL), return ERR_VAL;);
     }
     if ((p_to != NULL) && (p_to->len == p_to->tot_len)) {
       /* don't copy more than one packet! */
-      LWIP_ERROR("pbuf_copy() does not allow packet queues!",
+      LWIP_ERROR("pbuf_copy_partial_pbuf() does not allow packet queues!",
                  (p_to->next == NULL), return ERR_VAL;);
     }
-  } while (p_from);
+  } while (copy_len);
-  LWIP_DEBUGF(PBUF_DEBUG | LWIP_DBG_TRACE, ("pbuf_copy: end of chain reached.\n"));
+  LWIP_DEBUGF(PBUF_DEBUG | LWIP_DBG_TRACE, ("pbuf_copy_partial_pbuf: copy complete.\n"));
   return ERR_OK;
 }
 

src/include/lwip/pbuf.h

@@ -296,6 +296,7 @@ void pbuf_cat(struct pbuf *head, struct pbuf *tail);
 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);
+err_t pbuf_copy_partial_pbuf(struct pbuf *p_to, const struct pbuf *p_from, u16_t copy_len, u16_t offset);
 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);

test/unit/core/test_pbuf.c

@@ -146,6 +146,53 @@ START_TEST(test_pbuf_copy_unmatched_chains)
 }
 END_TEST
 
+START_TEST(test_pbuf_copy_partial_pbuf)
+{
+  struct pbuf *a, *b, *dest;
+  char lwip[] = "lwip ";
+  char packet[] = "packet";
+  err_t err;
+  LWIP_UNUSED_ARG(_i);
+
+  a = pbuf_alloc(PBUF_RAW, 5, PBUF_REF);
+  fail_unless(a != NULL);
+  a->payload = lwip;
+  b = pbuf_alloc(PBUF_RAW, 7, PBUF_REF);
+  fail_unless(b != NULL);
+  b->payload = packet;
+  pbuf_cat(a, b);
+  dest = pbuf_alloc(PBUF_RAW, 14, PBUF_RAM);
+  memset(dest->payload, 0, dest->len);
+  fail_unless(dest != NULL);
+
+  /* Don't copy if data will not fit */
+  err = pbuf_copy_partial_pbuf(dest, a, a->tot_len, 4);
+  fail_unless(err == ERR_ARG);
+  /* Don't copy if length is longer than source */
+  err = pbuf_copy_partial_pbuf(dest, a, a->tot_len + 1, 0);
+  fail_unless(err == ERR_ARG);
+  /* Normal copy */
+  err = pbuf_copy_partial_pbuf(dest, a, a->tot_len, 0);
+  fail_unless(err == ERR_OK);
+  fail_unless(strcmp("lwip packet", (char*)dest->payload) == 0);
+  /* Copy at offset */
+  err = pbuf_copy_partial_pbuf(dest, a, a->tot_len, 1);
+  fail_unless(err == ERR_OK);
+  fail_unless(strcmp("llwip packet", (char*)dest->payload) == 0);
+  /* Copy at offset with shorter length */
+  err = pbuf_copy_partial_pbuf(dest, a, 6, 6);
+  fail_unless(err == ERR_OK);
+  fail_unless(strcmp("llwip lwip p", (char*)dest->payload) == 0);
+  /* Copy with shorter length */
+  err = pbuf_copy_partial_pbuf(dest, a, 5, 0);
+  fail_unless(err == ERR_OK);
+  fail_unless(strcmp("lwip  lwip p", (char*)dest->payload) == 0);
+
+  pbuf_free(dest);
+  pbuf_free(a);
+}
+END_TEST
+
 START_TEST(test_pbuf_split_64k_on_small_pbufs)
 {
   struct pbuf *p, *rest=NULL;
@@ -303,6 +350,7 @@ pbuf_suite(void)
     TESTFUNC(test_pbuf_alloc_zero_pbufs),
     TESTFUNC(test_pbuf_copy_zero_pbuf),
     TESTFUNC(test_pbuf_copy_unmatched_chains),
+    TESTFUNC(test_pbuf_copy_partial_pbuf),
     TESTFUNC(test_pbuf_split_64k_on_small_pbufs),
     TESTFUNC(test_pbuf_queueing_bigger_than_64k),
     TESTFUNC(test_pbuf_take_at_edge),