diff --git a/CHANGELOG b/CHANGELOG index 2b9e4687..8fbe3cac 100644 --- a/CHANGELOG +++ b/CHANGELOG @@ -6,6 +6,9 @@ HISTORY ++ New features: + 2014-09-16: Simon Goldschmidt + * pbuf.h/.c: added pbuf_take_at() and pbuf_put_at() + 2014-09-15: Simon Goldschmidt * dns.c: added source port randomization to make the DNS client more robust (see bug #43144) diff --git a/src/core/pbuf.c b/src/core/pbuf.c index 27130455..e5206f70 100644 --- a/src/core/pbuf.c +++ b/src/core/pbuf.c @@ -1002,6 +1002,31 @@ void pbuf_split_64k(struct pbuf *p, struct pbuf **rest) } #endif /* LWIP_TCP && TCP_QUEUE_OOSEQ && LWIP_WND_SCALE */ +/** + * Skip a number of bytes at the start of a pbuf + * + * @param in input pbuf + * @param in_offset offset to skip + * @param out_offset resulting offset in the returned pbuf + * @return the pbuf in the queue where the offset is + */ +static struct pbuf* +pbuf_skip(struct pbuf* in, u16_t in_offset, u16_t* out_offset) +{ + u16_t offset_left = in_offset; + struct pbuf* q = in; + + /* get the correct pbuf */ + while ((q != NULL) && (q->len <= offset_left)) { + offset_left -= q->len; + q = q->next; + } + if (out_offset != NULL) { + *out_offset = offset_left; + } + return q; +} + /** * Copy application supplied data into a pbuf. * This function can only be used to copy the equivalent of buf->tot_len data. @@ -1044,6 +1069,40 @@ pbuf_take(struct pbuf *buf, const void *dataptr, u16_t len) return ERR_OK; } +/** + * Same as pbuf_take() but puts data at an offset + * + * @param buf pbuf to fill with data + * @param dataptr application supplied data buffer + * @param len length of the application supplied data buffer + * + * @return ERR_OK if successful, ERR_MEM if the pbuf is not big enough + */ +err_t +pbuf_take_at(struct pbuf *buf, const void *dataptr, u16_t len, u16_t offset) +{ + u16_t target_offset; + struct pbuf* q = pbuf_skip(buf, offset, &target_offset); + + /* return requested data if pbuf is OK */ + if ((q != NULL) && (q->tot_len >= target_offset + len)) { + u16_t remaining_len = len; + u8_t* src_ptr = (u8_t*)dataptr; + if (target_offset > 0) { + /* copy the part that goes into the first pbuf */ + u16_t first_copy_len = LWIP_MIN(q->len - target_offset, len); + MEMCPY(((u8_t*)q->payload) + target_offset, dataptr, first_copy_len); + remaining_len -= first_copy_len; + src_ptr += first_copy_len; + } + if (remaining_len > 0) { + return pbuf_take(q->next, src_ptr, remaining_len); + } + return ERR_OK; + } + return ERR_MEM; +} + /** * Creates a single pbuf out of a queue of pbufs. * @@ -1126,21 +1185,35 @@ pbuf_fill_chksum(struct pbuf *p, u16_t start_offset, const void *dataptr, u8_t pbuf_get_at(struct pbuf* p, u16_t offset) { - u16_t copy_from = offset; - struct pbuf* q = p; + u16_t q_idx; + struct pbuf* q = pbuf_skip(p, offset, &q_idx); - /* get the correct pbuf */ - while ((q != NULL) && (q->len <= copy_from)) { - copy_from -= q->len; - q = q->next; - } /* return requested data if pbuf is OK */ - if ((q != NULL) && (q->len > copy_from)) { - return ((u8_t*)q->payload)[copy_from]; + if ((q != NULL) && (q->len > q_idx)) { + return ((u8_t*)q->payload)[q_idx]; } return 0; } + /** Put one byte to the specified position in a pbuf + * WARNING: silently ignores offset >= p->tot_len + * + * @param p pbuf to fill + * @param offset offset into p of the byte to write + * @param data byte to write at an offset into p + */ +void +pbuf_put_at(struct pbuf* p, u16_t offset, u8_t data) +{ + u16_t q_idx; + struct pbuf* q = pbuf_skip(p, offset, &q_idx); + + /* write requested data if pbuf is OK */ + if ((q != NULL) && (q->len > q_idx)) { + ((u8_t*)q->payload)[q_idx] = data; + } +} + /** Compare pbuf contents at specified offset with memory s2, both of length n * * @param p pbuf to compare diff --git a/src/include/lwip/pbuf.h b/src/include/lwip/pbuf.h index 058e7013..b99e1c4a 100644 --- a/src/include/lwip/pbuf.h +++ b/src/include/lwip/pbuf.h @@ -167,6 +167,7 @@ struct pbuf *pbuf_dechain(struct pbuf *p); err_t pbuf_copy(struct pbuf *p_to, struct pbuf *p_from); u16_t pbuf_copy_partial(struct pbuf *p, void *dataptr, 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_coalesce(struct pbuf *p, pbuf_layer layer); #if LWIP_CHECKSUM_ON_COPY err_t pbuf_fill_chksum(struct pbuf *p, u16_t start_offset, const void *dataptr, @@ -177,6 +178,7 @@ void pbuf_split_64k(struct pbuf *p, struct pbuf **rest); #endif /* LWIP_TCP && TCP_QUEUE_OOSEQ && LWIP_WND_SCALE */ u8_t pbuf_get_at(struct pbuf* p, u16_t offset); +void pbuf_put_at(struct pbuf* p, u16_t offset, u8_t data); u16_t pbuf_memcmp(struct pbuf* p, u16_t offset, const void* s2, u16_t n); u16_t pbuf_memfind(struct pbuf* p, const void* mem, u16_t mem_len, u16_t start_offset); u16_t pbuf_strstr(struct pbuf* p, const char* substr);