nimble/hs: Fix flow control

Current implementation of hs flow control uses mbuf user header to
associate mbuf with a connection handle. This unfortunately does not
work well in most cases.

One major problem is that host can reassemble data. This is done by
os_mbuf_concat() which strips packet header from 2nd mbuf and this
means we basically lose connection handle information.

While this problem can be worked around by some extra mbuf counting,
another way to lose connection handle information is to basically pass
received buffer to application. For example, OIC uses user header for
own purposes so it will either overwrite connection handle information
(if our user header is large enough for OIC) or strip our packet header
if it decides to use own mbuf for packet header.

So it's better not to rely on user header to track store connection
handle information. This patch uses a simple private array to store
connection handle for each mbuf - array is indexed by mbuf index in a
mempool. This is a bit of a hack since index of an mbuf in a mempool
has to be calculated using mempool buffer address and block size, but
I do not really see any simple alternative here.

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)
 {
@@ -143,18 +160,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);
@@ -190,23 +202,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/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