From 010c5c2f203d77304dd4b993f41e46675f8aabd5 Mon Sep 17 00:00:00 2001 From: Wolfgang Puffitsch Date: Tue, 23 Feb 2021 15:41:44 +0100 Subject: [PATCH] Bluetooth: controller: Ignore connections from same peer Ignore connection indications from peers that are already connected. This is to bring the behavior of the controller in accordance with [5.2, Vol 6, Part B, 4.5 Connection state]: "If an advertiser receives a connection request from an initiator it is already connected to, it shall ignore that request." Signed-off-by: Wolfgang Puffitsch --- subsys/bluetooth/controller/Kconfig | 4 ++ .../bluetooth/controller/Kconfig.ll_sw_split | 12 ++++ subsys/bluetooth/controller/ll_sw/ull_adv.c | 35 ++++++++-- .../controller/ll_sw/ull_adv_types.h | 4 ++ subsys/bluetooth/controller/ll_sw/ull_conn.c | 22 +++++++ .../controller/ll_sw/ull_conn_internal.h | 2 + .../controller/ll_sw/ull_conn_types.h | 7 ++ .../bluetooth/controller/ll_sw/ull_filter.c | 34 ++++++++++ subsys/bluetooth/controller/ll_sw/ull_slave.c | 66 +++++++++++++------ 9 files changed, 160 insertions(+), 26 deletions(-) diff --git a/subsys/bluetooth/controller/Kconfig b/subsys/bluetooth/controller/Kconfig index 7073147ae15..c9d717f2593 100644 --- a/subsys/bluetooth/controller/Kconfig +++ b/subsys/bluetooth/controller/Kconfig @@ -372,6 +372,10 @@ config BT_CTLR_CONN_RSSI help Enable connection RSSI measurement. +config BT_CTLR_CHECK_SAME_PEER_CONN + bool + default BT_MAX_CONN > 1 && !BT_CTLR_ALLOW_SAME_PEER_CONN + endif # BT_CONN config BT_CTLR_FILTER diff --git a/subsys/bluetooth/controller/Kconfig.ll_sw_split b/subsys/bluetooth/controller/Kconfig.ll_sw_split index 8cef936fb3e..547652e8ec0 100644 --- a/subsys/bluetooth/controller/Kconfig.ll_sw_split +++ b/subsys/bluetooth/controller/Kconfig.ll_sw_split @@ -475,6 +475,18 @@ config BT_CTLR_CONN_RSSI_EVENT help Generate events for connection RSSI measurement. +config BT_CTLR_ALLOW_SAME_PEER_CONN + bool "Allow connection requests from same peer" + depends on BT_MAX_CONN > 1 + help + Allow connection requests from the same peer. While the + Bluetooth specification does not allow multiple connections + with the same peer, allowing such connections is useful + while debugging multiple connections. + + WARNING: This option enables behavior that violates the Bluetooth + specification. + endif # BT_CONN config BT_CTLR_ADV_INDICATION diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv.c b/subsys/bluetooth/controller/ll_sw/ull_adv.c index 05ba3a38937..fe07ba3e62b 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_adv.c +++ b/subsys/bluetooth/controller/ll_sw/ull_adv.c @@ -893,6 +893,13 @@ uint8_t ll_adv_enable(uint8_t enable) conn->supervision_expire = 0; conn->procedure_expire = 0; +#if defined(CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN) + conn->own_addr_type = BT_ADDR_LE_NONE->type; + memcpy(conn->own_addr, BT_ADDR_LE_NONE->a.val, sizeof(conn->own_addr)); + conn->peer_addr_type = BT_ADDR_LE_NONE->type; + memcpy(conn->peer_addr, BT_ADDR_LE_NONE->a.val, sizeof(conn->peer_addr)); +#endif /* CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN */ + conn->common.fex_valid = 0; conn->slave.latency_cancel = 0; @@ -2089,20 +2096,34 @@ static inline uint8_t *adv_pdu_adva_get(struct pdu_adv *pdu) static const uint8_t *adva_update(struct ll_adv_set *adv, struct pdu_adv *pdu) { #if defined(CONFIG_BT_CTLR_PRIVACY) - const uint8_t *tx_addr = ull_filter_adva_get(adv); + const uint8_t *rpa = ull_filter_adva_get(adv); #else - const uint8_t *tx_addr = NULL; + const uint8_t *rpa = NULL; #endif + const uint8_t *own_addr; + const uint8_t *tx_addr; uint8_t *adv_addr; - if (tx_addr) { - pdu->tx_addr = 1; + if (!rpa || IS_ENABLED(CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN)) { + if (0) { #if defined(CONFIG_BT_CTLR_ADV_EXT) - } else if (ll_adv_cmds_is_ext() && pdu->tx_addr) { - tx_addr = ll_adv_aux_random_addr_get(adv, NULL); + } else if (ll_adv_cmds_is_ext() && pdu->tx_addr) { + own_addr = ll_adv_aux_random_addr_get(adv, NULL); #endif + } else { + own_addr = ll_addr_get(pdu->tx_addr, NULL); + } + } + +#if defined(CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN) + memcpy(adv->own_addr, own_addr, BDADDR_SIZE); +#endif /* CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN */ + + if (rpa) { + pdu->tx_addr = 1; + tx_addr = rpa; } else { - tx_addr = ll_addr_get(pdu->tx_addr, NULL); + tx_addr = own_addr; } adv_addr = adv_pdu_adva_get(pdu); diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_types.h b/subsys/bluetooth/controller/ll_sw/ull_adv_types.h index e39978ac971..fbe9f40c776 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_adv_types.h +++ b/subsys/bluetooth/controller/ll_sw/ull_adv_types.h @@ -41,6 +41,10 @@ struct ll_adv_set { uint8_t peer_addr[BDADDR_SIZE]; #endif /* CONFIG_BT_CTLR_PRIVACY */ +#if defined(CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN) + uint8_t own_addr[BDADDR_SIZE]; +#endif /* CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN */ + #if defined(CONFIG_BT_CTLR_DF_ADV_CTE_TX) struct lll_df_adv_cfg *df_cfg; #endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */ diff --git a/subsys/bluetooth/controller/ll_sw/ull_conn.c b/subsys/bluetooth/controller/ll_sw/ull_conn.c index 28af2c53d48..0772ab3ee1e 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_conn.c +++ b/subsys/bluetooth/controller/ll_sw/ull_conn.c @@ -770,6 +770,28 @@ uint8_t ull_conn_default_phy_rx_get(void) } #endif /* CONFIG_BT_CTLR_PHY */ +#if defined(CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN) +bool ull_conn_peer_connected(uint8_t own_addr_type, uint8_t *own_addr, + uint8_t peer_addr_type, uint8_t *peer_addr) +{ + uint16_t handle; + + for (handle = 0U; handle < CONFIG_BT_MAX_CONN; handle++) { + struct ll_conn *conn = ll_connected_get(handle); + + if (conn && + conn->peer_addr_type == peer_addr_type && + !memcmp(conn->peer_addr, peer_addr, BDADDR_SIZE) && + conn->own_addr_type == own_addr_type && + !memcmp(conn->own_addr, own_addr, BDADDR_SIZE)) { + return true; + } + } + + return false; +} +#endif /* CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN */ + void ull_conn_setup(memq_link_t *link, struct node_rx_hdr *rx) { struct node_rx_ftr *ftr; diff --git a/subsys/bluetooth/controller/ll_sw/ull_conn_internal.h b/subsys/bluetooth/controller/ll_sw/ull_conn_internal.h index d4a69019865..83199c2bfde 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_conn_internal.h +++ b/subsys/bluetooth/controller/ll_sw/ull_conn_internal.h @@ -17,6 +17,8 @@ uint16_t ull_conn_default_tx_octets_get(void); uint16_t ull_conn_default_tx_time_get(void); uint8_t ull_conn_default_phy_tx_get(void); uint8_t ull_conn_default_phy_rx_get(void); +bool ull_conn_peer_connected(uint8_t own_addr_type, uint8_t *own_addr, + uint8_t peer_addr_type, uint8_t *peer_addr); void ull_conn_setup(memq_link_t *link, struct node_rx_hdr *rx); int ull_conn_rx(memq_link_t *link, struct node_rx_pdu **rx); int ull_conn_llcp(struct ll_conn *conn, uint32_t ticks_at_expire, uint16_t lazy); diff --git a/subsys/bluetooth/controller/ll_sw/ull_conn_types.h b/subsys/bluetooth/controller/ll_sw/ull_conn_types.h index 9231a3650cd..a1ac1f088d7 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_conn_types.h +++ b/subsys/bluetooth/controller/ll_sw/ull_conn_types.h @@ -56,6 +56,13 @@ struct ll_conn { #endif /* CONFIG_BT_CTLR_PHY */ #endif /* CONFIG_BT_CTLR_DATA_LENGTH */ +#if defined(CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN) + uint8_t own_addr_type:1; + uint8_t peer_addr_type:2; + uint8_t own_addr[BDADDR_SIZE]; + uint8_t peer_addr[BDADDR_SIZE]; +#endif /* CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN */ + union { struct { uint8_t fex_valid:1; diff --git a/subsys/bluetooth/controller/ll_sw/ull_filter.c b/subsys/bluetooth/controller/ll_sw/ull_filter.c index 127db1b4816..905abef4463 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_filter.c +++ b/subsys/bluetooth/controller/ll_sw/ull_filter.c @@ -31,11 +31,13 @@ #include "ull_adv_types.h" #include "ull_scan_types.h" +#include "ull_conn_types.h" #include "ull_filter.h" #include "ull_internal.h" #include "ull_adv_internal.h" #include "ull_scan_internal.h" +#include "ull_conn_internal.h" #include "ll.h" @@ -139,6 +141,11 @@ static void filter_insert(struct lll_filter *filter, int index, uint8_t addr_typ uint8_t *bdaddr); static void filter_clear(struct lll_filter *filter); +#if defined(CONFIG_BT_CTLR_PRIVACY) && \ + defined(CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN) +static void conn_rpa_update(uint8_t rl_idx); +#endif /* CONFIG_BT_CTLR_PRIVACY && CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN */ + #if defined(CONFIG_BT_CTLR_SW_DEFERRED_PRIVACY) static void prpa_cache_clear(void); static uint8_t prpa_cache_find(bt_addr_t *prpa_cache_addr); @@ -374,6 +381,9 @@ void ll_rl_crpa_set(uint8_t id_addr_type, uint8_t *id_addr, uint8_t rl_idx, uint if (rl_idx < ARRAY_SIZE(rl) && rl[rl_idx].taken) { memcpy(rl[rl_idx].curr_rpa.val, crpa, sizeof(bt_addr_t)); +#if defined(CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN) + conn_rpa_update(rl_idx); +#endif /* CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN) */ } } } @@ -1083,6 +1093,27 @@ static void filter_clear(struct lll_filter *filter) filter->addr_type_bitmask = 0; } +#if defined(CONFIG_BT_CTLR_PRIVACY) && \ + defined(CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN) +static void conn_rpa_update(uint8_t rl_idx) +{ + uint16_t handle; + + for (handle = 0U; handle < CONFIG_BT_MAX_CONN; handle++) { + struct ll_conn *conn = ll_connected_get(handle); + + /* The RPA of the connection matches the RPA that was just resolved */ + if (conn && + conn->peer_addr_type < 2U && + !memcmp(conn->peer_addr, rl[rl_idx].curr_rpa.val, BDADDR_SIZE)) { + memcpy(conn->peer_addr, rl[rl_idx].id_addr.val, BDADDR_SIZE); + conn->peer_addr_type += 2U; + break; + } + } +} +#endif /* CONFIG_BT_CTLR_PRIVACY && CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN */ + #if defined(CONFIG_BT_CTLR_SW_DEFERRED_PRIVACY) static void target_resolve(struct k_work *work) { @@ -1171,6 +1202,9 @@ static void prpa_cache_resolve(struct k_work *work) */ memcpy(rl[j].curr_rpa.val, search_rpa->val, sizeof(bt_addr_t)); +#if defined(CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN) + conn_rpa_update(j); +#endif /* CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN */ } } else { diff --git a/subsys/bluetooth/controller/ll_sw/ull_slave.c b/subsys/bluetooth/controller/ll_sw/ull_slave.c index f995c09a5f1..c66dab161b7 100644 --- a/subsys/bluetooth/controller/ll_sw/ull_slave.c +++ b/subsys/bluetooth/controller/ll_sw/ull_slave.c @@ -58,6 +58,7 @@ void ull_slave_setup(memq_link_t *link, struct node_rx_hdr *rx, { uint32_t conn_offset_us, conn_interval_us; uint8_t ticker_id_adv, ticker_id_conn; + uint8_t peer_id_addr[BDADDR_SIZE]; uint8_t peer_addr[BDADDR_SIZE]; uint32_t ticks_slot_overhead; uint32_t ticks_slot_offset; @@ -78,6 +79,46 @@ void ull_slave_setup(memq_link_t *link, struct node_rx_hdr *rx, /* Populate the slave context */ pdu_adv = (void *)((struct node_rx_pdu *)rx)->pdu; + + peer_addr_type = pdu_adv->tx_addr; + memcpy(peer_addr, pdu_adv->connect_ind.init_addr, BDADDR_SIZE); + +#if defined(CONFIG_BT_CTLR_PRIVACY) + uint8_t rl_idx = ftr->rl_idx; + + if (rl_idx != FILTER_IDX_NONE) { + /* Get identity address */ + ll_rl_id_addr_get(rl_idx, &peer_addr_type, peer_id_addr); + /* Mark it as identity address from RPA (0x02, 0x03) */ + peer_addr_type += 2; + } else { +#else /* CONFIG_BT_CTLR_PRIVACY */ + if (1) { +#endif /* CONFIG_BT_CTLR_PRIVACY */ + memcpy(peer_id_addr, peer_addr, BDADDR_SIZE); + } + +#if defined(CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN) + uint8_t own_addr_type = pdu_adv->rx_addr; + uint8_t *own_addr = adv->own_addr; + + /* Do not connect twice to the same peer */ + if (ull_conn_peer_connected(own_addr_type, own_addr, + peer_addr_type, peer_id_addr)) { + rx->type = NODE_RX_TYPE_RELEASE; + + ll_rx_put(link, rx); + ll_rx_sched(); + return; + } + + /* Remember peer and own identity */ + conn->peer_addr_type = peer_addr_type; + memcpy(conn->peer_addr, peer_id_addr, sizeof(conn->peer_addr)); + conn->own_addr_type = own_addr_type; + memcpy(conn->own_addr, own_addr, sizeof(conn->own_addr)); +#endif /* CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN */ + memcpy(&lll->crc_init[0], &pdu_adv->connect_ind.crc_init[0], 3); memcpy(&lll->access_addr[0], &pdu_adv->connect_ind.access_addr[0], 4); memcpy(&lll->data_chan_map[0], &pdu_adv->connect_ind.chan_map[0], @@ -176,9 +217,6 @@ void ull_slave_setup(memq_link_t *link, struct node_rx_hdr *rx, sizeof(conn->slave.force)); #endif /* CONFIG_BT_CTLR_CONN_RANDOM_FORCE */ - peer_addr_type = pdu_adv->tx_addr; - memcpy(peer_addr, pdu_adv->connect_ind.init_addr, BDADDR_SIZE); - if (0) { #if defined(CONFIG_BT_CTLR_ADV_EXT) } else if (adv->lll.aux) { @@ -193,8 +231,6 @@ void ull_slave_setup(memq_link_t *link, struct node_rx_hdr *rx, cc->role = 1U; #if defined(CONFIG_BT_CTLR_PRIVACY) - uint8_t rl_idx = ftr->rl_idx; - if (ull_filter_lll_lrpa_used(adv->lll.rl_idx)) { memcpy(&cc->local_rpa[0], &pdu_adv->connect_ind.adv_addr[0], BDADDR_SIZE); @@ -203,23 +239,15 @@ void ull_slave_setup(memq_link_t *link, struct node_rx_hdr *rx, } if (rl_idx != FILTER_IDX_NONE) { - /* TODO: store rl_idx instead if safe */ - /* Store identity address */ - ll_rl_id_addr_get(rl_idx, &cc->peer_addr_type, - &cc->peer_addr[0]); - /* Mark it as identity address from RPA (0x02, 0x03) */ - cc->peer_addr_type += 2; - /* Store peer RPA */ - memcpy(&cc->peer_rpa[0], &peer_addr[0], BDADDR_SIZE); + memcpy(cc->peer_rpa, peer_addr, BDADDR_SIZE); } else { - memset(&cc->peer_rpa[0], 0x0, BDADDR_SIZE); -#else - if (1) { -#endif /* CONFIG_BT_CTLR_PRIVACY */ - cc->peer_addr_type = peer_addr_type; - memcpy(cc->peer_addr, peer_addr, BDADDR_SIZE); + memset(cc->peer_rpa, 0x0, BDADDR_SIZE); } +#endif /* CONFIG_BT_CTLR_PRIVACY */ + + cc->peer_addr_type = peer_addr_type; + memcpy(cc->peer_addr, peer_id_addr, BDADDR_SIZE); cc->interval = lll->interval; cc->latency = lll->latency;