nimble/ll: Add PA ADI support

This adds ADI support for Periodic Advertising
in controller.

nimble/controller/include/controller/ble_ll_sync.h

@@ -45,7 +45,7 @@ int ble_ll_sync_transfer(const uint8_t *cmdbuf, uint8_t len,
  uint8_t *rspbuf, uint8_t *rsplen);
 
 void ble_ll_sync_periodic_ind(struct ble_ll_conn_sm *connsm,
- const uint8_t *sync_ind, bool reports_disabled,
+ const uint8_t *sync_ind, uint8_t mode,
  uint16_t max_skip, uint32_t sync_timeout);
 void ble_ll_sync_transfer_disconnected(struct ble_ll_conn_sm *connsm);
 

nimble/controller/src/ble_ll.c

@@ -1935,6 +1935,10 @@ ble_ll_init(void)
  features |= BLE_LL_FEAT_CS_PCT_QUALITY_IND;
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)
+ features |= BLE_LL_FEAT_PERIODIC_ADV_ADI;
+#endif
+
  lldata->ll_supp_features = features;
 
  /* Initialize random number generation */

nimble/controller/src/ble_ll_adv.c

@@ -167,6 +167,10 @@ struct ble_ll_adv_sm
  uint8_t periodic_adv_active : 1;
  uint8_t periodic_sync_active : 1;
  uint8_t periodic_sync_index : 1;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)
+ uint8_t periodic_include_adi : 1;
+ uint16_t periodic_adv_adi;
+#endif
  uint8_t periodic_num_used_chans;
  uint8_t periodic_chanmap[BLE_LL_CHAN_MAP_LEN];
  uint16_t periodic_adv_itvl;
@@ -2160,6 +2164,12 @@ ble_ll_adv_sync_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
  dptr += 1;
  }
 #endif
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)
+ if (sync->ext_hdr_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) {
+ put_le16(dptr, advsm->periodic_adv_adi);
+ dptr += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+ }
+#endif
 
  if (sync->ext_hdr_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) {
  if (!SYNC_NEXT(advsm)->sch.enqueued) {
@@ -2342,6 +2352,13 @@ ble_ll_adv_sync_calculate(struct ble_ll_adv_sm *advsm,
 
  ext_hdr_len = BLE_LL_EXT_ADV_HDR_LEN;
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)
+ if (advsm->periodic_include_adi) {
+ sync->ext_hdr_flags |= (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT);
+ ext_hdr_len += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+ }
+#endif
+
  /* TxPower if configured
  * Note: TxPower shall not be present in chain PDU for SYNC
  */
@@ -2599,6 +2616,9 @@ ble_ll_adv_update_periodic_data(struct ble_ll_adv_sm *advsm)
  os_mbuf_free_chain(advsm->periodic_adv_data);
  advsm->periodic_adv_data = advsm->periodic_new_data;
  advsm->periodic_new_data = NULL;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)
+ advsm->periodic_adv_adi = ble_ll_adv_update_did(advsm->periodic_adv_adi);
+#endif
  }
 
  ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_PERIODIC_NEW_DATA);
@@ -2665,6 +2685,14 @@ ble_ll_adv_sm_start_periodic(struct ble_ll_adv_sm *advsm)
  */
  advsm->adi = ble_ll_adv_update_did(advsm->adi);
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)
+ /*
+ * Since periodic advertising can be started without data we initialize DID here
+ * instead of after setting periodic advertising data.
+ */
+ advsm->periodic_adv_adi = ble_ll_adv_update_did(advsm->periodic_adv_adi);
+#endif
+
  advsm->periodic_adv_active = 1;
 
  /* keep channel map since we cannot change it later on */
@@ -3596,6 +3624,9 @@ ble_ll_adv_ext_set_param(const uint8_t *cmdbuf, uint8_t len,
  advsm->sec_phy = cmd->sec_phy;
  /* Update SID only */
  advsm->adi = (advsm->adi & 0x0fff) | ((cmd->sid << 12));
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)
+ advsm->periodic_adv_adi = (advsm->periodic_adv_adi & 0x0fff) | ((cmd->sid << 12));
+#endif
 
  advsm->props = props;
 
@@ -4101,6 +4132,23 @@ ble_ll_adv_periodic_set_data(const uint8_t *cmdbuf, uint8_t len)
  case BLE_HCI_LE_SET_DATA_OPER_COMPLETE:
  new_data = true;
  break;
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)
+ case BLE_HCI_LE_SET_DATA_OPER_UNCHANGED:
+ if (!advsm->periodic_adv_enabled) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ if (advsm->periodic_adv_data->om_len == 0) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ if (cmd->adv_data_len != 0) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ advsm->periodic_adv_adi = ble_ll_adv_update_did(advsm->periodic_adv_adi);
+ return BLE_ERR_SUCCESS;
+#endif
  default:
  return BLE_ERR_INV_HCI_CMD_PARMS;
  }
@@ -4177,10 +4225,14 @@ ble_ll_adv_periodic_enable(const uint8_t *cmdbuf, uint8_t len)
  return BLE_ERR_UNK_ADV_INDENT;
  }
 
-#if MYNEWT_VAL(BLE_VERSION) >= 53
+#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)
  if (cmd->enable & 0x02) {
  return BLE_ERR_UNSUPPORTED;
- } else if (cmd->enable & 0xfc) {
+ }
+#endif
+
+#if MYNEWT_VAL(BLE_VERSION) >= 53
+ if (cmd->enable & 0xfc) {
  return BLE_ERR_INV_HCI_CMD_PARMS;
  }
 #else
@@ -4189,7 +4241,7 @@ ble_ll_adv_periodic_enable(const uint8_t *cmdbuf, uint8_t len)
  }
 #endif
 
- if (cmd->enable) {
+ if (cmd->enable & 0x1) {
  if (advsm->props & (BLE_HCI_LE_SET_EXT_ADV_PROP_ANON_ADV |
  BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE |
  BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE |
@@ -4207,7 +4259,7 @@ ble_ll_adv_periodic_enable(const uint8_t *cmdbuf, uint8_t len)
 
  /* If Enable is set to 0x01 and the length of the periodic advertising
  * data is greater than the maximum that the Controller can transmit
- * within the chosen periodicadvertising interval, the Controller shall
+ * within the chosen periodic advertising interval, the Controller shall
  * return the error code Packet Too Long (0x45).
  */
  if (!ble_ll_adv_periodic_check_data_itvl(SYNC_DATA_LEN(advsm),
@@ -4217,6 +4269,10 @@ ble_ll_adv_periodic_enable(const uint8_t *cmdbuf, uint8_t len)
  return BLE_ERR_PACKET_TOO_LONG;
  }
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)
+ advsm->periodic_include_adi = !!(cmd->enable & 0x2);
+#endif
+
  /* If the advertising set is not currently enabled (see the
  * LE_Set_Extended_Advertising_Enable command), the periodic advertising
  * is not started until the advertising set is enabled.

nimble/controller/src/ble_ll_conn_hci.c

@@ -2069,11 +2069,18 @@ ble_ll_set_sync_transfer_params(const uint8_t *cmdbuf, uint8_t len,
  goto done;
  }
 
- if ((MYNEWT_VAL(BLE_VERSION) >= 53 ) && (cmd->mode == 0x03)) {
- /* We do not support ADI in periodic advertising thus cannot enable
- * duplicate filtering.
- */
- return BLE_ERR_UNSUPPORTED;
+ if (MYNEWT_VAL(BLE_VERSION) >= 53) {
+ if (cmd->mode > 0x03) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ if ((cmd->mode == 0x03) &&
+ !MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)) {
+ /* We do not support ADI in periodic advertising thus cannot enable
+ * duplicate filtering.
+ */
+ return BLE_ERR_UNSUPPORTED;
+ }
  } else if (cmd->mode > 0x02) {
  return BLE_ERR_INV_HCI_CMD_PARMS;
  }
@@ -2126,11 +2133,18 @@ ble_ll_set_default_sync_transfer_params(const uint8_t *cmdbuf, uint8_t len)
  return BLE_ERR_INV_HCI_CMD_PARMS;
  }
 
- if ((MYNEWT_VAL(BLE_VERSION) >= 53 ) && (cmd->mode == 0x03)) {
- /* We do not support ADI in periodic advertising thus cannot enable
- * duplicate filtering.
- */
- return BLE_ERR_UNSUPPORTED;
+ if (MYNEWT_VAL(BLE_VERSION) >= 53) {
+ if (cmd->mode > 0x03) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ if ((cmd->mode == 0x03) &&
+ !MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)) {
+ /* We do not support ADI in periodic advertising thus cannot enable
+ * duplicate filtering.
+ */
+ return BLE_ERR_UNSUPPORTED;
+ }
  } else if (cmd->mode > 0x02) {
  return BLE_ERR_INV_HCI_CMD_PARMS;
  }

nimble/controller/src/ble_ll_ctrl.c

@@ -1135,11 +1135,9 @@ ble_ll_ctrl_rx_phy_update_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
 static uint8_t
 ble_ll_ctrl_rx_periodic_sync_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
 {
- if (connsm->sync_transfer_mode) {
- ble_ll_sync_periodic_ind(connsm, dptr, connsm->sync_transfer_mode == 1,
- connsm->sync_transfer_skip,
- connsm->sync_transfer_sync_timeout);
- }
+ ble_ll_sync_periodic_ind(connsm, dptr, connsm->sync_transfer_mode,
+ connsm->sync_transfer_skip,
+ connsm->sync_transfer_sync_timeout);
  return BLE_ERR_MAX;
 }
 #endif

nimble/controller/src/ble_ll_sync.c

@@ -64,6 +64,7 @@
 #define BLE_LL_SYNC_SM_FLAG_HCI_TRUNCATED 0x0100
 #define BLE_LL_SYNC_SM_FLAG_NEW_CHANMAP 0x0200
 #define BLE_LL_SYNC_SM_FLAG_CHAIN 0x0400
+#define BLE_LL_SYNC_SM_FLAG_DUPLICATES 0x0800
 
 #define BLE_LL_SYNC_ITVL_USECS 1250
 
@@ -121,6 +122,10 @@ struct ble_ll_sync_sm {
  uint16_t event_cntr_last_received;
  uint8_t adv_addr_rpa[6];
 #endif
+
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)
+ uint16_t prev_adi;
+#endif
 };
 
 static struct ble_ll_sync_sm g_ble_ll_sync_sm[BLE_LL_SYNC_CNT];
@@ -548,7 +553,7 @@ 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,
- uint8_t **acad, uint8_t *acad_len)
+ uint8_t **acad, uint8_t *acad_len, uint16_t **adi)
 {
  uint8_t *rxbuf = om->om_data;
  uint8_t ext_hdr_flags;
@@ -587,8 +592,10 @@ ble_ll_sync_parse_ext_hdr(struct os_mbuf *om, uint8_t **aux, int8_t *tx_power,
  i += BLE_LL_EXT_ADV_CTE_INFO_SIZE;
  }
 
- /* there should be no ADI in Sync or chain, skip it */
  if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) {
+ if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT) {
+ *adi = (uint16_t *) ext_hdr + i;
+ }
  i += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
  }
 
@@ -1137,10 +1144,12 @@ ble_ll_sync_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr)
  uint8_t *aux = NULL;
  uint8_t *acad = NULL;
  uint8_t acad_len = 0;
+ uint16_t *adi = NULL;
  const uint8_t *biginfo = NULL;
  uint8_t biginfo_len = 0;
  int datalen;
  bool reports_enabled;
+ bool is_duplicate = false;
 
  BLE_LL_ASSERT(sm);
 
@@ -1186,14 +1195,22 @@ ble_ll_sync_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr)
  }
 
  /* get ext header data */
- datalen = ble_ll_sync_parse_ext_hdr(rxpdu, &aux, &tx_power, &acad, &acad_len);
+ datalen = ble_ll_sync_parse_ext_hdr(rxpdu, &aux, &tx_power, &acad, &acad_len, &adi);
  if (datalen < 0) {
  /* we got bad packet, end event */
  goto end_event;
  }
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)
+ if (adi != NULL) {
+ is_duplicate = (sm->flags & BLE_LL_SYNC_SM_FLAG_DUPLICATES) && (*adi == sm->prev_adi);
+ sm->prev_adi = *adi;
+ }
+#endif
+
  reports_enabled = ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_PERIODIC_ADV_RPT) &&
- !(sm->flags & BLE_LL_SYNC_SM_FLAG_DISABLED);
+ !(sm->flags & BLE_LL_SYNC_SM_FLAG_DISABLED) &&
+ !is_duplicate;
 
  /* no need to schedule for chain if reporting is disabled */
  if (reports_enabled) {
@@ -1539,6 +1556,12 @@ ble_ll_sync_info_event(struct ble_ll_scan_addr_data *addrd,
  }
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)
+ if (g_ble_ll_sync_create_params.options & BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_DUPLICATES) {
+ sm->flags |= BLE_LL_SYNC_SM_FLAG_DUPLICATES;
+ }
+#endif
+
  sm->flags &= ~BLE_LL_SYNC_SM_FLAG_RESERVED;
  sm->flags |= BLE_LL_SYNC_SM_FLAG_ESTABLISHING;
  sm->flags |= BLE_LL_SYNC_SM_FLAG_SYNC_INFO;
@@ -1583,7 +1606,8 @@ ble_ll_sync_create(const uint8_t *cmdbuf, uint8_t len)
  return BLE_ERR_INV_HCI_CMD_PARMS;
  }
  if (!(cmd->options & BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_DISABLED) &&
- (cmd->options & BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_DUPLICATES)) {
+ (cmd->options & BLE_HCI_LE_PERIODIC_ADV_CREATE_SYNC_OPT_DUPLICATES) &&
+ !MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)) {
  /* We do not support ADI in periodic advertising thus cannot enable
  * duplicate filtering.
  */
@@ -1870,7 +1894,8 @@ ble_ll_sync_receive_enable(const uint8_t *cmdbuf, uint8_t len)
  if (MYNEWT_VAL(BLE_VERSION) >= 53) {
  if (cmd->enable > 0x03) {
  return BLE_ERR_INV_HCI_CMD_PARMS;
- } else if (cmd->enable == 0x03) {
+ } else if ((cmd->enable == 0x03) &&
+ !MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_ADI_SUPPORT)) {
  /* We do not support ADI in periodic advertising thus cannot enable
  * duplicate filtering.
  */
@@ -1898,8 +1923,14 @@ ble_ll_sync_receive_enable(const uint8_t *cmdbuf, uint8_t len)
  return BLE_ERR_UNK_ADV_INDENT;
  }
 
- if (cmd->enable) {
+ if (cmd->enable & 0x1) {
  sm->flags &= ~BLE_LL_SYNC_SM_FLAG_DISABLED;
+
+ if (cmd->enable & 0x2) {
+ sm->flags |= BLE_LL_SYNC_SM_FLAG_DUPLICATES;
+ } else {
+ sm->flags &= ~BLE_LL_SYNC_SM_FLAG_DUPLICATES;
+ }
  } else {
  sm->flags |= BLE_LL_SYNC_SM_FLAG_DISABLED;
  }
@@ -1940,7 +1971,7 @@ ble_ll_sync_transfer_get(const uint8_t *addr, uint8_t addr_type, uint8_t sid)
 
 void
 ble_ll_sync_periodic_ind(struct ble_ll_conn_sm *connsm,
- const uint8_t *sync_ind, bool reports_disabled,
+ const uint8_t *sync_ind, uint8_t mode,
  uint16_t max_skip, uint32_t sync_timeout)
 {
  const uint8_t *syncinfo = sync_ind + 2;
@@ -1966,6 +1997,10 @@ ble_ll_sync_periodic_ind(struct ble_ll_conn_sm *connsm,
  uint8_t sca;
  os_sr_t sr;
 
+ if (!mode) {
+ return;
+ }
+
  phy_mode = ble_ll_ctrl_phy_from_phy_mask(sync_ind[25]);
  itvl = get_le16(syncinfo + 2);
  /* ignore if sync params are not valid */
@@ -2133,8 +2168,15 @@ ble_ll_sync_periodic_ind(struct ble_ll_conn_sm *connsm,
  sm->anchor_point = sm->sch.start_time + g_ble_ll_sched_offset_ticks;
  sm->anchor_point_usecs = sm->sch.remainder;
 
- if (reports_disabled) {
+ switch (mode) {
+ case 0x1:
  sm->flags |= BLE_LL_SYNC_SM_FLAG_DISABLED;
+ break;
+ case 0x3:
+ sm->flags |= BLE_LL_SYNC_SM_FLAG_DUPLICATES;
+ break;
+ default:
+ break;
  }
 }