From e9f824cfdb47686054ed40ca0556bae3d832861e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C5=81ukasz=20Rymanowski?= Date: Thu, 7 Sep 2017 10:15:05 +0200 Subject: [PATCH] nimble/controller: Fix scan long advertising packets This patch fixes corrupted long advertising data when BLE_EXT_ADV is used. It is allowed now that advertising data are in the chain of os_mbufs. X-Original-Commit: d18c3bbab23a57b29cce16e57b6262533f9e5bc8 --- .../include/controller/ble_ll_scan.h | 4 +- nimble/controller/src/ble_ll.c | 2 +- nimble/controller/src/ble_ll_scan.c | 41 +++++++++++-------- 3 files changed, 27 insertions(+), 20 deletions(-) diff --git a/nimble/controller/include/controller/ble_ll_scan.h b/nimble/controller/include/controller/ble_ll_scan.h index 3b2da392b..acae285f9 100644 --- a/nimble/controller/include/controller/ble_ll_scan.h +++ b/nimble/controller/include/controller/ble_ll_scan.h @@ -164,7 +164,7 @@ int ble_ll_scan_rx_isr_start(uint8_t pdu_type, uint16_t *rxflags); int ble_ll_scan_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok); /* Process a scan response PDU */ -void ble_ll_scan_rx_pkt_in(uint8_t pdu_type, uint8_t *rxbuf, +void ble_ll_scan_rx_pkt_in(uint8_t pdu_type, struct os_mbuf *om, struct ble_mbuf_hdr *hdr); /* Boolean function denoting whether or not the whitelist can be changed */ @@ -221,7 +221,7 @@ int ble_ll_scan_ext_initiator_start(struct hci_ext_create_conn *hcc, /* Called to parse extended advertising*/ struct ble_ll_ext_adv; -int ble_ll_scan_parse_ext_adv(uint8_t *rxbuf, struct ble_mbuf_hdr *ble_hdr, +int ble_ll_scan_parse_ext_adv(struct os_mbuf *om, struct ble_mbuf_hdr *ble_hdr, struct ble_ll_ext_adv *parsed_evt); void ble_ll_scan_aux_data_free(struct ble_ll_aux_data *aux_scan); diff --git a/nimble/controller/src/ble_ll.c b/nimble/controller/src/ble_ll.c index 6761e3b3b..4d02090c9 100644 --- a/nimble/controller/src/ble_ll.c +++ b/nimble/controller/src/ble_ll.c @@ -720,7 +720,7 @@ ble_ll_rx_pkt_in(void) ble_ll_adv_rx_pkt_in(pdu_type, rxbuf, ble_hdr); break; case BLE_LL_STATE_SCANNING: - ble_ll_scan_rx_pkt_in(pdu_type, rxbuf, ble_hdr); + ble_ll_scan_rx_pkt_in(pdu_type, m, ble_hdr); break; case BLE_LL_STATE_INITIATING: ble_ll_init_rx_pkt_in(pdu_type, rxbuf, ble_hdr); diff --git a/nimble/controller/src/ble_ll_scan.c b/nimble/controller/src/ble_ll_scan.c index 593f05d8d..90915d549 100644 --- a/nimble/controller/src/ble_ll_scan.c +++ b/nimble/controller/src/ble_ll_scan.c @@ -562,7 +562,7 @@ static int ble_ll_hci_send_legacy_ext_adv_report(uint8_t evtype, uint8_t addr_type, uint8_t *addr, uint8_t rssi, - uint8_t adv_data_len, uint8_t *adv_data, + uint8_t adv_data_len, struct os_mbuf *adv_data, uint8_t *inita) { struct ble_ll_ext_adv *evt; @@ -609,7 +609,7 @@ ble_ll_hci_send_legacy_ext_adv_report(uint8_t evtype, evt->event_len += BLE_DEV_ADDR_LEN + 1; } else if (adv_data_len <= (MYNEWT_VAL(BLE_HCI_EVT_BUF_SIZE) - sizeof(*evt))) { evt->adv_data_len = adv_data_len; - memcpy(evt->adv_data, adv_data, adv_data_len); + os_mbuf_copydata(adv_data, 0, adv_data_len, evt->adv_data); evt->event_len += adv_data_len; } @@ -623,7 +623,8 @@ ble_ll_hci_send_legacy_ext_adv_report(uint8_t evtype, static int ble_ll_hci_send_adv_report(uint8_t subev, uint8_t evtype,uint8_t event_len, uint8_t addr_type, uint8_t *addr, uint8_t rssi, - uint8_t adv_data_len, uint8_t *adv_data, uint8_t *inita) + uint8_t adv_data_len, struct os_mbuf *adv_data, + uint8_t *inita) { uint8_t *evbuf; uint8_t *tmp; @@ -653,7 +654,7 @@ ble_ll_hci_send_adv_report(uint8_t subev, uint8_t evtype,uint8_t event_len, tmp += BLE_DEV_ADDR_LEN + 1; } else if (subev == BLE_HCI_LE_SUBEV_ADV_RPT) { tmp[0] = adv_data_len; - memcpy(tmp + 1, adv_data, adv_data_len); + os_mbuf_copydata(adv_data, 0, adv_data_len, tmp + 1); tmp += adv_data_len + 1; } else { assert(0); @@ -677,7 +678,7 @@ ble_ll_hci_send_adv_report(uint8_t subev, uint8_t evtype,uint8_t event_len, * @param scansm */ static void -ble_ll_scan_send_adv_report(uint8_t pdu_type, uint8_t txadd, uint8_t *rxbuf, +ble_ll_scan_send_adv_report(uint8_t pdu_type, uint8_t txadd, struct os_mbuf *om, struct ble_mbuf_hdr *hdr, struct ble_ll_scan_sm *scansm) { @@ -685,13 +686,13 @@ ble_ll_scan_send_adv_report(uint8_t pdu_type, uint8_t txadd, uint8_t *rxbuf, #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) int index; #endif + uint8_t *rxbuf = om->om_data; uint8_t evtype; uint8_t subev; uint8_t *adv_addr; uint8_t *inita; uint8_t addr_type; uint8_t adv_data_len; - uint8_t *adv_data = NULL; uint8_t event_len; inita = NULL; @@ -721,7 +722,7 @@ ble_ll_scan_send_adv_report(uint8_t pdu_type, uint8_t txadd, uint8_t *rxbuf, adv_data_len = rxbuf[1] & BLE_ADV_PDU_HDR_LEN_MASK; adv_data_len -= BLE_DEV_ADDR_LEN; event_len = BLE_HCI_LE_ADV_RPT_MIN_LEN + adv_data_len; - adv_data = rxbuf + BLE_LL_PDU_HDR_LEN + BLE_DEV_ADDR_LEN; + os_mbuf_adj(om, BLE_LL_PDU_HDR_LEN + BLE_DEV_ADDR_LEN); } if (txadd) { @@ -752,20 +753,20 @@ ble_ll_scan_send_adv_report(uint8_t pdu_type, uint8_t txadd, uint8_t *rxbuf, rc = ble_ll_hci_send_legacy_ext_adv_report(evtype, addr_type, adv_addr, hdr->rxinfo.rssi, - adv_data_len, adv_data, + adv_data_len, om, inita); } else { rc = ble_ll_hci_send_adv_report(subev, evtype, event_len, addr_type, adv_addr, hdr->rxinfo.rssi, - adv_data_len, adv_data, + adv_data_len, om, inita); } #else rc = ble_ll_hci_send_adv_report(subev, evtype, event_len, addr_type, adv_addr, hdr->rxinfo.rssi, - adv_data_len, adv_data, + adv_data_len, om, inita); #endif if (!rc) { @@ -1643,7 +1644,7 @@ ble_ll_scan_get_aux_data(struct ble_ll_scan_sm *scansm, * */ int -ble_ll_scan_parse_ext_adv(uint8_t *rxbuf, struct ble_mbuf_hdr *ble_hdr, +ble_ll_scan_parse_ext_adv(struct os_mbuf *om, struct ble_mbuf_hdr *ble_hdr, struct ble_ll_ext_adv *out_evt) { uint8_t pdu_len; @@ -1653,6 +1654,7 @@ ble_ll_scan_parse_ext_adv(uint8_t *rxbuf, struct ble_mbuf_hdr *ble_hdr, struct ble_ll_scan_sm *scansm; struct ble_ll_aux_data *aux_data; int i = 1; + uint8_t *rxbuf = om->om_data; if (!out_evt) { return -1; @@ -1678,6 +1680,7 @@ ble_ll_scan_parse_ext_adv(uint8_t *rxbuf, struct ble_mbuf_hdr *ble_hdr, ext_hdr_len = rxbuf[2] & 0x3F; ext_hdr_flags = rxbuf[3]; ext_hdr = &rxbuf[4]; + os_mbuf_adj(om, 4); i = 0; if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) { @@ -1722,9 +1725,12 @@ ble_ll_scan_parse_ext_adv(uint8_t *rxbuf, struct ble_mbuf_hdr *ble_hdr, /* Skip ADAC if it is there */ i = ext_hdr_len; + /* Adjust for advertising data */ + os_mbuf_adj(om, i); + if ((pdu_len - i - 1 > 0)) { out_evt->adv_data_len = pdu_len - i - 1; - memcpy(out_evt->adv_data, ext_hdr + ext_hdr_len - 1, out_evt->adv_data_len); + os_mbuf_copydata(om, 0, out_evt->adv_data_len, out_evt->adv_data); } /* In the event we need information on primary and secondary PHY used during @@ -2151,7 +2157,7 @@ ble_ll_scan_aux_data_free(struct ble_ll_aux_data *aux_scan) } static void -ble_ll_hci_send_ext_adv_report(uint8_t ptype, uint8_t *rxbuf, +ble_ll_hci_send_ext_adv_report(uint8_t ptype, struct os_mbuf *om, struct ble_mbuf_hdr *hdr) { struct ble_ll_ext_adv *evt; @@ -2166,7 +2172,7 @@ ble_ll_hci_send_ext_adv_report(uint8_t ptype, uint8_t *rxbuf, return; } - rc = ble_ll_scan_parse_ext_adv(rxbuf, hdr, evt); + rc = ble_ll_scan_parse_ext_adv(om, hdr, evt); if (rc) { ble_hci_trans_buf_free((uint8_t *)evt); @@ -2188,11 +2194,12 @@ ble_ll_hci_send_ext_adv_report(uint8_t ptype, uint8_t *rxbuf, * @param rxbuf */ void -ble_ll_scan_rx_pkt_in(uint8_t ptype, uint8_t *rxbuf, struct ble_mbuf_hdr *hdr) +ble_ll_scan_rx_pkt_in(uint8_t ptype, struct os_mbuf *om, struct ble_mbuf_hdr *hdr) { #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) int index; #endif + uint8_t *rxbuf = om->om_data; uint8_t *adv_addr = NULL; uint8_t *adva; uint8_t *ident_addr; @@ -2307,7 +2314,7 @@ ble_ll_scan_rx_pkt_in(uint8_t ptype, uint8_t *rxbuf, struct ble_mbuf_hdr *hdr) STATS_INC(ble_ll_stats, aux_chain_cnt); } - ble_ll_hci_send_ext_adv_report(ptype, rxbuf, hdr); + ble_ll_hci_send_ext_adv_report(ptype, om, hdr); ble_ll_scan_switch_phy(scansm); if (scansm->scan_rsp_pending) { @@ -2330,7 +2337,7 @@ ble_ll_scan_rx_pkt_in(uint8_t ptype, uint8_t *rxbuf, struct ble_mbuf_hdr *hdr) #endif /* Send the advertising report */ - ble_ll_scan_send_adv_report(ptype, ident_addr_type, rxbuf, hdr, scansm); + ble_ll_scan_send_adv_report(ptype, ident_addr_type, om, hdr, scansm); scan_continue: /*