From ec8ebe6fd29f1a31106c11117a23e79d4680772d Mon Sep 17 00:00:00 2001
From: Will San Filippo <william.sanfilippo@juul.com>
Date: Tue, 5 Nov 2019 11:03:40 -0800
Subject: [PATCH] porting/nimble: os_mbuf_pack_chains

Adding the os_mbuf_pack_chains API from mynewt-core.
---
 porting/nimble/include/os/os_mbuf.h | 19 +++++++
 porting/nimble/src/os_mbuf.c        | 86 +++++++++++++++++++++++++++++
 2 files changed, 105 insertions(+)

diff --git a/porting/nimble/include/os/os_mbuf.h b/porting/nimble/include/os/os_mbuf.h
index b1e234039..f3857fe46 100644
--- a/porting/nimble/include/os/os_mbuf.h
+++ b/porting/nimble/include/os/os_mbuf.h
@@ -614,6 +614,25 @@ struct os_mbuf *os_mbuf_pullup(struct os_mbuf *om, uint16_t len);
  */
 struct os_mbuf *os_mbuf_trim_front(struct os_mbuf *om);
 
+/**
+ * Creates a single chained mbuf from m1 and m2 utilizing all
+ * the available buffer space in all mbufs in the resulting
+ * chain. In other words, ensures there is no leading space in
+ * any mbuf in the resulting chain and trailing space only in
+ * the last mbuf in the chain. Mbufs from either chain may be
+ * freed if not needed. No mbufs are allocated. Note that mbufs
+ * from m2 are added to the end of m1. If m1 has a packet
+ * header, it is retained and length updated. If m2 has a packet
+ * header it is discarded. If m1 is NULL, NULL is returned and
+ * m2 is left untouched.
+ *
+ * @param m1 Pointer to first mbuf chain to pack
+ * @param m2 Pointer to second mbuf chain to pack
+ *
+ * @return struct os_mbuf* Pointer to resulting mbuf chain
+ */
+struct os_mbuf *os_mbuf_pack_chains(struct os_mbuf *m1, struct os_mbuf *m2);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/porting/nimble/src/os_mbuf.c b/porting/nimble/src/os_mbuf.c
index 4ac6bbb7c..92ea935f5 100644
--- a/porting/nimble/src/os_mbuf.c
+++ b/porting/nimble/src/os_mbuf.c
@@ -1037,3 +1037,89 @@ os_mbuf_trim_front(struct os_mbuf *om)
     return om;
 }
 
+struct os_mbuf *
+os_mbuf_pack_chains(struct os_mbuf *m1, struct os_mbuf *m2)
+{
+    uint16_t rem_len;
+    uint16_t copylen;
+    uint8_t *dptr;
+    struct os_mbuf *cur;
+    struct os_mbuf *next;
+
+    /* If m1 is NULL, return NULL */
+    if (m1 == NULL) {
+        return NULL;
+    }
+
+    /*
+     * Concatenate the two chains to start. This will discard packet header in
+     * m2 and adjust packet length in m1 if m1 has a packet header.
+     */
+    if (m2 != NULL) {
+        os_mbuf_concat(m1, m2);
+    }
+
+    cur = m1;
+    while (1) {
+        /* If there is leading space in the mbuf, move data up */
+        if (OS_MBUF_LEADINGSPACE(cur)) {
+            dptr = &cur->om_databuf[0];
+            if (OS_MBUF_IS_PKTHDR(cur)) {
+                dptr += cur->om_pkthdr_len;
+            }
+            memmove(dptr, cur->om_data, cur->om_len);
+            cur->om_data = dptr;
+        }
+
+        /* Set pointer to where we will begin copying data in current mbuf */
+        dptr = cur->om_data + cur->om_len;
+
+        /* Get a pointer to the next buf we want to absorb */
+        next = SLIST_NEXT(cur, om_next);
+
+        /*
+         * Is there trailing space in the mbuf? If so, copy data from
+         * following mbufs into the current mbuf
+         */
+        rem_len = OS_MBUF_TRAILINGSPACE(cur);
+        while (rem_len && next) {
+            copylen = min(rem_len, next->om_len);
+            memcpy(dptr, next->om_data, copylen);
+            cur->om_len += copylen;
+            dptr += copylen;
+            rem_len -= copylen;
+
+            /*
+             * We copied bytes from the next mbuf. Move the data pointer
+             * and subtract from its length
+             */
+            next->om_data += copylen;
+            next->om_len -= copylen;
+
+            /*
+             * Keep removing and freeing consecutive zero length mbufs,
+             * stopping when we find one with data in it or we have
+             * reached the end. This will prevent any zero length mbufs
+             * from remaining in the chain.
+             */
+            while (next->om_len == 0) {
+                SLIST_NEXT(cur, om_next) = SLIST_NEXT(next, om_next);
+                os_mbuf_free(next);
+                next = SLIST_NEXT(cur, om_next);
+                if (next == NULL) {
+                    break;
+                }
+            }
+        }
+
+        /* If no mbufs are left, we are done */
+        if (next == NULL) {
+            break;
+        }
+
+        /* Move cur to next as we filled up current */
+        cur = next;
+    }
+
+    return m1;
+}