nimble/ll: Add support for Channel Map Update in periodic scanner
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 @@
#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 @@
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 @@
}
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 @@
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_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 @@
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 @@
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 @@
/* 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);
