Merge branch 'nimble/fix_host_flow_ctrl_v3.3' into 'nimble-1.1.0-idf-v3.3'

NimBLE: Fix host flow control related bugs (v3.3)

See merge request espressif/esp-nimble!69

nimble/host/src/ble_hs.c

@@ -684,7 +684,7 @@ ble_hs_rx_data(struct os_mbuf *om, void *arg)
     /* If flow control is enabled, mark this packet with its corresponding
      * connection handle.
      */
-    ble_hs_flow_fill_acl_usrhdr(om);
+    ble_hs_flow_track_data_mbuf(om);
 
     rc = ble_mqueue_put(&ble_hs_rx_q, ble_hs_evq, om);
     if (rc != 0) {

nimble/host/src/ble_hs_flow.c

@@ -40,6 +40,23 @@ static ble_npl_event_fn ble_hs_flow_event_cb;
 
 static struct ble_npl_event ble_hs_flow_ev;
 
+/* Connection handle associated with each mbuf in ACL pool */
+static uint16_t ble_hs_flow_mbuf_conn_handle[ MYNEWT_VAL(BLE_ACL_BUF_COUNT) ];
+
+static inline int
+ble_hs_flow_mbuf_index(const struct os_mbuf *om)
+{
+    const struct os_mempool *mp = om->om_omp->omp_pool;
+    uintptr_t addr = (uintptr_t)om;
+    int idx;
+
+    idx = (addr - mp->mp_membuf_addr) / mp->mp_block_size;
+
+    BLE_HS_DBG_ASSERT(mp->mp_membuf_addr + idx * mp->mp_block_size == addr);
+
+    return idx;
+}
+
 static int
 ble_hs_flow_tx_num_comp_pkts(void)
 {
@@ -147,18 +164,13 @@ ble_hs_flow_acl_free(struct os_mempool_ext *mpe, void *data, void *arg)
     struct ble_hs_conn *conn;
     const struct os_mbuf *om;
     uint16_t conn_handle;
+    int idx;
     int rc;
 
     om = data;
 
-    /* An ACL data packet must be a single mbuf, and it must contain the
-     * corresponding connection handle in its user header.
-     */
-    assert(OS_MBUF_IS_PKTHDR(om));
-    assert(OS_MBUF_USRHDR_LEN(om) >= sizeof conn_handle);
-
-    /* Copy the connection handle out of the mbuf. */
-    memcpy(&conn_handle, OS_MBUF_USRHDR(om), sizeof conn_handle);
+    idx = ble_hs_flow_mbuf_index(om);
+    conn_handle = ble_hs_flow_mbuf_conn_handle[idx];
 
     /* Free the mbuf back to its pool. */
     rc = os_memblock_put_from_cb(&mpe->mpe_mp, data);
@@ -194,23 +206,19 @@ ble_hs_flow_connection_broken(uint16_t conn_handle)
 }
 
 /**
- * Fills the user header of an incoming data packet.  On function return, the
- * header contains the connection handle associated with the sender.
+ * Associates incoming data packet with a connection handle of the sender.
  *
  * If flow control is disabled, this function is a no-op.
  */
 void
-ble_hs_flow_fill_acl_usrhdr(struct os_mbuf *om)
+ble_hs_flow_track_data_mbuf(struct os_mbuf *om)
 {
 #if MYNEWT_VAL(BLE_HS_FLOW_CTRL)
     const struct hci_data_hdr *hdr;
-    uint16_t *conn_handle;
-
-    BLE_HS_DBG_ASSERT(OS_MBUF_USRHDR_LEN(om) >= sizeof *conn_handle);
-    conn_handle = OS_MBUF_USRHDR(om);
+    int idx = ble_hs_flow_mbuf_index(om);
 
     hdr = (void *)om->om_data;
-    *conn_handle = BLE_HCI_DATA_HANDLE(hdr->hdh_handle_pb_bc);
+    ble_hs_flow_mbuf_conn_handle[idx] = BLE_HCI_DATA_HANDLE(hdr->hdh_handle_pb_bc);
 #endif
 }
 

nimble/host/src/ble_hs_flow_priv.h

@@ -26,7 +26,7 @@ extern "C" {
 #endif
 
 void ble_hs_flow_connection_broken(uint16_t conn_handle);
-void ble_hs_flow_fill_acl_usrhdr(struct os_mbuf *om);
+void ble_hs_flow_track_data_mbuf(struct os_mbuf *om);
 int ble_hs_flow_startup(void);
 
 #ifdef __cplusplus

nimble/host/src/ble_l2cap.c

@@ -188,17 +188,13 @@ ble_l2cap_remove_rx(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan)
 static void
 ble_l2cap_append_rx(struct ble_l2cap_chan *chan, struct os_mbuf *frag)
 {
-    int rc;
-
-    (void)rc;
-
 #if MYNEWT_VAL(BLE_L2CAP_JOIN_RX_FRAGS)
+    struct os_mbuf *m;
+
     /* Copy the data from the incoming fragment into the packet in progress. */
-    rc = os_mbuf_appendfrom(chan->rx_buf, frag, 0, OS_MBUF_PKTLEN(frag));
-    if (rc == 0) {
-        os_mbuf_free_chain(frag);
-        return;
-    }
+    m = os_mbuf_pack_chains(chan->rx_buf, frag);
+    assert(m);
+    return;
 #endif
 
     /* Join disabled or append failed due to mbuf shortage.  Just attach the

nimble/transport/emspi/src/ble_hci_emspi.c

@@ -295,15 +295,7 @@ done:
 static struct os_mbuf *
 ble_hci_trans_acl_buf_alloc(void)
 {
-    uint8_t usrhdr_len;
-
-#if MYNEWT_VAL(BLE_HS_FLOW_CTRL)
-    usrhdr_len = BLE_MBUF_HS_HDR_LEN;
-#else
-    usrhdr_len = 0;
-#endif
-
-    return os_mbuf_get_pkthdr(&ble_hci_emspi_acl_mbuf_pool, usrhdr_len);
+    return os_mbuf_get_pkthdr(&ble_hci_emspi_acl_mbuf_pool, 0);
 }
 
 /**

nimble/transport/uart/src/ble_hci_uart.c

@@ -195,8 +195,6 @@ ble_hci_trans_acl_buf_alloc(void)
 
 #if MYNEWT_VAL(BLE_CONTROLLER)
     usrhdr_len = sizeof(struct ble_mbuf_hdr);
-#elif MYNEWT_VAL(BLE_HS_FLOW_CTRL)
-    usrhdr_len = BLE_MBUF_HS_HDR_LEN;
 #else
     usrhdr_len = 0;
 #endif

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

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;
+}