diff --git a/src/core/ipv6/icmp6.c b/src/core/ipv6/icmp6.c index 4fd10216..ed0bd7b7 100644 --- a/src/core/ipv6/icmp6.c +++ b/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) */ - offset = 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; - } + /* copy fields from original packet */ + pbuf_copy_partial_pbuf(q, p, datalen, sizeof(struct icmp6_hdr)); /* calculate checksum */ icmp6hdr->chksum = 0; diff --git a/src/core/pbuf.c b/src/core/pbuf.c index 42c875a1..40378422 100644 --- a/src/core/pbuf.c +++ b/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) && - (p_from != NULL) && (p_to->tot_len >= p_from->tot_len)), return ERR_ARG;); + LWIP_ERROR("pbuf_copy_partial_pbuf: target not big enough", ((p_to != NULL) && + (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); - LWIP_DEBUGF(PBUF_DEBUG | LWIP_DBG_TRACE, ("pbuf_copy: end of chain reached.\n")); + } while (copy_len); + LWIP_DEBUGF(PBUF_DEBUG | LWIP_DBG_TRACE, ("pbuf_copy_partial_pbuf: copy complete.\n")); return ERR_OK; } diff --git a/src/include/lwip/pbuf.h b/src/include/lwip/pbuf.h index 84c7f95d..5a4fc88b 100644 --- a/src/include/lwip/pbuf.h +++ b/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); diff --git a/test/unit/core/test_pbuf.c b/test/unit/core/test_pbuf.c index be2491c4..1ceea08f 100644 --- a/test/unit/core/test_pbuf.c +++ b/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),