diff --git a/subsys/bluetooth/controller/hci/hci.c b/subsys/bluetooth/controller/hci/hci.c index a1ed1bf4c7e..ad54a5be54d 100644 --- a/subsys/bluetooth/controller/hci/hci.c +++ b/subsys/bluetooth/controller/hci/hci.c @@ -5523,232 +5523,190 @@ static void le_per_adv_sync_report(struct pdu_data *pdu_data, struct net_buf *buf) { struct bt_hci_evt_le_per_advertising_report *sep; + struct node_rx_ftr *ftr = &node_rx->hdr.rx_ftr; int8_t tx_pwr = BT_HCI_LE_ADV_TX_POWER_NO_PREF; struct pdu_adv *adv = (void *)pdu_data; + struct pdu_adv_aux_ptr *aux_ptr = NULL; uint8_t cte_type = BT_HCI_LE_NO_CTE; - struct node_rx_pdu *node_rx_curr; - struct node_rx_pdu *node_rx_next; - uint8_t total_data_len = 0U; + struct pdu_adv_com_ext_adv *p; + struct pdu_adv_ext_hdr *h; uint8_t data_status = 0U; + struct net_buf *evt_buf; uint8_t data_len = 0U; uint8_t *data = NULL; uint8_t data_max_len; + uint8_t hdr_buf_len; + uint8_t hdr_len; + uint8_t *ptr; int8_t rssi; if (!(event_mask & BT_EVT_MASK_LE_META_EVENT) || !(le_event_mask & BT_EVT_MASK_LE_PER_ADVERTISING_REPORT)) { - node_rx_extra_list_release(node_rx->hdr.rx_ftr.extra); return; } - node_rx_curr = node_rx; - node_rx_next = node_rx_curr->hdr.rx_ftr.extra; - do { - struct pdu_adv_com_ext_adv *p; - uint8_t data_len_curr = 0U; - uint8_t *data_curr = NULL; - uint8_t sec_phy_curr = 0U; - struct pdu_adv_ext_hdr *h; - uint8_t hdr_buf_len; - uint8_t hdr_len; - uint8_t *ptr; + /* The Link Layer currently returns RSSI as an absolute value */ + rssi = -(node_rx->hdr.rx_ftr.rssi); - /* The Link Layer currently returns RSSI as an absolute value */ - rssi = -(node_rx_curr->hdr.rx_ftr.rssi); + BT_DBG("len = %u, rssi = %d", adv->len, rssi); - BT_DBG("len = %u, rssi = %d", adv->len, rssi); + p = (void *)&adv->adv_ext_ind; + h = (void *)p->ext_hdr_adv_data; + ptr = (void *)h; - p = (void *)&adv->adv_ext_ind; - h = (void *)p->ext_hdr_adv_data; - ptr = (void *)h; + BT_DBG(" Per. adv mode= 0x%x, hdr len= %u", p->adv_mode, + p->ext_hdr_len); - BT_DBG(" Ext. adv mode= 0x%x, hdr len= %u", p->adv_mode, - p->ext_hdr_len); + if (!p->ext_hdr_len) { + hdr_len = PDU_AC_EXT_HEADER_SIZE_MIN; - if (!p->ext_hdr_len) { - hdr_len = PDU_AC_EXT_HEADER_SIZE_MIN; + goto no_ext_hdr; + } - goto no_ext_hdr; + ptr = h->data; + + if (h->adv_addr) { + ptr += BDADDR_SIZE; + } + + if (h->tgt_addr) { + ptr += BDADDR_SIZE; + } + + if (h->cte_info) { + struct pdu_cte_info *cte_info; + + cte_info = (void *)ptr; + cte_type = cte_info->type; + ptr++; + + BT_DBG(" CTE type= %d", cte_type); + } + + if (h->adi) { + ptr += sizeof(struct pdu_adv_adi); + } + + /* AuxPtr */ + if (h->aux_ptr) { + uint8_t aux_phy; + + aux_ptr = (void *)ptr; + if (aux_ptr->phy > EXT_ADV_AUX_PHY_LE_CODED) { + struct node_rx_ftr *ftr; + + ftr = &node_rx->hdr.rx_ftr; + node_rx_extra_list_release(ftr->extra); + return; } - ptr = h->data; + ptr += sizeof(*aux_ptr); - if (h->adv_addr) { - ptr += BDADDR_SIZE; - } + aux_phy = BIT(aux_ptr->phy); - if (h->tgt_addr) { - ptr += BDADDR_SIZE; - } + BT_DBG(" AuxPtr chan_idx = %u, ca = %u, offs_units " + "= %u offs = 0x%x, phy = 0x%x", + aux_ptr->chan_idx, aux_ptr->ca, + aux_ptr->offs_units, aux_ptr->offs, aux_phy); + } - if (h->cte_info) { - struct pdu_cte_info *cte_info; + /* No SyncInfo */ + if (h->sync_info) { + ptr += sizeof(struct pdu_adv_sync_info); + } - cte_info = (void *)ptr; - cte_type = cte_info->type; - ptr++; + /* Tx Power */ + if (h->tx_pwr) { + tx_pwr = *(int8_t *)ptr; + ptr++; - BT_DBG(" CTE type= %d", cte_type); - } + BT_DBG(" Tx pwr= %d dB", tx_pwr); + } - if (h->adi) { - ptr += sizeof(struct pdu_adv_adi); - } - - /* AuxPtr */ - if (h->aux_ptr) { - struct pdu_adv_aux_ptr *aux_ptr; - uint8_t aux_phy; - - aux_ptr = (void *)ptr; - if (aux_ptr->phy > EXT_ADV_AUX_PHY_LE_CODED) { - struct node_rx_ftr *ftr; - - ftr = &node_rx->hdr.rx_ftr; - node_rx_extra_list_release(ftr->extra); - return; - } - - ptr += sizeof(*aux_ptr); - - sec_phy_curr = aux_ptr->phy + 1; - - aux_phy = BIT(aux_ptr->phy); - - BT_DBG(" AuxPtr chan_idx = %u, ca = %u, offs_units " - "= %u offs = 0x%x, phy = 0x%x", - aux_ptr->chan_idx, aux_ptr->ca, - aux_ptr->offs_units, aux_ptr->offs, aux_phy); - } - - /* No SyncInfo */ - if (h->sync_info) { - ptr += sizeof(struct pdu_adv_sync_info); - } - - /* Tx Power */ - if (h->tx_pwr) { - tx_pwr = *(int8_t *)ptr; - ptr++; - - BT_DBG(" Tx pwr= %d dB", tx_pwr); - } - - hdr_len = ptr - (uint8_t *)p; + hdr_len = ptr - (uint8_t *)p; if (hdr_len <= (PDU_AC_EXT_HEADER_SIZE_MIN + sizeof(struct pdu_adv_ext_hdr))) { hdr_len = PDU_AC_EXT_HEADER_SIZE_MIN; ptr = (uint8_t *)h; } - hdr_buf_len = PDU_AC_EXT_HEADER_SIZE_MIN + p->ext_hdr_len; - if (hdr_len > hdr_buf_len) { - BT_WARN(" Header length %u/%u, INVALID.", hdr_len, - p->ext_hdr_len); - } else { - uint8_t acad_len = hdr_buf_len - hdr_len; + hdr_buf_len = PDU_AC_EXT_HEADER_SIZE_MIN + p->ext_hdr_len; + if (hdr_len > hdr_buf_len) { + BT_WARN(" Header length %u/%u, INVALID.", hdr_len, + p->ext_hdr_len); + } else { + uint8_t acad_len = hdr_buf_len - hdr_len; - if (acad_len) { - ptr += acad_len; - hdr_len += acad_len; + if (acad_len) { + ptr += acad_len; + hdr_len += acad_len; - BT_DBG("ACAD: "); - } + BT_DBG("ACAD: "); } + } no_ext_hdr: - if (hdr_len < adv->len) { - data_len_curr = adv->len - hdr_len; - data_curr = ptr; + if (hdr_len < adv->len) { + data_len = adv->len - hdr_len; + data = ptr; - BT_DBG(" AD Data (%u): ", data_len); - } + BT_DBG(" AD Data (%u): ", data_len); + } - if (node_rx_curr == node_rx) { - data_len = data_len_curr; - total_data_len = data_len; - data = data_curr; - } else { - /* TODO: Validate current value with previous ?? - */ + adv = (void *)node_rx->pdu; - if (!data) { - data_len = data_len_curr; - total_data_len = data_len; - data = data_curr; - } else { - total_data_len += data_len_curr; - - /* TODO: construct new HCI event for this - * fragment. - */ - } - } - - if (!node_rx_next) { - bool has_aux_ptr = !!sec_phy_curr; - - if (has_aux_ptr) { - data_status = - BT_HCI_LE_ADV_EVT_TYPE_DATA_STATUS_INCOMPLETE; - } - - break; - } - - node_rx_curr = node_rx_next; - node_rx_next = node_rx_curr->hdr.rx_ftr.extra; - adv = (void *)node_rx_curr->pdu; - } while (1); - - /* FIXME: move most of below into above loop to dispatch fragments of - * data in HCI event. - */ data_max_len = ADV_REPORT_EVT_MAX_LEN - sizeof(struct bt_hci_evt_le_meta_event) - sizeof(*sep); - /* If data complete */ - if (!data_status) { - /* Only copy data that fit the event buffer size, - * mark it as incomplete - */ - if (data_len > data_max_len) { - data_len = data_max_len; - data_status = - BT_HCI_LE_ADV_EVT_TYPE_DATA_STATUS_PARTIAL; - } + evt_buf = buf; - /* else, data incomplete */ - } else { - /* Data incomplete and no more to come */ - if ((tx_pwr == BT_HCI_LE_ADV_TX_POWER_NO_PREF) && !data) { - /* No Tx Power value and no valid AD data parsed in this - * chain of PDUs, skip HCI event generation. + do { + uint8_t data_len_frag; + + data_len_frag = MIN(data_len, data_max_len); + + /* Start constructing periodic advertising report */ + sep = meta_evt(evt_buf, + BT_HCI_EVT_LE_PER_ADVERTISING_REPORT, + sizeof(*sep) + data_len_frag); + + memcpy(&sep->data[0], data, data_len_frag); + data += data_len_frag; + data_len -= data_len_frag; + + if (data_len > 0) { + /* Some data left in PDU, mark as partial data. */ + data_status = BT_HCI_LE_ADV_EVT_TYPE_DATA_STATUS_PARTIAL; + } else if (!aux_ptr) { + /* No data left, no AuxPtr, mark as complete data. */ + data_status = BT_HCI_LE_ADV_EVT_TYPE_DATA_STATUS_COMPLETE; + } else if (ftr->aux_w4next) { + /* No data left, but have AuxPtr and scheduled aux scan, + * mark as partial data. */ - node_rx_extra_list_release(node_rx->hdr.rx_ftr.extra); - return; + data_status = BT_HCI_LE_ADV_EVT_TYPE_DATA_STATUS_PARTIAL; + } else { + /* No data left, have AuxPtr but not aux scan scheduled, + * mark as incomplete data. + */ + data_status = BT_HCI_LE_ADV_EVT_TYPE_DATA_STATUS_INCOMPLETE; } - /* Only copy data that fit the event buffer size */ - if (data_len > data_max_len) { - data_len = data_max_len; + sep->handle = sys_cpu_to_le16(node_rx->hdr.handle); + sep->tx_power = tx_pwr; + sep->rssi = rssi; + sep->cte_type = cte_type; + sep->data_status = data_status; + sep->length = data_len_frag; + + if (data_len > 0) { + evt_buf = bt_buf_get_rx(BT_BUF_EVT, K_FOREVER); + net_buf_frag_add(buf, evt_buf); + + tx_pwr = BT_HCI_LE_ADV_TX_POWER_NO_PREF; } - } - - /* Start constructing the event */ - sep = meta_evt(buf, BT_HCI_EVT_LE_PER_ADVERTISING_REPORT, - sizeof(*sep) + data_len); - - sep->handle = sys_cpu_to_le16(node_rx->hdr.handle); - sep->tx_power = tx_pwr; - sep->rssi = rssi; - sep->cte_type = cte_type; - sep->data_status = data_status; - sep->length = data_len; - memcpy(&sep->data[0], data, data_len); - - node_rx_extra_list_release(node_rx->hdr.rx_ftr.extra); + } while (data_len > 0); } static void le_per_adv_sync_lost(struct pdu_data *pdu_data, diff --git a/subsys/bluetooth/controller/ll_sw/lll.h b/subsys/bluetooth/controller/ll_sw/lll.h index 0179b82cc77..b1217112d49 100644 --- a/subsys/bluetooth/controller/ll_sw/lll.h +++ b/subsys/bluetooth/controller/ll_sw/lll.h @@ -276,13 +276,15 @@ struct node_rx_ftr { */ void *aux_ptr; uint8_t aux_phy; - uint8_t aux_sched; }; uint32_t ticks_anchor; uint32_t radio_end_us; uint8_t rssi; #if defined(CONFIG_BT_CTLR_ADV_EXT) && defined(CONFIG_BT_OBSERVER) uint8_t aux_sched_from_lll:1; + uint8_t aux_lll_sched:1; + uint8_t aux_w4next:1; + uint8_t phy_flags:1; uint8_t scan_req:1; uint8_t scan_rsp:1; diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan.c index 860738f9c3c..9e62d540893 100644 --- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan.c +++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan.c @@ -1454,11 +1454,11 @@ static int isr_rx_scan_report(struct lll_scan *lll, uint8_t rssi_ready, radio_rx_chain_delay_get(lll->phy, phy_flags_rx); ftr->phy_flags = phy_flags_rx; - ftr->aux_sched = + ftr->aux_lll_sched = lll_scan_aux_setup(lll, NULL, pdu_adv_rx, lll->phy, phy_flags_rx); - if (ftr->aux_sched) { + if (ftr->aux_lll_sched) { err = -EBUSY; } } diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan_aux.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan_aux.c index e9a0a56149a..05dfe86e498 100644 --- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan_aux.c +++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_scan_aux.c @@ -1099,7 +1099,7 @@ static int isr_rx_pdu(struct lll_scan *lll, struct lll_sync *lll_sync, struct ll ftr->rl_idx = irkmatch_ok ? rl_idx : FILTER_IDX_NONE; #endif /* CONFIG_BT_CTLR_PRIVACY */ - ftr->aux_sched = 0U; + ftr->aux_lll_sched = 0U; ull_rx_put(node_rx->hdr.link, node_rx); ull_rx_sched(); @@ -1157,8 +1157,8 @@ static int isr_rx_pdu(struct lll_scan *lll, struct lll_sync *lll_sync, struct ll ftr->rl_idx = irkmatch_ok ? rl_idx : FILTER_IDX_NONE; #endif /* CONFIG_BT_CTLR_PRIVACY */ - ftr->aux_sched = lll_scan_aux_setup(lll, lll_sync, pdu, phy_aux, - phy_aux_flags_rx); + ftr->aux_lll_sched = lll_scan_aux_setup(lll, lll_sync, pdu, phy_aux, + phy_aux_flags_rx); node_rx->hdr.type = NODE_RX_TYPE_EXT_AUX_REPORT; @@ -1178,7 +1178,7 @@ static int isr_rx_pdu(struct lll_scan *lll, struct lll_sync *lll_sync, struct ll /* Next aux scan is scheduled from LLL, we already handled radio * disable so prevent caller from doing it again. */ - if (ftr->aux_sched) { + if (ftr->aux_lll_sched) { return 0; } diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_sync.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_sync.c index 1a97ce84f50..bcc75bce5e7 100644 --- a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_sync.c +++ b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_sync.c @@ -350,9 +350,9 @@ static void isr_rx(void *param) pdu = (void *)node_rx->pdu; ftr->aux_sched_from_lll = 0; - ftr->aux_sched = lll_scan_aux_setup(NULL, lll, pdu, - lll->phy, 0); - if (ftr->aux_sched) { + ftr->aux_lll_sched = lll_scan_aux_setup(NULL, lll, pdu, + lll->phy, 0); + if (ftr->aux_lll_sched) { err = -EBUSY; } diff --git a/subsys/bluetooth/controller/ll_sw/ull_scan_aux.c b/subsys/bluetooth/controller/ll_sw/ull_scan_aux.c index 8ee627f8a3b..1122eac331b 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_scan_aux.c +++ b/subsys/bluetooth/controller/ll_sw/ull_scan_aux.c @@ -101,7 +101,6 @@ void ull_scan_aux_setup(memq_link_t *link, struct node_rx_hdr *rx) struct lll_scan *lll; struct pdu_adv *pdu; uint8_t aux_handle; - bool is_lll_sched; bool is_scan_req; uint8_t acad_len; uint8_t hdr_len; @@ -246,12 +245,11 @@ void ull_scan_aux_setup(memq_link_t *link, struct node_rx_hdr *rx) return; } - /* Copy to local flag since we need to clear 'extra' field */ - is_lll_sched = !!ftr->aux_sched; - rx->link = link; ftr->extra = NULL; + ftr->aux_w4next = 0; + pdu = (void *)((struct node_rx_pdu *)rx)->pdu; p = (void *)&pdu->adv_ext_ind; if (!p->ext_hdr_len) { @@ -388,11 +386,13 @@ void ull_scan_aux_setup(memq_link_t *link, struct node_rx_hdr *rx) lll_aux->chan = aux_ptr->chan_idx; lll_aux->phy = BIT(aux_ptr->phy); + ftr->aux_w4next = 1; + /* See if this was already scheduled from LLL. If so, store aux context * in global scan struct so we can pick it when scanned node is received * with a valid context. */ - if (is_lll_sched) { + if (ftr->aux_lll_sched) { if (lll) { lll->lll_aux = lll_aux; } else {