nimble/ll: Add PA ADI support

This adds ADI support for Periodic Advertising
in controller.

Author: m-gorecki

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;
@@ -2175,6 +2179,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) {
@@ -2356,6 +2366,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
      */
@@ -2613,6 +2630,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);
@@ -2679,6 +2699,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 */
@@ -3636,6 +3664,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;
 
@@ -4141,6 +4172,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;
     }
@@ -4217,10 +4265,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
@@ -4229,7 +4281,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 |
@@ -4247,7 +4299,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),
@@ -4257,6 +4309,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;
     }
 }