This is an automated email from the ASF dual-hosted git repository. janc pushed a commit to branch master in repository https://gitbox.apache.org/repos/asf/mynewt-nimble.git
The following commit(s) were added to refs/heads/master by this push: new 97482d4 nimble/ll: Add support for Channel Map Update in periodic scanner 97482d4 is described below commit 97482d497ddaba6fe5c7c442ceb6730f09846399 Author: Szymon Janc <szymon.j...@codecoup.pl> AuthorDate: Mon Mar 8 17:15:28 2021 +0100 nimble/ll: Add support for Channel Map Update in periodic scanner --- nimble/controller/include/controller/ble_ll.h | 8 ++ nimble/controller/src/ble_ll_sync.c | 138 ++++++++++++++++++++++---- 2 files changed, 126 insertions(+), 20 deletions(-) diff --git a/nimble/controller/include/controller/ble_ll.h b/nimble/controller/include/controller/ble_ll.h index 1c43e69..24d7db3 100644 --- a/nimble/controller/include/controller/ble_ll.h +++ b/nimble/controller/include/controller/ble_ll.h @@ -444,6 +444,14 @@ struct ble_dev_addr #define BLE_LL_ADDR_SUBTYPE_RPA (1) #define BLE_LL_ADDR_SUBTYPE_NRPA (2) +/* ACAD data types */ +#define BLE_LL_ACAD_CHANNEL_MAP_UPDATE_IND 0x28 + +struct ble_ll_acad_channel_map_update_ind { + uint8_t map[5]; + uint16_t instant; +} __attribute__((packed)); + /*--- External API ---*/ /* Initialize the Link Layer */ void ble_ll_init(void); diff --git a/nimble/controller/src/ble_ll_sync.c b/nimble/controller/src/ble_ll_sync.c index df80608..7c46d10 100644 --- a/nimble/controller/src/ble_ll_sync.c +++ b/nimble/controller/src/ble_ll_sync.c @@ -60,6 +60,7 @@ #define BLE_LL_SYNC_SM_FLAG_DISABLED 0x0040 #define BLE_LL_SYNC_SM_FLAG_ADDR_RESOLVED 0x0080 #define BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED 0x0100 +#define BLE_LL_SYNC_SM_FLAG_NEW_CHANMAP 0x0200 #define BLE_LL_SYNC_CHMAP_LEN 5 #define BLE_LL_SYNC_ITVL_USECS 1250 @@ -75,6 +76,9 @@ struct ble_ll_sync_sm { uint8_t chanmap[BLE_LL_SYNC_CHMAP_LEN]; uint8_t num_used_chans; + uint8_t chanmap_new[BLE_LL_SYNC_CHMAP_LEN]; + uint16_t chanmap_new_instant; + uint8_t chan_index; uint8_t chan_chain; @@ -525,7 +529,8 @@ ble_ll_sync_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *rxhdr) } static int -ble_ll_sync_parse_ext_hdr(struct os_mbuf *om, uint8_t **aux, int8_t *tx_power) +ble_ll_sync_parse_ext_hdr(struct os_mbuf *om, uint8_t **aux, int8_t *tx_power, + uint8_t **acad, uint8_t *acad_len) { uint8_t *rxbuf = om->om_data; uint8_t ext_hdr_flags; @@ -585,12 +590,17 @@ ble_ll_sync_parse_ext_hdr(struct os_mbuf *om, uint8_t **aux, int8_t *tx_power) i += BLE_LL_EXT_ADV_TX_POWER_SIZE; } - /* TODO Handle ACAD if needed */ - /* sanity check */ if (i > ext_hdr_len) { return -1; } + + /* ACAD */ + if (ext_hdr_len > (i + 1)) { + *acad = ext_hdr + i; + *acad_len = ext_hdr_len - i - 1; + } + } return pdu_len - ext_hdr_len - 1; @@ -996,6 +1006,70 @@ ble_ll_sync_check_failed(struct ble_ll_sync_sm *sm) ble_ll_sync_est_event_failed(BLE_ERR_CONN_ESTABLISHMENT); } +static bool +ble_ll_sync_check_acad(struct ble_ll_sync_sm *sm, + const uint8_t *acad, uint8_t acad_len) +{ + const struct ble_ll_acad_channel_map_update_ind *chmu; + unsigned int ad_len; + uint8_t ad_type; + + /* assume no empty fields */ + while (acad_len > 2) { + ad_len = acad[0]; + ad_type = acad[1]; + + /* early termination should not happen in ACAD */ + if (ad_len == 0) { + return false; + } + + /* check if not passing pass acad data */ + if (ad_len + 1 > acad_len) { + return false; + } + + switch (ad_type) { + case BLE_LL_ACAD_CHANNEL_MAP_UPDATE_IND: + chmu = (const void *)&acad[2]; + + if (ad_len - 1 != sizeof(*chmu)) { + return false; + } + + /* Channels Mask (37 bits) + * TODO should we check this? + */ + sm->chanmap_new[0] = chmu->map[0]; + sm->chanmap_new[1] = chmu->map[1]; + sm->chanmap_new[2] = chmu->map[2]; + sm->chanmap_new[3] = chmu->map[3]; + sm->chanmap_new[4] = chmu->map[4] & 0x1f; + + /* drop if channel map is invalid */ + if (ble_ll_utils_calc_num_used_chans(sm->chanmap_new) == 0) { + return false; + } + + sm->chanmap_new_instant = le16toh(chmu->instant); + sm->flags |= BLE_LL_SYNC_SM_FLAG_NEW_CHANMAP; + break; + default: + break; + } + + acad += ad_len + 1; + acad_len -= ad_len + 1; + } + + /* should have no trailing zeros */ + if (acad_len) { + return false; + } + + return true; +} + void ble_ll_sync_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr) { @@ -1003,7 +1077,10 @@ ble_ll_sync_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr) bool aux_scheduled = false; int8_t tx_power = 127; /* defaults to not available */ uint8_t *aux = NULL; + uint8_t *acad = NULL; + uint8_t acad_len; int datalen; + bool reports_enabled; BLE_LL_ASSERT(sm); @@ -1048,36 +1125,43 @@ ble_ll_sync_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr) goto end_event; } - if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_RPT) && - !(sm->flags & BLE_LL_SYNC_SM_FLAG_DISABLED)) { - /* get ext header data */ - datalen = ble_ll_sync_parse_ext_hdr(rxpdu, &aux, &tx_power); - if (datalen < 0) { - /* we got bad packet, end event */ - goto end_event; - } + /* get ext header data */ + datalen = ble_ll_sync_parse_ext_hdr(rxpdu, &aux, &tx_power, &acad, &acad_len); + if (datalen < 0) { + /* we got bad packet, end event */ + goto end_event; + } + reports_enabled = ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_RPT) && + !(sm->flags & BLE_LL_SYNC_SM_FLAG_DISABLED); + + /* no need to schedule for chain if reporting is disabled */ + if (reports_enabled) { /* if aux is present, we need to schedule ASAP */ if (aux && (ble_ll_sync_schedule_chain(sm, hdr, aux) == 0)) { aux_scheduled = true; } + } - /* in case data reporting is enabled we need to send sync established here */ - if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) { - ble_ll_sync_established(sm); - } + /* check ACAD, needs to be done before rxpdu is adjusted for ADV data */ + if (acad && !ble_ll_sync_check_acad(sm, acad, acad_len)) { + /* we got bad packet (bad ACAD data), end event */ + goto end_event; + } + + /* we need to establish link even if reporting was disabled */ + if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) { + ble_ll_sync_established(sm); + } + /* only if reporting is enabled */ + if (reports_enabled) { /* Adjust rxpdu to contain advertising data only */ ble_ll_sync_adjust_ext_hdr(rxpdu); /* send reports from this PDU */ ble_ll_sync_send_per_adv_rpt(sm, rxpdu, hdr->rxinfo.rssi, tx_power, datalen, aux, aux_scheduled); - } else { - /* we need to establish link even if reporting was disabled */ - if (sm->flags & BLE_LL_SYNC_SM_FLAG_ESTABLISHING) { - ble_ll_sync_established(sm); - } } /* if chain was scheduled we don't end event yet */ @@ -1131,6 +1215,20 @@ ble_ll_sync_next_event(struct ble_ll_sync_sm *sm, uint32_t cur_ww_adjust) /* Set event counter to the next event */ sm->event_cntr += 1 + skip; + /* update channel map if needed */ + if (sm->flags & BLE_LL_SYNC_SM_FLAG_NEW_CHANMAP) { + if (((int16_t)(sm->event_cntr - sm->chanmap_new_instant)) >= 0) { + /* map was verified on reception */ + sm->chanmap[0] = sm->chanmap_new[0]; + sm->chanmap[1] = sm->chanmap_new[1]; + sm->chanmap[2] = sm->chanmap_new[2]; + sm->chanmap[3] = sm->chanmap_new[3]; + sm->chanmap[4] = sm->chanmap_new[4]; + sm->num_used_chans = ble_ll_utils_calc_num_used_chans(sm->chanmap); + sm->flags &= ~BLE_LL_SYNC_SM_FLAG_NEW_CHANMAP; + } + } + /* Calculate channel index of next event */ sm->chan_index = ble_ll_utils_calc_dci_csa2(sm->event_cntr, sm->channel_id, sm->num_used_chans, sm->chanmap);