diff --git a/src/core/ipv6/icmp6.c b/src/core/ipv6/icmp6.c index 7c93668..b5c281d 100644 --- a/src/core/ipv6/icmp6.c +++ b/src/core/ipv6/icmp6.c @@ -387,17 +387,18 @@ 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); - /* ICMPv6 header + IPv6 header + data */ - q = pbuf_alloc(PBUF_IP, sizeof(struct icmp6_hdr) + IP6_HLEN + LWIP_ICMP6_DATASIZE, + /* ICMPv6 header + datalen (as much of the offending packet as possible) */ + q = pbuf_alloc(PBUF_IP, sizeof(struct icmp6_hdr) + datalen, PBUF_RAM); if (q == NULL) { LWIP_DEBUGF(ICMP_DEBUG, ("icmp_time_exceeded: failed to allocate pbuf for ICMPv6 packet.\n")); ICMP6_STATS_INC(icmp6.memerr); return; } - LWIP_ASSERT("check that first pbuf can hold icmp 6message", - (q->len >= (sizeof(struct icmp6_hdr) + IP6_HLEN + LWIP_ICMP6_DATASIZE))); + LWIP_ASSERT("check that first pbuf can hold icmp6 header", + (q->len >= (sizeof(struct icmp6_hdr)))); icmp6hdr = (struct icmp6_hdr *)q->payload; icmp6hdr->type = type; @@ -405,8 +406,7 @@ icmp6_send_response_with_addrs_and_netif(struct pbuf *p, u8_t code, u32_t data, icmp6hdr->data = lwip_htonl(data); /* copy fields from original packet */ - SMEMCPY((u8_t *)q->payload + sizeof(struct icmp6_hdr), (u8_t *)p->payload, - IP6_HLEN + LWIP_ICMP6_DATASIZE); + 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 7579d86..dd371f9 100644 --- a/src/core/pbuf.c +++ b/src/core/pbuf.c @@ -1011,6 +1011,83 @@ pbuf_copy(struct pbuf *p_to, const struct pbuf *p_from) return ERR_OK; } +/** + * @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_partial_pbuf: target not big enough", ((p_to != NULL) && + (p_to->tot_len >= (offset + copy_len))), return ERR_ARG;); + + /* iterate through pbuf chain */ + do { + /* copy one part of the original chain */ + if ((p_to->len - offset_to) >= (p_from->len - offset_from)) { + /* complete current p_from fits into current p_to */ + len = p_from->len - offset_from; + } else { + /* 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) || (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_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_partial_pbuf() does not allow packet queues!", + (p_to->next == NULL), return ERR_VAL;); + } + } while (copy_len); + LWIP_DEBUGF(PBUF_DEBUG | LWIP_DBG_TRACE, ("pbuf_copy_partial_pbuf: copy complete.\n")); + return ERR_OK; +} + /** * @ingroup pbuf * Copy (part of) the contents of a packet buffer diff --git a/src/include/lwip/pbuf.h b/src/include/lwip/pbuf.h index 328c772..6e52cf5 100644 --- a/src/include/lwip/pbuf.h +++ b/src/include/lwip/pbuf.h @@ -55,7 +55,7 @@ extern "C" { #define LWIP_SUPPORT_CUSTOM_PBUF ((IP_FRAG && !LWIP_NETIF_TX_SINGLE_PBUF) || (LWIP_IPV6 && LWIP_IPV6_FRAG)) #endif -/** @ingroup pbuf +/** @ingroup pbuf * PBUF_NEEDS_COPY(p): return a boolean value indicating whether the given * pbuf needs to be copied in order to be kept around beyond the current call * stack without risking being corrupted. The default setting provides safety: @@ -293,6 +293,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/src/netif/zepif.c b/src/netif/zepif.c index b7c9b9c..2898103 100644 --- a/src/netif/zepif.c +++ b/src/netif/zepif.c @@ -201,7 +201,7 @@ zepif_linkoutput(struct netif *netif, struct pbuf *p) state->seqno++; zep->len = (u8_t)p->tot_len; - err = pbuf_take_at(q, p->payload, p->tot_len, sizeof(struct zep_hdr)); + err = pbuf_copy_partial_pbuf(q, p, p->tot_len, sizeof(struct zep_hdr)); if (err == ERR_OK) { #if ZEPIF_LOOPBACK zepif_udp_recv(netif, state->pcb, pbuf_clone(PBUF_RAW, PBUF_RAM, q), NULL, 0);