diff --git a/nimble/host/src/ble_hs.c b/nimble/host/src/ble_hs.c index 1169f1dd3..a532d72e7 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/host/src/ble_l2cap.c b/nimble/host/src/ble_l2cap.c index a9252b8e0..d88eb1830 100644 --- a/nimble/host/src/ble_l2cap.c +++ b/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 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 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; +}