diff --git a/nimble/controller/include/controller/ble_ll_ctrl.h b/nimble/controller/include/controller/ble_ll_ctrl.h index 0b042a2af..90156d51d 100644 --- a/nimble/controller/include/controller/ble_ll_ctrl.h +++ b/nimble/controller/include/controller/ble_ll_ctrl.h @@ -253,7 +253,7 @@ uint8_t ble_ll_ctrl_conn_param_reply(struct ble_ll_conn_sm *connsm, struct ble_ll_conn_params *req); int ble_ll_ctrl_reject_ind_send(struct ble_ll_conn_sm *connsm, uint8_t rej_opcode, uint8_t err); -int ble_ll_ctrl_start_enc_send(struct ble_ll_conn_sm *connsm, uint8_t opcode); +int ble_ll_ctrl_start_enc_send(struct ble_ll_conn_sm *connsm); int ble_ll_ctrl_enc_allowed_pdu_rx(struct os_mbuf *rxpdu); int ble_ll_ctrl_enc_allowed_pdu_tx(struct os_mbuf_pkthdr *pkthdr); int ble_ll_ctrl_tx_done(struct os_mbuf *txpdu, struct ble_ll_conn_sm *connsm); diff --git a/nimble/controller/include/controller/ble_ll_scan.h b/nimble/controller/include/controller/ble_ll_scan.h index b4415b790..2a9ba826e 100644 --- a/nimble/controller/include/controller/ble_ll_scan.h +++ b/nimble/controller/include/controller/ble_ll_scan.h @@ -92,6 +92,7 @@ struct ble_ll_scan_params #define BLE_LL_AUX_INCOMPLETE_BIT 0x02 #define BLE_LL_AUX_INCOMPLETE_ERR_BIT 0x04 #define BLE_LL_AUX_HAS_ADDRA 0x08 +#define BLE_LL_AUX_IGNORE_BIT 0x10 #define BLE_LL_CHECK_AUX_FLAG(aux_data, flag) (!!((aux_data)->flags & flag)) @@ -102,12 +103,14 @@ struct ble_ll_aux_data { uint8_t mode; uint8_t scanning; uint8_t flags; - uint16_t did; + uint16_t adi; uint32_t offset; uint8_t offset_units; uint8_t addr[6]; uint8_t addr_type; + uint8_t evt_type; struct ble_ll_sched_item sch; + struct ble_ll_ext_adv_report *evt; }; struct ble_ll_scan_sm @@ -234,9 +237,9 @@ int ble_ll_scan_ext_initiator_start(struct hci_ext_create_conn *hcc, struct ble_ll_scan_sm **sm); /* Called to parse extended advertising*/ -struct ble_ll_ext_adv; +struct ble_ll_ext_adv_report; int ble_ll_scan_parse_ext_hdr(struct os_mbuf *om, struct ble_mbuf_hdr *ble_hdr, - struct ble_ll_ext_adv *parsed_evt); + struct ble_ll_ext_adv_report *parsed_evt); void ble_ll_scan_aux_data_free(struct ble_ll_aux_data *aux_scan); #endif diff --git a/nimble/controller/src/ble_ll_adv.c b/nimble/controller/src/ble_ll_adv.c index d813c94db..943d666d7 100644 --- a/nimble/controller/src/ble_ll_adv.c +++ b/nimble/controller/src/ble_ll_adv.c @@ -394,9 +394,6 @@ static void ble_ll_adv_put_aux_ptr(struct ble_ll_adv_sm *advsm, uint32_t offset, uint8_t *dptr) { - /* in usecs */ - offset = os_cputime_ticks_to_usecs(offset); - dptr[0] = advsm->adv_secondary_chan; if (offset > 245700) { @@ -468,7 +465,7 @@ ble_ll_adv_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) /* AuxPtr */ if (AUX_CURRENT(advsm)->sch.enqueued) { - offset = AUX_CURRENT(advsm)->start_time - advsm->adv_pdu_start_time; + offset = os_cputime_ticks_to_usecs(AUX_CURRENT(advsm)->start_time - advsm->adv_pdu_start_time); } else { offset = 0; } @@ -547,12 +544,10 @@ ble_ll_adv_aux_pdu_make(uint8_t *dptr, void *pducb_arg, uint8_t *hdr_byte) */ offset = 0; } else if (advsm->rx_ble_hdr) { - offset = advsm->rx_ble_hdr->rem_usecs + - ble_ll_pdu_tx_time_get(12, advsm->sec_phy) + BLE_LL_IFS + 30; - offset = AUX_NEXT(advsm)->start_time - advsm->rx_ble_hdr->beg_cputime - - os_cputime_usecs_to_ticks(offset); + offset = os_cputime_ticks_to_usecs(AUX_NEXT(advsm)->start_time - advsm->rx_ble_hdr->beg_cputime); + offset -= (advsm->rx_ble_hdr->rem_usecs + ble_ll_pdu_tx_time_get(12, advsm->sec_phy) + BLE_LL_IFS); } else { - offset = AUX_NEXT(advsm)->start_time - aux->start_time; + offset = os_cputime_ticks_to_usecs(AUX_NEXT(advsm)->start_time - aux->start_time); } ble_ll_adv_put_aux_ptr(advsm, offset, dptr); @@ -1112,14 +1107,18 @@ ble_ll_adv_aux_calculate(struct ble_ll_adv_sm *advsm, aux->aux_data_offset = aux_data_offset; aux->aux_data_len = 0; aux->payload_len = 0; + aux->ext_hdr = 0; rem_aux_data_len = AUX_DATA_LEN(advsm) - aux_data_offset; chainable = !(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE); - /* Flags and ADI */ - aux->ext_hdr = (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT); - hdr_len = BLE_LL_EXT_ADV_HDR_LEN + BLE_LL_EXT_ADV_FLAGS_SIZE + - BLE_LL_EXT_ADV_DATA_INFO_SIZE; + hdr_len = BLE_LL_EXT_ADV_HDR_LEN + BLE_LL_EXT_ADV_FLAGS_SIZE; + + if (!(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE)) { + /* Flags and ADI */ + aux->ext_hdr |= (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT); + hdr_len += BLE_LL_EXT_ADV_DATA_INFO_SIZE; + } /* AdvA for 1st PDU in chain (i.e. AUX_ADV_IND or AUX_SCAN_RSP) */ if (aux_data_offset == 0) { diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c index 3528940f1..0d25c34a9 100644 --- a/nimble/controller/src/ble_ll_conn.c +++ b/nimble/controller/src/ble_ll_conn.c @@ -2130,6 +2130,15 @@ ble_ll_conn_end(struct ble_ll_conn_sm *connsm, uint8_t ble_err) connsm->csmflags.cfbit.pending_hci_rd_features = 0; } + /* + * If there is still pending read features request HCI command, send an + * event to complete it. + */ + if (connsm->csmflags.cfbit.pending_hci_rd_features) { + ble_ll_hci_ev_rd_rem_used_feat(connsm, ble_err); + connsm->csmflags.cfbit.pending_hci_rd_features = 0; + } + /* * We need to send a disconnection complete event. Connection Complete for * canceling connection creation is sent from LE Create Connection Cancel @@ -2143,15 +2152,6 @@ ble_ll_conn_end(struct ble_ll_conn_sm *connsm, uint8_t ble_err) ble_ll_disconn_comp_event_send(connsm, ble_err); } - /* - * If there is still pending read features request HCI command, send an - * event to complete it. - */ - if (connsm->csmflags.cfbit.pending_hci_rd_features) { - ble_ll_hci_ev_rd_rem_used_feat(connsm, BLE_ERR_UNK_CONN_ID); - connsm->csmflags.cfbit.pending_hci_rd_features = 0; - } - /* Put connection state machine back on free list */ STAILQ_INSERT_TAIL(&g_ble_ll_conn_free_list, connsm, free_stqe); @@ -3907,7 +3907,8 @@ chk_rx_terminate_ind: } /* If we received a terminate IND, we must set some flags */ - if (is_ctrl && (opcode == BLE_LL_CTRL_TERMINATE_IND)) { + if (is_ctrl && (opcode == BLE_LL_CTRL_TERMINATE_IND) + && (rx_pyld_len == BLE_LL_CTRL_TERMINATE_IND_LEN)) { connsm->csmflags.cfbit.terminate_ind_rxd = 1; connsm->rxd_disconnect_reason = rxbuf[3]; reply = 1; diff --git a/nimble/controller/src/ble_ll_conn_hci.c b/nimble/controller/src/ble_ll_conn_hci.c index 5c086fbf3..526bad00d 100644 --- a/nimble/controller/src/ble_ll_conn_hci.c +++ b/nimble/controller/src/ble_ll_conn_hci.c @@ -853,7 +853,7 @@ ble_ll_conn_hci_update(uint8_t *cmdbuf) /* See if this feature is supported on both sides */ if ((connsm->conn_features & BLE_LL_FEAT_CONN_PARM_REQ) == 0) { if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) { - return BLE_ERR_CMD_DISALLOWED; + return BLE_ERR_UNSUPP_REM_FEATURE; } ctrl_proc = BLE_LL_CTRL_PROC_CONN_UPDATE; } else { @@ -1362,7 +1362,7 @@ ble_ll_conn_hci_le_ltk_reply(uint8_t *cmdbuf, uint8_t *rspbuf, uint8_t *rsplen) swap_buf(connsm->enc_data.enc_block.key, cmdbuf + 2, 16); ble_ll_calc_session_key(connsm); - ble_ll_ctrl_start_enc_send(connsm, BLE_LL_CTRL_START_ENC_REQ); + ble_ll_ctrl_start_enc_send(connsm); rc = BLE_ERR_SUCCESS; ltk_key_cmd_complete: diff --git a/nimble/controller/src/ble_ll_ctrl.c b/nimble/controller/src/ble_ll_ctrl.c index 50c0b724a..4ae03bb0f 100644 --- a/nimble/controller/src/ble_ll_ctrl.c +++ b/nimble/controller/src/ble_ll_ctrl.c @@ -356,6 +356,91 @@ conn_param_pdu_exit: return rsp_opcode; } +/** + * Called to make a connection update request LL control PDU + * + * Context: Link Layer + * + * @param connsm + * @param rsp + */ +static void +ble_ll_ctrl_conn_upd_make(struct ble_ll_conn_sm *connsm, uint8_t *pyld, + struct ble_ll_conn_params *cp) +{ + uint16_t instant; + uint32_t dt; + uint32_t num_old_ce; + uint32_t new_itvl_usecs; + uint32_t old_itvl_usecs; + struct hci_conn_update *hcu; + struct ble_ll_conn_upd_req *req; + + /* + * Set instant. We set the instant to the current event counter plus + * the amount of slave latency as the slave may not be listening + * at every connection interval and we are not sure when the connect + * request will actually get sent. We add one more event plus the + * minimum as per the spec of 6 connection events. + */ + instant = connsm->event_cntr + connsm->slave_latency + 6 + 1; + + /* + * XXX: This should change in the future, but for now we will just + * start the new instant at the same anchor using win offset 0. + */ + /* Copy parameters in connection update structure */ + hcu = &connsm->conn_param_req; + req = &connsm->conn_update_req; + if (cp) { + /* XXX: so we need to make the new anchor point some time away + * from txwinoffset by some amount of msecs. Not sure how to do + that here. We dont need to, but we should. */ + /* Calculate offset from requested offsets (if any) */ + if (cp->offset0 != 0xFFFF) { + new_itvl_usecs = cp->interval_max * BLE_LL_CONN_ITVL_USECS; + old_itvl_usecs = connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS; + if ((int16_t)(cp->ref_conn_event_cnt - instant) >= 0) { + num_old_ce = cp->ref_conn_event_cnt - instant; + dt = old_itvl_usecs * num_old_ce; + dt += (cp->offset0 * BLE_LL_CONN_ITVL_USECS); + dt = dt % new_itvl_usecs; + } else { + num_old_ce = instant - cp->ref_conn_event_cnt; + dt = old_itvl_usecs * num_old_ce; + dt -= (cp->offset0 * BLE_LL_CONN_ITVL_USECS); + dt = dt % new_itvl_usecs; + dt = new_itvl_usecs - dt; + } + req->winoffset = dt / BLE_LL_CONN_TX_WIN_USECS; + } else { + req->winoffset = 0; + } + req->interval = cp->interval_max; + req->timeout = cp->timeout; + req->latency = cp->latency; + req->winsize = 1; + } else { + req->interval = hcu->conn_itvl_max; + req->timeout = hcu->supervision_timeout; + req->latency = hcu->conn_latency; + req->winoffset = 0; + req->winsize = connsm->tx_win_size; + } + req->instant = instant; + + /* XXX: make sure this works for the connection parameter request proc. */ + pyld[0] = req->winsize; + put_le16(pyld + 1, req->winoffset); + put_le16(pyld + 3, req->interval); + put_le16(pyld + 5, req->latency); + put_le16(pyld + 7, req->timeout); + put_le16(pyld + 9, instant); + + /* Set flag in state machine to denote we have scheduled an update */ + connsm->csmflags.cfbit.conn_update_sched = 1; +} + /** * Called to process and UNKNOWN_RSP LL control packet. * @@ -363,8 +448,8 @@ conn_param_pdu_exit: * * @param dptr */ -static void -ble_ll_ctrl_proc_unk_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +static int +ble_ll_ctrl_proc_unk_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspdata) { uint8_t ctrl_proc; uint8_t opcode; @@ -383,8 +468,14 @@ ble_ll_ctrl_proc_unk_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr) case BLE_LL_CTRL_SLAVE_FEATURE_REQ: ctrl_proc = BLE_LL_CTRL_PROC_FEATURE_XCHG; break; - case BLE_LL_CTRL_CONN_PARM_RSP: case BLE_LL_CTRL_CONN_PARM_REQ: + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + ble_ll_ctrl_conn_upd_make(connsm, rspdata, NULL); + connsm->reject_reason = BLE_ERR_SUCCESS; + return BLE_LL_CTRL_CONN_UPDATE_IND; + } + /* note: fall-through intentional */ + case BLE_LL_CTRL_CONN_PARM_RSP: ctrl_proc = BLE_LL_CTRL_PROC_CONN_PARAM_REQ; break; case BLE_LL_CTRL_PING_REQ: @@ -418,6 +509,38 @@ ble_ll_ctrl_proc_unk_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr) connsm->csmflags.cfbit.pending_hci_rd_features = 0; } } + + return BLE_ERR_MAX; +} + +/** + * Callback when LL control procedure times out (for a given connection). If + * this is called, it means that we need to end the connection because it + * has not responded to a LL control request. + * + * Context: Link Layer + * + * @param arg Pointer to connection state machine. + */ +void +ble_ll_ctrl_proc_rsp_timer_cb(struct ble_npl_event *ev) +{ + /* Control procedure has timed out. Kill the connection */ + ble_ll_conn_timeout((struct ble_ll_conn_sm *)ble_npl_event_get_arg(ev), + BLE_ERR_LMP_LL_RSP_TMO); +} + +static void +ble_ll_ctrl_start_rsp_timer(struct ble_ll_conn_sm *connsm) +{ + ble_npl_callout_init(&connsm->ctrl_proc_rsp_timer, + &g_ble_ll_data.ll_evq, + ble_ll_ctrl_proc_rsp_timer_cb, + connsm); + + /* Re-start timer. Control procedure timeout is 40 seconds */ + ble_npl_callout_reset(&connsm->ctrl_proc_rsp_timer, + ble_npl_time_ms_to_ticks32(BLE_LL_CTRL_PROC_TIMEOUT_MS)); } #if (BLE_LL_BT5_PHY_SUPPORTED == 1) @@ -700,6 +823,10 @@ ble_ll_ctrl_rx_phy_req(struct ble_ll_conn_sm *connsm, uint8_t *req, CONN_F_PEER_PHY_UPDATE(connsm) = 1; ble_ll_ctrl_phy_req_rsp_make(connsm, rsp); rsp_opcode = BLE_LL_CTRL_PHY_RSP; + + /* Start response timer */ + connsm->cur_ctrl_proc = BLE_LL_CTRL_PROC_PHY_UPDATE; + ble_ll_ctrl_start_rsp_timer(connsm); } return rsp_opcode; } @@ -992,7 +1119,7 @@ ble_ll_ctrl_is_start_enc_rsp(struct os_mbuf *txpdu) } /** - * Called to create and send a LL_START_ENC_REQ or LL_START_ENC_RSP + * Called to create and send a LL_START_ENC_REQ * * @param connsm * @param err @@ -1000,7 +1127,7 @@ ble_ll_ctrl_is_start_enc_rsp(struct os_mbuf *txpdu) * @return int */ int -ble_ll_ctrl_start_enc_send(struct ble_ll_conn_sm *connsm, uint8_t opcode) +ble_ll_ctrl_start_enc_send(struct ble_ll_conn_sm *connsm) { int rc; struct os_mbuf *om; @@ -1008,8 +1135,13 @@ ble_ll_ctrl_start_enc_send(struct ble_ll_conn_sm *connsm, uint8_t opcode) om = os_msys_get_pkthdr(BLE_LL_CTRL_MAX_PDU_LEN, sizeof(struct ble_mbuf_hdr)); if (om) { - om->om_data[0] = opcode; + om->om_data[0] = BLE_LL_CTRL_START_ENC_REQ; ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, 1); + + /* Wait for LL_START_ENC_RSP */ + connsm->cur_ctrl_proc = BLE_LL_CTRL_PROC_ENCRYPT; + ble_ll_ctrl_start_rsp_timer(connsm); + rc = 0; } else { rc = -1; @@ -1226,11 +1358,12 @@ ble_ll_ctrl_rx_start_enc_rsp(struct ble_ll_conn_sm *connsm) return BLE_ERR_MAX; } + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_ENCRYPT); + /* If master, we are done. Stop control procedure and sent event to host */ if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { /* We are encrypted */ connsm->enc_data.enc_state = CONN_ENC_S_ENCRYPTED; - ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_ENCRYPT); #if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_PING) == 1) ble_ll_conn_auth_pyld_timer_start(connsm); #endif @@ -1336,91 +1469,6 @@ ble_ll_ctrl_chanmap_req_make(struct ble_ll_conn_sm *connsm, uint8_t *pyld) connsm->csmflags.cfbit.chanmap_update_scheduled = 1; } -/** - * Called to make a connection update request LL control PDU - * - * Context: Link Layer - * - * @param connsm - * @param rsp - */ -static void -ble_ll_ctrl_conn_upd_make(struct ble_ll_conn_sm *connsm, uint8_t *pyld, - struct ble_ll_conn_params *cp) -{ - uint16_t instant; - uint32_t dt; - uint32_t num_old_ce; - uint32_t new_itvl_usecs; - uint32_t old_itvl_usecs; - struct hci_conn_update *hcu; - struct ble_ll_conn_upd_req *req; - - /* - * Set instant. We set the instant to the current event counter plus - * the amount of slave latency as the slave may not be listening - * at every connection interval and we are not sure when the connect - * request will actually get sent. We add one more event plus the - * minimum as per the spec of 6 connection events. - */ - instant = connsm->event_cntr + connsm->slave_latency + 6 + 1; - - /* - * XXX: This should change in the future, but for now we will just - * start the new instant at the same anchor using win offset 0. - */ - /* Copy parameters in connection update structure */ - hcu = &connsm->conn_param_req; - req = &connsm->conn_update_req; - if (cp) { - /* XXX: so we need to make the new anchor point some time away - * from txwinoffset by some amount of msecs. Not sure how to do - that here. We dont need to, but we should. */ - /* Calculate offset from requested offsets (if any) */ - if (cp->offset0 != 0xFFFF) { - new_itvl_usecs = cp->interval_max * BLE_LL_CONN_ITVL_USECS; - old_itvl_usecs = connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS; - if ((int16_t)(cp->ref_conn_event_cnt - instant) >= 0) { - num_old_ce = cp->ref_conn_event_cnt - instant; - dt = old_itvl_usecs * num_old_ce; - dt += (cp->offset0 * BLE_LL_CONN_ITVL_USECS); - dt = dt % new_itvl_usecs; - } else { - num_old_ce = instant - cp->ref_conn_event_cnt; - dt = old_itvl_usecs * num_old_ce; - dt -= (cp->offset0 * BLE_LL_CONN_ITVL_USECS); - dt = dt % new_itvl_usecs; - dt = new_itvl_usecs - dt; - } - req->winoffset = dt / BLE_LL_CONN_TX_WIN_USECS; - } else { - req->winoffset = 0; - } - req->interval = cp->interval_max; - req->timeout = cp->timeout; - req->latency = cp->latency; - req->winsize = 1; - } else { - req->interval = hcu->conn_itvl_max; - req->timeout = hcu->supervision_timeout; - req->latency = hcu->conn_latency; - req->winoffset = 0; - req->winsize = connsm->tx_win_size; - } - req->instant = instant; - - /* XXX: make sure this works for the connection parameter request proc. */ - pyld[0] = req->winsize; - put_le16(pyld + 1, req->winoffset); - put_le16(pyld + 3, req->interval); - put_le16(pyld + 5, req->latency); - put_le16(pyld + 7, req->timeout); - put_le16(pyld + 9, instant); - - /* Set flag in state machine to denote we have scheduled an update */ - connsm->csmflags.cfbit.conn_update_sched = 1; -} - /** * Called to respond to a LL control PDU connection parameter request or * response. @@ -1458,11 +1506,12 @@ ble_ll_ctrl_conn_param_reply(struct ble_ll_conn_sm *connsm, uint8_t *rsp, * @param dptr * @param opcode */ -static void +static int ble_ll_ctrl_rx_reject_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr, - uint8_t opcode) + uint8_t opcode, uint8_t *rspdata) { uint8_t ble_error; + uint8_t rsp_opcode = BLE_ERR_MAX; /* Get error out of received PDU */ if (opcode == BLE_LL_CTRL_REJECT_IND) { @@ -1476,8 +1525,15 @@ ble_ll_ctrl_rx_reject_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr, switch (connsm->cur_ctrl_proc) { case BLE_LL_CTRL_PROC_CONN_PARAM_REQ: if (opcode == BLE_LL_CTRL_REJECT_IND_EXT) { - ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ); - ble_ll_hci_ev_conn_update(connsm, ble_error); + /* As a master we should send connection update indication in this point */ + if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) { + rsp_opcode = BLE_LL_CTRL_CONN_UPDATE_IND; + ble_ll_ctrl_conn_upd_make(connsm, rspdata, NULL); + connsm->reject_reason = BLE_ERR_SUCCESS; + } else { + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ); + ble_ll_hci_ev_conn_update(connsm, ble_error); + } } break; #if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1) @@ -1503,6 +1559,8 @@ ble_ll_ctrl_rx_reject_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr, default: break; } + + return rsp_opcode; } /** @@ -1806,23 +1864,6 @@ ble_ll_ctrl_rx_chanmap_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr) } } -/** - * Callback when LL control procedure times out (for a given connection). If - * this is called, it means that we need to end the connection because it - * has not responded to a LL control request. - * - * Context: Link Layer - * - * @param arg Pointer to connection state machine. - */ -void -ble_ll_ctrl_proc_rsp_timer_cb(struct ble_npl_event *ev) -{ - /* Control procedure has timed out. Kill the connection */ - ble_ll_conn_timeout((struct ble_ll_conn_sm *)ble_npl_event_get_arg(ev), - BLE_ERR_LMP_LL_RSP_TMO); -} - /** * Initiate LL control procedure. * @@ -2020,14 +2061,7 @@ ble_ll_ctrl_proc_start(struct ble_ll_conn_sm *connsm, int ctrl_proc) /* Initialize the procedure response timeout */ if (ctrl_proc != BLE_LL_CTRL_PROC_CHAN_MAP_UPD) { - ble_npl_callout_init(&connsm->ctrl_proc_rsp_timer, - &g_ble_ll_data.ll_evq, - ble_ll_ctrl_proc_rsp_timer_cb, - connsm); - - /* Re-start timer. Control procedure timeout is 40 seconds */ - ble_npl_callout_reset(&connsm->ctrl_proc_rsp_timer, - ble_npl_time_ms_to_ticks32(BLE_LL_CTRL_PROC_TIMEOUT_MS)); + ble_ll_ctrl_start_rsp_timer(connsm); } } } @@ -2270,7 +2304,7 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om) } break; case BLE_LL_CTRL_UNKNOWN_RSP: - ble_ll_ctrl_proc_unk_rsp(connsm, dptr); + rsp_opcode = ble_ll_ctrl_proc_unk_rsp(connsm, dptr, rspdata); break; case BLE_LL_CTRL_FEATURE_REQ: rsp_opcode = ble_ll_ctrl_rx_feature_req(connsm, dptr, rspbuf, opcode); @@ -2338,7 +2372,8 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om) /* Fall-through intentional... */ case BLE_LL_CTRL_REJECT_IND: case BLE_LL_CTRL_REJECT_IND_EXT: - ble_ll_ctrl_rx_reject_ind(connsm, dptr, opcode); + /* Sometimes reject triggers sending other LL CTRL msg */ + rsp_opcode = ble_ll_ctrl_rx_reject_ind(connsm, dptr, opcode, rspdata); break; #if (BLE_LL_BT5_PHY_SUPPORTED == 1) case BLE_LL_CTRL_PHY_REQ: @@ -2453,8 +2488,15 @@ ble_ll_ctrl_tx_done(struct os_mbuf *txpdu, struct ble_ll_conn_sm *connsm) break; case BLE_LL_CTRL_REJECT_IND_EXT: if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_CONN_PARAM_REQ) { - connsm->reject_reason = txpdu->om_data[2]; - connsm->csmflags.cfbit.host_expects_upd_event = 1; + /* If rejecting opcode is BLE_LL_CTRL_PROC_CONN_PARAM_REQ and + * reason is LMP collision that means we are master on the link and + * peer wanted to start procedure which we already started. + * Let's wait for response and do not close procedure. */ + if (txpdu->om_data[1] == BLE_LL_CTRL_CONN_PARM_REQ && + txpdu->om_data[2] != BLE_ERR_LMP_COLLISION) { + connsm->reject_reason = txpdu->om_data[2]; + connsm->csmflags.cfbit.host_expects_upd_event = 1; + } } #if (MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_ENCRYPTION) == 1) if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) { diff --git a/nimble/controller/src/ble_ll_hci.c b/nimble/controller/src/ble_ll_hci.c index 3d4ae900f..07f520bf2 100644 --- a/nimble/controller/src/ble_ll_hci.c +++ b/nimble/controller/src/ble_ll_hci.c @@ -321,16 +321,21 @@ ble_ll_hci_le_read_bufsize(uint8_t *rspbuf, uint8_t *rsplen) * @param txphy Pointer to output tx phy mask * @param rxphy Pointer to output rx phy mask * - * @return int BLE_ERR_SUCCESS or BLE_ERR_INV_HCI_CMD_PARMS + * @return int BLE_ERR_SUCCESS or BLE_ERR_INV_HCI_CMD_PARMS or BLE_ERR_UNSUPPORTED */ int ble_ll_hci_chk_phy_masks(uint8_t *cmdbuf, uint8_t *txphy, uint8_t *rxphy) { - int rc; uint8_t all_phys; uint8_t rx_phys; uint8_t tx_phys; + /* Check for RFU */ + if ((cmdbuf[1] & ~BLE_HCI_LE_PHY_PREF_MASK_ALL) || + (cmdbuf[2] & ~BLE_HCI_LE_PHY_PREF_MASK_ALL)) { + return BLE_ERR_UNSUPPORTED; + } + /* Check for valid values */ all_phys = cmdbuf[0]; tx_phys = cmdbuf[1] & BLE_HCI_LE_PHY_PREF_MASK_ALL; @@ -338,29 +343,34 @@ ble_ll_hci_chk_phy_masks(uint8_t *cmdbuf, uint8_t *txphy, uint8_t *rxphy) if ((!(all_phys & BLE_HCI_LE_PHY_NO_TX_PREF_MASK) && (tx_phys == 0)) || (!(all_phys & BLE_HCI_LE_PHY_NO_RX_PREF_MASK) && (rx_phys == 0))) { - rc = BLE_ERR_INV_HCI_CMD_PARMS; - } else { - /* If phy not supported, wipe its bit */ + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* If phy not supported, return error */ #if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_2M_PHY) - tx_phys &= ~BLE_HCI_LE_PHY_2M_PREF_MASK; - rx_phys &= ~BLE_HCI_LE_PHY_2M_PREF_MASK; + if((tx_phys & BLE_HCI_LE_PHY_2M_PREF_MASK) || + (rx_phys & BLE_HCI_LE_PHY_2M_PREF_MASK)) { + return BLE_ERR_UNSUPPORTED; + } #endif #if !MYNEWT_VAL(BLE_LL_CFG_FEAT_LE_CODED_PHY) - tx_phys &= ~BLE_HCI_LE_PHY_CODED_PREF_MASK; - rx_phys &= ~BLE_HCI_LE_PHY_CODED_PREF_MASK; -#endif - /* Set the default PHY preferences */ - if (all_phys & BLE_HCI_LE_PHY_NO_TX_PREF_MASK) { - tx_phys = BLE_HCI_LE_PHY_PREF_MASK_ALL; - } - *txphy = tx_phys; - if (all_phys & BLE_HCI_LE_PHY_NO_RX_PREF_MASK) { - rx_phys = BLE_HCI_LE_PHY_PREF_MASK_ALL; - } - *rxphy = rx_phys; - rc = BLE_ERR_SUCCESS; + if ((tx_phys & BLE_HCI_LE_PHY_CODED_PREF_MASK) || + (rx_phys & BLE_HCI_LE_PHY_CODED_PREF_MASK)) { + return BLE_ERR_UNSUPPORTED; } - return rc; +#endif + /* Set the default PHY preferences */ + if (all_phys & BLE_HCI_LE_PHY_NO_TX_PREF_MASK) { + tx_phys = BLE_HCI_LE_PHY_PREF_MASK_ALL; + } + *txphy = tx_phys; + + if (all_phys & BLE_HCI_LE_PHY_NO_RX_PREF_MASK) { + rx_phys = BLE_HCI_LE_PHY_PREF_MASK_ALL; + } + *rxphy = rx_phys; + + return BLE_ERR_SUCCESS; } /** diff --git a/nimble/controller/src/ble_ll_resolv.c b/nimble/controller/src/ble_ll_resolv.c index 4f0308d75..cc0dbfc1a 100644 --- a/nimble/controller/src/ble_ll_resolv.c +++ b/nimble/controller/src/ble_ll_resolv.c @@ -485,10 +485,6 @@ ble_ll_resolve_set_priv_mode(uint8_t *cmdbuf) return BLE_ERR_CMD_DISALLOWED; } - if (!ble_ll_resolv_enabled()) { - return BLE_ERR_CMD_DISALLOWED; - } - /* cmdbuf = addr_type(0) | addr(6) | priv_mode(1) */ rl = ble_ll_resolv_list_find(&cmdbuf[1], cmdbuf[0]); if (!rl) { diff --git a/nimble/controller/src/ble_ll_scan.c b/nimble/controller/src/ble_ll_scan.c index e6a47a31b..85762fbb1 100644 --- a/nimble/controller/src/ble_ll_scan.c +++ b/nimble/controller/src/ble_ll_scan.c @@ -95,7 +95,7 @@ struct ble_ll_ext_adv_hdr uint8_t hdr[0]; }; -struct ble_ll_ext_adv { +struct ble_ll_ext_adv_report { /* We support one report per event for now */ uint8_t event_meta; /* BLE_HCI_EVCODE_LE_META */ uint8_t event_len; @@ -572,12 +572,12 @@ ble_ll_scan_add_scan_rsp_adv(uint8_t *addr, uint8_t txadd) #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) -static struct ble_ll_ext_adv * -ble_ll_scan_init_ext_adv(struct ble_ll_ext_adv *copy_from) +static struct ble_ll_ext_adv_report * +ble_ll_scan_init_ext_adv_report(struct ble_ll_ext_adv_report *copy_from) { - struct ble_ll_ext_adv *evt; + struct ble_ll_ext_adv_report *evt; - evt = (struct ble_ll_ext_adv *) ble_hci_trans_buf_alloc( + evt = (struct ble_ll_ext_adv_report *) ble_hci_trans_buf_alloc( BLE_HCI_TRANS_BUF_EVT_LO); if (!evt) { return NULL; @@ -612,7 +612,7 @@ ble_ll_hci_send_legacy_ext_adv_report(uint8_t evtype, uint8_t adv_data_len, struct os_mbuf *adv_data, uint8_t *inita) { - struct ble_ll_ext_adv *evt; + struct ble_ll_ext_adv_report *evt; if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_EXT_ADV_RPT)) { return -1; @@ -624,7 +624,7 @@ ble_ll_hci_send_legacy_ext_adv_report(uint8_t evtype, return -1; } - evt = ble_ll_scan_init_ext_adv(NULL); + evt = ble_ll_scan_init_ext_adv_report(NULL); if (!evt) { return 0; } @@ -1591,7 +1591,7 @@ ble_ll_ext_scan_parse_aux_ptr(struct ble_ll_scan_sm *scansm, static void ble_ll_ext_scan_parse_adv_info(struct ble_ll_scan_sm *scansm, - struct ble_ll_ext_adv *evt, uint8_t *buf) + struct ble_ll_ext_adv_report *evt, uint8_t *buf) { uint16_t adv_info = get_le16(buf); @@ -1628,6 +1628,7 @@ ble_ll_scan_get_aux_data(struct ble_ll_scan_sm *scansm, uint8_t ext_hdr_flags; uint8_t *ext_hdr; uint8_t has_addr = 0; + uint8_t has_adi = 0; int i; struct ble_ll_aux_data tmp_aux_data = { 0 }; @@ -1639,7 +1640,7 @@ ble_ll_scan_get_aux_data(struct ble_ll_scan_sm *scansm, tmp_aux_data.mode = rxbuf[2] >> 6; ext_hdr_len = rxbuf[2] & 0x3F; - if (ext_hdr_len < BLE_LL_EXT_ADV_AUX_PTR_SIZE) { + if (ext_hdr_len < BLE_LL_EXT_ADV_AUX_PTR_SIZE && !scansm->cur_aux_data) { return -1; } @@ -1665,8 +1666,9 @@ ble_ll_scan_get_aux_data(struct ble_ll_scan_sm *scansm, } if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) { - tmp_aux_data.did = get_le16(ext_hdr + i); + tmp_aux_data.adi = get_le16(ext_hdr + i); i += BLE_LL_EXT_ADV_DATA_INFO_SIZE; + has_adi = 1; } if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_AUX_PTR_BIT)) { @@ -1682,7 +1684,8 @@ ble_ll_scan_get_aux_data(struct ble_ll_scan_sm *scansm, *aux_data = scansm->cur_aux_data; /* TODO Collision; Do smth smart when did does not match */ - if (tmp_aux_data.did != (*aux_data)->did) { + if (!((*aux_data)->evt_type & (BLE_HCI_ADV_SCAN_MASK)) + && (tmp_aux_data.adi != (*aux_data)->adi)) { STATS_INC(ble_ll_stats, aux_chain_err); (*aux_data)->flags |= BLE_LL_AUX_INCOMPLETE_ERR_BIT; } @@ -1703,7 +1706,10 @@ ble_ll_scan_get_aux_data(struct ble_ll_scan_sm *scansm, scansm->cur_aux_data = NULL; } - (*aux_data)->did = tmp_aux_data.did; + if (has_adi) { + (*aux_data)->adi = tmp_aux_data.adi; + } + (*aux_data)->chan = tmp_aux_data.chan; (*aux_data)->offset = tmp_aux_data.offset; (*aux_data)->mode = tmp_aux_data.mode; @@ -1736,7 +1742,7 @@ ble_ll_scan_get_aux_data(struct ble_ll_scan_sm *scansm, */ int ble_ll_scan_parse_ext_hdr(struct os_mbuf *om, struct ble_mbuf_hdr *ble_hdr, - struct ble_ll_ext_adv *out_evt) + struct ble_ll_ext_adv_report *out_evt) { uint8_t pdu_len; uint8_t ext_hdr_len; @@ -1768,6 +1774,10 @@ ble_ll_scan_parse_ext_hdr(struct os_mbuf *om, struct ble_mbuf_hdr *ble_hdr, return -1; } + if (BLE_MBUF_HDR_SCAN_RSP_RCV(ble_hdr)) { + out_evt->evt_type |= BLE_HCI_ADV_SCAN_RSP_MASK; + } + ext_hdr_len = rxbuf[2] & 0x3F; os_mbuf_adj(om, 3); @@ -1781,7 +1791,7 @@ ble_ll_scan_parse_ext_hdr(struct os_mbuf *om, struct ble_mbuf_hdr *ble_hdr, ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK); i += BLE_LL_EXT_ADV_ADVA_SIZE; } else { - if (aux_data->flags & BLE_LL_AUX_HAS_ADDRA) { + if (aux_data && (aux_data->flags & BLE_LL_AUX_HAS_ADDRA)) { /* Have address in aux_data */ memcpy(out_evt->addr, aux_data->addr, 6); out_evt->addr_type = aux_data->addr_type; @@ -1793,6 +1803,7 @@ ble_ll_scan_parse_ext_hdr(struct os_mbuf *om, struct ble_mbuf_hdr *ble_hdr, out_evt->dir_addr_type = ble_ll_get_addr_type(rxbuf[0] & BLE_ADV_PDU_HDR_RXADD_MASK); i += BLE_LL_EXT_ADV_TARGETA_SIZE; + out_evt->evt_type |= BLE_HCI_ADV_DIRECT_MASK; } if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_RFU_BIT)) { @@ -1803,6 +1814,8 @@ ble_ll_scan_parse_ext_hdr(struct os_mbuf *om, struct ble_mbuf_hdr *ble_hdr, if (ext_hdr_flags & (1 << BLE_LL_EXT_ADV_DATA_INFO_BIT)) { ble_ll_ext_scan_parse_adv_info(scansm, out_evt, (ext_hdr + i)); i += BLE_LL_EXT_ADV_DATA_INFO_SIZE; + } else if (out_evt->evt_type & BLE_HCI_ADV_SCAN_RSP_MASK) { + out_evt->sid = (aux_data->adi >> 12); } /* In this point of time we don't want to care about aux ptr */ @@ -1834,13 +1847,16 @@ ble_ll_scan_parse_ext_hdr(struct os_mbuf *om, struct ble_mbuf_hdr *ble_hdr, out_evt->sec_phy = aux_data->aux_phy; out_evt->prim_phy = aux_data->aux_primary_phy; - if (BLE_MBUF_HDR_SCAN_RSP_RCV(ble_hdr)) { - out_evt->evt_type |= BLE_HCI_ADV_SCAN_RSP_MASK; - } - /* Adjust mbuf to contain advertising data only */ os_mbuf_adj(om, i); + /* Let us first keep update event type in aux data. + * Note that in aux chain and aux scan response packets + * we do miss original event type, which we need for advertising report. + */ + aux_data->evt_type |= out_evt->evt_type; + out_evt->evt_type = aux_data->evt_type; + done: return pdu_len - i - 1; } @@ -2271,83 +2287,123 @@ void ble_ll_scan_aux_data_free(struct ble_ll_aux_data *aux_scan) { if (aux_scan) { + if (aux_scan->evt) { + ble_hci_trans_buf_free((uint8_t *)aux_scan->evt); + } os_memblock_put(&ext_adv_pool, aux_scan); } } -static void +/* + * Send extended advertising report + * + * @return -1 on error (data truncated or other error) + * 0 on success (data status is "completed") + * 1 on success (data status is not "completed") + */ +static int ble_ll_hci_send_ext_adv_report(uint8_t ptype, struct os_mbuf *om, struct ble_mbuf_hdr *hdr) { struct ble_ll_aux_data *aux_data = hdr->rxinfo.user_data; - struct ble_ll_ext_adv *evt; - struct ble_ll_ext_adv *next_evt; + struct ble_ll_ext_adv_report *evt; + struct ble_ll_ext_adv_report *next_evt; int offset; int datalen; + int rc; + bool need_event; if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_EXT_ADV_RPT)) { - return; + return -1; } - if (!aux_data) { - BLE_LL_ASSERT(0); - return; - } - - evt = ble_ll_scan_init_ext_adv(NULL); - if (!evt) { - return; + /* + * We keep one allocated event in aux_data to be able to truncate chain + * properly in case of error. If there is no event in aux_data it means this + * is the first event for this chain. + */ + if (aux_data && aux_data->evt) { + evt = aux_data->evt; + } else { + evt = ble_ll_scan_init_ext_adv_report(NULL); + if (!evt) { + return -1; + } } datalen = ble_ll_scan_parse_ext_hdr(om, hdr, evt); if (datalen < 0) { + /* XXX what should we do here? send some trimmed event? */ ble_hci_trans_buf_free((uint8_t *)evt); - return; + return -1; } offset = 0; do { + need_event = false; next_evt = NULL; evt->adv_data_len = min(BLE_LL_MAX_EVT_LEN - sizeof(*evt), datalen - offset); - os_mbuf_copydata(om, offset, evt->adv_data_len, evt->adv_data); + evt->event_len = (sizeof(*evt) - BLE_HCI_EVENT_HDR_LEN) + + evt->adv_data_len; + evt->rssi = hdr->rxinfo.rssi; + os_mbuf_copydata(om, offset, evt->adv_data_len, evt->adv_data); offset += evt->adv_data_len; + if (offset < datalen) { + /* Need another event for next fragment of this PDU */ + need_event = true; + } else if (BLE_LL_CHECK_AUX_FLAG(aux_data, BLE_LL_AUX_INCOMPLETE_ERR_BIT)) { + need_event = false; + } else if (BLE_LL_CHECK_AUX_FLAG(aux_data, BLE_LL_AUX_INCOMPLETE_BIT)) { + /* Need another event for next PDU in chain */ + need_event = true; + } else { + need_event = false; + } + + /* Assume data status is not "completed" */ + rc = 1; + + if (need_event) { /* - * We need to fragment this PDU as it does not fit in single HCI - * event. Subsequent event will have exactly the same header data. - * If we cannot allocate another HCI event from pool, just mark - * data as truncated. + * We will need another event so let's try to allocate one now. If + * we cannot do this, need to mark event as truncated. */ - next_evt = ble_ll_scan_init_ext_adv(evt); + next_evt = ble_ll_scan_init_ext_adv_report(evt); if (next_evt) { evt->evt_type |= (BLE_HCI_ADV_DATA_STATUS_INCOMPLETE); } else { evt->evt_type |= (BLE_HCI_ADV_DATA_STATUS_TRUNCATED); + rc = -1; + } + } else if (aux_data) { + if (BLE_LL_CHECK_AUX_FLAG(aux_data, BLE_LL_AUX_INCOMPLETE_ERR_BIT)) { + evt->evt_type |= (BLE_HCI_ADV_DATA_STATUS_TRUNCATED); + rc = -1; + } else if (BLE_LL_CHECK_AUX_FLAG(aux_data, BLE_LL_AUX_INCOMPLETE_BIT)) { + evt->evt_type |= (BLE_HCI_ADV_DATA_STATUS_INCOMPLETE); } } else { - if (BLE_LL_CHECK_AUX_FLAG(aux_data, BLE_LL_AUX_INCOMPLETE_BIT)) { - evt->evt_type |= (BLE_HCI_ADV_DATA_STATUS_INCOMPLETE); - } else if (BLE_LL_CHECK_AUX_FLAG(aux_data, BLE_LL_AUX_INCOMPLETE_ERR_BIT)) { - evt->evt_type |= (BLE_HCI_ADV_DATA_STATUS_TRUNCATED); - } + rc = 0; } - evt->event_len = (sizeof(*evt) - BLE_HCI_EVENT_HDR_LEN) + - evt->adv_data_len; - evt->rssi = hdr->rxinfo.rssi; - ble_ll_hci_event_send((uint8_t *)evt); evt = next_evt; - } while (evt); + } while ((offset < datalen) && evt); BLE_LL_ASSERT(offset <= datalen); + + /* Store any event left for later use */ + aux_data->evt = evt; + + return rc; } #endif /** @@ -2380,6 +2436,7 @@ ble_ll_scan_rx_pkt_in(uint8_t ptype, struct os_mbuf *om, struct ble_mbuf_hdr *hd int ext_adv_mode = -1; #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_EXT_ADV) struct ble_ll_aux_data *aux_data; + int rc; #endif /* Set scan response check flag */ @@ -2484,11 +2541,17 @@ ble_ll_scan_rx_pkt_in(uint8_t ptype, struct os_mbuf *om, struct ble_mbuf_hdr *hd aux_data = hdr->rxinfo.user_data; + if (BLE_LL_CHECK_AUX_FLAG(aux_data, BLE_LL_AUX_IGNORE_BIT)) { + goto scan_continue; + } + /* Let's see if that packet contains aux ptr*/ if (BLE_MBUF_HDR_WAIT_AUX(hdr)) { if (ble_ll_sched_aux_scan(hdr, scansm, hdr->rxinfo.user_data)) { /* We are here when could not schedule the aux ptr */ hdr->rxinfo.flags &= ~BLE_MBUF_HDR_F_AUX_PTR_WAIT; + /* Mark that chain is trimmed */ + aux_data->flags |= BLE_LL_AUX_INCOMPLETE_ERR_BIT; } if (!BLE_LL_CHECK_AUX_FLAG(aux_data, BLE_LL_AUX_CHAIN_BIT)) { @@ -2503,7 +2566,17 @@ ble_ll_scan_rx_pkt_in(uint8_t ptype, struct os_mbuf *om, struct ble_mbuf_hdr *hd STATS_INC(ble_ll_stats, aux_chain_cnt); } - ble_ll_hci_send_ext_adv_report(ptype, om, hdr); + rc = ble_ll_hci_send_ext_adv_report(ptype, om, hdr); + if (rc < 0) { + /* + * Data were trimmed so no need to scan this chain anymore. Also + * make sure we do not send any more events for this chain, just in + * case we managed to scan something before events were processed. + */ + ble_ll_sched_rmv_elem(&aux_data->sch); + hdr->rxinfo.flags &= ~BLE_MBUF_HDR_F_AUX_PTR_WAIT; + aux_data->flags |= BLE_LL_AUX_IGNORE_BIT; + } ble_ll_scan_switch_phy(scansm); if (BLE_MBUF_HDR_WAIT_AUX(hdr)) { @@ -2922,6 +2995,10 @@ ble_ll_scan_set_enable(uint8_t *cmd, uint8_t ext) if (scansm->cur_phy == PHY_NOT_CONFIGURED) { scansm->cur_phy = i; + /* Take own_addr_type from the first configured PHY. + * Note: All configured PHYs shall have the same own_addr_type + */ + scansm->own_addr_type = scanphy->own_addr_type; } else { scansm->next_phy = i; }