diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c index 65896c5b55f..fa588ea3a3b 100644 --- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c +++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c @@ -332,8 +332,10 @@ int lll_adv_data_release(struct lll_adv_pdu *pdu) last = pdu->last; p = pdu->pdu[last]; - pdu->pdu[last] = NULL; - mem_release(p, &mem_pdu.free); + if (p) { + pdu->pdu[last] = NULL; + mem_release(p, &mem_pdu.free); + } last++; if (last == DOUBLE_BUFFER_SIZE) { diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h index 8baccb50f8d..bc507378e7c 100644 --- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h +++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv_pdu.h @@ -89,6 +89,11 @@ lll_adv_data_latest_peek(const struct lll_adv *const lll) } #if defined(CONFIG_BT_CTLR_ADV_EXT) +static inline int lll_adv_aux_data_init(struct lll_adv_pdu *pdu) +{ + return lll_adv_data_init(pdu); +} + static inline struct pdu_adv *lll_adv_aux_data_alloc(struct lll_adv_aux *lll, uint8_t *idx) { @@ -118,6 +123,13 @@ static inline struct pdu_adv *lll_adv_aux_data_curr_get(struct lll_adv_aux *lll) return (void *)lll->data.pdu[lll->data.first]; } +static inline struct pdu_adv *lll_adv_aux_scan_rsp_alloc(struct lll_adv *lll, + uint8_t *idx) +{ + return lll_adv_pdu_alloc(&lll->scan_rsp, idx); +} + + #if defined(CONFIG_BT_CTLR_ADV_PERIODIC) int lll_adv_and_extra_data_release(struct lll_adv_pdu *pdu); @@ -125,6 +137,11 @@ int lll_adv_and_extra_data_release(struct lll_adv_pdu *pdu); void lll_adv_sync_pdu_b2b_update(struct lll_adv_sync *lll, uint8_t idx); #endif +static inline int lll_adv_sync_data_init(struct lll_adv_pdu *pdu) +{ + return lll_adv_data_init(pdu); +} + struct pdu_adv *lll_adv_pdu_and_extra_data_alloc(struct lll_adv_pdu *pdu, void **extra_data, uint8_t *idx); diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv.c b/subsys/bluetooth/controller/ll_sw/ull_adv.c index 6961b917762..1b6b2fd9d10 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_adv.c +++ b/subsys/bluetooth/controller/ll_sw/ull_adv.c @@ -687,16 +687,43 @@ uint8_t ll_adv_params_set(uint16_t interval, uint8_t adv_type, * scan response data. Existing sets keep whatever data was set. */ if (is_pdu_type_changed) { + uint8_t err; + + /* Make sure the scan response PDU is allocated from the right pool */ + (void)lll_adv_data_release(&adv->lll.scan_rsp); + lll_adv_data_reset(&adv->lll.scan_rsp); + err = lll_adv_aux_data_init(&adv->lll.scan_rsp); + if (err) { + return err; + } + pdu = lll_adv_scan_rsp_peek(&adv->lll); pdu->type = PDU_ADV_TYPE_AUX_SCAN_RSP; pdu->len = 0; } #endif /* CONFIG_BT_CTLR_ADV_EXT */ } else { + pdu = lll_adv_scan_rsp_peek(&adv->lll); + +#if defined(CONFIG_BT_CTLR_ADV_EXT) + if (is_pdu_type_changed || !pdu) { + uint8_t err; + + /* Make sure the scan response PDU is allocated from the right pool */ + (void)lll_adv_data_release(&adv->lll.scan_rsp); + lll_adv_data_reset(&adv->lll.scan_rsp); + err = lll_adv_data_init(&adv->lll.scan_rsp); + if (err) { + return err; + } + + pdu = lll_adv_scan_rsp_peek(&adv->lll); + } +#endif /* CONFIG_BT_CTLR_ADV_EXT */ + /* Make sure legacy advertising set has scan response data * initialized. */ - pdu = lll_adv_scan_rsp_peek(&adv->lll); pdu->type = PDU_ADV_TYPE_SCAN_RSP; pdu->rfu = 0; pdu->chan_sel = 0; @@ -892,6 +919,25 @@ uint8_t ll_adv_enable(uint8_t enable) pdu_adv = lll_adv_data_peek(lll); pdu_scan = lll_adv_scan_rsp_peek(lll); +#if defined(CONFIG_BT_CTLR_ADV_EXT) + if (!pdu_scan) { + uint8_t err; + + if (pdu_adv->type == PDU_ADV_TYPE_EXT_IND) { + /* Should never happen */ + return BT_HCI_ERR_CMD_DISALLOWED; + } + + err = lll_adv_data_init(&adv->lll.scan_rsp); + if (err) { + return err; + } + + pdu_scan = lll_adv_scan_rsp_peek(lll); + init_pdu(pdu_scan, PDU_ADV_TYPE_SCAN_RSP); + } +#endif /* CONFIG_BT_CTLR_ADV_EXT */ + /* Update Bluetooth Device address in advertising and scan response * PDUs. */ @@ -1854,6 +1900,18 @@ uint8_t ull_scan_rsp_set(struct ll_adv_set *adv, uint8_t len, /* update scan pdu fields. */ prev = lll_adv_scan_rsp_peek(&adv->lll); + if (!prev) { + uint8_t err; + + err = lll_adv_data_init(&adv->lll.scan_rsp); + if (err) { + return err; + } + + prev = lll_adv_scan_rsp_peek(&adv->lll); + init_pdu(prev, PDU_ADV_TYPE_SCAN_RSP); + } + pdu = lll_adv_scan_rsp_alloc(&adv->lll, &idx); pdu->type = PDU_ADV_TYPE_SCAN_RSP; pdu->rfu = 0; @@ -2156,7 +2214,14 @@ static int init_reset(void) for (handle = 0U; handle < BT_CTLR_ADV_SET; handle++) { lll_adv_data_init(&ll_adv[handle].lll.adv_data); + +#if defined(CONFIG_BT_CTLR_ADV_EXT) + /* scan_rsp is not init'ed until we know if it is a legacy or extended scan rsp */ + memset(&ll_adv[handle].lll.scan_rsp, 0, sizeof(ll_adv[handle].lll.scan_rsp)); +#else lll_adv_data_init(&ll_adv[handle].lll.scan_rsp); +#endif /* !CONFIG_BT_CTLR_ADV_EXT */ + #if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX) /* Pointer to DF configuration must be cleared on reset. In other case it will point * to a memory pool address that should be released. It may be used by the pool @@ -2960,5 +3025,8 @@ static void init_set(struct ll_adv_set *adv) #endif /* ONFIG_BT_CTLR_JIT_SCHEDULING */ init_pdu(lll_adv_data_peek(&ll_adv[0].lll), PDU_ADV_TYPE_ADV_IND); + +#if !defined(CONFIG_BT_CTLR_ADV_EXT) init_pdu(lll_adv_scan_rsp_peek(&ll_adv[0].lll), PDU_ADV_TYPE_SCAN_RSP); +#endif /* !CONFIG_BT_CTLR_ADV_EXT */ } diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c b/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c index 4f0e5215ed7..004a65d6e91 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c +++ b/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c @@ -726,7 +726,7 @@ uint8_t ll_adv_aux_sr_data_set(uint8_t handle, uint8_t op, uint8_t frag_pref, sr_pdu_prev = lll_adv_scan_rsp_peek(lll); /* Get reference to next scan response PDU */ - sr_pdu = lll_adv_scan_rsp_alloc(lll, &sr_idx); + sr_pdu = lll_adv_aux_scan_rsp_alloc(lll, &sr_idx); /* Prepare the AD data as parameter to update in PDU */ /* Use length = 0 and NULL pointer to retain old data in the PDU. @@ -2545,7 +2545,7 @@ struct ll_adv_aux_set *ull_adv_aux_acquire(struct lll_adv *lll) lll_aux->adv = lll; lll_adv_data_reset(&lll_aux->data); - err = lll_adv_data_init(&lll_aux->data); + err = lll_adv_aux_data_init(&lll_aux->data); if (err) { return NULL; } diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c index ceda2ed6bf2..8b149dbc20f 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c +++ b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c @@ -111,7 +111,7 @@ uint8_t ll_adv_sync_param_set(uint8_t handle, uint16_t interval, uint16_t flags) lll_sync->adv = lll; lll_adv_data_reset(&lll_sync->data); - err = lll_adv_data_init(&lll_sync->data); + err = lll_adv_sync_data_init(&lll_sync->data); if (err) { return BT_HCI_ERR_MEM_CAPACITY_EXCEEDED; }