diff --git a/nimble/controller/include/controller/ble_ll.h b/nimble/controller/include/controller/ble_ll.h index 12b978653..3a028369a 100644 --- a/nimble/controller/include/controller/ble_ll.h +++ b/nimble/controller/include/controller/ble_ll.h @@ -437,6 +437,9 @@ int ble_ll_rx_start(uint8_t *rxbuf, uint8_t chan, struct ble_mbuf_hdr *hdr); /* Called by the PHY when a packet reception ends */ int ble_ll_rx_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr); +/* Helper callback to tx mbuf using ble_phy_tx() */ +uint8_t ble_ll_tx_mbuf_pducb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte); + /*--- Controller API ---*/ void ble_ll_mbuf_init(struct os_mbuf *m, uint8_t pdulen, uint8_t hdr); diff --git a/nimble/controller/include/controller/ble_phy.h b/nimble/controller/include/controller/ble_phy.h index 3887a2a3e..c7d1d61fd 100644 --- a/nimble/controller/include/controller/ble_phy.h +++ b/nimble/controller/include/controller/ble_phy.h @@ -106,8 +106,11 @@ int ble_phy_rx_set_start_time(uint32_t cputime, uint8_t rem_usecs); /* Set the transmit end callback and argument */ void ble_phy_set_txend_cb(ble_phy_tx_end_func txend_cb, void *arg); +typedef uint8_t (*ble_phy_tx_pducb_t)(uint8_t *dptr, void *pducb_arg, + uint8_t *hdr_byte); + /* Place the PHY into transmit mode */ -int ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans); +int ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans); /* Place the PHY into receive mode */ int ble_phy_rx(void); diff --git a/nimble/controller/src/ble_ll.c b/nimble/controller/src/ble_ll.c index 3674a19b1..649a59cc8 100644 --- a/nimble/controller/src/ble_ll.c +++ b/nimble/controller/src/ble_ll.c @@ -1013,6 +1013,24 @@ ble_ll_rx_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr) return rc; } +uint8_t +ble_ll_tx_mbuf_pducb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) +{ + struct os_mbuf *txpdu; + struct ble_mbuf_hdr *ble_hdr; + + txpdu = pducb_arg; + assert(txpdu); + ble_hdr = BLE_MBUF_HDR_PTR(txpdu); + + os_mbuf_copydata(txpdu, ble_hdr->txinfo.offset, ble_hdr->txinfo.pyld_len, + dptr); + + *hdr_byte = ble_hdr->txinfo.hdr_byte; + + return ble_hdr->txinfo.pyld_len; +} + static void ble_ll_event_rx_pkt(struct os_event *ev) { diff --git a/nimble/controller/src/ble_ll_adv.c b/nimble/controller/src/ble_ll_adv.c index 8e175c97c..3a62dc505 100644 --- a/nimble/controller/src/ble_ll_adv.c +++ b/nimble/controller/src/ble_ll_adv.c @@ -276,14 +276,16 @@ ble_ll_adv_final_chan(struct ble_ll_adv_sm *advsm) * * @param advsm Pointer to advertisement state machine */ -static void -ble_ll_adv_legacy_pdu_make(struct ble_ll_adv_sm *advsm, struct os_mbuf *m) +static uint8_t +ble_ll_adv_legacy_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) { + struct ble_ll_adv_sm *advsm; uint8_t adv_data_len; - uint8_t *dptr; uint8_t pdulen; uint8_t pdu_type; + advsm = pducb_arg; + /* assume this is not a direct ind */ adv_data_len = ADV_DATA_LEN(advsm); pdulen = BLE_DEV_ADDR_LEN + adv_data_len; @@ -324,11 +326,9 @@ ble_ll_adv_legacy_pdu_make(struct ble_ll_adv_sm *advsm, struct os_mbuf *m) pdu_type |= BLE_ADV_PDU_HDR_TXADD_RAND; } - /* Get the advertising PDU and initialize it*/ - ble_ll_mbuf_init(m, pdulen, pdu_type); + *hdr_byte = pdu_type; /* Construct advertisement */ - dptr = m->om_data; memcpy(dptr, advsm->adva, BLE_DEV_ADDR_LEN); dptr += BLE_DEV_ADDR_LEN; @@ -341,6 +341,8 @@ ble_ll_adv_legacy_pdu_make(struct ble_ll_adv_sm *advsm, struct os_mbuf *m) if (adv_data_len != 0) { os_mbuf_copydata(advsm->adv_data, 0, adv_data_len, dptr); } + + return pdulen; } #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) @@ -395,10 +397,10 @@ ble_ll_adv_aux_scan_rsp_payload_len(struct ble_ll_adv_sm *advsm) /** * Create the advertising PDU */ -static void -ble_ll_adv_pdu_make(struct ble_ll_adv_sm *advsm, struct os_mbuf *m) +static uint8_t +ble_ll_adv_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) { - uint8_t *dptr; + struct ble_ll_adv_sm *advsm; uint8_t pdulen; uint8_t pdu_type; @@ -413,10 +415,12 @@ ble_ll_adv_pdu_make(struct ble_ll_adv_sm *advsm, struct os_mbuf *m) uint8_t ext_hdr_flags; uint32_t offset; + advsm = pducb_arg; + assert(advsm->adv_data); if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { - return ble_ll_adv_legacy_pdu_make(advsm, m); + return ble_ll_adv_legacy_pdu_make(dptr, advsm, hdr_byte); } pdulen = BLE_LL_EXT_ADV_HDR_LEN; @@ -504,10 +508,7 @@ ble_ll_adv_pdu_make(struct ble_ll_adv_sm *advsm, struct os_mbuf *m) /* Set the PDU length in the state machine (includes header) */ advsm->adv_pdu_len = pdulen + BLE_LL_PDU_HDR_LEN; - ble_ll_mbuf_init(m, pdulen, pdu_type); - - /* Construct advertisement */ - dptr = m->om_data; + *hdr_byte = pdu_type; /* ext hdr len and adv mode */ dptr[0] = ext_hdr_len | (adv_mode << 6); @@ -563,24 +564,21 @@ ble_ll_adv_pdu_make(struct ble_ll_adv_sm *advsm, struct os_mbuf *m) os_mbuf_copydata(advsm->adv_data, 0, ADV_DATA_LEN(advsm), dptr); dptr += ADV_DATA_LEN(advsm); } + + return pdulen; } #endif -static struct os_mbuf * -ble_ll_adv_scan_rsp_legacy_pdu_make(struct ble_ll_adv_sm *advsm) +static uint8_t +ble_ll_adv_scan_rsp_legacy_pdu_make(uint8_t *dptr, void *pducb_arg, + uint8_t *hdr_byte) { + struct ble_ll_adv_sm *advsm; uint8_t scan_rsp_len; - uint8_t *dptr; uint8_t pdulen; uint8_t hdr; - struct os_mbuf *m; - /* Obtain scan response buffer */ - m = os_msys_get_pkthdr(BLE_SCAN_RSP_LEGACY_DATA_MAX_LEN + BLE_DEV_ADDR_LEN, - sizeof(struct ble_mbuf_hdr)); - if (!m) { - return NULL; - } + advsm = pducb_arg; /* Make sure that the length is valid */ scan_rsp_len = advsm->scan_rsp_len; @@ -593,7 +591,7 @@ ble_ll_adv_scan_rsp_legacy_pdu_make(struct ble_ll_adv_sm *advsm) hdr |= BLE_ADV_PDU_HDR_TXADD_RAND; } - ble_ll_mbuf_init(m, pdulen, hdr); + *hdr_byte = hdr; /* * The adva in this packet will be the same one that was being advertised @@ -604,13 +602,12 @@ ble_ll_adv_scan_rsp_legacy_pdu_make(struct ble_ll_adv_sm *advsm) */ /* Construct scan response */ - dptr = m->om_data; memcpy(dptr, advsm->adva, BLE_DEV_ADDR_LEN); if (scan_rsp_len != 0) { memcpy(dptr + BLE_DEV_ADDR_LEN, advsm->scan_rsp_data, scan_rsp_len); } - return m; + return pdulen; } #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) @@ -619,18 +616,19 @@ ble_ll_adv_scan_rsp_legacy_pdu_make(struct ble_ll_adv_sm *advsm) * * @param advsm */ -static struct os_mbuf * -ble_ll_adv_scan_rsp_pdu_make(struct ble_ll_adv_sm *advsm) +static uint8_t +ble_ll_adv_scan_rsp_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) { - uint8_t *dptr; + struct ble_ll_adv_sm *advsm; uint8_t pdulen; uint8_t ext_hdr_len; uint8_t ext_hdr_flags; uint8_t hdr; - struct os_mbuf *m; + + advsm = pducb_arg; if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { - return ble_ll_adv_scan_rsp_legacy_pdu_make(advsm); + return ble_ll_adv_scan_rsp_legacy_pdu_make(dptr, pducb_arg, hdr_byte); } ext_hdr_len = BLE_LL_EXT_ADV_FLAGS_SIZE + BLE_LL_EXT_ADV_ADVA_SIZE; @@ -642,22 +640,13 @@ ble_ll_adv_scan_rsp_pdu_make(struct ble_ll_adv_sm *advsm) pdulen = BLE_LL_EXT_ADV_HDR_LEN + ext_hdr_len + advsm->scan_rsp_len; - /* Obtain scan response buffer */ - m = os_msys_get_pkthdr(pdulen, sizeof(struct ble_mbuf_hdr)); - if (!m) { - return NULL; - } - /* Set BLE transmit header */ hdr = BLE_ADV_PDU_TYPE_AUX_SCAN_RSP; if (advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) { hdr |= BLE_ADV_PDU_HDR_TXADD_RAND; } - ble_ll_mbuf_init(m, pdulen, hdr); - - /* Construct scan response */ - dptr = m->om_data; + *hdr_byte = hdr; /* ext hdr len and adv mode (00b) */ dptr[0] = ext_hdr_len; @@ -685,24 +674,31 @@ ble_ll_adv_scan_rsp_pdu_make(struct ble_ll_adv_sm *advsm) memcpy(dptr, advsm->scan_rsp_data, advsm->scan_rsp_len); dptr += advsm->scan_rsp_len; - return m; + return pdulen; } +struct aux_conn_rsp_data { + struct ble_ll_adv_sm *advsm; + uint8_t *peer; + uint8_t rxadd; +}; + /** * Create a AUX connect response PDU * * @param advsm */ -static struct os_mbuf * -ble_ll_adv_aux_conn_rsp_pdu_make(struct ble_ll_adv_sm *advsm, uint8_t *peer, - uint8_t rxadd) +static uint8_t +ble_ll_adv_aux_conn_rsp_pdu_make(uint8_t *dptr, void *pducb_arg, + uint8_t *hdr_byte) { - uint8_t *dptr; + struct aux_conn_rsp_data *rsp_data; uint8_t pdulen; uint8_t ext_hdr_len; uint8_t ext_hdr_flags; uint8_t hdr; - struct os_mbuf *m; + + rsp_data = pducb_arg; /* flags,AdvA and TargetA */ ext_hdr_len = BLE_LL_EXT_ADV_FLAGS_SIZE + BLE_LL_EXT_ADV_ADVA_SIZE + @@ -712,22 +708,13 @@ ble_ll_adv_aux_conn_rsp_pdu_make(struct ble_ll_adv_sm *advsm, uint8_t *peer, pdulen = BLE_LL_EXT_ADV_HDR_LEN + ext_hdr_len; - /* Obtain scan response buffer */ - m = os_msys_get_pkthdr(pdulen, sizeof(struct ble_mbuf_hdr)); - if (!m) { - return NULL; - } - /* Set BLE transmit header */ - hdr = BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP | rxadd; - if (advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) { + hdr = BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP | rsp_data->rxadd; + if (rsp_data->advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) { hdr |= BLE_ADV_PDU_HDR_TXADD_MASK; } - ble_ll_mbuf_init(m, pdulen, hdr); - - /* Construct connect response */ - dptr = m->om_data; + *hdr_byte = hdr; /* ext hdr len and adv mode (00b) */ dptr[0] = ext_hdr_len; @@ -737,13 +724,13 @@ ble_ll_adv_aux_conn_rsp_pdu_make(struct ble_ll_adv_sm *advsm, uint8_t *peer, dptr[0] = ext_hdr_flags; dptr += 1; - memcpy(dptr, advsm->adva, BLE_LL_EXT_ADV_ADVA_SIZE); + memcpy(dptr, rsp_data->advsm->adva, BLE_LL_EXT_ADV_ADVA_SIZE); dptr += BLE_LL_EXT_ADV_ADVA_SIZE; - memcpy(dptr, peer, BLE_LL_EXT_ADV_TARGETA_SIZE); + memcpy(dptr, rsp_data->peer, BLE_LL_EXT_ADV_TARGETA_SIZE); dptr += BLE_LL_EXT_ADV_ADVA_SIZE; - return m; + return pdulen; } #endif @@ -821,7 +808,6 @@ ble_ll_adv_tx_start_cb(struct ble_ll_sched_item *sch) uint8_t end_trans; uint32_t txstart; struct ble_ll_adv_sm *advsm; - struct os_mbuf *adv_pdu; /* Get the state machine for the event */ advsm = (struct ble_ll_adv_sm *)sch->cb_arg; @@ -884,23 +870,12 @@ ble_ll_adv_tx_start_cb(struct ble_ll_sched_item *sch) ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm); } - /* Get an advertising mbuf (packet header) */ - adv_pdu = os_msys_get_pkthdr(BLE_ADV_LEGACY_MAX_PKT_LEN, - sizeof(struct ble_mbuf_hdr)); - if (!adv_pdu) { - ble_phy_disable(); - goto adv_tx_done; - } - -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - ble_ll_adv_pdu_make(advsm, adv_pdu); -#else - ble_ll_adv_legacy_pdu_make(advsm, adv_pdu); -#endif - /* Transmit advertisement */ - rc = ble_phy_tx(adv_pdu, end_trans); - os_mbuf_free_chain(adv_pdu); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + rc = ble_phy_tx(ble_ll_adv_pdu_make, advsm, end_trans); +#else + rc = ble_phy_tx(ble_ll_adv_legacy_pdu_make, advsm, end_trans); +#endif if (rc) { goto adv_tx_done; } @@ -977,7 +952,6 @@ ble_ll_adv_secondary_tx_start_cb(struct ble_ll_sched_item *sch) uint8_t end_trans; uint32_t txstart; struct ble_ll_adv_sm *advsm; - struct os_mbuf *adv_pdu; /* Get the state machine for the event */ advsm = (struct ble_ll_adv_sm *)sch->cb_arg; @@ -1031,19 +1005,8 @@ ble_ll_adv_secondary_tx_start_cb(struct ble_ll_sched_item *sch) ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm); } - /* Get an advertising mbuf (packet header) */ - adv_pdu = os_msys_get_pkthdr(BLE_ADV_LEGACY_MAX_PKT_LEN, - sizeof(struct ble_mbuf_hdr)); - if (!adv_pdu) { - ble_phy_disable(); - goto adv_tx_done; - } - - ble_ll_adv_pdu_make(advsm, adv_pdu); - /* Transmit advertisement */ - rc = ble_phy_tx(adv_pdu, end_trans); - os_mbuf_free_chain(adv_pdu); + rc = ble_phy_tx(ble_ll_adv_pdu_make, advsm, end_trans); if (rc) { goto adv_tx_done; } @@ -2241,7 +2204,9 @@ ble_ll_adv_rx_req(uint8_t pdu_type, struct os_mbuf *rxpdu) uint8_t *peer; struct ble_mbuf_hdr *ble_hdr; struct ble_ll_adv_sm *advsm; - struct os_mbuf *rsp; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + struct aux_conn_rsp_data rsp_data; +#endif /* See if adva in the request (scan or connect) matches what we sent */ advsm = g_ble_ll_cur_adv_sm; @@ -2303,25 +2268,25 @@ ble_ll_adv_rx_req(uint8_t pdu_type, struct os_mbuf *rxpdu) rc = -1; if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_REQ) { + /* XXX TODO: assume we do not need to change phy mode */ + ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm); + #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) if (advsm->flags & BLE_LL_ADV_SM_FLAG_SCAN_REQ_NOTIF) { ble_ll_hci_ev_send_scan_req_recv(advsm->adv_instance, peer, peer_addr_type); } - rsp = ble_ll_adv_scan_rsp_pdu_make(advsm); + rc = ble_phy_tx(ble_ll_adv_scan_rsp_pdu_make, advsm, + BLE_PHY_TRANSITION_NONE); #else - rsp = ble_ll_adv_scan_rsp_legacy_pdu_make(advsm); + rc = ble_phy_tx(ble_ll_adv_scan_rsp_legacy_pdu_make, advsm, + BLE_PHY_TRANSITION_NONE); #endif - if (rsp) { - /* XXX TODO: assume we do not need to change phy mode */ - ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm); - rc = ble_phy_tx(rsp, BLE_PHY_TRANSITION_NONE); - if (!rc) { - ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_SCAN_RSP_TXD; - STATS_INC(ble_ll_stats, scan_rsp_txg); - } - os_mbuf_free_chain(rsp); + + if (!rc) { + ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_SCAN_RSP_TXD; + STATS_INC(ble_ll_stats, scan_rsp_txg); } } else if (pdu_type == BLE_ADV_PDU_TYPE_AUX_CONNECT_REQ) { /* @@ -2340,17 +2305,16 @@ ble_ll_adv_rx_req(uint8_t pdu_type, struct os_mbuf *rxpdu) } /* use remote address used over the air */ - rsp = ble_ll_adv_aux_conn_rsp_pdu_make(advsm, - rxbuf + BLE_LL_PDU_HDR_LEN, - rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK); - if (rsp) { - ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm); - rc = ble_phy_tx(rsp, BLE_PHY_TRANSITION_NONE); - if (!rc) { - advsm->flags |= BLE_LL_ADV_SM_FLAG_CONN_RSP_TXD; - STATS_INC(ble_ll_stats, aux_conn_rsp_tx); - } - os_mbuf_free_chain(rsp); + rsp_data.advsm = advsm; + rsp_data.peer = rxbuf + BLE_LL_PDU_HDR_LEN; + rsp_data.rxadd = rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK; + + ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm); + rc = ble_phy_tx(ble_ll_adv_aux_conn_rsp_pdu_make, &rsp_data, + BLE_PHY_TRANSITION_NONE); + if (!rc) { + advsm->flags |= BLE_LL_ADV_SM_FLAG_CONN_RSP_TXD; + STATS_INC(ble_ll_stats, aux_conn_rsp_tx); } #endif } diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c index 040c657ac..03930660f 100644 --- a/nimble/controller/src/ble_ll_conn.c +++ b/nimble/controller/src/ble_ll_conn.c @@ -1383,7 +1383,7 @@ conn_tx_pdu: /* Set transmit end callback */ ble_phy_set_txend_cb(txend_func, connsm); - rc = ble_phy_tx(m, end_transition); + rc = ble_phy_tx(ble_ll_tx_mbuf_pducb, m, end_transition); if (!rc) { /* Log transmit on connection state */ cur_txlen = ble_hdr->txinfo.pyld_len; @@ -2865,7 +2865,7 @@ ble_ll_conn_request_send(uint8_t addr_type, uint8_t *adva, uint16_t txoffset, } else { ble_phy_set_txend_cb(ble_ll_conn_req_txend_init, NULL); } - rc = ble_phy_tx(m, end_trans); + rc = ble_phy_tx(ble_ll_tx_mbuf_pducb, m, end_trans); return rc; } diff --git a/nimble/controller/src/ble_ll_scan.c b/nimble/controller/src/ble_ll_scan.c index adbd719be..f1935d2c1 100644 --- a/nimble/controller/src/ble_ll_scan.c +++ b/nimble/controller/src/ble_ll_scan.c @@ -2147,7 +2147,8 @@ ble_ll_scan_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok) #endif /* XXX: TODO assume we are on correct phy */ ble_ll_scan_req_pdu_make(scansm, adv_addr, addr_type); - rc = ble_phy_tx(scansm->scan_req_pdu, BLE_PHY_TRANSITION_TX_RX); + rc = ble_phy_tx(ble_ll_tx_mbuf_pducb, scansm->scan_req_pdu, + BLE_PHY_TRANSITION_TX_RX); if (rc == 0) { /* Set "waiting for scan response" flag */ diff --git a/nimble/drivers/native/src/ble_phy.c b/nimble/drivers/native/src/ble_phy.c index 2333eb45c..1450585ba 100644 --- a/nimble/drivers/native/src/ble_phy.c +++ b/nimble/drivers/native/src/ble_phy.c @@ -67,6 +67,8 @@ struct ble_phy_statistics struct ble_phy_statistics g_ble_phy_stats; +static uint8_t g_ble_phy_tx_buf[BLE_PHY_MAX_PDU_LEN]; + /* XCVR object to emulate transceiver */ struct xcvr_data { @@ -413,13 +415,11 @@ ble_phy_rx_set_start_time(uint32_t cputime, uint8_t rem_usecs) int -ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans) +ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans) { + uint8_t hdr_byte; int rc; - /* Better have a pdu! */ - assert(txpdu != NULL); - if (ble_phy_state_get() != BLE_PHY_STATE_IDLE) { ble_phy_disable(); ++g_ble_phy_stats.radio_state_errs; @@ -439,8 +439,8 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans) /* Set phy state to transmitting and count packet statistics */ g_ble_phy_data.phy_state = BLE_PHY_STATE_TX; ++g_ble_phy_stats.tx_good; - g_ble_phy_stats.tx_bytes += OS_MBUF_PKTHDR(txpdu)->omp_len + - BLE_LL_PDU_HDR_LEN; + g_ble_phy_stats.tx_bytes += pducb(g_ble_phy_tx_buf, pducb_arg, &hdr_byte) + + BLE_LL_PDU_HDR_LEN; rc = BLE_ERR_SUCCESS; return rc; diff --git a/nimble/drivers/nrf51/src/ble_phy.c b/nimble/drivers/nrf51/src/ble_phy.c index 175c652b5..b231fe73e 100644 --- a/nimble/drivers/nrf51/src/ble_phy.c +++ b/nimble/drivers/nrf51/src/ble_phy.c @@ -1084,17 +1084,15 @@ ble_phy_rx_set_start_time(uint32_t cputime, uint8_t rem_usecs) } int -ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans) +ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans) { int rc; uint8_t *dptr; uint8_t payload_len; + uint8_t payload_off; + uint8_t hdr_byte; uint32_t state; uint32_t shortcuts; - struct ble_mbuf_hdr *ble_hdr; - - /* Better have a pdu! */ - assert(txpdu != NULL); /* * This check is to make sure that the radio is not in a state where @@ -1102,9 +1100,6 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans) */ nrf_wait_disabled(); - ble_hdr = BLE_MBUF_HDR_PTR(txpdu); - payload_len = ble_hdr->txinfo.pyld_len; - /* * XXX: Although we may not have to do this here, I clear all the PPI * that should not be used when transmitting. Some of them are only enabled @@ -1119,10 +1114,7 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans) if (g_ble_phy_data.phy_encrypted) { /* RAM representation has S0, LENGTH and S1 fields. (3 bytes) */ dptr = (uint8_t *)&g_ble_phy_enc_buf[0]; - dptr[0] = ble_hdr->txinfo.hdr_byte; - dptr[1] = payload_len; - dptr[2] = 0; - dptr += 3; + payload_off = 3; NRF_CCM->SHORTS = 1; NRF_CCM->INPTR = (uint32_t)&g_ble_phy_enc_buf[0]; @@ -1141,9 +1133,7 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans) #endif /* RAM representation has S0 and LENGTH fields (2 bytes) */ dptr = (uint8_t *)&g_ble_phy_tx_buf[0]; - dptr[0] = ble_hdr->txinfo.hdr_byte; - dptr[1] = payload_len; - dptr += 2; + payload_off = 2; } #else @@ -1155,9 +1145,7 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans) /* RAM representation has S0 and LENGTH fields (2 bytes) */ dptr = (uint8_t *)&g_ble_phy_tx_buf[0]; - dptr[0] = ble_hdr->txinfo.hdr_byte; - dptr[1] = payload_len; - dptr += 2; + payload_off = 2; #endif NRF_RADIO->PACKETPTR = (uint32_t)&g_ble_phy_tx_buf[0]; @@ -1175,17 +1163,25 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans) NRF_RADIO->SHORTS = shortcuts; NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk; - /* Set transmitted payload length */ - g_ble_phy_data.phy_tx_pyld_len = payload_len; + /* Set PDU payload */ + payload_len = pducb(&dptr[payload_off], pducb_arg, &hdr_byte); + + /* Set PDU header */ + dptr[0] = hdr_byte; + dptr[1] = payload_len; + if (payload_off > 2) { + dptr[2] = 0; + } /* Set the PHY transition */ g_ble_phy_data.phy_transition = end_trans; + /* Set transmitted payload length */ + g_ble_phy_data.phy_tx_pyld_len = payload_len; + /* If we already started transmitting, abort it! */ state = NRF_RADIO->STATE; if (state != RADIO_STATE_STATE_Tx) { - /* Copy data from mbuf into transmit buffer */ - os_mbuf_copydata(txpdu, ble_hdr->txinfo.offset, payload_len, dptr); /* Set phy state to transmitting and count packet statistics */ g_ble_phy_data.phy_state = BLE_PHY_STATE_TX; diff --git a/nimble/drivers/nrf52/src/ble_phy.c b/nimble/drivers/nrf52/src/ble_phy.c index 010b744dc..bb457a17a 100644 --- a/nimble/drivers/nrf52/src/ble_phy.c +++ b/nimble/drivers/nrf52/src/ble_phy.c @@ -1438,18 +1438,15 @@ ble_phy_rx_set_start_time(uint32_t cputime, uint8_t rem_usecs) } int -ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans) +ble_phy_tx(ble_phy_tx_pducb_t pducb, void *pducb_arg, uint8_t end_trans) { int rc; uint8_t *dptr; uint8_t *pktptr; uint8_t payload_len; + uint8_t hdr_byte; uint32_t state; uint32_t shortcuts; - struct ble_mbuf_hdr *ble_hdr; - - /* Better have a pdu! */ - assert(txpdu != NULL); /* * This check is to make sure that the radio is not in a state where @@ -1457,9 +1454,6 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans) */ nrf_wait_disabled(); - ble_hdr = BLE_MBUF_HDR_PTR(txpdu); - payload_len = ble_hdr->txinfo.pyld_len; - /* * XXX: Although we may not have to do this here, I clear all the PPI * that should not be used when transmitting. Some of them are only enabled @@ -1497,11 +1491,6 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans) pktptr = dptr; #endif - /* RAM representation has S0, LENGTH and S1 fields. (3 bytes) */ - dptr[0] = ble_hdr->txinfo.hdr_byte; - dptr[1] = payload_len; - dptr[2] = 0; - dptr += 3; NRF_RADIO->PACKETPTR = (uint32_t)pktptr; /* Clear the ready, end and disabled events */ @@ -1514,18 +1503,23 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t end_trans) NRF_RADIO->SHORTS = shortcuts; NRF_RADIO->INTENSET = RADIO_INTENSET_DISABLED_Msk; - /* Set transmitted payload length */ - g_ble_phy_data.phy_tx_pyld_len = payload_len; + /* Set PDU payload */ + payload_len = pducb(&dptr[3], pducb_arg, &hdr_byte); + + /* RAM representation has S0, LENGTH and S1 fields. (3 bytes) */ + dptr[0] = hdr_byte; + dptr[1] = payload_len; + dptr[2] = 0; /* Set the PHY transition */ g_ble_phy_data.phy_transition = end_trans; + /* Set transmitted payload length */ + g_ble_phy_data.phy_tx_pyld_len = payload_len; + /* If we already started transmitting, abort it! */ state = NRF_RADIO->STATE; if (state != RADIO_STATE_STATE_Tx) { - /* Copy data from mbuf into transmit buffer */ - os_mbuf_copydata(txpdu, ble_hdr->txinfo.offset, payload_len, dptr); - /* Set phy state to transmitting and count packet statistics */ g_ble_phy_data.phy_state = BLE_PHY_STATE_TX; STATS_INC(ble_phy_stats, tx_good);