From cd239549c147b36df8e8bcccef579d353ee77eb9 Mon Sep 17 00:00:00 2001 From: Andrzej Kaczmarek Date: Mon, 25 May 2020 11:55:41 +0200 Subject: [PATCH] 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 | 2 +- nimble/host/src/ble_hs_flow.c | 40 +++++++++++++--------- nimble/host/src/ble_hs_flow_priv.h | 2 +- nimble/transport/emspi/src/ble_hci_emspi.c | 10 +----- nimble/transport/uart/src/ble_hci_uart.c | 2 -- 5 files changed, 27 insertions(+), 29 deletions(-) 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