andrzej-kaczmarek closed pull request #770: nimble/ll: Add support for 
advertising and scan response data fragmentation
URL: https://github.com/apache/mynewt-core/pull/770
 
 
   

This is a PR merged from a forked repository.
As GitHub hides the original diff on merge, it is displayed below for
the sake of provenance:

As this is a foreign pull request (from a fork), the diff is supplied
below (as it won't show otherwise due to GitHub magic):

diff --git a/net/nimble/controller/include/controller/ble_ll.h 
b/net/nimble/controller/include/controller/ble_ll.h
index 97338dc68..c81a79e77 100644
--- a/net/nimble/controller/include/controller/ble_ll.h
+++ b/net/nimble/controller/include/controller/ble_ll.h
@@ -240,8 +240,9 @@ struct ble_dev_addr
 #define BLE_LL_ACC_ADDR_LEN     (4)
 #define BLE_LL_CRC_LEN          (3)
 #define BLE_LL_PDU_HDR_LEN      (2)
+#define BLE_LL_MAX_PAYLOAD_LEN  (255)
 #define BLE_LL_MIN_PDU_LEN      (BLE_LL_PDU_HDR_LEN)
-#define BLE_LL_MAX_PDU_LEN      (257)
+#define BLE_LL_MAX_PDU_LEN      ((BLE_LL_PDU_HDR_LEN) + 
(BLE_LL_MAX_PAYLOAD_LEN))
 #define BLE_LL_CRCINIT_ADV      (0x555555)
 
 /* Access address for advertising channels */
diff --git a/net/nimble/controller/include/controller/ble_ll_sched.h 
b/net/nimble/controller/include/controller/ble_ll_sched.h
index 3545a4008..15f750077 100644
--- a/net/nimble/controller/include/controller/ble_ll_sched.h
+++ b/net/nimble/controller/include/controller/ble_ll_sched.h
@@ -152,10 +152,12 @@ int ble_ll_sched_master_new(struct ble_ll_conn_sm *connsm,
 int ble_ll_sched_slave_new(struct ble_ll_conn_sm *connsm);
 
 struct ble_ll_adv_sm;
-typedef void ble_ll_sched_adv_new_cb(struct ble_ll_adv_sm *advsm, uint32_t 
sch_start);
+typedef void ble_ll_sched_adv_new_cb(struct ble_ll_adv_sm *advsm,
+                                     uint32_t sch_start, void *arg);
 
 /* Schedule a new advertising event */
-int ble_ll_sched_adv_new(struct ble_ll_sched_item *sch, 
ble_ll_sched_adv_new_cb cb);
+int ble_ll_sched_adv_new(struct ble_ll_sched_item *sch,
+                         ble_ll_sched_adv_new_cb cb, void *arg);
 
 /* Reschedule an advertising event */
 int ble_ll_sched_adv_reschedule(struct ble_ll_sched_item *sch, uint32_t *start,
diff --git a/net/nimble/controller/src/ble_ll_adv.c 
b/net/nimble/controller/src/ble_ll_adv.c
index 50ac5334b..f50aea79c 100644
--- a/net/nimble/controller/src/ble_ll_adv.c
+++ b/net/nimble/controller/src/ble_ll_adv.c
@@ -51,6 +51,16 @@
  * that currently.
  */
 
+/* Scheduling data for secondary channel */
+struct ble_ll_adv_aux {
+    struct ble_ll_sched_item sch;
+    uint32_t start_time;
+    uint16_t aux_data_offset;
+    uint8_t ext_hdr;
+    uint8_t aux_data_len;
+    uint8_t payload_len;
+};
+
 /*
  * Advertising state machine
  *
@@ -76,7 +86,6 @@ struct ble_ll_adv_sm
     uint8_t own_addr_type;
     uint8_t peer_addr_type;
     uint8_t adv_chan;
-    uint8_t scan_rsp_len;
     uint8_t adv_pdu_len;
     int8_t adv_rpa_index;
     uint8_t flags;
@@ -94,13 +103,18 @@ struct ble_ll_adv_sm
     uint8_t peer_addr[BLE_DEV_ADDR_LEN];
     uint8_t initiator_addr[BLE_DEV_ADDR_LEN];
     struct os_mbuf *adv_data;
-    uint8_t scan_rsp_data[BLE_SCAN_RSP_DATA_MAX_LEN];
+    struct os_mbuf *scan_rsp_data;
     uint8_t *conn_comp_ev;
     struct os_event adv_txdone_ev;
     struct ble_ll_sched_item adv_sch;
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
-    uint32_t adv_secondary_start_time;
-    struct ble_ll_sched_item adv_secondary_sch;
+    uint8_t aux_active : 1;
+    uint8_t aux_index : 1;
+    uint8_t aux_first_pdu : 1;
+    uint8_t aux_not_scanned : 1;
+    struct ble_mbuf_hdr *rx_ble_hdr;
+    struct os_mbuf **aux_data;
+    struct ble_ll_adv_aux aux[2];
     struct os_event adv_sec_txdone_ev;
     uint16_t duration;
     uint16_t adi;
@@ -122,6 +136,13 @@ struct ble_ll_adv_sm
 
 #define ADV_DATA_LEN(_advsm) \
                 ((_advsm->adv_data) ? OS_MBUF_PKTLEN(advsm->adv_data) : 0)
+#define SCAN_RSP_DATA_LEN(_advsm) \
+                ((_advsm->scan_rsp_data) ? 
OS_MBUF_PKTLEN(advsm->scan_rsp_data) : 0)
+#define AUX_DATA_LEN(_advsm) \
+                (*(_advsm->aux_data) ? OS_MBUF_PKTLEN(*advsm->aux_data) : 0)
+
+#define AUX_CURRENT(_advsm)     (&(_advsm->aux[_advsm->aux_index]))
+#define AUX_NEXT(_advsm)        (&(_advsm->aux[_advsm->aux_index ^ 1]))
 
 static inline int
 ble_ll_adv_active_chanset_is_pri(struct ble_ll_adv_sm *advsm)
@@ -347,52 +368,24 @@ ble_ll_adv_legacy_pdu_make(uint8_t *dptr, void 
*pducb_arg, uint8_t *hdr_byte)
 }
 
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
-/* TODO this shouldn't be needed
- *
- * PDU could be constructed before scheduling and held in mbuf until
- * transmission
- *
- */
-static uint8_t
-ble_ll_adv_secondary_pdu_payload_len(struct ble_ll_adv_sm *advsm)
-{
-    uint8_t len;
-
-    len = BLE_LL_EXT_ADV_HDR_LEN + BLE_LL_EXT_ADV_FLAGS_SIZE +
-          BLE_LL_EXT_ADV_DATA_INFO_SIZE;
-
-    if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) &&
-            ADV_DATA_LEN(advsm)) {
-        len += ADV_DATA_LEN(advsm);
-    }
-
-    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR) {
-        len += BLE_LL_EXT_ADV_TX_POWER_SIZE;
-    }
-
-    if ((advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) ||
-            (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE)) {
-        len += BLE_LL_EXT_ADV_TARGETA_SIZE;
-    }
-
-    return len;
-}
-
-static uint8_t
-ble_ll_adv_aux_scan_rsp_payload_len(struct ble_ll_adv_sm *advsm)
+static void
+ble_ll_adv_put_aux_ptr(struct ble_ll_adv_sm *advsm, uint32_t offset,
+                       uint8_t *dptr)
 {
-    uint8_t len;
+    /* in usecs */
+    offset = os_cputime_ticks_to_usecs(offset);
 
-    len = BLE_LL_EXT_ADV_HDR_LEN + BLE_LL_EXT_ADV_FLAGS_SIZE +
-          BLE_LL_EXT_ADV_ADVA_SIZE;
+    dptr[0] = advsm->adv_secondary_chan;
 
-    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR) {
-        len += BLE_LL_EXT_ADV_TX_POWER_SIZE;
+    if (offset > 245700) {
+        dptr[0] |= 0x80;
+        offset = offset / 300;
+    } else {
+        offset = offset / 30;
     }
 
-    len += advsm->scan_rsp_len;
-
-    return len;
+    dptr[1] = (offset & 0x000000ff);
+    dptr[2] = ((offset >> 8) & 0x0000001f) | (advsm->sec_phy - 1) << 5; //TODO;
 }
 
 /**
@@ -402,15 +395,7 @@ static uint8_t
 ble_ll_adv_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
 {
     struct ble_ll_adv_sm *advsm;
-    uint8_t pdulen;
     uint8_t pdu_type;
-
-    bool adva = false;
-    bool targeta = false;
-    bool adi = false;
-    bool aux_ptr = false;
-    bool tx_power = false;
-    bool adv_data = false;
     uint8_t adv_mode;
     uint8_t ext_hdr_len;
     uint8_t ext_hdr_flags;
@@ -418,152 +403,209 @@ ble_ll_adv_pdu_make(uint8_t *dptr, void *pducb_arg, 
uint8_t *hdr_byte)
 
     advsm = pducb_arg;
 
+    assert(ble_ll_adv_active_chanset_is_pri(advsm));
     if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
         return ble_ll_adv_legacy_pdu_make(dptr, advsm, hdr_byte);
     }
 
-    pdulen = BLE_LL_EXT_ADV_HDR_LEN;
-
-    ext_hdr_len = BLE_LL_EXT_ADV_FLAGS_SIZE;
-    ext_hdr_flags = 0;
-
-    if (ble_ll_adv_active_chanset_is_sec(advsm)) {
-        pdu_type = BLE_ADV_PDU_TYPE_AUX_ADV_IND;
-
-        adi = true;
-        adva = true;
-
-        if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) &&
-                ADV_DATA_LEN(advsm)) {
-            adv_data = true;
-        }
-
-        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR) {
-            tx_power = true;
-        }
-
-        if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) {
-            targeta = true;
-        }
-
-    } else {
-        assert(ble_ll_adv_active_chanset_is_pri(advsm));
+    /* only ADV_EXT_IND goes on primary advertising channels */
+    pdu_type = BLE_ADV_PDU_TYPE_ADV_EXT_IND;
 
-        /* only ADV_EXT_IND goes on primary advertising channels */
-        pdu_type = BLE_ADV_PDU_TYPE_ADV_EXT_IND;
-
-        /* TODO in some cases we could avoid auxiliary packet */
-        aux_ptr = true;
-        adi = true;
+    /* Set TxAdd to random if needed. */
+    if (advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) {
+        pdu_type |= BLE_ADV_PDU_HDR_TXADD_RAND;
     }
 
+    *hdr_byte = pdu_type;
+
     adv_mode = 0;
     if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) {
         adv_mode |= BLE_LL_EXT_ADV_MODE_CONN;
     }
-
     if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) {
-            adv_mode |= BLE_LL_EXT_ADV_MODE_SCAN;
+        adv_mode |= BLE_LL_EXT_ADV_MODE_SCAN;
     }
 
-    if (adva) {
-        ext_hdr_flags |= (1 << BLE_LL_EXT_ADV_ADVA_BIT);
-        ext_hdr_len += BLE_LL_EXT_ADV_ADVA_SIZE;
-    }
+    ext_hdr_len = BLE_LL_EXT_ADV_FLAGS_SIZE + BLE_LL_EXT_ADV_DATA_INFO_SIZE +
+                  BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+    ext_hdr_flags = (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT) |
+                    (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT);
 
-    if (targeta) {
-        ext_hdr_flags |= (1 << BLE_LL_EXT_ADV_TARGETA_BIT);
-        ext_hdr_len += BLE_LL_EXT_ADV_TARGETA_SIZE;
-    }
+    /* ext hdr len and adv mode */
+    dptr[0] = ext_hdr_len | (adv_mode << 6);
+    dptr += 1;
 
-    if (adi) {
-        ext_hdr_flags |= (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT);
-        ext_hdr_len += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
-    }
+    /* ext hdr flags */
+    dptr[0] = ext_hdr_flags;
+    dptr += 1;
 
-    if (aux_ptr) {
-        ext_hdr_flags |= (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT);
-        ext_hdr_len += BLE_LL_EXT_ADV_AUX_PTR_SIZE;
-    }
+    /* ADI */
+    dptr[0] = advsm->adi & 0x00ff;
+    dptr[1] = advsm->adi >> 8;
+    dptr += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
 
-    if (tx_power) {
-        ext_hdr_flags |= (1 << BLE_LL_EXT_ADV_TX_POWER_BIT);
-        ext_hdr_len += BLE_LL_EXT_ADV_TX_POWER_SIZE;
-    }
+    /* AuxPtr */
+    assert(AUX_CURRENT(advsm)->sch.enqueued);
+    offset = AUX_CURRENT(advsm)->start_time - advsm->adv_pdu_start_time;
+    ble_ll_adv_put_aux_ptr(advsm, offset, dptr);
 
-    /* TODO ACAD */
+    return BLE_LL_EXT_ADV_HDR_LEN + ext_hdr_len;
+}
 
-    if (adv_data) {
-        pdulen += ADV_DATA_LEN(advsm);
-    }
+/**
+ * Create the AUX PDU
+ */
+static uint8_t
+ble_ll_adv_aux_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
+{
+    struct ble_ll_adv_sm *advsm;
+    struct ble_ll_adv_aux *aux;
+    uint8_t adv_mode;
+    uint8_t pdu_type;
+    uint8_t ext_hdr_len;
+    uint32_t offset;
+
+    advsm = pducb_arg;
+    aux = AUX_CURRENT(advsm);
+
+    assert(!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY));
+    assert(ble_ll_adv_active_chanset_is_sec(advsm));
+
+    /* It's the same for AUX_ADV_IND and AUX_CHAIN_IND */
+    pdu_type = BLE_ADV_PDU_TYPE_AUX_ADV_IND;
 
     /* Set TxAdd to random if needed. */
     if (advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) {
         pdu_type |= BLE_ADV_PDU_HDR_TXADD_RAND;
     }
 
-    pdulen += ext_hdr_len;
-
-    /* Set the PDU length in the state machine (includes header) */
-    advsm->adv_pdu_len = pdulen + BLE_LL_PDU_HDR_LEN;
-
     *hdr_byte = pdu_type;
 
-    /* ext hdr len and adv mode */
-    dptr[0] = ext_hdr_len | (adv_mode << 6);
+    /* We do not create scannable PDUs here - this is handled separately */
+    adv_mode = 0;
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) {
+        adv_mode |= BLE_LL_EXT_ADV_MODE_CONN;
+    }
+
+    ext_hdr_len = aux->payload_len - BLE_LL_EXT_ADV_HDR_LEN - 
aux->aux_data_len;
+    dptr[0] = (adv_mode << 6) | ext_hdr_len;
     dptr += 1;
 
-    /* ext hdr flags */
-    dptr[0] = ext_hdr_flags;
+    dptr[0] = aux->ext_hdr;
     dptr += 1;
 
-    if (adva) {
+    if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) {
         memcpy(dptr, advsm->adva, BLE_LL_EXT_ADV_ADVA_SIZE);
         dptr += BLE_LL_EXT_ADV_ADVA_SIZE;
     }
 
-    if (targeta) {
+    if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) {
         memcpy(dptr, advsm->initiator_addr, BLE_LL_EXT_ADV_TARGETA_SIZE);
         dptr += BLE_LL_EXT_ADV_TARGETA_SIZE;
     }
 
-    if (adi) {
+    if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) {
         dptr[0] = advsm->adi & 0x00ff;
         dptr[1] = advsm->adi >> 8;
         dptr += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
     }
 
-    if (aux_ptr) {
-        offset = advsm->adv_secondary_start_time - advsm->adv_pdu_start_time;
-
-        /* in usecs */
-        offset = os_cputime_ticks_to_usecs(offset);
+    if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) {
+        assert(AUX_NEXT(advsm)->sch.enqueued);
 
-        dptr[0] = advsm->adv_secondary_chan;
-
-        if (offset > 245700) {
-            dptr[0] |= 0x80;
-            offset = offset / 300;
+        if (advsm->rx_ble_hdr) {
+            offset = advsm->rx_ble_hdr->rem_usecs +
+                     ble_ll_pdu_tx_time_get(12, advsm->sec_phy) + BLE_LL_IFS + 
30;
+            offset = AUX_NEXT(advsm)->start_time - 
advsm->rx_ble_hdr->beg_cputime -
+                     os_cputime_usecs_to_ticks(offset);
         } else {
-            offset = offset / 30;
+            offset = AUX_NEXT(advsm)->start_time - aux->start_time;
         }
 
-        dptr[1] = (offset & 0x000000ff);
-        dptr[2] = ((offset >> 8) & 0x0000001f) | (advsm->sec_phy - 1) << 5; 
//TODO;
+        ble_ll_adv_put_aux_ptr(advsm, offset, dptr);
 
         dptr += BLE_LL_EXT_ADV_AUX_PTR_SIZE;
     }
 
-    if (tx_power) {
+    if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_TX_POWER_BIT)) {
         dptr[0] = advsm->adv_txpwr;
         dptr += BLE_LL_EXT_ADV_TX_POWER_SIZE;
     }
 
-    if (adv_data) {
-        os_mbuf_copydata(advsm->adv_data, 0, ADV_DATA_LEN(advsm), dptr);
-        dptr += ADV_DATA_LEN(advsm);
+    if (aux->aux_data_len) {
+        os_mbuf_copydata(*advsm->aux_data, aux->aux_data_offset,
+                         aux->aux_data_len, dptr);
     }
 
+    return aux->payload_len;
+}
+
+static uint8_t
+ble_ll_adv_aux_scannable_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t 
*hdr_byte)
+{
+    struct ble_ll_adv_sm *advsm;
+    uint8_t pdu_type;
+    uint8_t *ext_hdr_len;
+    uint8_t *ext_hdr;
+    uint8_t pdulen;
+
+    advsm = pducb_arg;
+
+    assert(!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY));
+    assert(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE);
+    assert(advsm->aux_first_pdu);
+    assert(ble_ll_adv_active_chanset_is_sec(advsm));
+
+    pdu_type = BLE_ADV_PDU_TYPE_AUX_ADV_IND;
+
+    /* Set TxAdd to random if needed. */
+    if (advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) {
+        pdu_type |= BLE_ADV_PDU_HDR_TXADD_RAND;
+    }
+
+    *hdr_byte = pdu_type;
+
+    ext_hdr_len = &dptr[0];
+    ext_hdr = &dptr[1];
+    dptr += 2;
+
+    /* Flags always */
+    *ext_hdr_len = BLE_LL_EXT_ADV_FLAGS_SIZE;
+    *ext_hdr = 0;
+
+    /* AdvA always */
+    *ext_hdr_len += BLE_LL_EXT_ADV_ADVA_SIZE;
+    *ext_hdr |= (1 << BLE_LL_EXT_ADV_ADVA_BIT);
+    memcpy(dptr, advsm->adva, BLE_LL_EXT_ADV_ADVA_SIZE);
+    dptr += BLE_LL_EXT_ADV_ADVA_SIZE;
+
+    /* TargetA only for directed */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) {
+        *ext_hdr_len += BLE_LL_EXT_ADV_TARGETA_SIZE;
+        *ext_hdr |= (1 << BLE_LL_EXT_ADV_TARGETA_BIT);
+        memcpy(dptr, advsm->initiator_addr, BLE_LL_EXT_ADV_TARGETA_SIZE);
+        dptr += BLE_LL_EXT_ADV_TARGETA_SIZE;
+    }
+
+    /* ADI always */
+    *ext_hdr_len += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+    *ext_hdr |= (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT);
+    dptr[0] = advsm->adi & 0x00ff;
+    dptr[1] = advsm->adi >> 8;
+    dptr += BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+
+    /* TxPower if configured */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR) {
+        *ext_hdr_len += BLE_LL_EXT_ADV_TX_POWER_SIZE;
+        *ext_hdr |= (1 << BLE_LL_EXT_ADV_TX_POWER_BIT);
+        dptr[0] = advsm->adv_txpwr;
+        dptr += BLE_LL_EXT_ADV_TX_POWER_SIZE;
+    }
+
+    pdulen = BLE_LL_EXT_ADV_HDR_LEN + *ext_hdr_len;
+
+    *ext_hdr_len |= (BLE_LL_EXT_ADV_MODE_SCAN << 6);
+
     return pdulen;
 }
 #endif
@@ -580,7 +622,7 @@ ble_ll_adv_scan_rsp_legacy_pdu_make(uint8_t *dptr, void 
*pducb_arg,
     advsm = pducb_arg;
 
     /* Make sure that the length is valid */
-    scan_rsp_len = advsm->scan_rsp_len;
+    scan_rsp_len = SCAN_RSP_DATA_LEN(advsm);
     assert(scan_rsp_len <= BLE_SCAN_RSP_LEGACY_DATA_MAX_LEN);
 
     /* Set BLE transmit header */
@@ -603,7 +645,8 @@ ble_ll_adv_scan_rsp_legacy_pdu_make(uint8_t *dptr, void 
*pducb_arg,
     /* Construct scan response */
     memcpy(dptr, advsm->adva, BLE_DEV_ADDR_LEN);
     if (scan_rsp_len != 0) {
-        memcpy(dptr + BLE_DEV_ADDR_LEN, advsm->scan_rsp_data, scan_rsp_len);
+        os_mbuf_copydata(advsm->scan_rsp_data, 0, scan_rsp_len,
+                         dptr + BLE_DEV_ADDR_LEN);
     }
 
     return pdulen;
@@ -619,10 +662,6 @@ static uint8_t
 ble_ll_adv_scan_rsp_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte)
 {
     struct ble_ll_adv_sm *advsm;
-    uint8_t     pdulen;
-    uint8_t     ext_hdr_len;
-    uint8_t     ext_hdr_flags;
-    uint8_t     hdr;
 
     advsm = pducb_arg;
 
@@ -630,50 +669,7 @@ ble_ll_adv_scan_rsp_pdu_make(uint8_t *dptr, void 
*pducb_arg, uint8_t *hdr_byte)
         return ble_ll_adv_scan_rsp_legacy_pdu_make(dptr, pducb_arg, hdr_byte);
     }
 
-    ext_hdr_len = BLE_LL_EXT_ADV_FLAGS_SIZE + BLE_LL_EXT_ADV_ADVA_SIZE;
-    ext_hdr_flags = (1 << BLE_LL_EXT_ADV_ADVA_BIT);
-
-    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR) {
-        ext_hdr_len += BLE_LL_EXT_ADV_TX_POWER_SIZE;
-    }
-
-    pdulen = BLE_LL_EXT_ADV_HDR_LEN + ext_hdr_len + advsm->scan_rsp_len;
-
-    /* Set BLE transmit header */
-    hdr = BLE_ADV_PDU_TYPE_AUX_SCAN_RSP;
-    if (advsm->flags & BLE_LL_ADV_SM_FLAG_TX_ADD) {
-        hdr |= BLE_ADV_PDU_HDR_TXADD_RAND;
-    }
-
-    *hdr_byte = hdr;
-
-    /* ext hdr len and adv mode (00b) */
-    dptr[0] = ext_hdr_len;
-    dptr += 1;
-
-    /* ext hdr flags */
-    dptr[0] = ext_hdr_flags;
-    dptr += 1;
-
-    /*
-     * The adva in this packet will be the same one that was being advertised
-     * and is based on the peer identity address in the set advertising
-     * parameters. If a different peer sends us a scan request (for some 
reason)
-     * we will reply with an adva that was not generated based on the local irk
-     * of the peer sending the scan request.
-     */
-    memcpy(dptr, advsm->adva, BLE_LL_EXT_ADV_ADVA_SIZE);
-    dptr += BLE_LL_EXT_ADV_ADVA_SIZE;
-
-    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR) {
-        dptr[0] = advsm->adv_txpwr;
-        dptr += BLE_LL_EXT_ADV_TX_POWER_SIZE;
-    }
-
-    memcpy(dptr, advsm->scan_rsp_data, advsm->scan_rsp_len);
-    dptr += advsm->scan_rsp_len;
-
-    return pdulen;
+    return ble_ll_adv_aux_pdu_make(dptr, pducb_arg, hdr_byte);
 }
 
 struct aux_conn_rsp_data {
@@ -951,6 +947,7 @@ ble_ll_adv_secondary_tx_start_cb(struct ble_ll_sched_item 
*sch)
     uint8_t end_trans;
     uint32_t txstart;
     struct ble_ll_adv_sm *advsm;
+    ble_phy_tx_pducb_t pducb;
 
     /* Get the state machine for the event */
     advsm = (struct ble_ll_adv_sm *)sch->cb_arg;
@@ -995,17 +992,23 @@ ble_ll_adv_secondary_tx_start_cb(struct ble_ll_sched_item 
*sch)
 #endif
 
     /* Set phy mode based on type of advertisement */
-    if ((advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) ||
-        (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE)) {
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) {
         end_trans = BLE_PHY_TRANSITION_TX_RX;
         ble_phy_set_txend_cb(NULL, NULL);
+        pducb = ble_ll_adv_aux_pdu_make;
+    } else if ((advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) &&
+               advsm->aux_first_pdu) {
+        end_trans = BLE_PHY_TRANSITION_TX_RX;
+        ble_phy_set_txend_cb(NULL, NULL);
+        pducb = ble_ll_adv_aux_scannable_pdu_make;
     } else {
         end_trans = BLE_PHY_TRANSITION_NONE;
         ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm);
+        pducb = ble_ll_adv_aux_pdu_make;
     }
 
     /* Transmit advertisement */
-    rc = ble_phy_tx(ble_ll_adv_pdu_make, advsm, end_trans);
+    rc = ble_phy_tx(pducb, advsm, end_trans);
     if (rc) {
         goto adv_tx_done;
     }
@@ -1030,16 +1033,164 @@ ble_ll_adv_secondary_tx_start_cb(struct 
ble_ll_sched_item *sch)
     return BLE_LL_SCHED_STATE_DONE;
 }
 
+static uint8_t
+ble_ll_adv_aux_scannable_pdu_payload_len(struct ble_ll_adv_sm *advsm)
+{
+    uint8_t len;
+
+    /* Flags, AdvA and ADI always */
+    len = BLE_LL_EXT_ADV_HDR_LEN + BLE_LL_EXT_ADV_FLAGS_SIZE +
+          BLE_LL_EXT_ADV_ADVA_SIZE + BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+
+    /* TargetA only for directed */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) {
+        len += BLE_LL_EXT_ADV_TARGETA_SIZE;
+    }
+
+    /* TxPower if configured */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR) {
+        len += BLE_LL_EXT_ADV_TX_POWER_SIZE;
+    }
+
+    return len;
+}
+
+static void
+ble_ll_adv_aux_calculate(struct ble_ll_adv_sm *advsm,
+                         struct ble_ll_adv_aux *aux, uint16_t aux_data_offset)
+{
+    uint16_t rem_aux_data_len;
+    uint8_t hdr_len;
+    bool chainable;
+
+    assert(!aux->sch.enqueued);
+    assert((AUX_DATA_LEN(advsm) > aux_data_offset) ||
+           (AUX_DATA_LEN(advsm) == 0 && aux_data_offset == 0));
+
+    aux->aux_data_offset = aux_data_offset;
+    aux->aux_data_len = 0;
+    aux->payload_len = 0;
+
+    rem_aux_data_len = AUX_DATA_LEN(advsm) - aux_data_offset;
+    chainable = !(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE);
+
+    /* Flags and ADI */
+    aux->ext_hdr = (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT);
+    hdr_len = BLE_LL_EXT_ADV_HDR_LEN + BLE_LL_EXT_ADV_FLAGS_SIZE +
+              BLE_LL_EXT_ADV_DATA_INFO_SIZE;
+
+    /* AdvA for 1st PDU in chain (i.e. AUX_ADV_IND or AUX_SCAN_RSP) */
+    if (aux_data_offset == 0) {
+        aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_ADVA_BIT);
+        hdr_len += BLE_LL_EXT_ADV_ADVA_SIZE;
+    }
+
+    /* TargetA for directed connectable */
+    if ((advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) &&
+        (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE)) {
+        aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_TARGETA_BIT);
+        hdr_len += BLE_LL_EXT_ADV_TARGETA_SIZE;
+    }
+
+    /* TxPower if configured */
+    if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR) {
+        aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_TX_POWER_BIT);
+        hdr_len += BLE_LL_EXT_ADV_TX_POWER_SIZE;
+    }
+
+    /* AdvData always */
+    aux->aux_data_len = min(BLE_LL_MAX_PAYLOAD_LEN - hdr_len, 
rem_aux_data_len);
+
+    /* AuxPtr if there are more AdvData remaining that we can fit here */
+    if (chainable && (rem_aux_data_len > aux->aux_data_len)) {
+            aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT);
+            hdr_len += BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+            aux->aux_data_len -= BLE_LL_EXT_ADV_AUX_PTR_SIZE;
+
+            /* PDU payload should be full if chained */
+            assert(hdr_len + aux->aux_data_len == BLE_LL_MAX_PAYLOAD_LEN);
+    }
+
+    aux->payload_len = hdr_len + aux->aux_data_len;
+}
+
 static void
-ble_ll_adv_secondary_set_sched(struct ble_ll_adv_sm *advsm)
+ble_ll_adv_aux_scheduled(struct ble_ll_adv_sm *advsm, uint32_t sch_start,
+                         void *arg)
 {
+    struct ble_ll_adv_aux *aux = arg;
+
+    aux->start_time = sch_start + g_ble_ll_sched_offset_ticks;
+}
+
+static void
+ble_ll_adv_aux_schedule_next(struct ble_ll_adv_sm *advsm)
+{
+    struct ble_ll_adv_aux *aux;
+    struct ble_ll_adv_aux *aux_next;
+    struct ble_ll_sched_item *sch;
+    uint16_t rem_aux_data_len;
+    uint16_t next_aux_data_offset;
     uint32_t max_usecs;
+
+    assert(advsm->aux_active);
+
+    aux = AUX_CURRENT(advsm);
+    aux_next = AUX_NEXT(advsm);
+
+    assert(aux->sch.enqueued);
+    assert(!aux_next->sch.enqueued);
+
+    /*
+     * In general we do not schedule next aux if current aux does not have
+     * AuxPtr in extended header as this means we do not need subsequent
+     * ADV_CHAIN_IND to be sent.
+     * However, if current aux is scannable we allow to schedule next aux as
+     * this will be 1st ADV_CHAIN_IND of scan response.
+     */
+    if (!(aux->ext_hdr & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT))) {
+        return;
+    }
+
+    next_aux_data_offset = aux->aux_data_offset + aux->aux_data_len;
+
+    assert(AUX_DATA_LEN(advsm) >= next_aux_data_offset);
+
+    rem_aux_data_len = AUX_DATA_LEN(advsm) - next_aux_data_offset;
+    assert(rem_aux_data_len > 0);
+
+    ble_ll_adv_aux_calculate(advsm, aux_next, next_aux_data_offset);
+    max_usecs = ble_ll_pdu_tx_time_get(aux_next->payload_len, advsm->sec_phy);
+    max_usecs += XCVR_PROC_DELAY_USECS;
+
+    aux_next->start_time = aux->sch.end_time +
+                          ble_ll_usecs_to_ticks_round_up(BLE_LL_MAFS);
+
+    sch = &aux_next->sch;
+    sch->start_time = aux_next->start_time - g_ble_ll_sched_offset_ticks;
+    sch->remainder = 0;
+    sch->end_time = aux_next->start_time + 
os_cputime_usecs_to_ticks(max_usecs);
+    ble_ll_sched_adv_new(&aux_next->sch, ble_ll_adv_aux_scheduled, aux_next);
+}
+
+static void
+ble_ll_adv_aux_schedule_first(struct ble_ll_adv_sm *advsm)
+{
+    struct ble_ll_adv_aux *aux;
     struct ble_ll_sched_item *sch;
+    uint32_t max_usecs;
 
-    sch = &advsm->adv_secondary_sch;
-    sch->cb_arg = advsm;
-    sch->sched_cb = ble_ll_adv_secondary_tx_start_cb;
-    sch->sched_type = BLE_LL_SCHED_TYPE_ADV;
+    assert(!advsm->aux_active);
+    assert(!advsm->aux[0].sch.enqueued);
+    assert(!advsm->aux[1].sch.enqueued);
+
+    advsm->aux_active = 1;
+    advsm->aux_index = 0;
+    advsm->aux_first_pdu = 1;
+    advsm->aux_not_scanned = 0;
+
+    aux = AUX_CURRENT(advsm);
+    ble_ll_adv_aux_calculate(advsm, aux, 0);
 
     /* TODO we could use CSA2 for this
      * (will be needed for periodic advertising anyway)
@@ -1047,24 +1198,26 @@ ble_ll_adv_secondary_set_sched(struct ble_ll_adv_sm 
*advsm)
     advsm->adv_secondary_chan = rand() % BLE_PHY_NUM_DATA_CHANS;
 
     /* Set end time to maximum time this schedule item may take */
-    max_usecs = 
ble_ll_pdu_tx_time_get(ble_ll_adv_secondary_pdu_payload_len(advsm),
-                                       advsm->sec_phy);
-
     if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE) {
-        max_usecs += BLE_LL_IFS +
-                     /* AUX_CONN_REQ */
-                     ble_ll_pdu_tx_time_get(34 + 14, advsm->sec_phy)  +
-                     BLE_LL_IFS +
-                     /* AUX_CONN_RSP */
-                     ble_ll_pdu_tx_time_get(14, advsm->sec_phy);
+        max_usecs = ble_ll_pdu_tx_time_get(aux->payload_len, advsm->sec_phy) +
+                    BLE_LL_IFS +
+                    /* AUX_CONN_REQ */
+                    ble_ll_pdu_tx_time_get(34 + 14, advsm->sec_phy)  +
+                    BLE_LL_IFS +
+                    /* AUX_CONN_RSP */
+                    ble_ll_pdu_tx_time_get(14, advsm->sec_phy);
     } else if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) {
-        max_usecs += BLE_LL_IFS +
-                     /* AUX_SCAN_REQ */
-                     ble_ll_pdu_tx_time_get(12, advsm->sec_phy)  +
-                     BLE_LL_IFS +
-                     /* AUX_SCAN_RSP */
-                     
ble_ll_pdu_tx_time_get(ble_ll_adv_aux_scan_rsp_payload_len(advsm),
-                                            advsm->sec_phy);
+        /* Scheduled aux is calculated for AUX_SCAN_RSP, 1st aux is created 
separately */
+        max_usecs = 
ble_ll_pdu_tx_time_get(ble_ll_adv_aux_scannable_pdu_payload_len(advsm),
+                                           advsm->sec_phy) +
+                    BLE_LL_IFS +
+                    /* AUX_SCAN_REQ */
+                    ble_ll_pdu_tx_time_get(12, advsm->sec_phy)  +
+                    BLE_LL_IFS +
+                    /* AUX_SCAN_RSP */
+                    ble_ll_pdu_tx_time_get(aux->payload_len, advsm->sec_phy);
+    } else {
+        max_usecs = ble_ll_pdu_tx_time_get(aux->payload_len, advsm->sec_phy);
     }
 
     /*
@@ -1073,10 +1226,73 @@ ble_ll_adv_secondary_set_sched(struct ble_ll_adv_sm 
*advsm)
      */
     max_usecs += XCVR_PROC_DELAY_USECS;
 
-    sch->start_time = advsm->adv_secondary_start_time - 
g_ble_ll_sched_offset_ticks;
+    sch = &aux->sch;
+    sch->start_time = aux->start_time - g_ble_ll_sched_offset_ticks;
     sch->remainder = 0;
-    sch->end_time = advsm->adv_secondary_start_time +
-        os_cputime_usecs_to_ticks(max_usecs);
+    sch->end_time = aux->start_time + os_cputime_usecs_to_ticks(max_usecs);
+    ble_ll_sched_adv_new(sch, ble_ll_adv_aux_scheduled, aux);
+
+}
+
+static void
+ble_ll_adv_aux_set_start_time(struct ble_ll_adv_sm *advsm)
+{
+    static const uint8_t bits[8] = {0, 1, 1, 2, 1, 2, 2, 3};
+    struct ble_ll_sched_item *sched = &advsm->adv_sch;
+    uint32_t adv_pdu_dur;
+    uint32_t adv_event_dur;
+    uint8_t chans;
+
+    assert(!advsm->aux_active);
+    assert(!advsm->aux[0].sch.enqueued);
+    assert(!advsm->aux[1].sch.enqueued);
+
+    assert(advsm->adv_chanmask > 0 &&
+           advsm->adv_chanmask <= BLE_HCI_ADV_CHANMASK_DEF);
+
+    chans = bits[advsm->adv_chanmask];
+
+    /*
+     * We want to schedule auxiliary packet as soon as possible after the end
+     * of advertising event, but no sooner than T_MAFS. The interval between
+     * advertising packets is 250 usecs (8.19 ticks) on LE Coded and a bit less
+     * on 1M, but it can vary a bit due to scheduling which we can't really
+     * control. Since we round ticks up for both interval and T_MAFS, we still
+     * have some margin here. The worst thing that can happen is that we skip
+     * last advertising packet which is not a bit problem so leave it as-is, no
+     * need to make code more complicated.
+     */
+
+    /*
+     * XXX: this could be improved if phy has TX-TX transition with controlled
+     *      or predefined interval, but since it makes advertising code even
+     *      more complicated let's skip it for now...
+     */
+
+    adv_pdu_dur = (int32_t)(sched->end_time - sched->start_time) -
+                  g_ble_ll_sched_offset_ticks;
+
+    /* 9 is 8.19 ticks rounded up - see comment above */
+    adv_event_dur = (adv_pdu_dur * chans) + (9 * (chans - 1));
+
+    advsm->aux[0].start_time = advsm->adv_event_start_time + adv_event_dur +
+                               ble_ll_usecs_to_ticks_round_up(BLE_LL_MAFS);
+}
+
+static void
+ble_ll_adv_aux_schedule(struct ble_ll_adv_sm *advsm)
+{
+    /*
+     * For secondary channel we always start by scheduling two consecutive
+     * auxiliary packets at once. Then, after sending one packet we try to
+     * schedule another one as long as there are some data left to send. This
+     * is to make sure we can always calculate AuxPtr to subsequent packet
+     * without need to scheduled it in an interrupt.
+     */
+
+    ble_ll_adv_aux_set_start_time(advsm);
+    ble_ll_adv_aux_schedule_first(advsm);
+    ble_ll_adv_aux_schedule_next(advsm);
 }
 #endif
 
@@ -1258,7 +1474,9 @@ ble_ll_adv_sm_stop(struct ble_ll_adv_sm *advsm)
         /* Remove any scheduled advertising items */
         ble_ll_sched_rmv_elem(&advsm->adv_sch);
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
-        ble_ll_sched_rmv_elem(&advsm->adv_secondary_sch);
+        advsm->aux_active = 0;
+        ble_ll_sched_rmv_elem(&advsm->aux[0].sch);
+        ble_ll_sched_rmv_elem(&advsm->aux[1].sch);
 #endif
 
         /* Set to standby if we are no longer advertising */
@@ -1304,7 +1522,7 @@ ble_ll_adv_sm_stop(struct ble_ll_adv_sm *advsm)
 }
 
 static void
-ble_ll_adv_scheduled(struct ble_ll_adv_sm *advsm, uint32_t sch_start)
+ble_ll_adv_scheduled(struct ble_ll_adv_sm *advsm, uint32_t sch_start, void 
*arg)
 {
     /* The event start time is when we start transmission of the adv PDU */
     advsm->adv_event_start_time = sch_start + g_ble_ll_sched_offset_ticks;
@@ -1329,53 +1547,6 @@ ble_ll_adv_scheduled(struct ble_ll_adv_sm *advsm, 
uint32_t sch_start)
 }
 
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
-static void
-ble_ll_set_adv_secondary_start_time(struct ble_ll_adv_sm *advsm)
-{
-    static const uint8_t bits[8] = {0, 1, 1, 2, 1, 2, 2, 3};
-    struct ble_ll_sched_item *sched = &advsm->adv_sch;
-    uint32_t adv_pdu_dur;
-    uint32_t adv_event_dur;
-    uint8_t chans;
-
-    assert(advsm->adv_chanmask > 0 &&
-           advsm->adv_chanmask <= BLE_HCI_ADV_CHANMASK_DEF);
-
-    chans = bits[advsm->adv_chanmask];
-
-    /*
-     * We want to schedule auxiliary packet as soon as possible after the end
-     * of advertising event, but no sooner than T_MAFS. The interval between
-     * advertising packets is 250 usecs (8.19 ticks) on LE Coded and a bit less
-     * on 1M, but it can vary a bit due to scheduling which we can't really
-     * control. Since we round ticks up for both interval and T_MAFS, we still
-     * have some margin here. The worst thing that can happen is that we skip
-     * last advertising packet which is not a bit problem so leave it as-is, no
-     * need to make code more complicated.
-     */
-
-    /*
-     * XXX: this could be improved if phy has TX-TX transition with controlled
-     *      or predefined interval, but since it makes advertising code even
-     *      more complicated let's skip it for now...
-     */
-
-    adv_pdu_dur = (int32_t)(sched->end_time - sched->start_time) -
-                  g_ble_ll_sched_offset_ticks;
-
-    /* 9 is 8.19 ticks rounded up - see comment above */
-    adv_event_dur = (adv_pdu_dur * chans) + (9 * (chans - 1));
-
-    advsm->adv_secondary_start_time = advsm->adv_event_start_time +
-                                      adv_event_dur +
-                                      
ble_ll_usecs_to_ticks_round_up(BLE_LL_MAFS);
-}
-
-static void
-ble_ll_adv_secondary_scheduled(struct ble_ll_adv_sm *advsm, uint32_t sch_start)
-{
-    advsm->adv_secondary_start_time = sch_start + g_ble_ll_sched_offset_ticks;
-}
 #endif
 
 /**
@@ -1487,14 +1658,11 @@ ble_ll_adv_sm_start(struct ble_ll_adv_sm *advsm)
      * times to the earliest possible start/end.
      */
     ble_ll_adv_set_sched(advsm);
-    ble_ll_sched_adv_new(&advsm->adv_sch, ble_ll_adv_scheduled);
+    ble_ll_sched_adv_new(&advsm->adv_sch, ble_ll_adv_scheduled, NULL);
 
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
     if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY)) {
-        ble_ll_set_adv_secondary_start_time(advsm);
-        ble_ll_adv_secondary_set_sched(advsm);
-        ble_ll_sched_adv_new(&advsm->adv_secondary_sch,
-                             ble_ll_adv_secondary_scheduled);
+        ble_ll_adv_aux_schedule(advsm);
     }
 #endif
 
@@ -1573,6 +1741,44 @@ ble_ll_adv_set_enable(uint8_t instance, uint8_t enable, 
int duration,
     return rc;
 }
 
+static void
+ble_ll_adv_update_data_mbuf(struct os_mbuf **omp, bool new_data, uint16_t 
maxlen,
+                            const void *data, uint16_t datalen)
+{
+    struct os_mbuf *om;
+    int ret;
+
+    om = *omp;
+
+    if (new_data) {
+        if (om) {
+            os_mbuf_free_chain(om);
+        }
+
+        om = os_msys_get_pkthdr(datalen, 0);
+        if (!om) {
+            goto done;
+        }
+    }
+
+    assert(om);
+
+    if (OS_MBUF_PKTLEN(om) + datalen > maxlen) {
+        os_mbuf_free_chain(om);
+        om = NULL;
+        goto done;
+    }
+
+    ret = os_mbuf_append(om, data, datalen);
+    if (ret) {
+        os_mbuf_free_chain(om);
+        om = NULL;
+    }
+
+done:
+    *omp = om;
+}
+
 /**
  * Set the scan response data that the controller will send.
  *
@@ -1585,8 +1791,8 @@ int
 ble_ll_adv_set_scan_rsp_data(uint8_t *cmd, uint8_t instance, uint8_t operation)
 {
     uint8_t datalen;
-    uint8_t off = 0;
     struct ble_ll_adv_sm *advsm;
+    bool new_data;
 
     if (instance >= BLE_ADV_INSTANCES) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
@@ -1614,6 +1820,10 @@ ble_ll_adv_set_scan_rsp_data(uint8_t *cmd, uint8_t 
instance, uint8_t operation)
         /* TODO mark scan rsp as complete? */
         /* fall through */
     case BLE_HCI_LE_SET_EXT_SCAN_RSP_DATA_OPER_INT:
+        if (!advsm->scan_rsp_data) {
+            return BLE_ERR_INV_HCI_CMD_PARMS;
+        }
+
         if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
             return BLE_ERR_INV_HCI_CMD_PARMS;
         }
@@ -1625,8 +1835,6 @@ ble_ll_adv_set_scan_rsp_data(uint8_t *cmd, uint8_t 
instance, uint8_t operation)
         if (!datalen) {
             return BLE_ERR_INV_HCI_CMD_PARMS;
         }
-
-        off = advsm->scan_rsp_len;
         break;
     case BLE_HCI_LE_SET_EXT_SCAN_RSP_DATA_OPER_FIRST:
         if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
@@ -1647,9 +1855,12 @@ ble_ll_adv_set_scan_rsp_data(uint8_t *cmd, uint8_t 
instance, uint8_t operation)
         return BLE_ERR_INV_HCI_CMD_PARMS;
     }
 
-    /* Check for valid advertising data length */
-    if (datalen + off > BLE_SCAN_RSP_DATA_MAX_LEN) {
-        advsm->scan_rsp_len = 0;
+    new_data = (operation == BLE_HCI_LE_SET_EXT_SCAN_RSP_DATA_OPER_COMPLETE) ||
+               (operation == BLE_HCI_LE_SET_EXT_SCAN_RSP_DATA_OPER_FIRST);
+
+    ble_ll_adv_update_data_mbuf(&advsm->scan_rsp_data, new_data,
+                                BLE_SCAN_RSP_DATA_MAX_LEN, cmd + 1, datalen);
+    if (!advsm->scan_rsp_data) {
         return BLE_ERR_MEM_CAPACITY;
     }
 
@@ -1658,10 +1869,6 @@ ble_ll_adv_set_scan_rsp_data(uint8_t *cmd, uint8_t 
instance, uint8_t operation)
     advsm->adi = (advsm->adi & 0xf000) | (rand() & 0x0fff);
 #endif
 
-    /* Copy the new data into the advertising structure. */
-    advsm->scan_rsp_len = datalen + off;
-    memcpy(advsm->scan_rsp_data + off, cmd + 1, datalen);
-
     return BLE_ERR_SUCCESS;
 }
 
@@ -1679,7 +1886,7 @@ ble_ll_adv_set_adv_data(uint8_t *cmd, uint8_t instance, 
uint8_t operation)
 {
     uint8_t datalen;
     struct ble_ll_adv_sm *advsm;
-    int ret;
+    bool new_data;
 
     if (instance >= BLE_ADV_INSTANCES) {
         return BLE_ERR_INV_HCI_CMD_PARMS;
@@ -1762,25 +1969,12 @@ ble_ll_adv_set_adv_data(uint8_t *cmd, uint8_t instance, 
uint8_t operation)
         return BLE_ERR_INV_HCI_CMD_PARMS;
     }
 
-    /* Need to allocate new mbuf if this is beginning of new data */
-    if (operation == BLE_HCI_LE_SET_EXT_ADV_DATA_OPER_COMPLETE ||
-                        operation == BLE_HCI_LE_SET_EXT_ADV_DATA_OPER_FIRST) {
-        if (advsm->adv_data) {
-            os_mbuf_free_chain(advsm->adv_data);
-        }
-
-        advsm->adv_data = os_msys_get_pkthdr(datalen, 0);
-        if (!advsm->adv_data) {
-            return BLE_ERR_MEM_CAPACITY;
-        }
-    }
-
-    assert(advsm->adv_data);
+    new_data = (operation == BLE_HCI_LE_SET_EXT_ADV_DATA_OPER_COMPLETE) ||
+               (operation == BLE_HCI_LE_SET_EXT_ADV_DATA_OPER_FIRST);
 
-    /* Check for valid advertising data length */
-    if (datalen + ADV_DATA_LEN(advsm) > BLE_ADV_DATA_MAX_LEN) {
-        os_mbuf_free_chain(advsm->adv_data);
-        advsm->adv_data = NULL;
+    ble_ll_adv_update_data_mbuf(&advsm->adv_data, new_data, 
BLE_ADV_DATA_MAX_LEN,
+                                cmd + 1, datalen);
+    if (!advsm->adv_data) {
         return BLE_ERR_MEM_CAPACITY;
     }
 
@@ -1789,14 +1983,6 @@ ble_ll_adv_set_adv_data(uint8_t *cmd, uint8_t instance, 
uint8_t operation)
     advsm->adi = (advsm->adi & 0xf000) | (rand() & 0x0fff);
 #endif
 
-    /* Copy the new data into the advertising structure. */
-    ret = os_mbuf_append(advsm->adv_data, cmd + 1, datalen);
-    if (ret) {
-        os_mbuf_free_chain(advsm->adv_data);
-        advsm->adv_data = NULL;
-        return BLE_ERR_MEM_CAPACITY;
-    }
-
     return BLE_ERR_SUCCESS;
 }
 
@@ -1839,7 +2025,7 @@ ble_ll_adv_ext_set_param(uint8_t *cmdbuf, uint8_t 
*rspbuf, uint8_t *rsplen)
 
     if (props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
         if (ADV_DATA_LEN(advsm) > BLE_ADV_LEGACY_DATA_MAX_LEN ||
-            advsm->scan_rsp_len > BLE_SCAN_RSP_LEGACY_DATA_MAX_LEN) {
+            SCAN_RSP_DATA_LEN(advsm) > BLE_SCAN_RSP_LEGACY_DATA_MAX_LEN) {
             return BLE_ERR_INV_HCI_CMD_PARMS;
         }
 
@@ -1874,7 +2060,7 @@ ble_ll_adv_ext_set_param(uint8_t *cmdbuf, uint8_t 
*rspbuf, uint8_t *rsplen)
     }
 
     if (props & BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED) {
-        if (ADV_DATA_LEN(advsm) || advsm->scan_rsp_len) {
+        if (ADV_DATA_LEN(advsm) || SCAN_RSP_DATA_LEN(advsm)) {
             return BLE_ERR_INV_HCI_CMD_PARMS;
         }
 
@@ -1973,6 +2159,15 @@ ble_ll_adv_ext_set_param(uint8_t *cmdbuf, uint8_t 
*rspbuf, uint8_t *rsplen)
 
     advsm->props = props;
 
+    /* Set proper mbuf chain for aux data */
+    if (props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) {
+        advsm->aux_data = NULL;
+    } else if (props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) {
+        advsm->aux_data = &advsm->scan_rsp_data;
+    } else {
+        advsm->aux_data = &advsm->adv_data;
+    }
+
     if (scan_req_notif) {
         advsm->flags |= BLE_LL_ADV_SM_FLAG_SCAN_REQ_NOTIF;
     } else {
@@ -2155,7 +2350,12 @@ ble_ll_adv_remove(uint8_t instance)
         return BLE_ERR_CMD_DISALLOWED;
     }
 
-    os_mbuf_free_chain(advsm->adv_data);
+    if (advsm->adv_data) {
+        os_mbuf_free_chain(advsm->adv_data);
+    }
+    if (advsm->scan_rsp_data) {
+        os_mbuf_free_chain(advsm->scan_rsp_data);
+    }
 
     ble_ll_adv_sm_init(advsm);
 
@@ -2282,8 +2482,16 @@ ble_ll_adv_rx_req(uint8_t pdu_type, struct os_mbuf 
*rxpdu)
                                              peer_addr_type);
         }
 
+        /*
+         * We need to store current rxed packet header temporarily so AuxPtr
+         * can be calculated (if necessary) relative to AUX_SCAN_RSP instead of
+         * AUX_ADV_IND.
+         */
+
+        advsm->rx_ble_hdr = ble_hdr;
         rc = ble_phy_tx(ble_ll_adv_scan_rsp_pdu_make, advsm,
                         BLE_PHY_TRANSITION_NONE);
+        advsm->rx_ble_hdr = NULL;
 #else
         rc = ble_phy_tx(ble_ll_adv_scan_rsp_legacy_pdu_make, advsm,
                         BLE_PHY_TRANSITION_NONE);
@@ -2604,7 +2812,8 @@ ble_ll_adv_drop_event(struct ble_ll_adv_sm *advsm)
 
     ble_ll_sched_rmv_elem(&advsm->adv_sch);
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
-    ble_ll_sched_rmv_elem(&advsm->adv_secondary_sch);
+    ble_ll_sched_rmv_elem(&advsm->aux[0].sch);
+    ble_ll_sched_rmv_elem(&advsm->aux[1].sch);
 #endif
 
     advsm->adv_chan = ble_ll_adv_final_chan(advsm);
@@ -2642,18 +2851,8 @@ ble_ll_adv_reschedule_event(struct ble_ll_adv_sm *advsm)
 
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
     if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) &&
-                                        !advsm->adv_secondary_sch.enqueued) {
-        ble_ll_set_adv_secondary_start_time(advsm);
-        ble_ll_adv_secondary_set_sched(advsm);
-
-         rc = ble_ll_sched_adv_reschedule(&advsm->adv_secondary_sch,
-                                          &advsm->adv_secondary_start_time, 0);
-         if (rc) {
-             ble_ll_adv_drop_event(advsm);
-             return;
-         }
-
-         advsm->adv_secondary_start_time += g_ble_ll_sched_offset_ticks;
+                                                        !advsm->aux_active) {
+        ble_ll_adv_aux_schedule(advsm);
     }
 #endif
 }
@@ -2770,7 +2969,8 @@ ble_ll_adv_done(struct ble_ll_adv_sm *advsm)
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
         /* If we're past aux (unlikely, but can happen), just drop an event */
         if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) &&
-                (advsm->adv_pdu_start_time > advsm->adv_secondary_start_time)) 
{
+                advsm->aux_active &&
+                advsm->adv_pdu_start_time > AUX_CURRENT(advsm)->start_time) {
             ble_ll_adv_drop_event(advsm);
             return;
         }
@@ -2881,10 +3081,21 @@ ble_ll_adv_event_done(struct os_event *ev)
 static void
 ble_ll_adv_sec_done(struct ble_ll_adv_sm *advsm)
 {
+    struct ble_ll_adv_aux *aux;
+    struct ble_ll_adv_aux *aux_next;
+
     assert(advsm->adv_enabled);
+    assert(advsm->aux_active);
+
+    aux = AUX_CURRENT(advsm);
+
+    if (advsm->aux_not_scanned) {
+        aux_next = AUX_NEXT(advsm);
+        ble_ll_sched_rmv_elem(&aux_next->sch);
+    }
 
     /* Remove anything else scheduled for secondary channel */
-    ble_ll_sched_rmv_elem(&advsm->adv_secondary_sch);
+    ble_ll_sched_rmv_elem(&aux->sch);
     os_eventq_remove(&g_ble_ll_data.ll_evq, &advsm->adv_sec_txdone_ev);
 
     /* Stop advertising due to transmitting connection response */
@@ -2893,12 +3104,20 @@ ble_ll_adv_sec_done(struct ble_ll_adv_sm *advsm)
         return;
     }
 
+    /* If we have next AUX scheduled, try to schedule another one */
+    if (AUX_NEXT(advsm)->sch.enqueued) {
+        advsm->aux_index ^= 1;
+        advsm->aux_first_pdu = 0;
+        ble_ll_adv_aux_schedule_next(advsm);
+        return;
+    }
+
     /* Check if we need to resume scanning */
     ble_ll_scan_chk_resume();
 
     /* Check if advertising timed out */
     if (advsm->duration &&
-        advsm->adv_secondary_start_time >= advsm->adv_end_time) {
+        aux->start_time >= advsm->adv_end_time) {
         ble_ll_hci_ev_send_adv_set_terminated(BLE_ERR_DIR_ADV_TMO,
                                               advsm->adv_instance, 0, 0);
 
@@ -2926,6 +3145,7 @@ ble_ll_adv_sec_done(struct ble_ll_adv_sm *advsm)
          return;
     }
 
+    advsm->aux_active = 0;
     ble_ll_adv_reschedule_event(advsm);
 }
 
@@ -3057,6 +3277,10 @@ ble_ll_adv_get_peer_rpa(struct ble_ll_adv_sm *advsm)
 void
 ble_ll_adv_wfr_timer_exp(void)
 {
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    g_ble_ll_cur_adv_sm->aux_not_scanned = 1;
+#endif
+
     ble_phy_disable();
     ble_ll_adv_tx_done(g_ble_ll_cur_adv_sm);
 }
@@ -3113,6 +3337,17 @@ ble_ll_adv_sm_init(struct ble_ll_adv_sm *advsm)
     advsm->adv_sec_txdone_ev.ev_arg = advsm;
 #endif
 
+#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV)
+    /* Initialize aux schedulers */
+    advsm->aux_active = 0;
+    advsm->aux[0].sch.cb_arg = advsm;
+    advsm->aux[0].sch.sched_cb = ble_ll_adv_secondary_tx_start_cb;
+    advsm->aux[0].sch.sched_type = BLE_LL_SCHED_TYPE_ADV;
+    advsm->aux[1].sch.cb_arg = advsm;
+    advsm->aux[1].sch.sched_cb = ble_ll_adv_secondary_tx_start_cb;
+    advsm->aux[1].sch.sched_type = BLE_LL_SCHED_TYPE_ADV;
+#endif
+
     /*XXX Configure instances to be legacy on start */
     advsm->props |= BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE;
     advsm->props |= BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY;
diff --git a/net/nimble/controller/src/ble_ll_sched.c 
b/net/nimble/controller/src/ble_ll_sched.c
index 8b44025f1..8d32121b6 100644
--- a/net/nimble/controller/src/ble_ll_sched.c
+++ b/net/nimble/controller/src/ble_ll_sched.c
@@ -825,7 +825,8 @@ ble_ll_sched_slave_new(struct ble_ll_conn_sm *connsm)
 }
 
 int
-ble_ll_sched_adv_new(struct ble_ll_sched_item *sch, ble_ll_sched_adv_new_cb cb)
+ble_ll_sched_adv_new(struct ble_ll_sched_item *sch, ble_ll_sched_adv_new_cb cb,
+                     void *arg)
 {
     int rc;
     os_sr_t sr;
@@ -877,7 +878,7 @@ ble_ll_sched_adv_new(struct ble_ll_sched_item *sch, 
ble_ll_sched_adv_new_cb cb)
     }
 
     if (cb) {
-        cb((struct ble_ll_adv_sm *)orig->cb_arg, adv_start);
+        cb((struct ble_ll_adv_sm *)orig->cb_arg, adv_start, arg);
     }
 
 #ifdef BLE_XCVR_RFCLK


 

----------------------------------------------------------------
This is an automated message from the Apache Git Service.
To respond to the message, please log on GitHub and use the
URL above to go to the specific comment.
 
For queries about this service, please contact Infrastructure at:
us...@infra.apache.org


With regards,
Apache Git Services

Reply via email to