net: Add nbuf APIs to read/write across multiple fragments

Helper functions to read and write data across multiple fragments.

Write function always adds at the end of last fragment. If there is
no space in last fragment new data fragment will be created and added
to input buffer.

Read function start reading from offset of input fragment. If the
required data is located in mutliple fragments it reads from list
of fragments and returns position and fragment where read stopped.

Skip function start skipping from offset of input fragment. If the
required length is across multiple fragments it skip till length and
returns position and fragment where read stopped. This is useful
when reading with unwanted data (reserved or unhandled data).

These functions are quite useful when handling with multiple fragments.

Change-Id: I348b869108724602ae780a1cba4fe17d3af7ffc2
Signed-off-by: Ravi kumar Veeramally <ravikumar.veeramally@linux.intel.com>

include/net/yaip/nbuf.h

@@ -565,6 +565,69 @@ struct net_buf *net_nbuf_push(struct net_buf *parent, struct net_buf *buf,
  */
 struct net_buf *net_nbuf_pull(struct net_buf *buf, size_t amount);
 
+/**
+ * @brief Add data to last fragment in fragment list
+ *
+ * @details Add data to last fragment. If there is no enough space in last
+ * fragment then new data fragment will be created and will be added to
+ * fragment list. Caller has to take care of endianness if needed.
+ *
+ * @param buf Network buffer fragment list.
+ * @param len Total length of input data
+ * @param data Data to be added
+ *
+ * @return True if all the data is placed at end of fragment list,
+ * False otherwie (In-case of false buf might contain input data in the
+ * process of placing into fragments).
+ */
+bool net_nbuf_write(struct net_buf *buf, uint16_t len, uint8_t *data);
+
+/**
+ * @brief Get data from buffer
+ *
+ * @details Get N number of bytes starting from fragment's offset. If the total
+ * length of data is placed in multiple framgents, this function will read from
+ * all fragments until it reaches N number of bytes. Caller has to take care of
+ * endianness if needed.
+ *
+ * @param buf Network buffer fragment.
+ * @param offset Offset of input buffer.
+ * @param pos Pointer to position of offset after reading n number of bytes,
+ *            this is with respect to return buffer(fragment).
+ * @param len Total length of data to be read.
+ * @param data Data will be copied here.
+ *
+ * @return Pointer to fragment after successful read,
+ *         NULL otherwise (if pos is 0, NULL is not a failure case).
+ */
+struct net_buf *net_nbuf_read(struct net_buf *buf, uint16_t offset,
+			      uint16_t *pos, uint16_t len, uint8_t *data);
+
+/**
+ * @brief Skip N number of bytes while reading buffer
+ *
+ * @details Skip N number of bytes starting from fragment's offset. If the total
+ * length of data is placed in multiple framgents, this function will skip from
+ * all fragments until it reaches N number of bytes. This function is useful
+ * when unwanted data (e.g. reserved or not supported data in message) is part
+ * of fragment and want to skip it.
+ *
+ * @param buf Network buffer fragment.
+ * @param offset Offset of input buffer.
+ * @param pos Pointer to position of offset after reading n number of bytes,
+ *            this is with respect to return buffer(fragment).
+ * @param len Total length of data to be read.
+ *
+ * @return Pointer to fragment after successful skip,
+ *         NULL otherwise (if pos is 0, NULL is not a failure case).
+ */
+static inline struct net_buf *net_nbuf_skip(struct net_buf *buf,
+					    uint16_t offset,
+					    uint16_t *pos, uint16_t len)
+{
+	return net_nbuf_read(buf, offset, pos, len, NULL);
+}
+
 #if defined(CONFIG_NET_DEBUG_NET_BUF)
 /**
  * @brief Debug helper to print out the buffer allocations

net/yaip/nbuf.c

@@ -923,6 +923,100 @@ struct net_buf *net_nbuf_pull(struct net_buf *buf, size_t amount)
 	return first;
 }
 
+/* This helper routine will put single byte, if there is no place for
+ * the byte in current fragment then create new fragment and add it to
+ * the buffer. Every next byte retrieve last fragment and start placing it.
+ */
+static inline bool net_nbuf_write_byte(struct net_buf *buf, uint8_t value)
+{
+	struct net_buf *frag;
+
+	frag = net_buf_frag_last(buf->frags);
+	if (!frag) {
+		return false;
+	}
+
+	if (net_buf_tailroom(frag)) {
+		frag->data[frag->len] = value;
+		net_buf_add(frag, 1);
+
+		return true;
+	}
+
+	frag = net_nbuf_get_reserve_data(net_nbuf_ll_reserve(buf));
+	if (!frag) {
+		return false;
+	}
+
+	net_buf_frag_add(buf, frag);
+
+	frag->data[0] = value;
+
+	net_buf_add(frag, 1);
+
+	return true;
+}
+
+bool net_nbuf_write(struct net_buf *buf, uint16_t len, uint8_t *data)
+{
+	uint16_t offset = 0;
+
+	NET_ASSERT(buf && data);
+
+	while (len-- > 0) {
+		if (!net_nbuf_write_byte(buf, *(data + offset++))) {
+
+			return false;
+		}
+	}
+
+	return true;
+}
+
+/* Helper routine to retrieve single byte from fragment and move
+ * offset. If required byte is last byte in framgent then return
+ * next fragment and set offset = 0.
+ */
+static inline struct net_buf *net_nbuf_read_byte(struct net_buf *buf,
+						 uint16_t offset,
+						 uint16_t *pos,
+						 uint8_t *data)
+{
+	if (data) {
+		*data = buf->data[offset];
+	}
+
+	*pos = offset + 1;
+
+	if (*pos >= buf->len) {
+		*pos = 0;
+
+		return buf->frags;
+	}
+
+	return buf;
+}
+
+struct net_buf *net_nbuf_read(struct net_buf *buf, uint16_t offset,
+			      uint16_t *pos, uint16_t len, uint8_t *data)
+{
+	uint16_t copy = 0;
+
+	NET_ASSERT(buf);
+
+	*pos = offset;
+
+	while (len-- > 0 && buf) {
+		if (data) {
+			buf = net_nbuf_read_byte(buf, *pos, pos, data + copy++);
+		} else {
+			buf = net_nbuf_read_byte(buf, *pos, pos, NULL);
+		}
+	}
+
+	return buf;
+}
+
 #if NET_DEBUG
 void net_nbuf_print(void)
 {