[wip] periodic adv adi support

apache · Sep 5, 2024 · 6631330 · 6631330
1 parent baf2930
commit 6631330
Showing 1 changed file with 37 additions and 4 deletions.
diff --git a/nimble/controller/src/ble_ll_adv.c b/nimble/controller/src/ble_ll_adv.c
@@ -167,6 +167,8 @@ struct ble_ll_adv_sm
     uint8_t periodic_adv_active : 1;
     uint8_t periodic_sync_active : 1;
     uint8_t periodic_sync_index : 1;
+    uint8_t periodic_include_adi : 1;
+    uint16_t periodic_adv_adi;
     uint8_t periodic_num_used_chans;
     uint8_t periodic_chanmap[BLE_LL_CHAN_MAP_LEN];
     uint16_t periodic_adv_itvl;
@@ -2152,6 +2154,11 @@ ble_ll_adv_sync_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
         dptr += 1;
     }
 #endif
+    if (sync->ext_hdr_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) {
+        dptr[0] = advsm->periodic_adv_adi & 0x00ff;
+        dptr[1] = advsm->periodic_adv_adi >> 8;
+        dptr += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+    }
 
     if (sync->ext_hdr_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) {
         if (!SYNC_NEXT(advsm)->sch.enqueued) {
@@ -2334,6 +2341,11 @@ ble_ll_adv_sync_calculate(struct ble_ll_adv_sm *advsm,
 
     ext_hdr_len = BLE_LL_EXT_ADV_HDR_LEN;
 
+    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;
+    }
+
     /* TxPower if configured
      * Note: TxPower shall not be present in chain PDU for SYNC
      */
@@ -2564,6 +2576,24 @@ ble_ll_adv_reschedule_periodic_event(struct ble_ll_adv_sm *advsm)
     ble_ll_adv_sync_schedule(advsm, false);
 }
 
+static void
+ble_ll_adv_update_did_periodic(struct ble_ll_adv_sm *advsm)
+{
+    uint16_t old_adi = advsm->periodic_adv_adi;
+
+    /*
+     * The Advertising DID for a given advertising set shall be initialized
+     * with a randomly chosen value. Whenever the Host provides new periodic
+     * advertising data for a given advertising set (whether it is the
+     * same as the previous data or not), the Advertising DID shall be updated.
+     * The new value shall be a randomly chosen value that is not the same as
+     * the previously used value.
+     */
+    do {
+        advsm->periodic_adv_adi = (advsm->periodic_adv_adi & 0xf000) | (ble_ll_rand() & 0x0fff);
+    } while (old_adi == advsm->periodic_adv_adi);
+}
+
 static void
 ble_ll_adv_update_periodic_data(struct ble_ll_adv_sm *advsm)
 {
@@ -2579,6 +2609,7 @@ 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;
+        ble_ll_adv_update_did_periodic(advsm);
     }
 
     ble_ll_adv_flags_clear(advsm, BLE_LL_ADV_SM_FLAG_PERIODIC_NEW_DATA);
@@ -2646,6 +2677,7 @@ ble_ll_adv_sm_start_periodic(struct ble_ll_adv_sm *advsm)
      * train is enabled.
      */
     ble_ll_adv_update_did(advsm);
+    ble_ll_adv_update_did_periodic(advsm);
 
     advsm->periodic_adv_active = 1;
 
@@ -3585,6 +3617,7 @@ 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));
+    advsm->periodic_adv_adi = (advsm->periodic_adv_adi & 0x0fff) | ((cmd->sid << 12));
 
     advsm->props = props;
 
@@ -4115,9 +4148,7 @@ ble_ll_adv_periodic_enable(const uint8_t *cmdbuf, uint8_t len)
     }
 
 #if MYNEWT_VAL(BLE_VERSION) >= 53
-    if (cmd->enable & 0x02) {
-        return BLE_ERR_UNSUPPORTED;
-    } else if (cmd->enable & 0xfc) {
+    if (cmd->enable & 0xfc) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
     }
 #else
@@ -4126,7 +4157,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 |
@@ -4154,6 +4185,8 @@ ble_ll_adv_periodic_enable(const uint8_t *cmdbuf, uint8_t len)
             return BLE_ERR_PACKET_TOO_LONG;
         }
 
+        advsm->periodic_include_adi = ((cmd->enable >> 1) & 0x1);
+
         /* 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.