diff --git a/nimble/controller/include/controller/ble_ll.h b/nimble/controller/include/controller/ble_ll.h index 4a832c655..556883422 100644 --- a/nimble/controller/include/controller/ble_ll.h +++ b/nimble/controller/include/controller/ble_ll.h @@ -216,7 +216,6 @@ extern STATS_SECT_DECL(ble_ll_stats) ble_ll_stats; #define BLE_LL_STATE_STANDBY (0) #define BLE_LL_STATE_ADV (1) #define BLE_LL_STATE_SCANNING (2) -#define BLE_LL_STATE_INITIATING (3) #define BLE_LL_STATE_CONNECTION (4) #define BLE_LL_STATE_DTM (5) #define BLE_LL_STATE_SYNC (6) diff --git a/nimble/controller/include/controller/ble_ll_conn.h b/nimble/controller/include/controller/ble_ll_conn.h index daf7db584..7be3a4315 100644 --- a/nimble/controller/include/controller/ble_ll_conn.h +++ b/nimble/controller/include/controller/ble_ll_conn.h @@ -225,9 +225,6 @@ struct ble_ll_conn_sm /* RSSI */ int8_t conn_rssi; - /* For privacy */ - int8_t rpa_index; - /* Connection data length management */ uint8_t max_tx_octets; uint8_t max_rx_octets; @@ -328,6 +325,9 @@ struct ble_ll_conn_sm uint8_t own_addr_type; uint8_t peer_addr_type; uint8_t peer_addr[BLE_DEV_ADDR_LEN]; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + uint8_t peer_addr_resolved; +#endif /* * XXX: TODO. Could save memory. Have single event at LL and put these @@ -379,7 +379,6 @@ struct ble_ll_conn_sm /* XXX: for now, just store them all */ struct ble_ll_conn_params conn_cp; - struct ble_ll_scan_sm *scansm; #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) struct hci_ext_create_conn initial_params; #endif @@ -426,6 +425,34 @@ uint8_t ble_ll_conn_calc_dci(struct ble_ll_conn_sm *conn, uint16_t latency); void ble_ll_conn_get_anchor(struct ble_ll_conn_sm *connsm, uint16_t conn_event, uint32_t *anchor, uint8_t *anchor_usecs); +struct ble_ll_scan_addr_data; +struct ble_ll_scan_pdu_data; + +uint8_t ble_ll_conn_tx_connect_ind_pducb(uint8_t *dptr, void *pducb_arg, + uint8_t *hdr_byte); +void ble_ll_conn_prepare_connect_ind(struct ble_ll_conn_sm *connsm, + struct ble_ll_scan_pdu_data *pdu_data, + uint8_t adva_type, uint8_t *adva, + uint8_t inita_type, uint8_t *inita, + int rpa_index, uint8_t channel); + +/* Send CONNECT_IND/AUX_CONNECT_REQ */ +int ble_ll_conn_send_connect_req(struct os_mbuf *rxpdu, + struct ble_ll_scan_addr_data *addrd, + uint8_t ext); +/* Cancel connection after AUX_CONNECT_REQ was sent */ +void ble_ll_conn_send_connect_req_cancel(void); +/* Signal connection created via CONNECT_IND */ +void ble_ll_conn_created_on_legacy(struct os_mbuf *rxpdu, + struct ble_ll_scan_addr_data *addrd, + uint8_t *targeta); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +/* Signal connection created via AUX_CONNECT_REQ */ +void ble_ll_conn_created_on_aux(struct os_mbuf *rxpdu, + struct ble_ll_scan_addr_data *addrd, + uint8_t *targeta); +#endif + #ifdef __cplusplus } #endif diff --git a/nimble/controller/include/controller/ble_ll_scan.h b/nimble/controller/include/controller/ble_ll_scan.h index d3097f2b1..eae0c3945 100644 --- a/nimble/controller/include/controller/ble_ll_scan.h +++ b/nimble/controller/include/controller/ble_ll_scan.h @@ -89,56 +89,6 @@ struct ble_ll_scan_phy struct ble_ll_scan_timing timing; }; -#define BLE_LL_AUX_HAS_ADVA 0x01 -#define BLE_LL_AUX_HAS_TARGETA 0x02 -#define BLE_LL_AUX_HAS_ADI 0x04 -#define BLE_LL_AUX_IS_MATCHED 0x08 -#define BLE_LL_AUX_IS_TARGETA_RESOLVED 0x10 - -#define BLE_LL_AUX_FLAG_HCI_SENT_ANY 0x02 -#define BLE_LL_AUX_FLAG_HCI_SENT_COMPLETED 0x04 -#define BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED 0x08 -#define BLE_LL_AUX_FLAG_SCAN_COMPLETE 0x10 -#define BLE_LL_AUX_FLAG_SCAN_ERROR 0x20 -#define BLE_LL_AUX_FLAG_AUX_ADV_RECEIVED 0x40 -#define BLE_LL_AUX_FLAG_AUX_CHAIN_RECEIVED 0x80 - -struct ble_ll_aux_data { - uint8_t flags; - - /* - * Since aux_data can be accessed from ISR and LL, we have separate copies - * of flags to make sure that ISR does not modify flags while LL uses them. - * ISR updates 'flags_isr' and LL adds these to 'flags_ll' which it then - * uses for further processing allowing to update 'flags_isr' if another - * scan for given 'aux_data' is scheduled. Note that flags must not be unset - * while aux_data is valid. - */ - uint8_t flags_isr; - uint8_t flags_ll; - - uint8_t ref_cnt; - uint8_t chan; - uint8_t aux_phy; - uint8_t aux_primary_phy; - uint8_t mode; - uint8_t scanning; -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) - int8_t rpa_index; -#endif - uint16_t adi; - uint32_t offset; - uint8_t offset_units; - uint8_t adva[6]; - uint8_t adva_type; - uint8_t targeta[6]; - uint8_t targeta_type; - uint16_t evt_type; - struct ble_ll_sched_item sch; - struct ble_hci_ev *evt; - struct ble_npl_event ev; -}; - struct ble_ll_scan_pdu_data { uint8_t hdr_byte; /* ScanA for SCAN_REQ and InitA for CONNECT_IND */ @@ -197,11 +147,13 @@ struct ble_ll_scan_sm #endif uint8_t restart_timer_needed; - struct ble_ll_aux_data *cur_aux_data; struct ble_ll_scan_phy *scanp; struct ble_ll_scan_phy *scanp_next; struct ble_ll_scan_phy scan_phys[BLE_LL_SCAN_PHY_NUMBER]; + + /* Connection sm for initiator scan */ + struct ble_ll_conn_sm *connsm; }; /* Scan types */ @@ -244,13 +196,11 @@ int ble_ll_scan_can_chg_whitelist(void); /* Boolean function returning true if scanning enabled */ int ble_ll_scan_enabled(void); -/* Boolean function returns true if whitelist is enabled for scanning */ -int ble_ll_scan_whitelist_enabled(void); - /* Initialize the scanner when we start initiating */ struct hci_create_conn; -int ble_ll_scan_initiator_start(struct hci_create_conn *hcc, - struct ble_ll_scan_sm **sm); +int +ble_ll_scan_initiator_start(struct hci_create_conn *hcc, + struct ble_ll_conn_sm *connsm); /* Returns storage for PDU data (for SCAN_REQ or CONNECT_IND) */ struct ble_ll_scan_pdu_data *ble_ll_scan_get_pdu_data(void); @@ -276,24 +226,14 @@ void ble_ll_scan_wfr_timer_exp(void); /* Called when scan could be interrupted */ void ble_ll_scan_interrupted(struct ble_ll_scan_sm *scansm); -int ble_ll_scan_adv_decode_addr(uint8_t pdu_type, uint8_t *rxbuf, - struct ble_mbuf_hdr *ble_hdr, - uint8_t **addr, uint8_t *addr_type, - uint8_t **inita, uint8_t *init_addr_type, - int *ext_mode); - #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) -int ble_ll_scan_update_aux_data(struct ble_mbuf_hdr *ble_hdr, uint8_t *rxbuf, - bool *adva_present); - /* Initialize the extended scanner when we start initiating */ struct hci_ext_create_conn; -int ble_ll_scan_ext_initiator_start(struct hci_ext_create_conn *hcc, - struct ble_ll_scan_sm **sm); +int +ble_ll_scan_ext_initiator_start(struct hci_ext_create_conn *hcc, + struct ble_ll_conn_sm *connsm); /* Called to parse extended advertising*/ -struct ble_ll_aux_data *ble_ll_scan_aux_data_ref(struct ble_ll_aux_data *aux_scan); -void ble_ll_scan_aux_data_unref(struct ble_ll_aux_data *aux_scan); void ble_ll_scan_end_adv_evt(struct ble_ll_aux_data *aux_data); #endif @@ -316,8 +256,10 @@ int ble_ll_scan_have_rxd_scan_rsp(uint8_t *addr, uint8_t txadd, uint8_t ext_adv, void ble_ll_scan_add_scan_rsp_adv(uint8_t *addr, uint8_t txadd, uint8_t ext_adv, uint16_t adi); -int ble_ll_scan_rx_filter(uint8_t own_addr_type, uint8_t scan_filt_policy, - struct ble_ll_scan_addr_data *addrd, uint8_t *scan_ok); +int +ble_ll_scan_rx_filter(uint8_t own_addr_type, uint8_t scan_filt_policy, + struct ble_ll_scan_addr_data *addrd, uint8_t *scan_ok); +int ble_ll_scan_rx_check_init(struct ble_ll_scan_addr_data *addrd); #ifdef __cplusplus } diff --git a/nimble/controller/include/controller/ble_ll_sched.h b/nimble/controller/include/controller/ble_ll_sched.h index 15f331998..45e148cbd 100644 --- a/nimble/controller/include/controller/ble_ll_sched.h +++ b/nimble/controller/include/controller/ble_ll_sched.h @@ -67,7 +67,6 @@ extern uint8_t g_ble_ll_sched_offset_ticks; #define BLE_LL_SCHED_TYPE_ADV (1) #define BLE_LL_SCHED_TYPE_SCAN (2) #define BLE_LL_SCHED_TYPE_CONN (3) -#define BLE_LL_SCHED_TYPE_AUX_SCAN (4) #define BLE_LL_SCHED_TYPE_DTM (5) #define BLE_LL_SCHED_TYPE_PERIODIC (6) #define BLE_LL_SCHED_TYPE_SYNC (7) diff --git a/nimble/controller/src/ble_ll.c b/nimble/controller/src/ble_ll.c index 8c7095a7e..2702b7c53 100644 --- a/nimble/controller/src/ble_ll.c +++ b/nimble/controller/src/ble_ll.c @@ -677,9 +677,6 @@ ble_ll_wfr_timer_exp(void *arg) case BLE_LL_STATE_SCANNING: ble_ll_scan_wfr_timer_exp(); break; - case BLE_LL_STATE_INITIATING: - ble_ll_conn_init_wfr_timer_exp(); - break; #if MYNEWT_VAL(BLE_LL_DTM) case BLE_LL_STATE_DTM: ble_ll_dtm_wfr_timer_exp(); @@ -842,9 +839,6 @@ ble_ll_rx_pkt_in(void) case BLE_LL_STATE_SCANNING: ble_ll_scan_rx_pkt_in(pdu_type, m, ble_hdr); break; - case BLE_LL_STATE_INITIATING: - ble_ll_init_rx_pkt_in(pdu_type, rxbuf, ble_hdr); - break; #if MYNEWT_VAL(BLE_LL_DTM) case BLE_LL_STATE_DTM: ble_ll_dtm_rx_pkt_in(m, ble_hdr); @@ -980,9 +974,6 @@ ble_ll_rx_start(uint8_t *rxbuf, uint8_t chan, struct ble_mbuf_hdr *rxhdr) case BLE_LL_STATE_ADV: rc = ble_ll_adv_rx_isr_start(pdu_type); break; - case BLE_LL_STATE_INITIATING: - rc = ble_ll_init_rx_isr_start(pdu_type, rxhdr); - break; case BLE_LL_STATE_SCANNING: rc = ble_ll_scan_rx_isr_start(pdu_type, &rxhdr->rxinfo.flags); break; @@ -1122,9 +1113,6 @@ ble_ll_rx_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr) } rc = ble_ll_scan_rx_isr_end(rxpdu, crcok); break; - case BLE_LL_STATE_INITIATING: - rc = ble_ll_init_rx_isr_end(rxbuf, crcok, rxhdr); - break; #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) case BLE_LL_STATE_SCAN_AUX: if (!badpkt) { diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c index 0f930d94e..76dd18e4b 100644 --- a/nimble/controller/src/ble_ll_conn.c +++ b/nimble/controller/src/ble_ll_conn.c @@ -674,64 +674,6 @@ ble_ll_conn_wfr_timer_exp(void) STATS_INC(ble_ll_conn_stats, wfr_expirations); } -void -ble_ll_conn_reset_pending_aux_conn_rsp(void) -{ -#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - return; -#endif - struct ble_ll_conn_sm *connsm; - - connsm = g_ble_ll_conn_create_sm; - if (!connsm) { - return; - } - - if (CONN_F_AUX_CONN_REQ(connsm)) { - STATS_INC(ble_ll_stats, aux_conn_rsp_err); - CONN_F_CONN_REQ_TXD(connsm) = 0; - CONN_F_AUX_CONN_REQ(connsm) = 0; - ble_ll_sched_rmv_elem(&connsm->conn_sch); - return; - } - - return; -} - -bool -ble_ll_conn_init_pending_aux_conn_rsp(void) -{ -#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - return false; -#endif - struct ble_ll_conn_sm *connsm; - - connsm = g_ble_ll_conn_create_sm; - if (!connsm) { - return false; - } - - return CONN_F_AUX_CONN_REQ(connsm); -} - -void -ble_ll_conn_init_wfr_timer_exp(void) -{ -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - struct ble_ll_conn_sm *connsm; - - connsm = g_ble_ll_conn_create_sm; - if (!connsm) { - return; - } - - ble_ll_conn_reset_pending_aux_conn_rsp(); - connsm->inita_identity_used = 0; - - ble_ll_scan_interrupted(connsm->scansm); - -#endif -} /** * Callback for slave when it transmits a data pdu and the connection event * ends after the transmission. @@ -1764,6 +1706,13 @@ void ble_ll_conn_ext_set_params(struct ble_ll_conn_sm *connsm, struct hci_ext_conn_params *hcc_params, int phy) { + connsm->slave_latency = hcc_params->conn_latency; + connsm->supervision_tmo = hcc_params->supervision_timeout; + + connsm->conn_itvl = hcc_params->conn_itvl; + connsm->conn_itvl_ticks = hcc_params->conn_itvl_ticks; + connsm->conn_itvl_usecs = hcc_params->conn_itvl_usecs; + /* Check the min/max CE lengths are less than connection interval */ if (hcc_params->min_ce_len > (connsm->conn_itvl * 2)) { connsm->min_ce_len = connsm->conn_itvl * 2; @@ -1833,7 +1782,6 @@ ble_ll_conn_sm_new(struct ble_ll_conn_sm *connsm) connsm->sub_vers_nr = 0; connsm->reject_reason = BLE_ERR_SUCCESS; connsm->conn_rssi = BLE_LL_CONN_UNKNOWN_RSSI; - connsm->rpa_index = -1; connsm->inita_identity_used = 0; #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) @@ -2667,8 +2615,8 @@ ble_ll_conn_event_end(struct ble_npl_event *ev) * @param txoffset The tx window offset for this connection */ -static void -ble_ll_conn_connect_ind_prepare(struct ble_ll_conn_sm *connsm, +void +ble_ll_conn_prepare_connect_ind(struct ble_ll_conn_sm *connsm, struct ble_ll_scan_pdu_data *pdu_data, uint8_t adva_type, uint8_t *adva, uint8_t inita_type, uint8_t *inita, @@ -2753,110 +2701,8 @@ ble_ll_conn_connect_ind_prepare(struct ble_ll_conn_sm *connsm, pdu_data->hdr_byte = hdr; } -/* Returns true if the address matches the connection peer address having in - * mind privacy mode - */ -static int -ble_ll_conn_is_peer_adv(uint8_t addr_type, uint8_t *adva, int index) -{ - int rc; - uint8_t *peer_addr = NULL; - struct ble_ll_conn_sm *connsm; -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) - struct ble_ll_resolv_entry *rl; -#endif - - /* XXX: Deal with different types of random addresses here! */ - connsm = g_ble_ll_conn_create_sm; - if (!connsm) { - return 0; - } - - switch (connsm->peer_addr_type) { - /* Fall-through intentional */ - case BLE_HCI_CONN_PEER_ADDR_PUBLIC: - case BLE_HCI_CONN_PEER_ADDR_RANDOM: -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) - if (ble_ll_addr_is_id(adva, addr_type)) { - /* Peer uses its identity address. Let's verify privacy mode. - * - * Note: Core Spec 5.0 Vol 6, Part B - * If the Host has added the peer device to the resolving list - * with an all-zero peer IRK, the Controller shall only accept - * the peer's identity address. - */ - if (ble_ll_resolv_enabled()) { - rl = ble_ll_resolv_list_find(adva, addr_type); - if (rl && (rl->rl_priv_mode == BLE_HCI_PRIVACY_NETWORK) && - rl->rl_has_peer) { - return 0; - } - } - } - - /* Check if peer uses RPA. If so and it match, use it as controller - * supports privacy mode - */ - if ((index >= 0) && - (g_ble_ll_resolv_list[index].rl_addr_type == connsm->peer_addr_type)) { - peer_addr = g_ble_ll_resolv_list[index].rl_identity_addr; - } -#endif - /* - * If we are here it means we don't know the device, lets - * check if type is what we are looking for and later - * if address matches - */ - if ((connsm->peer_addr_type == addr_type) && !peer_addr) { - peer_addr = adva; - } - - break; -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) - case BLE_HCI_CONN_PEER_ADDR_PUBLIC_IDENT: - if ((index < 0) || - (g_ble_ll_resolv_list[index].rl_addr_type != 0)) { - return 0; - } - peer_addr = g_ble_ll_resolv_list[index].rl_identity_addr; - break; - case BLE_HCI_CONN_PEER_ADDR_RANDOM_IDENT: - if ((index < 0) || - (g_ble_ll_resolv_list[index].rl_addr_type != 1)) { - return 0; - } - peer_addr = g_ble_ll_resolv_list[index].rl_identity_addr; - break; -#endif - default: - peer_addr = NULL; - break; - } - - rc = 0; - if (peer_addr) { - if (!memcmp(peer_addr, connsm->peer_addr, BLE_DEV_ADDR_LEN)) { - rc = 1; - } - } - - return rc; -} - -static void -ble_ll_conn_connect_ind_txend_to_standby(void *arg) -{ - ble_ll_state_set(BLE_LL_STATE_STANDBY); -} - -static void -ble_ll_conn_connect_ind_txend_to_init(void *arg) -{ - ble_ll_state_set(BLE_LL_STATE_INITIATING); -} - -static uint8_t -ble_ll_conn_connect_ind_tx_pducb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) +uint8_t +ble_ll_conn_tx_connect_ind_pducb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) { struct ble_ll_conn_sm *connsm; struct ble_ll_scan_pdu_data *pdu_data; @@ -2890,30 +2736,6 @@ ble_ll_conn_connect_ind_tx_pducb(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_by return 34; } -/** - * Send a connection requestion to an advertiser - * - * Context: Interrupt - * - * @param addr_type Address type of advertiser - * @param adva Address of advertiser - */ -int -ble_ll_conn_connect_ind_send(struct ble_ll_conn_sm *connsm, uint8_t end_trans) -{ - int rc; - - if (end_trans == BLE_PHY_TRANSITION_NONE) { - ble_phy_set_txend_cb(ble_ll_conn_connect_ind_txend_to_standby, NULL); - } else { - ble_phy_set_txend_cb(ble_ll_conn_connect_ind_txend_to_init, NULL); - } - - rc = ble_phy_tx(ble_ll_conn_connect_ind_tx_pducb, connsm, end_trans); - - return rc; -} - /** * Called when a schedule item overlaps the currently running connection * event. This generally should not happen, but if it does we stop the @@ -2933,596 +2755,142 @@ ble_ll_conn_event_halt(void) } } -/** - * Process a received PDU while in the initiating state. - * - * Context: Link Layer task. - * - * @param pdu_type - * @param rxbuf - * @param ble_hdr - */ -void -ble_ll_init_rx_pkt_in(uint8_t pdu_type, uint8_t *rxbuf, - struct ble_mbuf_hdr *ble_hdr) -{ - uint8_t addr_type; - uint8_t *addr; - uint8_t *adv_addr; - uint8_t *inita; - uint8_t inita_type; - struct ble_ll_conn_sm *connsm; - int ext_adv_mode = -1; -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - struct ble_ll_aux_data *aux_data = NULL; - - if (ble_hdr->rxinfo.user_data) { - /* aux_data just a local helper, no need to ref - * as ble_hdr->rxinfo.user_data is unref in the end of this function - */ - aux_data = ble_hdr->rxinfo.user_data; - } -#endif - - /* Get the connection state machine we are trying to create */ - connsm = g_ble_ll_conn_create_sm; - if (!connsm) { -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - if (aux_data) { - ble_ll_scan_aux_data_unref(ble_hdr->rxinfo.user_data); - ble_hdr->rxinfo.user_data = NULL; - } -#endif - return; - } - - if (!BLE_MBUF_HDR_CRC_OK(ble_hdr)) { - goto scan_continue; - } - -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - if (BLE_MBUF_HDR_AUX_INVALID(ble_hdr)) { - goto scan_continue; - } - - if (pdu_type == BLE_ADV_PDU_TYPE_ADV_EXT_IND) { - if (BLE_MBUF_HDR_WAIT_AUX(ble_hdr)) { - /* Just continue scanning. We are waiting for AUX */ - if (!ble_ll_sched_aux_scan(ble_hdr, connsm->scansm, aux_data)) { - /* ref for aux ptr in the scheduler */ - ble_ll_scan_aux_data_unref(ble_hdr->rxinfo.user_data); - ble_hdr->rxinfo.user_data = NULL; - ble_ll_scan_chk_resume(); - return; - } - goto scan_continue; - } - } - - if (CONN_F_AUX_CONN_REQ(connsm)) { - if (pdu_type != BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP) { - /* Wait for connection response, in this point of time aux is NULL */ - BLE_LL_ASSERT(ble_hdr->rxinfo.user_data == NULL); - return; - } - } -#endif - - /* If we have sent a connect request, we need to enter CONNECTION state */ - if (connsm && CONN_F_CONN_REQ_TXD(connsm)) { - /* Set address of advertiser to which we are connecting. */ - - if (ble_ll_scan_adv_decode_addr(pdu_type, rxbuf, ble_hdr, - &adv_addr, &addr_type, - &inita, &inita_type, &ext_adv_mode)) { - /* Something got wrong, keep trying to connect */ - goto scan_continue; - } - -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) - /* - * Did we resolve this address? If so, set correct peer address - * and peer address type. - */ - if (connsm->rpa_index >= 0) { - addr_type = g_ble_ll_resolv_list[connsm->rpa_index].rl_addr_type + 2; - addr = g_ble_ll_resolv_list[connsm->rpa_index].rl_identity_addr; - } else { - addr = adv_addr; - } -#else - addr = adv_addr; -#endif - - if (connsm->rpa_index >= 0) { - connsm->peer_addr_type = addr_type; - memcpy(connsm->peer_addr, addr, BLE_DEV_ADDR_LEN); - - ble_ll_scan_set_peer_rpa(adv_addr); - -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) - /* Update resolving list with current peer RPA */ - ble_ll_resolv_set_peer_rpa(connsm->rpa_index, rxbuf + BLE_LL_PDU_HDR_LEN); - if (ble_ll_is_rpa(inita, inita_type)) { - ble_ll_resolv_set_local_rpa(connsm->rpa_index, inita); - } - -#endif - } else if (ble_ll_scan_whitelist_enabled()) { - /* if WL is used we need to store peer addr also if it was not - * resolved - */ - connsm->peer_addr_type = addr_type; - memcpy(connsm->peer_addr, addr, BLE_DEV_ADDR_LEN); - } - - /* Connection has been created. Stop scanning */ - g_ble_ll_conn_create_sm = NULL; - ble_ll_scan_sm_stop(0); - - /* For AUX Connect CSA2 is mandatory. Otherwise we need to check bit - * mask - */ - if (ble_hdr->rxinfo.channel < BLE_PHY_NUM_DATA_CHANS) { - ble_ll_conn_set_csa(connsm, 1); - } else { - ble_ll_conn_set_csa(connsm, rxbuf[0] & BLE_ADV_PDU_HDR_CHSEL_MASK); - } - -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) -#if (BLE_LL_BT5_PHY_SUPPORTED == 1) - /* Lets take last used phy */ - ble_ll_conn_init_phy(connsm, ble_hdr->rxinfo.phy); -#endif - if (aux_data) { - ble_ll_scan_aux_data_unref(ble_hdr->rxinfo.user_data); - ble_hdr->rxinfo.user_data = NULL; - } -#endif - ble_ll_conn_created(connsm, NULL); - return; - } - -scan_continue: -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - /* Drop last reference and keep continue to connect */ - if (aux_data) { - ble_ll_scan_aux_data_unref(ble_hdr->rxinfo.user_data); - ble_hdr->rxinfo.user_data = NULL; - } -#endif - ble_ll_scan_chk_resume(); -} - -/** - * Called when a receive PDU has started and we are in the initiating state. - * - * Context: Interrupt - * - * @param pdu_type - * @param ble_hdr - * - * @return int - * 0: we will not attempt to reply to this frame - * 1: we may send a response to this frame. - */ int -ble_ll_init_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *ble_hdr) +ble_ll_conn_send_connect_req(struct os_mbuf *rxpdu, + struct ble_ll_scan_addr_data *addrd, + uint8_t ext) { struct ble_ll_conn_sm *connsm; + struct ble_mbuf_hdr *rxhdr; + int8_t rpa_index; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + struct hci_ext_conn_params *ecp; + uint8_t phy; +#endif + int rc; connsm = g_ble_ll_conn_create_sm; - if (!connsm) { - return 0; - } - - if ((pdu_type == BLE_ADV_PDU_TYPE_ADV_IND) || - (pdu_type == BLE_ADV_PDU_TYPE_ADV_DIRECT_IND || - pdu_type == BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP)) { - return 1; - } + rxhdr = BLE_MBUF_HDR_PTR(rxpdu); #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - if (pdu_type == BLE_ADV_PDU_TYPE_ADV_EXT_IND && - connsm->scansm->ext_scanning) { - if (connsm->scansm->cur_aux_data) { - STATS_INC(ble_ll_stats, aux_received); - } - - ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_EXT_ADV; - return 1; + if (ext) { +#if BLE_LL_BT5_PHY_SUPPORTED + phy = rxhdr->rxinfo.phy; +#else + phy = BLE_PHY_1M; +#endif + ecp = &connsm->initial_params.params[phy - 1]; + ble_ll_conn_ext_set_params(connsm, ecp, phy); } #endif + if (ble_ll_sched_master_new(connsm, rxhdr, 0)) { + return -1; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + rpa_index = addrd->rpa_index; +#else + rpa_index = -1; +#endif + ble_ll_conn_prepare_connect_ind(connsm, ble_ll_scan_get_pdu_data(), + addrd->adva_type, addrd->adva, + addrd->targeta_type, addrd->targeta, + rpa_index, rxhdr->rxinfo.channel); + + ble_phy_set_txend_cb(NULL, NULL); + rc = ble_phy_tx(ble_ll_conn_tx_connect_ind_pducb, connsm, + ext ? BLE_PHY_TRANSITION_TX_RX : BLE_PHY_TRANSITION_NONE); + if (rc) { + ble_ll_conn_send_connect_req_cancel(); + return -1; + } + return 0; } -/** - * Called when a receive PDU has ended and we are in the initiating state. - * - * Context: Interrupt - * - * @param rxpdu - * @param crcok - * @param ble_hdr - * - * @return int - * < 0: Disable the phy after reception. - * == 0: Success. Do not disable the PHY. - * > 0: Do not disable PHY as that has already been done. - */ -int -ble_ll_init_rx_isr_end(uint8_t *rxbuf, uint8_t crcok, - struct ble_mbuf_hdr *ble_hdr) +void +ble_ll_conn_send_connect_req_cancel(void) { - int rc; - int resolved; - int chk_wl; - int index; - uint8_t pdu_type; - uint8_t adv_addr_type; - uint8_t peer_addr_type; - uint8_t *adv_addr = NULL; - uint8_t *peer; - uint8_t *init_addr = NULL; - uint8_t init_addr_type; - uint8_t pyld_len; - uint8_t inita_is_rpa; - uint8_t conn_req_end_trans; - struct os_mbuf *rxpdu; struct ble_ll_conn_sm *connsm; -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) - struct ble_ll_resolv_entry *rl; -#endif -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - struct hci_ext_conn_params *hcp; - struct ble_ll_scan_sm *scansm; - uint8_t phy; -#endif - int ext_adv_mode = -1; - /* Get connection state machine to use if connection to be established */ connsm = g_ble_ll_conn_create_sm; - /* This could happen if connection init was cancelled while isr end was - * already pending - */ - if (!connsm) { - ble_ll_state_set(BLE_LL_STATE_STANDBY); - return -1; - } - rc = -1; - pdu_type = rxbuf[0] & BLE_ADV_PDU_HDR_TYPE_MASK; - pyld_len = rxbuf[1]; + ble_ll_sched_rmv_elem(&connsm->conn_sch); +} -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - scansm = connsm->scansm; - if (scansm->cur_aux_data) { - ble_hdr->rxinfo.user_data = scansm->cur_aux_data; - scansm->cur_aux_data = NULL; - } -#endif +static void +ble_ll_conn_master_start(uint8_t phy, uint8_t csa, + struct ble_ll_scan_addr_data *addrd, uint8_t *targeta) +{ + struct ble_ll_conn_sm *connsm; - if (!crcok) { -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - /* Invalid packet - make sure we do not wait for AUX_CONNECT_RSP */ - ble_ll_conn_reset_pending_aux_conn_rsp(); -#endif + connsm = g_ble_ll_conn_create_sm; + g_ble_ll_conn_create_sm = NULL; - /* Ignore this packet */ - goto init_rx_isr_exit; - } - -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - /* If we sent AUX_CONNECT_REQ, we only expect AUX_CONNECT_RSP here */ - if (CONN_F_AUX_CONN_REQ(connsm)) { - if (pdu_type != BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP) { - STATS_INC(ble_ll_stats, aux_conn_rsp_err); - CONN_F_CONN_REQ_TXD(connsm) = 0; - CONN_F_AUX_CONN_REQ(connsm) = 0; - ble_ll_sched_rmv_elem(&connsm->conn_sch); - } - goto init_rx_isr_exit; - } -#endif - - inita_is_rpa = 0; - -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - if (pdu_type == BLE_ADV_PDU_TYPE_ADV_EXT_IND) { - if (!scansm->ext_scanning) { - goto init_rx_isr_exit; - } - - rc = ble_ll_scan_update_aux_data(ble_hdr, rxbuf, NULL); - if (rc < 0) { - /* No memory or broken packet */ - ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_AUX_INVALID; - goto init_rx_isr_exit; - } - } -#endif - - /* Lets get addresses from advertising report*/ - if (ble_ll_scan_adv_decode_addr(pdu_type, rxbuf, ble_hdr, - &adv_addr, &adv_addr_type, - &init_addr, &init_addr_type, - &ext_adv_mode)) { -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_AUX_INVALID; -#endif - goto init_rx_isr_exit; - } - - switch (pdu_type) { - case BLE_ADV_PDU_TYPE_ADV_IND: - break; - -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - case BLE_ADV_PDU_TYPE_ADV_EXT_IND: - rc = -1; - - /* If this is not connectable adv mode, lets skip it */ - if (!(ext_adv_mode & BLE_LL_EXT_ADV_MODE_CONN)) { - goto init_rx_isr_exit; - } - - if (!adv_addr) { - ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_AUX_PTR_WAIT; - goto init_rx_isr_exit; - } - - if (!init_addr) { - break; - } - /* if there is direct address lets fall down and check it.*/ - // no break -#endif - case BLE_ADV_PDU_TYPE_ADV_DIRECT_IND: - inita_is_rpa = (uint8_t)ble_ll_is_rpa(init_addr, init_addr_type); - if (!inita_is_rpa) { - - /* Resolving will be done later. Check if identity InitA matches */ - if (!ble_ll_is_our_devaddr(init_addr, init_addr_type)) { - goto init_rx_isr_exit; - } - } -#if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) - else { - /* If privacy is off - reject RPA InitA*/ - goto init_rx_isr_exit; - } -#endif - - break; - default: - goto init_rx_isr_exit; - } - - /* Should we send a connect request? */ - index = -1; - peer = adv_addr; - peer_addr_type = adv_addr_type; - - resolved = 0; - chk_wl = ble_ll_scan_whitelist_enabled(); + connsm->peer_addr_type = addrd->adv_addr_type; + memcpy(connsm->peer_addr, addrd->adv_addr, 6); #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) - if (ble_ll_is_rpa(adv_addr, adv_addr_type) && ble_ll_resolv_enabled()) { - index = ble_hw_resolv_list_match(); - if (index >= 0) { - rl = &g_ble_ll_resolv_list[index]; - - ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_RESOLVED; - connsm->rpa_index = index; - peer = rl->rl_identity_addr; - peer_addr_type = rl->rl_addr_type; - resolved = 1; - - /* Assure privacy */ - if ((rl->rl_priv_mode == BLE_HCI_PRIVACY_NETWORK) && init_addr && - !inita_is_rpa && rl->rl_has_local) { - goto init_rx_isr_exit; - } - - /* - * If the InitA is a RPA, we must see if it resolves based on the - * identity address of the resolved ADVA. - */ - if (init_addr && inita_is_rpa) { - if (!ble_ll_resolv_rpa(init_addr, - g_ble_ll_resolv_list[index].rl_local_irk)) { - goto init_rx_isr_exit; - } - - /* Core Specification Vol 6, Part B, Section 6.4: - * "The Link Layer should not set the InitA field to the same - * value as the TargetA field in the received advertising PDU." - * - * We update the received PDU directly here, so ble_ll_init_rx_pkt_in - * can process it as is. - */ - memcpy(init_addr, rl->rl_local_rpa, BLE_DEV_ADDR_LEN); - } - - } else { - if (chk_wl) { - goto init_rx_isr_exit; - } - - /* Could not resolved InitA */ - if (init_addr && inita_is_rpa) { - goto init_rx_isr_exit; - } - } - } else if (init_addr) { - - /* If resolving is off and InitA is RPA we reject advertising */ - if (inita_is_rpa && !ble_ll_resolv_enabled()) { - goto init_rx_isr_exit; - } - - /* Let's see if we have IRK with that peer.*/ - rl = ble_ll_resolv_list_find(adv_addr, adv_addr_type); - - /* Lets make sure privacy mode is correct together with InitA in case it - * is identity address - */ - if (rl && !inita_is_rpa && - (rl->rl_priv_mode == BLE_HCI_PRIVACY_NETWORK) && - rl->rl_has_local) { - goto init_rx_isr_exit; - } - - /* - * If the InitA is a RPA, we must see if it resolves based on the - * identity address of the resolved ADVA. - */ - if (inita_is_rpa) { - if (!rl || !ble_ll_resolv_rpa(init_addr, rl->rl_local_irk)) { - goto init_rx_isr_exit; - } - - /* Core Specification Vol 6, Part B, Section 6.4: - * "The Link Layer should not set the InitA field to the same - * value as the TargetA field in the received advertising PDU." - * - * We update the received PDU directly here, so ble_ll_init_rx_pkt_in - * can process it as is. - */ - memcpy(init_addr, rl->rl_local_rpa, BLE_DEV_ADDR_LEN); - } - } else if (!ble_ll_is_rpa(adv_addr, adv_addr_type)) { - /* undirected with ID address, assure privacy if on RL */ - rl = ble_ll_resolv_list_find(adv_addr, adv_addr_type); - if (rl && (rl->rl_priv_mode == BLE_HCI_PRIVACY_NETWORK) && - rl->rl_has_peer) { - goto init_rx_isr_exit; - } - } -#endif - - /* Check filter policy */ - if (chk_wl) { - if (!ble_ll_whitelist_match(peer, peer_addr_type, resolved)) { - goto init_rx_isr_exit; - } + if (addrd->adva_resolved) { + BLE_LL_ASSERT(addrd->rpa_index >= 0); + connsm->peer_addr_resolved = 1; + ble_ll_resolv_set_peer_rpa(addrd->rpa_index, addrd->adva); + ble_ll_scan_set_peer_rpa(addrd->adva); } else { - /* Must match the connection address */ - if (!ble_ll_conn_is_peer_adv(adv_addr_type, adv_addr, index)) { - goto init_rx_isr_exit; - } - } - ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_DEVMATCH; - - /* For CONNECT_IND we don't go into RX state */ - conn_req_end_trans = BLE_PHY_TRANSITION_NONE; - -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - /* Check if we should send AUX_CONNECT_REQ and wait for AUX_CONNECT_RSP */ - if (ble_hdr->rxinfo.channel < BLE_PHY_NUM_DATA_CHANS) { - conn_req_end_trans = BLE_PHY_TRANSITION_TX_RX; + connsm->peer_addr_resolved = 0; } - if (connsm->scansm->ext_scanning) { - phy = ble_hdr->rxinfo.phy; - - hcp = &connsm->initial_params.params[phy - 1]; - - connsm->slave_latency = hcp->conn_latency; - connsm->supervision_tmo = hcp->supervision_timeout; - - connsm->conn_itvl = hcp->conn_itvl; - connsm->conn_itvl_ticks = hcp->conn_itvl_ticks; - connsm->conn_itvl_usecs = hcp->conn_itvl_usecs; + if (addrd->targeta_resolved) { + BLE_LL_ASSERT(addrd->rpa_index >= 0); + BLE_LL_ASSERT(targeta); + ble_ll_resolv_set_local_rpa(addrd->rpa_index, targeta); } #endif - /* Schedule new connection */ - if (ble_ll_sched_master_new(connsm, ble_hdr, pyld_len)) { - STATS_INC(ble_ll_conn_stats, cant_set_sched); - goto init_rx_isr_exit; - } - - /* Prepare data for connect request */ - ble_ll_conn_connect_ind_prepare(connsm, - ble_ll_scan_get_pdu_data(), - adv_addr_type, adv_addr, - init_addr_type, init_addr, - index, ble_hdr->rxinfo.channel); - - /* Setup to transmit the connect request */ - rc = ble_ll_conn_connect_ind_send(connsm, conn_req_end_trans); - if (rc) { - ble_ll_sched_rmv_elem(&connsm->conn_sch); - goto init_rx_isr_exit; - } - - if (init_addr && !inita_is_rpa) { - connsm->inita_identity_used = 1; - } - - CONN_F_CONN_REQ_TXD(connsm) = 1; - -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - if (ble_hdr->rxinfo.channel < BLE_PHY_NUM_DATA_CHANS) { - /* Lets wait for AUX_CONNECT_RSP */ - CONN_F_AUX_CONN_REQ(connsm) = 1; - /* Keep aux data until we get scan response */ - scansm->cur_aux_data = ble_hdr->rxinfo.user_data; - ble_hdr->rxinfo.user_data = NULL; - STATS_INC(ble_ll_stats, aux_conn_req_tx); - } - - if (connsm->scansm->ext_scanning) { - phy = ble_hdr->rxinfo.phy; - - /* Update connection state machine with appropriate parameters for - * certain PHY - */ - ble_ll_conn_ext_set_params(connsm, - &connsm->initial_params.params[phy - 1], - phy); - - } + ble_ll_conn_set_csa(connsm, csa); +#if BLE_LL_BT5_PHY_SUPPORTED + ble_ll_conn_init_phy(connsm, phy); #endif - - STATS_INC(ble_ll_conn_stats, conn_req_txd); - -init_rx_isr_exit: - - /* - * We have to restart receive if we cant hand up pdu. We return 0 so that - * the phy does not get disabled. - */ - rxpdu = ble_ll_rxpdu_alloc(pyld_len + BLE_LL_PDU_HDR_LEN); - if (rxpdu == NULL) { - /* - * XXX: possible allocate the PDU when we start initiating? - * I cannot say I like this solution, but if we cannot allocate a PDU - * to hand up to the LL, we need to remove the connection we just - * scheduled since the connection state machine will not get processed - * by link layer properly. For now, just remove it from the scheduler - */ - if (CONN_F_CONN_REQ_TXD(connsm) == 1) { - CONN_F_CONN_REQ_TXD(connsm) = 0; - CONN_F_AUX_CONN_REQ(connsm) = 0; - ble_ll_sched_rmv_elem(&connsm->conn_sch); - } - ble_phy_restart_rx(); - rc = 0; - } else { - ble_phy_rxpdu_copy(rxbuf, rxpdu); - ble_ll_rx_pdu_in(rxpdu); - } - - if (rc) { - ble_ll_state_set(BLE_LL_STATE_STANDBY); - } - - return rc; + ble_ll_conn_created(connsm, NULL); } +void +ble_ll_conn_created_on_legacy(struct os_mbuf *rxpdu, + struct ble_ll_scan_addr_data *addrd, + uint8_t *targeta) +{ + uint8_t *rxbuf; + uint8_t csa; + + rxbuf = rxpdu->om_data; + csa = rxbuf[0] & BLE_ADV_PDU_HDR_CHSEL_MASK; + + ble_ll_conn_master_start(BLE_PHY_1M, csa, addrd, targeta); +} + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) +void +ble_ll_conn_created_on_aux(struct os_mbuf *rxpdu, + struct ble_ll_scan_addr_data *addrd, + uint8_t *targeta) +{ +#if BLE_LL_BT5_PHY_SUPPORTED + struct ble_mbuf_hdr *rxhdr; +#endif + uint8_t phy; + +#if BLE_LL_BT5_PHY_SUPPORTED + rxhdr = BLE_MBUF_HDR_PTR(rxpdu); + phy = rxhdr->rxinfo.phy; +#else + phy = BLE_PHY_1M; +#endif + + ble_ll_conn_master_start(phy, 1, addrd, targeta); +} +#endif /* BLE_LL_CFG_FEAT_LL_EXT_ADV */ + /** * Function called when a timeout has occurred for a connection. There are * two types of timeouts: a connection supervision timeout and control diff --git a/nimble/controller/src/ble_ll_conn_hci.c b/nimble/controller/src/ble_ll_conn_hci.c index fe384c414..e53918f96 100644 --- a/nimble/controller/src/ble_ll_conn_hci.c +++ b/nimble/controller/src/ble_ll_conn_hci.c @@ -179,13 +179,12 @@ ble_ll_conn_comp_event_send(struct ble_ll_conn_sm *connsm, uint8_t status, memcpy(enh_ev->local_rpa, rpa, BLE_DEV_ADDR_LEN); } - /* We need to adjust peer type if device connected using RPA - * and was resolved since RPA needs to be added to HCI event. - */ - if (connsm->peer_addr_type < BLE_HCI_CONN_PEER_ADDR_PUBLIC_IDENT - && (connsm->rpa_index > -1)) { +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + /* Adjust address type if peer address was resolved */ + if (connsm->peer_addr_resolved) { enh_ev->peer_addr_type += 2; } +#endif if (enh_ev->peer_addr_type > BLE_HCI_CONN_PEER_ADDR_RANDOM) { if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { @@ -220,10 +219,6 @@ ble_ll_conn_comp_event_send(struct ble_ll_conn_sm *connsm, uint8_t status, ev->conn_handle = htole16(connsm->conn_handle); ev->role = connsm->conn_role - 1; ev->peer_addr_type = connsm->peer_addr_type; - - if (ev->peer_addr_type > BLE_HCI_CONN_PEER_ADDR_RANDOM) { - ev->peer_addr_type -= 2; - } memcpy(ev->peer_addr, connsm->peer_addr, BLE_DEV_ADDR_LEN); ev->conn_itvl = htole16(connsm->conn_itvl); ev->conn_latency = htole16(connsm->slave_latency); @@ -520,7 +515,7 @@ ble_ll_conn_create(const uint8_t *cmdbuf, uint8_t len) /* CSA will be selected when advertising is received */ /* Start scanning */ - rc = ble_ll_scan_initiator_start(&hcc, &connsm->scansm); + rc = ble_ll_scan_initiator_start(&hcc, connsm); if (rc) { SLIST_REMOVE(&g_ble_ll_conn_active_list,connsm,ble_ll_conn_sm,act_sle); STAILQ_INSERT_TAIL(&g_ble_ll_conn_free_list, connsm, free_stqe); @@ -777,10 +772,8 @@ ble_ll_ext_conn_create(const uint8_t *cmdbuf, uint8_t len) ble_ll_conn_ext_master_init(connsm, &hcc); ble_ll_conn_sm_new(connsm); - /* CSA will be selected when advertising is received */ - /* Start scanning */ - rc = ble_ll_scan_ext_initiator_start(&hcc, &connsm->scansm); + rc = ble_ll_scan_ext_initiator_start(&hcc, connsm); if (rc) { SLIST_REMOVE(&g_ble_ll_conn_active_list,connsm,ble_ll_conn_sm,act_sle); STAILQ_INSERT_TAIL(&g_ble_ll_conn_free_list, connsm, free_stqe); diff --git a/nimble/controller/src/ble_ll_conn_priv.h b/nimble/controller/src/ble_ll_conn_priv.h index 915a4d2a6..099ce77f5 100644 --- a/nimble/controller/src/ble_ll_conn_priv.h +++ b/nimble/controller/src/ble_ll_conn_priv.h @@ -148,18 +148,10 @@ void ble_ll_conn_tx_pkt_in(struct os_mbuf *om, uint16_t handle, uint16_t len); int ble_ll_conn_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa); int ble_ll_conn_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr); void ble_ll_conn_rx_data_pdu(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr); -void ble_ll_init_rx_pkt_in(uint8_t pdu_type, uint8_t *rxbuf, - struct ble_mbuf_hdr *ble_hdr); -int ble_ll_init_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *ble_hdr); -int ble_ll_init_rx_isr_end(uint8_t *rxbuf, uint8_t crcok, - struct ble_mbuf_hdr *ble_hdr); void ble_ll_conn_wfr_timer_exp(void); -void ble_ll_conn_init_wfr_timer_exp(void); int ble_ll_conn_is_lru(struct ble_ll_conn_sm *s1, struct ble_ll_conn_sm *s2); uint32_t ble_ll_conn_get_ce_end_time(void); void ble_ll_conn_event_halt(void); -void ble_ll_conn_reset_pending_aux_conn_rsp(void); -bool ble_ll_conn_init_pending_aux_conn_rsp(void); /* HCI */ void ble_ll_disconn_comp_event_send(struct ble_ll_conn_sm *connsm, uint8_t reason); diff --git a/nimble/controller/src/ble_ll_scan.c b/nimble/controller/src/ble_ll_scan.c index 9ce7877b1..0d9d36f66 100644 --- a/nimble/controller/src/ble_ll_scan.c +++ b/nimble/controller/src/ble_ll_scan.c @@ -25,7 +25,6 @@ #include "os/os.h" #include "os/os_cputime.h" #include "nimble/ble.h" -#include "nimble/nimble_opt.h" #include "nimble/hci_common.h" #include "nimble/ble_hci_trans.h" #include "controller/ble_phy.h" @@ -129,122 +128,9 @@ static struct os_mempool g_scan_dup_pool; static TAILQ_HEAD(ble_ll_scan_dup_list, ble_ll_scan_dup_entry) g_scan_dup_list; #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) -#if MYNEWT_VAL(BLE_LL_EXT_ADV_AUX_PTR_CNT) != 0 -static os_membuf_t ext_scan_aux_mem[ OS_MEMPOOL_SIZE( - MYNEWT_VAL(BLE_LL_EXT_ADV_AUX_PTR_CNT), - sizeof (struct ble_ll_aux_data)) -]; -#else -#define ext_scan_aux_mem NULL -#endif - -static struct os_mempool ext_scan_aux_pool; - -static int ble_ll_scan_start(struct ble_ll_scan_sm *scansm, - struct ble_ll_sched_item *sch); - -static void -ble_ll_aux_scan_drop_event_cb(struct ble_npl_event *ev) -{ - struct ble_ll_aux_data *aux_data = ble_npl_event_get_arg(ev); - - ble_ll_scan_end_adv_evt(aux_data); - ble_ll_scan_aux_data_unref(aux_data); -} - -static void -ble_ll_aux_scan_drop(struct ble_ll_aux_data *aux_data) -{ - BLE_LL_ASSERT(aux_data); - - STATS_INC(ble_ll_stats, aux_scan_drop); - - ble_npl_event_init(&aux_data->ev, ble_ll_aux_scan_drop_event_cb, aux_data); - ble_ll_event_send(&aux_data->ev); -} - static int -ble_ll_aux_scan_cb(struct ble_ll_sched_item *sch) -{ - struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; - uint8_t lls = ble_ll_state_get(); - uint32_t wfr_usec; +ble_ll_scan_start(struct ble_ll_scan_sm *scansm); - STATS_INC(ble_ll_stats, aux_sched_cb); - - /* Drop the scheduled item if scan was disable or there is aux or scan - * response pending - */ - if (!scansm->scan_enabled || scansm->cur_aux_data || - scansm->scan_rsp_pending) { - ble_ll_aux_scan_drop(sch->cb_arg); - sch->cb_arg = NULL; - goto done; - } - - /* Check if there is no aux connect sent. If so drop the sched item */ - if (lls == BLE_LL_STATE_INITIATING && ble_ll_conn_init_pending_aux_conn_rsp()) { - ble_ll_aux_scan_drop(sch->cb_arg); - sch->cb_arg = NULL; - goto done; - } - - /* This function is called only when scanner is running. This can happen - * in 3 states: - * BLE_LL_STATE_SCANNING - * BLE_LL_STATE_INITIATING - * BLE_LL_STATE_STANDBY - */ - if (lls != BLE_LL_STATE_STANDBY) { - ble_phy_disable(); - ble_ll_state_set(BLE_LL_STATE_STANDBY); - } - - /* When doing RX for AUX pkt, cur_aux_data keeps valid aux data */ - scansm->cur_aux_data = sch->cb_arg; - sch->cb_arg = NULL; - BLE_LL_ASSERT(scansm->cur_aux_data != NULL); - scansm->cur_aux_data->scanning = 1; - - if (ble_ll_scan_start(scansm, sch)) { - ble_ll_scan_interrupted(scansm); - goto done; - } - - STATS_INC(ble_ll_stats, aux_fired_for_read); - - wfr_usec = scansm->cur_aux_data->offset_units ? 300 : 30; - ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_RX, 0, wfr_usec); - -done: - - return BLE_LL_SCHED_STATE_DONE; -} - -static int -ble_ll_scan_ext_adv_init(struct ble_ll_aux_data **aux_data) -{ - struct ble_ll_aux_data *e; - - e = os_memblock_get(&ext_scan_aux_pool); - if (!e) { - return -1; - } - - memset(e, 0, sizeof(*e)); - e->sch.sched_cb = ble_ll_aux_scan_cb; - e->sch.sched_type = BLE_LL_SCHED_TYPE_AUX_SCAN; - e->ref_cnt = 1; -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) - e->rpa_index = -1; -#endif - ble_ll_trace_u32x2(BLE_LL_TRACE_ID_AUX_REF, (uint32_t)e, e->ref_cnt); - - *aux_data = e; - STATS_INC(ble_ll_stats, aux_allocated); - - return 0; -} #endif static inline uint32_t @@ -481,28 +367,11 @@ ble_ll_scan_end_adv_evt(struct ble_ll_aux_data *aux_data) } #endif -static void -ble_ll_scan_clean_cur_aux_data(void) -{ -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; - - /* If scanner was reading aux ptr, we need to clean it up */ - if (scansm->cur_aux_data) { - ble_ll_scan_end_adv_evt(scansm->cur_aux_data); - ble_ll_scan_aux_data_unref(scansm->cur_aux_data); - scansm->cur_aux_data = NULL; - } -#endif -} - void ble_ll_scan_halt(void) { struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; - ble_ll_scan_clean_cur_aux_data(); - /* Update backoff if we failed to receive scan response */ if (scansm->scan_rsp_pending) { scansm->scan_rsp_pending = 0; @@ -859,27 +728,6 @@ done: } } -static void -ble_ll_get_chan_to_scan(struct ble_ll_scan_sm *scansm, uint8_t *chan, - int *phy) -{ - struct ble_ll_scan_phy *scanp = scansm->scanp; -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - struct ble_ll_aux_data *aux_data = scansm->cur_aux_data; - - if (!scansm->ext_scanning || !aux_data || !aux_data->scanning) { - *chan = scanp->scan_chan; - *phy = scanp->phy; - return; - } - - *chan = aux_data->chan; - *phy = aux_data->aux_phy; -#else - *chan = scanp->scan_chan; - *phy = scanp->phy; -#endif -} /** * Called to enable the receiver for scanning. * @@ -890,29 +738,21 @@ ble_ll_get_chan_to_scan(struct ble_ll_scan_sm *scansm, uint8_t *chan, * @return int */ static int -ble_ll_scan_start(struct ble_ll_scan_sm *scansm, struct ble_ll_sched_item *sch) +ble_ll_scan_start(struct ble_ll_scan_sm *scansm) { int rc; struct ble_ll_scan_phy *scanp = scansm->scanp; - uint8_t scan_chan; + uint8_t chan; #if (BLE_LL_BT5_PHY_SUPPORTED == 1) uint8_t phy_mode; -#endif int phy; +#endif BLE_LL_ASSERT(scansm->scan_rsp_pending == 0); - ble_ll_get_chan_to_scan(scansm, &scan_chan, &phy); - - /* XXX: right now scheduled item is only present if we schedule for aux - * scan just make sanity check that we have proper combination of - * sch and resulting scan_chan - */ - BLE_LL_ASSERT(!sch || scan_chan < BLE_PHY_ADV_CHAN_START); - BLE_LL_ASSERT(sch || scan_chan >= BLE_PHY_ADV_CHAN_START); - /* Set channel */ - rc = ble_phy_setchan(scan_chan, BLE_ACCESS_ADDR_ADV, BLE_LL_CRCINIT_ADV); + chan = scanp->scan_chan; + rc = ble_phy_setchan(chan, BLE_ACCESS_ADDR_ADV, BLE_LL_CRCINIT_ADV); BLE_LL_ASSERT(rc == 0); /* @@ -934,20 +774,13 @@ ble_ll_scan_start(struct ble_ll_scan_sm *scansm, struct ble_ll_sched_item *sch) #endif #if (BLE_LL_BT5_PHY_SUPPORTED == 1) + phy = scanp->phy; phy_mode = ble_ll_phy_to_phy_mode(phy, BLE_HCI_LE_PHY_CODED_ANY); ble_phy_mode_set(phy_mode, phy_mode); #endif - /* XXX: probably need to make sure hfxo is running too */ - /* XXX: can make this better; want to just start asap. */ - if (sch) { - rc = ble_phy_rx_set_start_time(sch->start_time + - g_ble_ll_sched_offset_ticks, - sch->remainder); - } else { - rc = ble_phy_rx_set_start_time(os_cputime_get32() + - g_ble_ll_sched_offset_ticks, 0); - } + rc = ble_phy_rx_set_start_time(os_cputime_get32() + + g_ble_ll_sched_offset_ticks, 0); if (!rc || rc == BLE_PHY_ERR_RX_LATE) { /* If we are late here, it is still OK because we keep scanning. * Clear error @@ -961,12 +794,7 @@ ble_ll_scan_start(struct ble_ll_scan_sm *scansm, struct ble_ll_sched_item *sch) ble_ll_whitelist_disable(); } - /* Set link layer state to scanning */ - if (scanp->scan_type == BLE_SCAN_TYPE_INITIATE) { - ble_ll_state_set(BLE_LL_STATE_INITIATING); - } else { - ble_ll_state_set(BLE_LL_STATE_SCANNING); - } + ble_ll_state_set(BLE_LL_STATE_SCANNING); } return rc; @@ -1020,76 +848,6 @@ ble_ll_scan_is_inside_window(struct ble_ll_scan_phy *scanp, uint32_t time) CPUTIME_LT(time, start_time + scanp->timing.window); } -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) -static void -ble_ll_scan_aux_data_free(struct ble_ll_aux_data *aux_data) -{ - if (aux_data) { - if (aux_data->evt) { - ble_hci_trans_buf_free((uint8_t *)aux_data->evt); - aux_data->evt = NULL; - } - os_memblock_put(&ext_scan_aux_pool, aux_data); - STATS_INC(ble_ll_stats, aux_freed); - } -} - -struct ble_ll_aux_data * -ble_ll_scan_aux_data_ref(struct ble_ll_aux_data *aux_data) -{ - os_sr_t sr; - - BLE_LL_ASSERT(aux_data); - - OS_ENTER_CRITICAL(sr); - aux_data->ref_cnt++; - ble_ll_trace_u32x2(BLE_LL_TRACE_ID_AUX_REF, (uint32_t) aux_data, aux_data->ref_cnt); - - OS_EXIT_CRITICAL(sr); - - return aux_data; -} - -void -ble_ll_scan_aux_data_unref(struct ble_ll_aux_data *aux_data) -{ - os_sr_t sr; - - BLE_LL_ASSERT(aux_data); - - OS_ENTER_CRITICAL(sr); - aux_data->ref_cnt--; - ble_ll_trace_u32x2(BLE_LL_TRACE_ID_AUX_UNREF, (uint32_t) aux_data, aux_data->ref_cnt); - - if (aux_data->ref_cnt == 0) { - /* - * Some validation to make sure that we completed scan properly: - * - we either did not send any report or sent completed/truncated - * - we only sent one of completed/truncated - * - in case of error, we wither did not send anything or sent truncated - */ - BLE_LL_ASSERT(!(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY) || - ((aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY) && - (aux_data->flags_ll & (BLE_LL_AUX_FLAG_HCI_SENT_COMPLETED | BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED)))); - BLE_LL_ASSERT(!(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_COMPLETED) || !(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED)); - BLE_LL_ASSERT(!(aux_data->flags_ll & BLE_LL_AUX_FLAG_SCAN_ERROR) || - !(aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_ANY) || - (aux_data->flags_ll & BLE_LL_AUX_FLAG_HCI_SENT_TRUNCATED)); - - ble_ll_scan_aux_data_free(aux_data); - } - - OS_EXIT_CRITICAL(sr); -} - -static void -ble_ll_scan_sched_remove(struct ble_ll_sched_item *sch) -{ - ble_ll_scan_end_adv_evt(sch->cb_arg); - ble_ll_scan_aux_data_unref(sch->cb_arg); - sch->cb_arg = NULL; -} -#endif /** * Stop the scanning state machine */ @@ -1109,8 +867,7 @@ ble_ll_scan_sm_stop(int chk_disable) OS_ENTER_CRITICAL(sr); lls = ble_ll_state_get(); - if ((lls == BLE_LL_STATE_SCANNING) || - (lls == BLE_LL_STATE_INITIATING && chk_disable == 1)) { + if (lls == BLE_LL_STATE_SCANNING) { /* Disable phy */ ble_phy_disable(); @@ -1122,6 +879,8 @@ ble_ll_scan_sm_stop(int chk_disable) OS_ENTER_CRITICAL(sr); + scansm->connsm = NULL; + /* Disable scanning state machine */ scansm->scan_enabled = 0; scansm->restart_timer_needed = 0; @@ -1129,8 +888,6 @@ ble_ll_scan_sm_stop(int chk_disable) #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) if (scansm->ext_scanning) { ble_ll_sched_rmv_elem_type(BLE_LL_SCHED_TYPE_SCAN_AUX, ble_ll_scan_aux_sched_remove); - ble_ll_scan_clean_cur_aux_data(); - ble_ll_sched_rmv_elem_type(BLE_LL_SCHED_TYPE_AUX_SCAN, ble_ll_scan_sched_remove); scansm->ext_scanning = 0; } #endif @@ -1209,45 +966,15 @@ ble_ll_scan_sm_start(struct ble_ll_scan_sm *scansm) return BLE_ERR_SUCCESS; } -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) -static void -ble_ll_aux_scan_rsp_failed(struct ble_ll_scan_sm *scansm) -{ - if (!scansm->cur_aux_data) { - return; - } - - STATS_INC(ble_ll_stats, aux_scan_rsp_err); - ble_ll_scan_interrupted(scansm); -} -#endif - static void ble_ll_scan_interrupted_event_cb(struct ble_npl_event *ev) { struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - struct ble_ll_aux_data *aux_data; -#endif if (!scansm->scan_enabled) { return; } -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - aux_data = ble_npl_event_get_arg(ev); - - if (aux_data) { - if (scansm->scan_rsp_pending) { - STATS_INC(ble_ll_stats, aux_scan_rsp_err); - } - ble_ll_scan_end_adv_evt(aux_data); - ble_ll_scan_aux_data_unref(aux_data); - ble_npl_event_set_arg(ev, NULL); - STATS_INC(ble_ll_stats, aux_missed_adv); - } -#endif - /* * If we timed out waiting for a response, the scan response pending * flag should be set. Deal with scan backoff. Put device back into rx. @@ -1301,7 +1028,7 @@ ble_ll_scan_event_proc(struct ble_npl_event *ev) return; } - if (scansm->cur_aux_data || scansm->scan_rsp_pending) { + if (scansm->scan_rsp_pending) { /* Aux scan in progress. Wait */ STATS_INC(ble_ll_stats, scan_timer_stopped); scansm->restart_timer_needed = 1; @@ -1366,15 +1093,6 @@ ble_ll_scan_event_proc(struct ble_npl_event *ev) case BLE_LL_STATE_SCAN_AUX: start_scan = false; break; - case BLE_LL_STATE_INITIATING: - /* Must disable PHY since we will move to a new channel */ - ble_phy_disable(); - if (!inside_window) { - ble_ll_state_set(BLE_LL_STATE_STANDBY); - } - /* PHY is disabled - make sure we do not wait for AUX_CONNECT_RSP */ - ble_ll_conn_reset_pending_aux_conn_rsp(); - break; case BLE_LL_STATE_SCANNING: /* Must disable PHY since we will move to a new channel */ ble_phy_disable(); @@ -1390,7 +1108,7 @@ ble_ll_scan_event_proc(struct ble_npl_event *ev) } if (start_scan) { - ble_ll_scan_start(scansm, NULL); + ble_ll_scan_start(scansm); } else { ble_ll_rfmgmt_release(); } @@ -1440,10 +1158,6 @@ ble_ll_scan_rx_isr_start(uint8_t pdu_type, uint16_t *rxflags) } #endif - if (scansm->cur_aux_data && !scansm->scan_rsp_pending ) { - STATS_INC(ble_ll_stats, aux_received); - } - /* * If this is the first PDU after we sent the scan response (as * denoted by the scan rsp pending flag), we set a bit in the ble @@ -1460,9 +1174,6 @@ ble_ll_scan_rx_isr_start(uint8_t pdu_type, uint16_t *rxflags) *rxflags |= BLE_MBUF_HDR_F_SCAN_RSP_RXD; } else { ble_ll_scan_req_backoff(scansm, 0); -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - ble_ll_aux_scan_rsp_failed(scansm); -#endif } } break; @@ -1473,6 +1184,19 @@ ble_ll_scan_rx_isr_start(uint8_t pdu_type, uint16_t *rxflags) } break; #endif + case BLE_SCAN_TYPE_INITIATE: + if ((pdu_type == BLE_ADV_PDU_TYPE_ADV_IND) || + (pdu_type == BLE_ADV_PDU_TYPE_ADV_DIRECT_IND)) { + rc = 1; + } + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + if ((pdu_type == BLE_ADV_PDU_TYPE_ADV_EXT_IND && scansm->ext_scanning)) { + *rxflags |= BLE_MBUF_HDR_F_EXT_ADV; + rc = 1; + } +#endif + break; default: break; } @@ -1480,321 +1204,6 @@ ble_ll_scan_rx_isr_start(uint8_t pdu_type, uint16_t *rxflags) return rc; } -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) -static uint8_t -ble_ll_ext_adv_phy_mode_to_local_phy(uint8_t adv_phy_mode) -{ - switch (adv_phy_mode) { - case 0x00: - return BLE_PHY_1M; -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) - case 0x01: - return BLE_PHY_2M; -#endif -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) - case 0x02: - return BLE_PHY_CODED; -#endif - } - - return 0; -} - -static int -ble_ll_ext_scan_parse_aux_ptr(struct ble_ll_aux_data *aux_data, uint8_t *buf) -{ - uint32_t aux_ptr_field = get_le32(buf) & 0x00FFFFFF; - - aux_data->chan = (aux_ptr_field) & 0x3F; - if (aux_data->chan >= BLE_PHY_NUM_DATA_CHANS) { - return -1; - } - - /* TODO use CA aux_ptr_field >> 6 */ - - aux_data->offset = 30 * ((aux_ptr_field >> 8) & 0x1FFF); - - if ((aux_ptr_field >> 7) & 0x01) { - aux_data->offset *= 10; - aux_data->offset_units = 1; - } - - if (aux_data->offset < BLE_LL_MAFS) { - return -1; - } - - aux_data->aux_phy = - ble_ll_ext_adv_phy_mode_to_local_phy((aux_ptr_field >> 21) & 0x07); - if (aux_data->aux_phy == 0) { - return -1; - } - - return 0; -} - -/** - * ble_ll_scan_update_aux_data - * - * Update aux_data stored in ble_hdr.rxinfo.user_data. If no aux_data is present - * (i.e. processing ADV_EXT_IND) this will try to allocate new aux_data. - * - * Context: Interrupt - * - * @param ble_hdr - * @param rxbuf - * - * @return int - * 1: do not scan for next AUX (no AuxPtr or malformed data) - * 0: scan for next AUX (valid AuxPtr found) - * -1: error - */ -int -ble_ll_scan_update_aux_data(struct ble_mbuf_hdr *ble_hdr, uint8_t *rxbuf, - bool *adva_present) -{ - uint8_t pdu_hdr; - uint8_t pdu_len; - uint8_t adv_mode; - uint8_t eh_len; - uint8_t eh_flags; - uint8_t *eh; - struct ble_ll_aux_data *aux_data; - bool is_aux; - - aux_data = ble_hdr->rxinfo.user_data; - /* aux_data is initially not set only for ADV_EXT_IND */ - is_aux = aux_data; - - pdu_hdr = rxbuf[0]; - pdu_len = rxbuf[1]; - - /* PDU without at least Extended Header Length is invalid */ - if (pdu_len == 0) { - return -1; - } - - adv_mode = rxbuf[2] >> 6; - eh_len = rxbuf[2] & 0x3f; - eh_flags = rxbuf[3]; - eh = &rxbuf[4]; - - /* - * PDU without Extended Header is valid in case of last AUX_CHAIN_IND in - * chain so aux_data has to be set and advertising mode has to be 00b, - * otherwise it's an invalid PDU. - */ - if (eh_len == 0) { - if (!aux_data || adv_mode) { - return -1; - } - aux_data->flags_isr |= BLE_LL_AUX_FLAG_SCAN_COMPLETE; - return 1; - } - - /* - * If aux_data is not set, this is ADV_EXT_IND which starts new extended - * advertising event. - */ - if (!aux_data) { - if (ble_ll_scan_ext_adv_init(&aux_data)) { - return -1; - } - - aux_data->aux_primary_phy = ble_hdr->rxinfo.phy; - } else { - if (aux_data->flags_isr & BLE_LL_AUX_FLAG_AUX_ADV_RECEIVED) { - aux_data->flags_isr |= BLE_LL_AUX_FLAG_AUX_CHAIN_RECEIVED; - } else { - aux_data->flags_isr |= BLE_LL_AUX_FLAG_AUX_ADV_RECEIVED; - } - } - - /* Now parse extended header... */ - - if (eh_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) { - aux_data->flags |= BLE_LL_AUX_HAS_ADVA; - memcpy(aux_data->adva, eh, 6); - aux_data->adva_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_TXADD_MASK); - eh += BLE_LL_EXT_ADV_ADVA_SIZE; - - if (adva_present) { - *adva_present = true; - } - } else if (adva_present) { - *adva_present = false; - } - - if (eh_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) { - aux_data->flags |= BLE_LL_AUX_HAS_TARGETA; - memcpy(aux_data->targeta, eh, 6); - aux_data->targeta_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_RXADD_MASK); - eh += BLE_LL_EXT_ADV_TARGETA_SIZE; - } - - - if (eh_flags & (1 << BLE_LL_EXT_ADV_CTE_INFO_BIT)) { - eh += 1; - } - - if (eh_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) { - aux_data->flags |= BLE_LL_AUX_HAS_ADI; - if (is_aux) { - if (get_le16(eh) != aux_data->adi) { - aux_data->flags_isr |= BLE_LL_AUX_FLAG_SCAN_ERROR; - STATS_INC(ble_ll_stats, aux_chain_err); - } - } else { - aux_data->adi = get_le16(eh); - } - eh += BLE_LL_EXT_ADV_DATA_INFO_SIZE; - } - - if (eh_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) { - if (ble_ll_ext_scan_parse_aux_ptr(aux_data, eh)) { - aux_data->flags_isr |= BLE_LL_AUX_FLAG_SCAN_ERROR; - } - } else if (!(adv_mode & BLE_LL_EXT_ADV_MODE_SCAN)) { - /* No AuxPtr for scannable PDU is ignored since we can still scan it */ - aux_data->flags_isr |= BLE_LL_AUX_FLAG_SCAN_COMPLETE; - } - - ble_hdr->rxinfo.user_data = aux_data; - - /* Do not scan for next AUX if either no AuxPtr or malformed data found */ - return !(eh_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) || - (aux_data->flags_isr & BLE_LL_AUX_FLAG_SCAN_ERROR); -} - -static int -ble_ll_scan_get_addr_from_ext_adv(uint8_t *rxbuf, struct ble_mbuf_hdr *ble_hdr, - uint8_t **addr, uint8_t *addr_type, - uint8_t **inita, uint8_t *inita_type, - int *ext_mode) -{ - uint8_t pdu_len; - uint8_t ext_hdr_len; - uint8_t ext_hdr_flags; - uint8_t *ext_hdr; - bool has_adva = false; - bool has_inita = false; - int i; - struct ble_ll_aux_data *aux_data = ble_hdr->rxinfo.user_data; - - *addr = NULL; - *inita = NULL; - - pdu_len = rxbuf[1]; - if (pdu_len == 0) { - return -1; - } - - *ext_mode = rxbuf[2] >> 6; - if (*ext_mode > BLE_LL_EXT_ADV_MODE_SCAN) { - return -1; - } - - ext_hdr_len = rxbuf[2] & 0x3F; - if (ext_hdr_len == 0) { - goto done; - } - - ext_hdr_flags = rxbuf[3]; - ext_hdr = &rxbuf[4]; - - i = 0; - if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_ADVA_BIT)) { - if (ext_hdr_len < BLE_LL_EXT_ADV_ADVA_SIZE) { - return -1; - } - - *addr = ext_hdr + i; - *addr_type = - ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK); - i += BLE_LL_EXT_ADV_ADVA_SIZE; - - has_adva = true; - } - - if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_TARGETA_BIT)) { - *inita = ext_hdr + i; - *inita_type = - ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_RXADD_MASK); - i += BLE_LL_EXT_ADV_TARGETA_SIZE; - - has_inita = true; - } - -done: - /* Check if we had address already. If yes, replace it with new one */ - - if (aux_data) { - /* If address has been provided, we do have it already in aux_data.*/ - if (aux_data->flags & BLE_LL_AUX_HAS_ADVA) { - if (!has_adva) { - *addr = aux_data->adva; - *addr_type = aux_data->adva_type; - } else { - memcpy(aux_data->adva, *addr, 6); - aux_data->adva_type = *addr_type; - } - } - - if (aux_data->flags & BLE_LL_AUX_HAS_TARGETA) { - if (!has_inita) { - *inita = aux_data->targeta; - *inita_type = aux_data->targeta_type; - } else { - memcpy(aux_data->targeta, *inita, 6); - aux_data->targeta_type = *inita_type; - } - } - } - - return 0; -} -#endif - -int -ble_ll_scan_adv_decode_addr(uint8_t pdu_type, uint8_t *rxbuf, - struct ble_mbuf_hdr *ble_hdr, - uint8_t **addr, uint8_t *addr_type, - uint8_t **inita, uint8_t *inita_type, - int *ext_mode) -{ - /* - * XXX this should be only used for legacy advertising, but need to refactor - * code in ble_ll_init first so it does not call this for ext - */ - - if (pdu_type != BLE_ADV_PDU_TYPE_ADV_EXT_IND && - pdu_type != BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP) { - /* Legacy advertising */ - *addr_type = ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK); - *addr = rxbuf + BLE_LL_PDU_HDR_LEN; - - if (pdu_type != BLE_ADV_PDU_TYPE_ADV_DIRECT_IND) { - *inita = NULL; - *inita_type = 0; - return 0; - } - - *inita = rxbuf + BLE_LL_PDU_HDR_LEN + BLE_DEV_ADDR_LEN; - *inita_type = ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_RXADD_MASK); - - return 0; - } - - /* Extended advertising */ -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - return ble_ll_scan_get_addr_from_ext_adv(rxbuf, ble_hdr, addr, addr_type, - inita, inita_type, ext_mode); -#else - return -1; -#endif - - return 0; -} - static void ble_ll_scan_get_addr_data_from_legacy(uint8_t pdu_type, uint8_t *rxbuf, struct ble_ll_scan_addr_data *addrd) @@ -1918,11 +1327,12 @@ ble_ll_scan_rx_filter(uint8_t own_addr_type, uint8_t scan_filt_policy, resolved = false; #endif - /* Check on WL if required by scan filter policy */ - if ((scan_filt_policy & 0x01) && - !ble_ll_whitelist_match(addrd->adv_addr, addrd->adv_addr_type, - resolved)) { + if (scan_filt_policy & 0x01) { + /* Check on WL if required by scan filter policy */ + if (!ble_ll_whitelist_match(addrd->adv_addr, addrd->adv_addr_type, + resolved)) { return -2; + } } if (scan_ok) { @@ -1932,10 +1342,33 @@ ble_ll_scan_rx_filter(uint8_t own_addr_type, uint8_t scan_filt_policy, return 0; } +int +ble_ll_scan_rx_check_init(struct ble_ll_scan_addr_data *addrd) +{ + struct ble_ll_scan_sm *scansm; + struct ble_ll_conn_sm *connsm; + + scansm = &g_ble_ll_scan_sm; + connsm = scansm->connsm; + BLE_LL_ASSERT(connsm); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + if ((connsm->peer_addr_type > BLE_ADDR_RANDOM) && !addrd->adva_resolved) { + return -1; + } +#endif + if ((addrd->adv_addr_type != (connsm->peer_addr_type & 0x01)) || + memcmp(addrd->adv_addr, connsm->peer_addr, 6) != 0) { + return -1; + } + + return 0; +} + static int -ble_ll_scan_rx_isr_on_adv(uint8_t pdu_type, uint8_t *rxbuf, - struct ble_mbuf_hdr *hdr, - struct ble_ll_scan_addr_data *addrd) +ble_ll_scan_rx_isr_end_on_adv(uint8_t pdu_type, uint8_t *rxbuf, + struct ble_mbuf_hdr *hdr, + struct ble_ll_scan_addr_data *addrd) { struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; struct ble_ll_scan_phy *scanp = scansm->scanp; @@ -1949,12 +1382,20 @@ ble_ll_scan_rx_isr_on_adv(uint8_t pdu_type, uint8_t *rxbuf, addrd->rpa_index = ble_hw_resolv_list_match(); #endif - rc = ble_ll_scan_rx_filter(scansm->own_addr_type, scansm->scan_filt_policy, - addrd, &scan_ok); + rc = ble_ll_scan_rx_filter(scansm->own_addr_type, + scansm->scan_filt_policy, addrd, &scan_ok); if (rc < 0) { return 0; } + if ((scanp->scan_type == BLE_SCAN_TYPE_INITIATE) && + !(scansm->scan_filt_policy & 0x01)) { + rc = ble_ll_scan_rx_check_init(addrd); + if (rc < 0) { + return 0; + } + } + rxinfo->flags |= BLE_MBUF_HDR_F_DEVMATCH; #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) rxinfo->rpa_index = addrd->rpa_index; @@ -1971,15 +1412,19 @@ ble_ll_scan_rx_isr_on_adv(uint8_t pdu_type, uint8_t *rxbuf, return 0; } - return (scanp->scan_type == BLE_SCAN_TYPE_ACTIVE) && + /* Allow responding to all PDUs when initiating since unwanted PDUs were + * already filtered out in isr_start. + */ + return ((scanp->scan_type == BLE_SCAN_TYPE_ACTIVE) && ((pdu_type == BLE_ADV_PDU_TYPE_ADV_IND) || - (pdu_type == BLE_ADV_PDU_TYPE_ADV_SCAN_IND)); + (pdu_type == BLE_ADV_PDU_TYPE_ADV_SCAN_IND))) || + (scanp->scan_type == BLE_SCAN_TYPE_INITIATE); } static int -ble_ll_scan_rx_isr_on_scan_rsp(uint8_t pdu_type, uint8_t *rxbuf, - struct ble_mbuf_hdr *hdr, - struct ble_ll_scan_addr_data *addrd) +ble_ll_scan_rx_isr_end_on_scan_rsp(uint8_t pdu_type, uint8_t *rxbuf, + struct ble_mbuf_hdr *hdr, + struct ble_ll_scan_addr_data *addrd) { struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; struct ble_mbuf_hdr_rxinfo *rxinfo = &hdr->rxinfo; @@ -2119,10 +1564,10 @@ ble_ll_scan_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok) case BLE_ADV_PDU_TYPE_ADV_DIRECT_IND: case BLE_ADV_PDU_TYPE_ADV_NONCONN_IND: case BLE_ADV_PDU_TYPE_ADV_SCAN_IND: - rc = ble_ll_scan_rx_isr_on_adv(pdu_type, rxbuf, hdr, &addrd); + rc = ble_ll_scan_rx_isr_end_on_adv(pdu_type, rxbuf, hdr, &addrd); break; case BLE_ADV_PDU_TYPE_SCAN_RSP: - rc = ble_ll_scan_rx_isr_on_scan_rsp(pdu_type, rxbuf, hdr, &addrd); + rc = ble_ll_scan_rx_isr_end_on_scan_rsp(pdu_type, rxbuf, hdr, &addrd); break; #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) case BLE_ADV_PDU_TYPE_ADV_EXT_IND: @@ -2144,10 +1589,22 @@ ble_ll_scan_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok) if (rc == -1) { goto scan_rx_isr_ignore; - } else if (rc == 1) { - if (ble_ll_scan_send_scan_req(pdu_type, rxbuf, hdr, &addrd)) { - /* Keep PHY active and LL in scanning state */ - return 0; + } + + if (rc == 1) { + switch (scansm->scanp->scan_type) { + case BLE_SCAN_TYPE_ACTIVE: + if (ble_ll_scan_send_scan_req(pdu_type, rxbuf, hdr, &addrd)) { + /* Keep PHY active and LL in scanning state */ + return 0; + } + break; + case BLE_SCAN_TYPE_INITIATE: + if (ble_ll_conn_send_connect_req(rxpdu, &addrd, 0) == 0) { + hdr->rxinfo.flags |= BLE_MBUF_HDR_F_CONNECT_IND_TXD; + return 0; + } + break; } } @@ -2193,7 +1650,7 @@ ble_ll_scan_chk_resume(void) if (ble_ll_state_get() == BLE_LL_STATE_STANDBY && ble_ll_scan_is_inside_window(scansm->scanp, now)) { /* Turn on the receiver and set state */ - ble_ll_scan_start(scansm, NULL); + ble_ll_scan_start(scansm); } OS_EXIT_CRITICAL(sr); } @@ -2219,11 +1676,6 @@ ble_ll_scan_timer_cb(void *arg) void ble_ll_scan_interrupted(struct ble_ll_scan_sm *scansm) { -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - ble_npl_event_set_arg(&scansm->scan_interrupted_ev, scansm->cur_aux_data); - scansm->cur_aux_data = NULL; -#endif - ble_ll_event_send(&scansm->scan_interrupted_ev); } @@ -2236,16 +1688,7 @@ ble_ll_scan_interrupted(struct ble_ll_scan_sm *scansm) void ble_ll_scan_wfr_timer_exp(void) { - struct ble_ll_scan_sm *scansm; - uint8_t chan; - int phy; - int rc; -#if (BLE_LL_BT5_PHY_SUPPORTED == 1) - uint8_t phy_mode; -#endif - uint32_t now; - - scansm = &g_ble_ll_scan_sm; + struct ble_ll_scan_sm *scansm = &g_ble_ll_scan_sm; /* Update backoff if we failed to receive scan response */ if (scansm->scan_rsp_pending) { @@ -2253,34 +1696,6 @@ ble_ll_scan_wfr_timer_exp(void) ble_ll_scan_req_backoff(scansm, 0); } - if (scansm->cur_aux_data) { - /* We actually care about interrupted scan only for EXT ADV because only - * then we might consider to send truncated event to the host. - */ - ble_ll_scan_interrupted(scansm); - - /* Need to disable phy since we are going to move to BLE_LL_STATE_STANDBY - * or we will need to change channel to primary one - */ - ble_phy_disable(); - - now = os_cputime_get32(); - if (!ble_ll_scan_is_inside_window(scansm->scanp, now)) { - /* Outside the window scan */ - ble_ll_state_set(BLE_LL_STATE_STANDBY); - return; - } - - ble_ll_get_chan_to_scan(scansm, &chan, &phy); -#if (BLE_LL_BT5_PHY_SUPPORTED == 1) - phy_mode = ble_ll_phy_to_phy_mode(phy, BLE_HCI_LE_PHY_CODED_ANY); - ble_phy_mode_set(phy_mode, phy_mode); -#endif - rc = ble_phy_setchan(chan, BLE_ACCESS_ADDR_ADV, BLE_LL_CRCINIT_ADV); - BLE_LL_ASSERT(rc == 0); - } - - ble_phy_restart_rx(); } @@ -2509,7 +1924,10 @@ ble_ll_scan_rx_pkt_in_on_legacy(uint8_t pdu_type, struct os_mbuf *om, void ble_ll_scan_rx_pkt_in(uint8_t ptype, struct os_mbuf *om, struct ble_mbuf_hdr *hdr) { + struct ble_mbuf_hdr_rxinfo *rxinfo; + struct ble_ll_scan_sm *scansm; struct ble_ll_scan_addr_data addrd; + uint8_t *targeta; #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) if (ptype == BLE_ADV_PDU_TYPE_ADV_EXT_IND) { @@ -2519,7 +1937,26 @@ ble_ll_scan_rx_pkt_in(uint8_t ptype, struct os_mbuf *om, struct ble_mbuf_hdr *hd } #endif - ble_ll_scan_rx_pkt_in_on_legacy(ptype, om, hdr, &addrd); + scansm = &g_ble_ll_scan_sm; + rxinfo = &hdr->rxinfo; + + if (scansm->scanp->scan_type == BLE_SCAN_TYPE_INITIATE) { + if (rxinfo->flags & BLE_MBUF_HDR_F_CONNECT_IND_TXD) { + /* We need to keep original TargetA in case it was resolved, so rl + * can be updated properly. + */ + ble_ll_scan_get_addr_data_from_legacy(ptype, om->om_data, &addrd); + targeta = addrd.targeta; + ble_ll_scan_rx_pkt_in_restore_addr_data(hdr, &addrd); + + ble_ll_scan_sm_stop(0); + ble_ll_conn_created_on_legacy(om, &addrd, targeta); + return; + } + } else { + ble_ll_scan_rx_pkt_in_on_legacy(ptype, om, hdr, &addrd); + } + ble_ll_scan_chk_resume(); } @@ -2975,7 +2412,7 @@ ble_ll_scan_can_chg_whitelist(void) int ble_ll_scan_initiator_start(struct hci_create_conn *hcc, - struct ble_ll_scan_sm **sm) + struct ble_ll_conn_sm *connsm) { struct ble_ll_scan_sm *scansm; struct ble_ll_scan_phy *scanp; @@ -2987,6 +2424,8 @@ ble_ll_scan_initiator_start(struct hci_create_conn *hcc, #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) scansm->ext_scanning = 0; #endif + scansm->connsm = connsm; + scansm->scanp = &scansm->scan_phys[PHY_UNCODED]; scansm->scanp_next = NULL; @@ -3002,15 +2441,6 @@ ble_ll_scan_initiator_start(struct hci_create_conn *hcc, #endif rc = ble_ll_scan_sm_start(scansm); - if (sm == NULL) { - return rc; - } - - if (rc == BLE_ERR_SUCCESS) { - *sm = scansm; - } else { - *sm = NULL; - } return rc; } @@ -3018,7 +2448,7 @@ ble_ll_scan_initiator_start(struct hci_create_conn *hcc, #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) int ble_ll_scan_ext_initiator_start(struct hci_ext_create_conn *hcc, - struct ble_ll_scan_sm **sm) + struct ble_ll_conn_sm *connsm) { struct ble_ll_scan_sm *scansm; struct ble_ll_scan_phy *scanp_uncoded; @@ -3032,6 +2462,7 @@ ble_ll_scan_ext_initiator_start(struct hci_ext_create_conn *hcc, scansm->scanp = NULL; scansm->scanp_next = NULL; scansm->ext_scanning = 1; + scansm->connsm = connsm; params = &hcc->params[0]; scanp_uncoded = &scansm->scan_phys[PHY_UNCODED]; @@ -3080,15 +2511,6 @@ ble_ll_scan_ext_initiator_start(struct hci_ext_create_conn *hcc, } rc = ble_ll_scan_sm_start(scansm); - if (sm == NULL) { - return rc; - } - - if (rc == BLE_ERR_SUCCESS) { - *sm = scansm; - } else { - *sm = NULL; - } return rc; } @@ -3155,13 +2577,6 @@ ble_ll_scan_get_pdu_data(void) return &g_ble_ll_scan_sm.pdu_data; } -/* Returns true if whitelist is enabled for scanning */ -int -ble_ll_scan_whitelist_enabled(void) -{ - return g_ble_ll_scan_sm.scan_filt_policy & 1; -} - static void ble_ll_scan_common_init(void) { @@ -3243,13 +2658,11 @@ ble_ll_scan_reset(void) os_mempool_clear(&g_scan_dup_pool); TAILQ_INIT(&g_scan_dup_list); -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - /* clear memory pool for AUX scan results */ - os_mempool_clear(&ext_scan_aux_pool); -#endif - /* Call the common init function again */ ble_ll_scan_common_init(); +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) + ble_ll_scan_aux_init(); +#endif } /** @@ -3263,15 +2676,6 @@ ble_ll_scan_init(void) { os_error_t err; -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) - err = os_mempool_init(&ext_scan_aux_pool, - MYNEWT_VAL(BLE_LL_EXT_ADV_AUX_PTR_CNT), - sizeof (struct ble_ll_aux_data), - ext_scan_aux_mem, - "ble_ll_aux_scan_pool"); - BLE_LL_ASSERT(err == 0); -#endif - err = os_mempool_init(&g_scan_dup_pool, MYNEWT_VAL(BLE_LL_NUM_SCAN_DUP_ADVS), sizeof(struct ble_ll_scan_dup_entry), diff --git a/nimble/controller/src/ble_ll_scan_aux.c b/nimble/controller/src/ble_ll_scan_aux.c index 128b45a7c..94f776df9 100644 --- a/nimble/controller/src/ble_ll_scan_aux.c +++ b/nimble/controller/src/ble_ll_scan_aux.c @@ -50,6 +50,8 @@ #define BLE_LL_SCAN_AUX_F_HAS_ADI 0x0080 #define BLE_LL_SCAN_AUX_F_RESOLVED_ADVA 0x0100 #define BLE_LL_SCAN_AUX_F_RESOLVED_TARGETA 0x0200 +#define BLE_LL_SCAN_AUX_F_CONNECTABLE 0x0400 +#define BLE_LL_SCAN_AUX_F_W4_CONNECT_RSP 0x0800 #define BLE_LL_SCAN_AUX_H_SENT_ANY 0x01 #define BLE_LL_SCAN_AUX_H_DONE 0x02 @@ -668,6 +670,10 @@ ble_ll_scan_aux_break_ev(struct ble_npl_event *ev) void ble_ll_scan_aux_break(struct ble_ll_scan_aux_data *aux) { + if (aux->flags & BLE_LL_SCAN_AUX_F_W4_CONNECT_RSP) { + ble_ll_conn_send_connect_req_cancel(); + } + ble_npl_event_init(&aux->break_ev, ble_ll_scan_aux_break_ev, aux); ble_ll_event_send(&aux->break_ev); } @@ -751,18 +757,29 @@ ble_ll_scan_aux_rx_isr_start(uint8_t pdu_type, struct ble_mbuf_hdr *rxhdr) { struct ble_ll_scan_aux_data *aux; - if (pdu_type != BLE_ADV_PDU_TYPE_ADV_EXT_IND) { - return -1; - } - BLE_LL_ASSERT(aux_data_current); aux = aux_data_current; + if (aux->flags & BLE_LL_SCAN_AUX_F_W4_CONNECT_RSP) { + if (pdu_type != BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP) { + aux_data_current = NULL; + ble_ll_scan_aux_break(aux); + return -1; + } + return 0; + } + + if (pdu_type != BLE_ADV_PDU_TYPE_ADV_EXT_IND) { + aux_data_current = NULL; + ble_ll_scan_aux_break(aux); + return -1; + } + /* * Prepare TX transition when doing active scanning and receiving 1st PDU * since we may want to send AUX_SCAN_REQ. */ - if ((aux->scan_type == BLE_SCAN_TYPE_ACTIVE) && + if ((aux->scan_type != BLE_SCAN_TYPE_PASSIVE) && !(aux->flags & BLE_LL_SCAN_AUX_F_AUX_ADV)) { return 1; } @@ -1012,6 +1029,10 @@ ble_ll_scan_aux_rx_isr_end_on_ext(struct ble_ll_scan_sm *scansm, return -1; } + /* Don't care about initiator here, there are no AdvA and TargetA + * in connectable ADV_EXT_IND so no filtering to do. + */ + if (!aux_ptr) { /* We do not allocate aux_data for ADV_EXT_IND without AuxPtr so * need to pass match data in rxinfo. @@ -1046,6 +1067,10 @@ ble_ll_scan_aux_rx_isr_end_on_ext(struct ble_ll_scan_sm *scansm, aux->adi = adi; aux->flags |= BLE_LL_SCAN_AUX_F_HAS_ADI; + if (aux->scan_type == BLE_SCAN_TYPE_INITIATE) { + aux->flags |= BLE_LL_SCAN_AUX_F_CONNECTABLE; + } + if (do_match) { aux->flags |= BLE_LL_SCAN_AUX_F_MATCHED; ble_ll_scan_aux_update_aux_data_from_addrd(&addrd, aux); @@ -1187,6 +1212,7 @@ ble_ll_scan_aux_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok) struct ble_mbuf_hdr_rxinfo *rxinfo; struct ble_ll_scan_aux_data *aux; struct ble_mbuf_hdr *rxhdr; + uint8_t scan_filt_policy; uint8_t scan_ok; uint8_t adv_mode; uint8_t *rxbuf; @@ -1212,6 +1238,12 @@ ble_ll_scan_aux_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok) rxbuf = rxpdu->om_data; + if (aux->flags & BLE_LL_SCAN_AUX_F_W4_CONNECT_RSP) { + aux->flags &= ~BLE_LL_SCAN_AUX_F_W4_CONNECT_RSP; + rxinfo->flags |= BLE_MBUF_HDR_F_CONNECT_RSP_RXD; + goto done; + } + if (aux->flags & BLE_LL_SCAN_AUX_F_W4_SCAN_RSP) { aux->flags &= ~BLE_LL_SCAN_AUX_F_W4_SCAN_RSP; aux->flags |= BLE_LL_SCAN_AUX_F_SCANNED; @@ -1251,9 +1283,10 @@ ble_ll_scan_aux_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok) addrd.rpa_index = aux->rpa_index; #endif + scan_filt_policy = ble_ll_scan_get_filt_policy(); + rc = ble_ll_scan_rx_filter(ble_ll_scan_get_own_addr_type(), - ble_ll_scan_get_filt_policy(), - &addrd, &scan_ok); + scan_filt_policy, &addrd, &scan_ok); if (rc < 0) { rxinfo->flags |= BLE_MBUF_HDR_F_IGNORED; /* @@ -1269,21 +1302,42 @@ ble_ll_scan_aux_rx_isr_end(struct os_mbuf *rxpdu, uint8_t crcok) goto done; } + if ((aux->scan_type == BLE_SCAN_TYPE_INITIATE) && + !(scan_filt_policy & 0x01)) { + rc = ble_ll_scan_rx_check_init(&addrd); + if (rc < 0) { + goto done; + } + } + aux->flags |= BLE_LL_SCAN_AUX_F_MATCHED; ble_ll_scan_aux_update_aux_data_from_addrd(&addrd, aux); adv_mode = rxbuf[2] >> 6; - if ((aux->scan_type == BLE_SCAN_TYPE_ACTIVE) && - (adv_mode == BLE_LL_EXT_ADV_MODE_SCAN) && scan_ok) { - if (ble_ll_scan_aux_send_scan_req(aux, &addrd)) { + switch (aux->scan_type) { + case BLE_SCAN_TYPE_ACTIVE: + if ((adv_mode == BLE_LL_EXT_ADV_MODE_SCAN) && scan_ok && + ble_ll_scan_aux_send_scan_req(aux, &addrd)) { /* AUX_SCAN_REQ sent, keep PHY enabled to continue */ aux->flags |= BLE_LL_SCAN_AUX_F_W4_SCAN_RSP; rxinfo->flags |= BLE_MBUF_HDR_F_SCAN_REQ_TXD; aux_data_current = aux; return 0; } + break; + case BLE_SCAN_TYPE_INITIATE: + if (ble_ll_conn_send_connect_req(rxpdu, &addrd, 1) == 0) { + /* AUX_CONNECT_REQ sent, keep PHY enabled to continue */ + aux->flags |= BLE_LL_SCAN_AUX_F_W4_CONNECT_RSP; + rxinfo->flags |= BLE_MBUF_HDR_F_CONNECT_REQ_TXD; + aux_data_current = aux; + return 0; + } else { + rxinfo->flags |= BLE_MBUF_HDR_F_IGNORED; + } + break; } done: @@ -1399,6 +1453,137 @@ ble_ll_scan_aux_sync_check(struct os_mbuf *rxpdu, } #endif + +static int +ble_ll_scan_aux_check_connect_rsp(uint8_t *rxbuf, + struct ble_ll_scan_pdu_data *pdu_data, + struct ble_ll_scan_addr_data *addrd) +{ + uint8_t pdu_hdr; + uint8_t pdu_len; + uint8_t adv_mode; + uint8_t eh_len; + uint8_t eh_flags; + uint8_t *eh_data; + uint8_t adva_type; + uint8_t *adva; + uint8_t targeta_type; + uint8_t *targeta; +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + struct ble_ll_resolv_entry *rl = NULL; +#endif + + pdu_hdr = rxbuf[0]; + pdu_len = rxbuf[1]; + + if (pdu_len == 0) { + return -1; + } + + adv_mode = rxbuf[2] >> 6; + eh_len = rxbuf[2] & 0x3f; + + if ((adv_mode != 0) || (eh_len == 0)) { + return -1; + } + + eh_flags = rxbuf[3]; + eh_data = &rxbuf[4]; + + /* AUX_CONNECT_RSP without AdvA or TargetA is not valid */ + if (!(eh_flags & ((1 << BLE_LL_EXT_ADV_ADVA_BIT) | + (1 << BLE_LL_EXT_ADV_TARGETA_BIT)))) { + return -1; + } + + adva = eh_data; + adva_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_TXADD_RAND); + eh_data += BLE_LL_EXT_ADV_ADVA_SIZE; + + targeta = eh_data; + targeta_type = !!(pdu_hdr & BLE_ADV_PDU_HDR_RXADD_RAND); + +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) + /* If AdvA was initially resolved, we need to check if current AdvA also + * resolved with the same IRK since it may have changes due to RPA timeout. + * Otherwise it shall be the same as in AUX_CONNECT_REQ. + */ + if (addrd->adva_resolved) { + if (!adva_type) { + return -1; + } + + BLE_LL_ASSERT(addrd->rpa_index >= 0); + rl = &g_ble_ll_resolv_list[addrd->rpa_index]; + + if (!ble_ll_resolv_rpa(adva, rl->rl_peer_irk)) { + return -1; + } + + addrd->adva = adva; + } else if ((adva_type != + !!(pdu_data->hdr_byte & BLE_ADV_PDU_HDR_RXADD_MASK)) || + (memcmp(adva, pdu_data->adva, 6) != 0)) { + return -1; + } +#else + if ((adva_type != !!(pdu_data->hdr_byte & BLE_ADV_PDU_HDR_RXADD_MASK)) || + (memcmp(adva, pdu_data->adva, 6) != 0)) { + return -1; + } +#endif /* BLE_LL_CFG_FEAT_LL_PRIVACY */ + + if ((targeta_type != !!(pdu_data->hdr_byte & BLE_ADV_PDU_HDR_RXADD_MASK)) || + (memcmp(targeta, pdu_data->inita, 6) != 0)) { + return -1; + } + + return 0; +} + +static void +ble_ll_scan_aux_rx_pkt_in_for_initiator(struct os_mbuf *rxpdu, + struct ble_mbuf_hdr *rxhdr) +{ + struct ble_ll_scan_addr_data addrd; + struct ble_mbuf_hdr_rxinfo *rxinfo; + struct ble_ll_scan_aux_data *aux; + + rxinfo = &rxhdr->rxinfo; + aux = rxinfo->user_data; + + if (rxinfo->flags & BLE_MBUF_HDR_F_IGNORED) { + ble_ll_scan_chk_resume(); + goto done; + } + + if (!(rxinfo->flags & BLE_MBUF_HDR_F_CONNECT_RSP_RXD)) { + BLE_LL_ASSERT(rxinfo->flags & BLE_MBUF_HDR_F_CONNECT_REQ_TXD); + /* Waiting for AUX_CONNECT_RSP, do nothing */ + return; + } + + ble_ll_scan_aux_init_addrd_from_aux_data(aux, &addrd); + + if (ble_ll_scan_aux_check_connect_rsp(rxpdu->om_data, + ble_ll_scan_get_pdu_data(), + &addrd) < 0) { + ble_ll_scan_chk_resume(); + goto done; + } + + aux->flags &= ~BLE_LL_SCAN_AUX_F_W4_CONNECT_RSP; + + ble_ll_scan_sm_stop(0); + ble_ll_conn_created_on_aux(rxpdu, &addrd, aux->targeta); + +done: + if (aux->flags & BLE_LL_SCAN_AUX_F_W4_CONNECT_RSP) { + ble_ll_conn_send_connect_req_cancel(); + } + ble_ll_scan_aux_free(aux); +} + void ble_ll_scan_aux_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *rxhdr) { @@ -1416,6 +1601,11 @@ ble_ll_scan_aux_rx_pkt_in(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *rxhdr) BLE_LL_ASSERT(aux); + if (aux->scan_type == BLE_SCAN_TYPE_INITIATE) { + ble_ll_scan_aux_rx_pkt_in_for_initiator(rxpdu, rxhdr); + return; + } + #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) sync_check = ble_ll_sync_enabled() && !(aux->flags & BLE_LL_SCAN_AUX_F_AUX_CHAIN); diff --git a/nimble/controller/src/ble_ll_sched.c b/nimble/controller/src/ble_ll_sched.c index 1c7db41b9..03dea23a7 100644 --- a/nimble/controller/src/ble_ll_sched.c +++ b/nimble/controller/src/ble_ll_sched.c @@ -134,9 +134,6 @@ ble_ll_sched_preempt(struct ble_ll_sched_item *sch, case BLE_LL_SCHED_TYPE_SCAN_AUX: ble_ll_scan_aux_break(entry->cb_arg); break; - case BLE_LL_SCHED_TYPE_AUX_SCAN: - ble_ll_scan_end_adv_evt(entry->cb_arg); - break; #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV) case BLE_LL_SCHED_TYPE_PERIODIC: ble_ll_adv_periodic_rmvd_from_sched(entry->cb_arg); @@ -1133,20 +1130,6 @@ ble_ll_sched_execute_item(struct ble_ll_sched_item *sch) goto sched; } - /* If aux scan scheduled and LL is in state when scanner is running - * in 3 states: - * BLE_LL_STATE_SCANNING - * BLE_LL_STATE_INITIATING - * BLE_LL_STATE_STANDBY - * - * Let scanner to decide to disable phy or not. - */ - if (sch->sched_type == BLE_LL_SCHED_TYPE_AUX_SCAN) { - if (lls == BLE_LL_STATE_INITIATING || lls == BLE_LL_STATE_SCANNING) { - goto sched; - } - } - /* * This is either an advertising event or connection event start. If * we are scanning or initiating just stop it. @@ -1158,11 +1141,6 @@ ble_ll_sched_execute_item(struct ble_ll_sched_item *sch) if (lls == BLE_LL_STATE_SCANNING) { ble_ll_state_set(BLE_LL_STATE_STANDBY); ble_ll_scan_halt(); - } else if (lls == BLE_LL_STATE_INITIATING) { - ble_ll_state_set(BLE_LL_STATE_STANDBY); - ble_ll_scan_halt(); - /* PHY is disabled - make sure we do not wait for AUX_CONNECT_RSP */ - ble_ll_conn_reset_pending_aux_conn_rsp(); } else if (lls == BLE_LL_STATE_ADV) { STATS_INC(ble_ll_stats, sched_state_adv_errs); ble_ll_adv_halt(); @@ -1262,78 +1240,6 @@ ble_ll_sched_next_time(uint32_t *next_event_time) } #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) -/** - * Called to schedule a aux scan. - * - * Context: Interrupt - * - * @param ble_hdr - * @param scansm - * @param aux_scan - * - * @return 0 on success, 1 otherwise - */ -int -ble_ll_sched_aux_scan(struct ble_mbuf_hdr *ble_hdr, - struct ble_ll_scan_sm *scansm, - struct ble_ll_aux_data *aux_scan) -{ - int rc = 1; - os_sr_t sr; - uint32_t off_ticks; - uint32_t off_rem_usecs; - uint32_t start_time; - uint32_t start_time_rem_usecs; - uint32_t end_time; - uint32_t dur; - struct ble_ll_sched_item *sch; - int phy_mode; - - sch = &aux_scan->sch; - BLE_LL_ASSERT(sch->cb_arg == NULL); - - off_ticks = os_cputime_usecs_to_ticks(aux_scan->offset); - off_rem_usecs = aux_scan->offset - os_cputime_ticks_to_usecs(off_ticks); - - start_time = ble_hdr->beg_cputime + off_ticks; - start_time_rem_usecs = ble_hdr->rem_usecs + off_rem_usecs; - if (start_time_rem_usecs >= 31) { - start_time++; - start_time_rem_usecs -= 31; - } - start_time -= g_ble_ll_sched_offset_ticks; - - /* Let's calculate time we reserve for aux packet. For now we assume to wait - * for fixed number of bytes and handle possible interrupting it in - * ble_ll_sched_execute_item(). This is because aux packet can be up to - * 256bytes and we don't want to block sched that long - */ - phy_mode = ble_ll_phy_to_phy_mode(aux_scan->aux_phy, - BLE_HCI_LE_PHY_CODED_ANY); - dur = ble_ll_pdu_tx_time_get(MYNEWT_VAL(BLE_LL_SCHED_SCAN_AUX_PDU_LEN), - phy_mode); - end_time = start_time + os_cputime_usecs_to_ticks(dur); - - sch->start_time = start_time; - sch->remainder = start_time_rem_usecs; - sch->end_time = end_time; - - OS_ENTER_CRITICAL(sr); - - rc = ble_ll_sched_insert(sch, 0, preempt_none); - - if (rc == 0) { - sch->cb_arg = ble_ll_scan_aux_data_ref(aux_scan); - STATS_INC(ble_ll_stats, aux_scheduled); - } - - OS_EXIT_CRITICAL(sr); - - ble_ll_sched_restart(); - - return rc; -} - int ble_ll_sched_scan_aux(struct ble_ll_sched_item *sch, uint32_t pdu_time, uint8_t pdu_time_rem, uint32_t offset_us) diff --git a/nimble/include/nimble/ble.h b/nimble/include/nimble/ble.h index 1ddd5773d..d349afebb 100644 --- a/nimble/include/nimble/ble.h +++ b/nimble/include/nimble/ble.h @@ -88,6 +88,9 @@ struct ble_mbuf_hdr_rxinfo * set for the same PDU (e.g. one use by scanner, other one used by * connection) */ +#define BLE_MBUF_HDR_F_CONNECT_IND_TXD (0x4000) +#define BLE_MBUF_HDR_F_CONNECT_REQ_TXD (0x4000) +#define BLE_MBUF_HDR_F_CONNECT_RSP_RXD (0x0008) #define BLE_MBUF_HDR_F_CONN_CREDIT (0x8000) #define BLE_MBUF_HDR_F_IGNORED (0x8000) #define BLE_MBUF_HDR_F_SCAN_REQ_TXD (0x4000)