diff --git a/nimble/host/src/ble_hs.c b/nimble/host/src/ble_hs.c index adb8046a7..34b65f5ff 100644 --- a/nimble/host/src/ble_hs.c +++ b/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) { diff --git a/nimble/host/src/ble_hs_flow.c b/nimble/host/src/ble_hs_flow.c index 35e3cd9ab..b059b46d3 100644 --- a/nimble/host/src/ble_hs_flow.c +++ b/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 } diff --git a/nimble/host/src/ble_hs_flow_priv.h b/nimble/host/src/ble_hs_flow_priv.h index b1aa8c2fc..753eaf8ff 100644 --- a/nimble/host/src/ble_hs_flow_priv.h +++ b/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 diff --git a/nimble/transport/emspi/src/ble_hci_emspi.c b/nimble/transport/emspi/src/ble_hci_emspi.c index 2c1f951dd..2a24cfe2f 100644 --- a/nimble/transport/emspi/src/ble_hci_emspi.c +++ b/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); } /** diff --git a/nimble/transport/uart/src/ble_hci_uart.c b/nimble/transport/uart/src/ble_hci_uart.c index ff0dbec34..c7b7819d0 100644 --- a/nimble/transport/uart/src/ble_hci_uart.c +++ b/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