diff --git a/device/src/bt_conn.c b/device/src/bt_conn.c index 0c907ba0..6caf1ad7 100644 --- a/device/src/bt_conn.c +++ b/device/src/bt_conn.c @@ -1,4 +1,6 @@ +#include "keyboard/oled/framebuffer.h" #include +#include #include #include #ifdef CONFIG_BT_SCAN @@ -140,6 +142,21 @@ static void configureLatency(struct bt_conn *conn) { } } +void BtConn_UpdateHostConnectionPeerAllocations() { + //for each peer + for (uint8_t peerId = PeerIdFirstHost; peerId <= PeerIdLastHost; peerId++) { + struct bt_conn* conn = Peers[peerId].conn; + if (conn) { + connection_id_t currentId = Peers[peerId].connectionId; + connection_id_t newId = Connections_GetConnectionIdByHostAddr(bt_conn_get_dst(conn)); + printk("Reallocating peer %s from connection %d -> %d\n", Peers[peerId].name, currentId, newId); + if (newId != ConnectionId_Invalid && newId != currentId) { + Connections_MoveConnection(peerId, currentId, newId); + } + } + } +} + static uint8_t allocateHostPeer(uint8_t connectionType) { switch (connectionType) { case ConnectionType_NusLeft: diff --git a/device/src/connections.c b/device/src/connections.c index 920e57b4..2447b174 100644 --- a/device/src/connections.c +++ b/device/src/connections.c @@ -4,10 +4,12 @@ #include "host_connection.h" #include "device_state.h" #include "host_connection.h" +#include "keyboard/oled/widgets/widget_store.h" #include "messenger.h" #include "state_sync.h" #include #include "connections.h" +#include "stubs.h" connection_t Connections[ConnectionId_Count] = { [ConnectionId_UsbHidRight] = { .isAlias = true }, @@ -191,13 +193,17 @@ connection_target_t Connections_Target(connection_id_t connectionId) { return ConnectionTarget_None; } -connection_id_t Connections_GetConnectionIdByBtAddr(const bt_addr_le_t *addr) { +static connection_id_t getConnIdByPeer(const bt_addr_le_t *addr) { for (uint8_t peerId = 0; peerId < PeerCount; peerId++) { if (BtAddrEq(addr, &Peers[peerId].addr)) { return Peers[peerId].connectionId; } } + return ConnectionId_Invalid; +} + +static connection_id_t getConnIdByAddr(const bt_addr_le_t *addr) { for (uint8_t connectionId = ConnectionId_HostConnectionFirst; connectionId <= ConnectionId_HostConnectionLast; connectionId++) { host_connection_t *hostConnection = HostConnection(connectionId); switch (hostConnection->type) { @@ -220,6 +226,26 @@ connection_id_t Connections_GetConnectionIdByBtAddr(const bt_addr_le_t *addr) { return ConnectionId_Invalid; } +connection_id_t Connections_GetConnectionIdByBtAddr(const bt_addr_le_t *addr) { + connection_id_t res = ConnectionId_Invalid; + + res = getConnIdByPeer(addr); + if (res != ConnectionId_Invalid) { + return res; + } + + res = getConnIdByAddr(addr); + if (res != ConnectionId_Invalid) { + return res; + } + + return ConnectionId_Invalid; +} + +connection_id_t Connections_GetConnectionIdByHostAddr(const bt_addr_le_t *addr) { + return getConnIdByAddr(addr); +} + connection_id_t Connections_GetNewBtHidConnectionId() { for (uint8_t connectionId = ConnectionId_HostConnectionFirst; connectionId <= ConnectionId_HostConnectionLast; connectionId++) { host_connection_t *hostConnection = HostConnection(connectionId); @@ -234,6 +260,26 @@ connection_id_t Connections_GetNewBtHidConnectionId() { return ConnectionId_Invalid; } +void Connections_MoveConnection(uint8_t peerId, connection_id_t oldConnectionId, connection_id_t newConnectionId) { + bool isActiveConnection = oldConnectionId == ActiveHostConnectionId; + bool isSelectedConnection = oldConnectionId == SelectedHostConnectionId; + + Connections[newConnectionId].peerId = Connections[oldConnectionId].peerId; + Connections[newConnectionId].state = Connections[oldConnectionId].state; + + Peers[Connections[oldConnectionId].peerId].connectionId = newConnectionId; + + Connections[oldConnectionId].peerId = PeerIdUnknown; + Connections[oldConnectionId].state = ConnectionState_Disconnected; + + ActiveHostConnectionId = isActiveConnection ? newConnectionId : ActiveHostConnectionId; + SelectedHostConnectionId = isSelectedConnection ? newConnectionId : ActiveHostConnectionId; + + if (isActiveConnection) { + WIDGET_REFRESH(&TargetWidget); + } +} + bool Connections_IsHostConnection(connection_id_t connectionId) { return ConnectionId_HostConnectionFirst <= connectionId && connectionId <= ConnectionId_HostConnectionLast; } diff --git a/device/src/connections.h b/device/src/connections.h index 344f4ea3..15d13227 100644 --- a/device/src/connections.h +++ b/device/src/connections.h @@ -84,6 +84,9 @@ connection_id_t Connections_GetNewBtHidConnectionId(); connection_id_t Connections_GetConnectionIdByBtAddr(const bt_addr_le_t *addr); + connection_id_t Connections_GetConnectionIdByHostAddr(const bt_addr_le_t *addr); + + void Connections_MoveConnection(uint8_t peerId, connection_id_t oldConnectionId, connection_id_t newConnectionId); bool Connections_IsHostConnection(connection_id_t connectionId); bool Connections_IsReady(connection_id_t connectionId); diff --git a/right/src/config_parser/parse_config.c b/right/src/config_parser/parse_config.c index 54d3c1ae..27d5adff 100644 --- a/right/src/config_parser/parse_config.c +++ b/right/src/config_parser/parse_config.c @@ -335,6 +335,7 @@ parser_error_t parseConfig(config_buffer_t *buffer) LedManager_RecalculateLedBrightness(); LedManager_UpdateSleepModes(); BtPair_ClearUnknownBonds(); + BtConn_UpdateHostConnectionPeerAllocations(); // Update counts diff --git a/right/src/stubs.c b/right/src/stubs.c index 069162ac..2e13701d 100644 --- a/right/src/stubs.c +++ b/right/src/stubs.c @@ -33,3 +33,4 @@ ATTRS uint8_t BtAdvertise_Type() { return 0; }; ATTRS int BtScan_Start(void) { return 0; }; ATTRS void BtManager_StartScanningAndAdvertising() {}; + ATTRS void BtConn_UpdateHostConnectionPeerAllocations() {}; diff --git a/right/src/stubs.h b/right/src/stubs.h index 645ab1ba..b0571e73 100644 --- a/right/src/stubs.h +++ b/right/src/stubs.h @@ -47,6 +47,7 @@ extern uint8_t BtAdvertise_Type(); extern int BtScan_Start(void); extern void BtManager_StartScanningAndAdvertising(); + extern void BtConn_UpdateHostConnectionPeerAllocations(); #if DEVICE_HAS_OLED #define WIDGET_REFRESH(W) Widget_Refresh(W)