diff --git a/subsys/bluetooth/host/conn.c b/subsys/bluetooth/host/conn.c index 0076d74af8f..c5034183ca5 100644 --- a/subsys/bluetooth/host/conn.c +++ b/subsys/bluetooth/host/conn.c @@ -1204,6 +1204,21 @@ struct bt_conn *bt_conn_lookup_handle(uint16_t handle) return NULL; } +int bt_conn_addr_le_cmp(const struct bt_conn *conn, const bt_addr_le_t *peer) +{ + /* Check against conn dst address as it may be the identity address */ + if (!bt_addr_le_cmp(peer, &conn->le.dst)) { + return 0; + } + + /* Check against initial connection address */ + if (conn->role == BT_HCI_ROLE_MASTER) { + return bt_addr_le_cmp(peer, &conn->le.resp_addr); + } + + return bt_addr_le_cmp(peer, &conn->le.init_addr); +} + struct bt_conn *bt_conn_lookup_addr_le(const bt_addr_le_t *peer) { int i; @@ -1217,7 +1232,7 @@ struct bt_conn *bt_conn_lookup_addr_le(const bt_addr_le_t *peer) continue; } - if (!bt_addr_le_cmp(peer, &conns[i].le.dst)) { + if (!bt_conn_addr_le_cmp(&conns[i], peer)) { return bt_conn_ref(&conns[i]); } } @@ -1239,7 +1254,7 @@ struct bt_conn *bt_conn_lookup_state_le(const bt_addr_le_t *peer, continue; } - if (peer && bt_addr_le_cmp(peer, &conns[i].le.dst)) { + if (peer && bt_conn_addr_le_cmp(&conns[i], peer)) { continue; } diff --git a/subsys/bluetooth/host/conn_internal.h b/subsys/bluetooth/host/conn_internal.h index 3951b08aa02..d281b35e074 100644 --- a/subsys/bluetooth/host/conn_internal.h +++ b/subsys/bluetooth/host/conn_internal.h @@ -150,6 +150,9 @@ void bt_conn_disconnect_all(void); /* Look up an existing connection */ struct bt_conn *bt_conn_lookup_handle(uint16_t handle); +/* Compare an address with bt_conn destination address */ +int bt_conn_addr_le_cmp(const struct bt_conn *conn, const bt_addr_le_t *peer); + /* Look up a connection state. For BT_ADDR_LE_ANY, returns the first connection * with the specific state */