From 5e82e31b7435a4ce09c4d0503a91b2de5ef68a98 Mon Sep 17 00:00:00 2001 From: Andrzej Kaczmarek Date: Thu, 18 Jan 2018 17:11:45 +0100 Subject: [PATCH] nimble/ll: Add support for adv/scanrsp data fragmentation This patch adds support for fragmentation of advertising and scan response data on secondary advertising channels. The controller can support up to 1650 bytes of data (spec limit). The scheduling code for secondary channels is refactored to be more generic and allow to easily schedule chains of aux packets for both advertising and scan response data. We now have two scheduler instances for auxiliary packets so we can have two consecutive packets scheduled. This way AuxPtr can be calculated immediately, without need to schedule new auxiliary packet in interrupt context. Subsequent auxiliary packets are automatically scheduled as long as there are some data left unsent. X-Original-Commit: 8eaa177d1a55e0dda724cda8b5bda03ec0f3c8ce --- nimble/controller/include/controller/ble_ll.h | 3 +- .../include/controller/ble_ll_sched.h | 6 +- nimble/controller/src/ble_ll_adv.c | 805 +++++++++++------- nimble/controller/src/ble_ll_sched.c | 5 +- 4 files changed, 517 insertions(+), 302 deletions(-) diff --git a/nimble/controller/include/controller/ble_ll.h b/nimble/controller/include/controller/ble_ll.h index 97338dc68..c81a79e77 100644 --- a/nimble/controller/include/controller/ble_ll.h +++ b/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/nimble/controller/include/controller/ble_ll_sched.h b/nimble/controller/include/controller/ble_ll_sched.h index 3545a4008..15f750077 100644 --- a/nimble/controller/include/controller/ble_ll_sched.h +++ b/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/nimble/controller/src/ble_ll_adv.c b/nimble/controller/src/ble_ll_adv.c index 9bfc0f00e..f50aea79c 100644 --- a/nimble/controller/src/ble_ll_adv.c +++ b/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 * @@ -98,8 +108,13 @@ struct ble_ll_adv_sm 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; @@ -123,6 +138,11 @@ struct ble_ll_adv_sm ((_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) @@ -348,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) +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_DATA_INFO_SIZE; + dptr[0] = advsm->adv_secondary_chan; - if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE) && - ADV_DATA_LEN(advsm)) { - len += ADV_DATA_LEN(advsm); + if (offset > 245700) { + dptr[0] |= 0x80; + offset = offset / 300; + } else { + offset = offset / 30; } - 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) -{ - uint8_t len; - - len = BLE_LL_EXT_ADV_HDR_LEN + BLE_LL_EXT_ADV_FLAGS_SIZE + - BLE_LL_EXT_ADV_ADVA_SIZE; - - if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR) { - len += BLE_LL_EXT_ADV_TX_POWER_SIZE; - } - - len += advsm->scan_rsp_len; - - return len; + dptr[1] = (offset & 0x000000ff); + dptr[2] = ((offset >> 8) & 0x0000001f) | (advsm->sec_phy - 1) << 5; //TODO; } /** @@ -403,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; @@ -419,97 +403,34 @@ 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; - - /* TODO in some cases we could avoid auxiliary packet */ - aux_ptr = true; - adi = true; - } - - 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; - } - - if (adva) { - ext_hdr_flags |= (1 << BLE_LL_EXT_ADV_ADVA_BIT); - ext_hdr_len += BLE_LL_EXT_ADV_ADVA_SIZE; - } - - if (targeta) { - ext_hdr_flags |= (1 << BLE_LL_EXT_ADV_TARGETA_BIT); - ext_hdr_len += BLE_LL_EXT_ADV_TARGETA_SIZE; - } - - if (adi) { - ext_hdr_flags |= (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT); - ext_hdr_len += BLE_LL_EXT_ADV_DATA_INFO_SIZE; - } - - if (aux_ptr) { - ext_hdr_flags |= (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT); - ext_hdr_len += BLE_LL_EXT_ADV_AUX_PTR_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; - } - - /* TODO ACAD */ - - if (adv_data) { - pdulen += ADV_DATA_LEN(advsm); - } + /* only ADV_EXT_IND goes on primary advertising channels */ + pdu_type = BLE_ADV_PDU_TYPE_ADV_EXT_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; + 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; + } + + 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); + /* ext hdr len and adv mode */ dptr[0] = ext_hdr_len | (adv_mode << 6); dptr += 1; @@ -518,53 +439,173 @@ ble_ll_adv_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) dptr[0] = ext_hdr_flags; dptr += 1; - if (adva) { + /* ADI */ + dptr[0] = advsm->adi & 0x00ff; + dptr[1] = advsm->adi >> 8; + dptr += BLE_LL_EXT_ADV_DATA_INFO_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); + + return BLE_LL_EXT_ADV_HDR_LEN + ext_hdr_len; +} + +/** + * 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; + } + + *hdr_byte = pdu_type; + + /* 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; + + dptr[0] = aux->ext_hdr; + dptr += 1; + + 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; + if (aux->ext_hdr & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) { + assert(AUX_NEXT(advsm)->sch.enqueued); - /* in usecs */ - offset = os_cputime_ticks_to_usecs(offset); - - 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 @@ -621,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; @@ -632,49 +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 + SCAN_RSP_DATA_LEN(advsm); - - /* 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; - } - - os_mbuf_copydata(advsm->scan_rsp_data, 0, SCAN_RSP_DATA_LEN(advsm), dptr); - - return pdulen; + return ble_ll_adv_aux_pdu_make(dptr, pducb_arg, hdr_byte); } struct aux_conn_rsp_data { @@ -952,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; @@ -996,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; } @@ -1031,16 +1033,164 @@ adv_tx_done: return BLE_LL_SCHED_STATE_DONE; } -static void -ble_ll_adv_secondary_set_sched(struct ble_ll_adv_sm *advsm) +static uint8_t +ble_ll_adv_aux_scannable_pdu_payload_len(struct ble_ll_adv_sm *advsm) { - uint32_t max_usecs; - struct ble_ll_sched_item *sch; + uint8_t len; - 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; + /* 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_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; + + 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) @@ -1048,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); } /* @@ -1074,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 @@ -1259,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 */ @@ -1305,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; @@ -1330,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 /** @@ -1488,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 @@ -1992,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 { @@ -2306,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); @@ -2628,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); @@ -2666,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 } @@ -2794,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; } @@ -2905,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 */ @@ -2917,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); @@ -2950,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); } @@ -3081,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); } @@ -3137,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/nimble/controller/src/ble_ll_sched.c b/nimble/controller/src/ble_ll_sched.c index 8b44025f1..8d32121b6 100644 --- a/nimble/controller/src/ble_ll_sched.c +++ b/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