net: Utility function to compact net_buf fragments

Provide a function that checks if there is any free space
in the individual fragment and remove such slack. If there
are fragments that would become empty, then those fragments
are removed from the fragment list and freed to the fragment
pool.

Change-Id: Ieb1c953a1458622c4552c23ef38e330873cd27c3
Signed-off-by: Jukka Rissanen <jukka.rissanen@linux.intel.com>

include/net/nbuf.h

@@ -331,6 +331,32 @@ static inline struct net_buf *net_nbuf_copy_all(struct net_buf *buf,
 	return net_nbuf_copy(buf, net_buf_frags_len(buf), reserve);
 }
 
+/**
+ * @brief Compact the fragment list.
+ *
+ * @details After this there is no more any free space in individual fragments.
+ *
+ * @param buf Network buffer fragment. This should be the first fragment (data)
+ * in the fragment list.
+ *
+ * @return Pointer to the start of the fragment list if ok, NULL otherwise.
+ */
+struct net_buf *net_nbuf_compact(struct net_buf *buf);
+
+/**
+ * @brief Check if the buffer chain is compact or not.
+ *
+ * @details The compact here means that is there any free space in the
+ * fragments. Only the last fragment can have some free space if the fragment
+ * list is compact.
+ *
+ * @param buf Network buffer.
+ *
+ * @return True if there is no free space in the fragment list,
+ * false otherwise.
+ */
+bool net_nbuf_is_compact(struct net_buf *buf);
+
 #ifdef __cplusplus
 }
 #endif

net/yaip/nbuf.c

@@ -723,6 +723,131 @@ struct net_buf *net_nbuf_copy(struct net_buf *orig, size_t amount,
 	return first;
 }
 
+bool net_nbuf_is_compact(struct net_buf *buf)
+{
+	struct net_buf *last;
+	size_t total = 0, calc;
+	int count = 0;
+
+	last = NULL;
+
+	if (buf->user_data_size) {
+		/* Skip the first element that does not contain any data.
+		 */
+		buf = buf->frags;
+	}
+
+	while (buf) {
+		total += buf->len;
+		count++;
+
+		last = buf;
+		buf = buf->frags;
+	}
+
+	NET_ASSERT(last);
+
+	if (!last) {
+		return false;
+	}
+
+	calc = count * last->size - net_buf_tailroom(last) -
+		count * net_buf_headroom(last);
+
+	if (total == calc) {
+		return true;
+	}
+
+	NET_DBG("Not compacted total %u real %u", total, calc);
+
+	return false;
+}
+
+struct net_buf *net_nbuf_compact(struct net_buf *buf)
+{
+	struct net_buf *first, *prev;
+
+	first = buf;
+
+	if (buf->user_data_size) {
+		NET_DBG("Buffer %p is not a data fragment", buf);
+		buf = buf->frags;
+	}
+
+	prev = NULL;
+
+	NET_DBG("Compacting data to buf %p", first);
+
+	while (buf) {
+		if (buf->frags) {
+			/* Copy amount of data from next fragment to this
+			 * fragment.
+			 */
+			size_t copy_len;
+
+			copy_len = buf->frags->len;
+			if (copy_len > net_buf_tailroom(buf)) {
+				copy_len = net_buf_tailroom(buf);
+			}
+
+			memcpy(net_buf_tail(buf), buf->frags->data, copy_len);
+			net_buf_add(buf, copy_len);
+
+			memmove(buf->frags->data,
+				buf->frags->data + copy_len,
+				buf->frags->len - copy_len);
+
+			buf->frags->len -= copy_len;
+
+			/* Is there any more space in this fragment */
+			if (net_buf_tailroom(buf)) {
+				struct net_buf *frag;
+
+				/* There is. This also means that the next
+				 * fragment is empty as otherwise we could
+				 * not have copied all data.
+				 */
+				frag = buf->frags;
+
+				/* Remove next fragment as there is no
+				 * data in it any more.
+				 */
+				net_buf_frag_del(buf, buf->frags);
+
+				net_nbuf_unref(frag);
+
+				/* Then check next fragment */
+				continue;
+			}
+		} else {
+			if (!buf->len) {
+				/* Remove the last fragment because there is no
+				 * data in it.
+				 */
+				NET_ASSERT_INFO(prev,
+					"First element cannot be deleted!");
+
+				net_buf_frag_del(prev, buf);
+			}
+		}
+
+		prev = buf;
+		buf = buf->frags;
+	}
+
+	/* If the buf exists, then it is the last fragment and can be removed.
+	 */
+	if (buf) {
+		net_nbuf_unref(buf);
+
+		if (prev) {
+			prev->frags = NULL;
+		}
+	}
+
+	return first;
+}
+
 void net_nbuf_init(void)
 {
 	NET_DBG("Allocating %d RX (%d bytes), %d TX (%d bytes) "