added pbuf_take_at() and pbuf_put_at() (in preparation for dns.c changes)

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)

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

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);