From acc80719988d8704bc156f77452c8bf100ba1382 Mon Sep 17 00:00:00 2001 From: Szymon Janc Date: Fri, 9 Jun 2017 12:51:26 +0200 Subject: [PATCH] nimble/controller: Reply with AUX_CONN_RSP for connection request This allows to create connection while advertising using Extended Advertising PDUs. CSA2 is mandatory for this type of connection so ChSel bit is RFU and is not used. X-Original-Commit: 3f280f04d8e45985796ba2f8868b2f43b2e3627d --- nimble/controller/src/ble_ll_adv.c | 114 +++++++++++++++++++++-- nimble/controller/src/ble_ll_conn.c | 7 +- nimble/controller/src/ble_ll_conn_priv.h | 2 +- 3 files changed, 113 insertions(+), 10 deletions(-) diff --git a/nimble/controller/src/ble_ll_adv.c b/nimble/controller/src/ble_ll_adv.c index 28c7c1d57..4e78ca320 100644 --- a/nimble/controller/src/ble_ll_adv.c +++ b/nimble/controller/src/ble_ll_adv.c @@ -116,6 +116,7 @@ struct ble_ll_adv_sm uint8_t sec_phy; /* TODO */ uint8_t sid; /* TODO */ uint8_t scan_req_notif; /* TODO */ + uint8_t conn_rsp_tx; #endif }; @@ -637,6 +638,66 @@ ble_ll_adv_scan_rsp_pdu_make(struct ble_ll_adv_sm *advsm) return m; } +/** + * Create a AUX connect response PDU + * + * @param advsm + */ +static struct os_mbuf * +ble_ll_adv_aux_conn_rsp_pdu_make(struct ble_ll_adv_sm *advsm, uint8_t *peer, + uint8_t rxadd) +{ + uint8_t *dptr; + uint8_t pdulen; + uint8_t ext_hdr_len; + uint8_t ext_hdr_flags; + uint8_t hdr; + struct os_mbuf *m; + + /* ext hdr len */ + pdulen = 1; + + /* flags,AdvA and TargetA */ + ext_hdr_len = 1 + BLE_LL_EXT_ADV_ADVA_SIZE + BLE_LL_EXT_ADV_TARGETA_SIZE; + ext_hdr_flags = (1 << BLE_LL_EXT_ADV_ADVA_BIT); + ext_hdr_flags |= (1 << BLE_LL_EXT_ADV_TARGETA_BIT); + + pdulen += ext_hdr_len; + + /* Obtain scan response buffer */ + m = os_msys_get_pkthdr(pdulen, sizeof(struct ble_mbuf_hdr)); + if (!m) { + return NULL; + } + + /* Set BLE transmit header */ + hdr = BLE_ADV_PDU_TYPE_AUX_CONNECT_RSP | rxadd; + if (advsm->adv_txadd) { + hdr |= BLE_ADV_PDU_HDR_TXADD_MASK; + } + + ble_ll_mbuf_init(m, pdulen, hdr); + + /* Construct connect response */ + dptr = m->om_data; + + /* ext hdr len and adv mode (00b) */ + dptr[0] = ext_hdr_len; + dptr += 1; + + /* ext hdr flags */ + dptr[0] = ext_hdr_flags; + dptr += 1; + + memcpy(dptr, advsm->adva, BLE_LL_EXT_ADV_ADVA_SIZE); + dptr += BLE_LL_EXT_ADV_ADVA_SIZE; + + memcpy(dptr, peer, BLE_LL_EXT_ADV_TARGETA_SIZE); + dptr += BLE_LL_EXT_ADV_ADVA_SIZE; + + return m; +} + /** * Called to indicate the advertising event is over. * @@ -2052,7 +2113,7 @@ ble_ll_adv_rx_req(uint8_t pdu_type, struct os_mbuf *rxpdu) uint8_t *peer; struct ble_mbuf_hdr *ble_hdr; struct ble_ll_adv_sm *advsm; - struct os_mbuf *scan_rsp; + struct os_mbuf *rsp; /* See if adva in the request (scan or connect) matches what we sent */ advsm = g_ble_ll_cur_adv_sm; @@ -2112,22 +2173,51 @@ ble_ll_adv_rx_req(uint8_t pdu_type, struct os_mbuf *rxpdu) /* Setup to transmit the scan response if appropriate */ rc = -1; + if (pdu_type == BLE_ADV_PDU_TYPE_SCAN_REQ) { if (advsm->scan_req_notif) { ble_ll_hci_ev_send_scan_req_recv(advsm->adv_instance, peer, peer_addr_type); } - scan_rsp = ble_ll_adv_scan_rsp_pdu_make(advsm); - if (scan_rsp) { + rsp = ble_ll_adv_scan_rsp_pdu_make(advsm); + if (rsp) { /* XXX TODO: assume we do not need to change phy mode */ ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm); - rc = ble_phy_tx(scan_rsp, BLE_PHY_TRANSITION_NONE); + rc = ble_phy_tx(rsp, BLE_PHY_TRANSITION_NONE); if (!rc) { ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_SCAN_RSP_TXD; STATS_INC(ble_ll_stats, scan_rsp_txg); } - os_mbuf_free_chain(scan_rsp); + os_mbuf_free_chain(rsp); + } + } else if (pdu_type == BLE_ADV_PDU_TYPE_AUX_CONNECT_REQ) { + /* + * Only accept connect requests from the desired address if we + * are doing directed advertising + */ + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED) { + if (memcmp(advsm->peer_addr, peer, BLE_DEV_ADDR_LEN)) { + return -1; + } + } + + if (advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY) { + return -1; + } + + /* use remote address used over the air */ + rsp = ble_ll_adv_aux_conn_rsp_pdu_make(advsm, + rxbuf + BLE_LL_PDU_HDR_LEN, + rxbuf[0] & BLE_ADV_PDU_HDR_TXADD_MASK); + if (rsp) { + ble_phy_set_txend_cb(ble_ll_adv_tx_done, advsm); + rc = ble_phy_tx(rsp, BLE_PHY_TRANSITION_NONE); + if (!rc) { + advsm->conn_rsp_tx = 1; + //STATS_INC(ble_ll_stats, scan_rsp_txg); TODO + } + os_mbuf_free_chain(rsp); } } @@ -2219,9 +2309,13 @@ ble_ll_adv_conn_req_rxd(uint8_t *rxbuf, struct ble_mbuf_hdr *hdr, #endif /* Try to start slave connection. If successful, stop advertising */ - valid = ble_ll_conn_slave_start(rxbuf, addr_type, hdr); + valid = ble_ll_conn_slave_start(rxbuf, addr_type, hdr, + !(advsm->props & BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY)); if (valid) { - ble_ll_adv_sm_stop(advsm); + /* stop advertising only if not transmitting connection response */ + if (!advsm->conn_rsp_tx) { + ble_ll_adv_sm_stop(advsm); + } } } @@ -2448,6 +2542,12 @@ ble_ll_adv_done(struct ble_ll_adv_sm *advsm) assert(advsm->adv_enabled); + /* stop advertising this was due to transmitting connection response */ + if (advsm->conn_rsp_tx) { + ble_ll_adv_sm_stop(advsm); + return; + } + if (advsm->adv_secondary) { ble_ll_adv_secondary_done(advsm); return; diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c index 67a8f209a..438484495 100644 --- a/nimble/controller/src/ble_ll_conn.c +++ b/nimble/controller/src/ble_ll_conn.c @@ -3714,7 +3714,8 @@ ble_ll_conn_set_global_chanmap(uint8_t num_used_chans, uint8_t *chanmap) * @return 0: connection not started; 1 connecton started */ int -ble_ll_conn_slave_start(uint8_t *rxbuf, uint8_t pat, struct ble_mbuf_hdr *rxhdr) +ble_ll_conn_slave_start(uint8_t *rxbuf, uint8_t pat, struct ble_mbuf_hdr *rxhdr, + bool force_csa2) { int rc; uint32_t temp; @@ -3804,7 +3805,9 @@ ble_ll_conn_slave_start(uint8_t *rxbuf, uint8_t pat, struct ble_mbuf_hdr *rxhdr) /* Start the connection state machine */ connsm->conn_role = BLE_LL_CONN_ROLE_SLAVE; ble_ll_conn_sm_new(connsm); - ble_ll_conn_set_csa(connsm, rxbuf[0] & BLE_ADV_PDU_HDR_CHSEL_MASK); + + ble_ll_conn_set_csa(connsm, + force_csa2 || (rxbuf[0] & BLE_ADV_PDU_HDR_CHSEL_MASK)); /* Set initial schedule callback */ connsm->conn_sch.sched_cb = ble_ll_conn_event_start_cb; diff --git a/nimble/controller/src/ble_ll_conn_priv.h b/nimble/controller/src/ble_ll_conn_priv.h index 1e169fc67..60f7aa273 100644 --- a/nimble/controller/src/ble_ll_conn_priv.h +++ b/nimble/controller/src/ble_ll_conn_priv.h @@ -112,7 +112,7 @@ void ble_ll_conn_datalen_update(struct ble_ll_conn_sm *connsm, /* Advertising interface */ int ble_ll_conn_slave_start(uint8_t *rxbuf, uint8_t pat, - struct ble_mbuf_hdr *rxhdr); + struct ble_mbuf_hdr *rxhdr, bool force_csa2); /* Link Layer interface */ void ble_ll_conn_module_init(void);