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 <wopu@demant.com>

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

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

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) {
+	if (!rpa || IS_ENABLED(CONFIG_BT_CTLR_CHECK_SAME_PEER_CONN)) {
-		pdu->tx_addr = 1;
+		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);
+			own_addr = ll_adv_aux_random_addr_get(adv, NULL);
 #endif
 		} else {
-		tx_addr = ll_addr_get(pdu->tx_addr, NULL);
+			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 = own_addr;
 	}
 
 	adv_addr = adv_pdu_adva_get(pdu);

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 */

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;

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);

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;

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 {

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);
+		memset(cc->peer_rpa, 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);
 	}
+#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;