From 5d9ae8fffa869f36e3e0911bed92d41ed8640826 Mon Sep 17 00:00:00 2001 From: Xudong Zheng <7pkvm5aw@slicealias.com> Date: Fri, 17 Jun 2022 17:44:31 -0400 Subject: [PATCH] feat(split): allow central to connect to multiple peripherals --- app/include/zmk/ble.h | 6 +- app/src/ble.c | 54 ++++++++---- app/src/split/bluetooth/Kconfig | 4 + app/src/split/bluetooth/central.c | 138 ++++++++++++++++++------------ 4 files changed, 130 insertions(+), 72 deletions(-) diff --git a/app/include/zmk/ble.h b/app/include/zmk/ble.h index 1c84777d..435fde49 100644 --- a/app/include/zmk/ble.h +++ b/app/include/zmk/ble.h @@ -14,8 +14,8 @@ IS_ENABLED(CONFIG_ZMK_SPLIT_ROLE_CENTRAL)) #if ZMK_BLE_IS_CENTRAL -#define ZMK_BLE_PROFILE_COUNT (CONFIG_BT_MAX_PAIRED - 1) -#define ZMK_SPLIT_BLE_PERIPHERAL_COUNT 1 +#define ZMK_BLE_PROFILE_COUNT (CONFIG_BT_MAX_PAIRED - CONFIG_ZMK_SPLIT_BLE_CENTRAL_PERIPHERALS) +#define ZMK_SPLIT_BLE_PERIPHERAL_COUNT CONFIG_ZMK_SPLIT_BLE_CENTRAL_PERIPHERALS #else #define ZMK_BLE_PROFILE_COUNT CONFIG_BT_MAX_PAIRED #endif @@ -34,5 +34,5 @@ char *zmk_ble_active_profile_name(); int zmk_ble_unpair_all(); #if IS_ENABLED(CONFIG_ZMK_SPLIT_ROLE_CENTRAL) -void zmk_ble_set_peripheral_addr(bt_addr_le_t *addr); +int zmk_ble_put_peripheral_addr(const bt_addr_le_t *addr); #endif /* IS_ENABLED(CONFIG_ZMK_SPLIT_ROLE_CENTRAL) */ diff --git a/app/src/ble.c b/app/src/ble.c index 383a727d..ff82f3cf 100644 --- a/app/src/ble.c +++ b/app/src/ble.c @@ -47,12 +47,6 @@ RING_BUF_DECLARE(passkey_entries, PASSKEY_DIGITS); #endif /* IS_ENABLED(CONFIG_ZMK_BLE_PASSKEY_ENTRY) */ -#if IS_ENABLED(CONFIG_ZMK_SPLIT_ROLE_CENTRAL) -#define PROFILE_COUNT (CONFIG_BT_MAX_PAIRED - 1) -#else -#define PROFILE_COUNT CONFIG_BT_MAX_PAIRED -#endif - enum advertising_type { ZMK_ADV_NONE, ZMK_ADV_DIR, @@ -84,7 +78,7 @@ static const struct bt_data zmk_ble_ad[] = { #if IS_ENABLED(CONFIG_ZMK_SPLIT_ROLE_CENTRAL) -static bt_addr_le_t peripheral_addr; +static bt_addr_le_t peripheral_addrs[ZMK_SPLIT_BLE_PERIPHERAL_COUNT]; #endif /* IS_ENABLED(CONFIG_ZMK_SPLIT_ROLE_CENTRAL) */ @@ -283,9 +277,34 @@ char *zmk_ble_active_profile_name() { return profiles[active_profile].name; } #if IS_ENABLED(CONFIG_ZMK_SPLIT_ROLE_CENTRAL) -void zmk_ble_set_peripheral_addr(bt_addr_le_t *addr) { - memcpy(&peripheral_addr, addr, sizeof(bt_addr_le_t)); - settings_save_one("ble/peripheral_address", addr, sizeof(bt_addr_le_t)); +int zmk_ble_put_peripheral_addr(const bt_addr_le_t *addr) { + for (int i = 0; i < ZMK_SPLIT_BLE_PERIPHERAL_COUNT; i++) { + // If the address is recognized and already stored in settings, return + // index and no additional action is necessary. + if (!bt_addr_le_cmp(&peripheral_addrs[i], addr)) { + return i; + } + + // If the peripheral address slot is open, store new peripheral in the + // slot and return index. This compares against BT_ADDR_LE_ANY as that + // is the zero value. + if (!bt_addr_le_cmp(&peripheral_addrs[i], BT_ADDR_LE_ANY)) { + char addr_str[BT_ADDR_LE_STR_LEN]; + bt_addr_le_to_str(addr, addr_str, sizeof(addr_str)); + LOG_DBG("Storing peripheral %s in slot %d", addr_str, i); + bt_addr_le_copy(&peripheral_addrs[i], addr); + + char setting_name[32]; + sprintf(setting_name, "ble/peripheral_addresses/%d", i); + settings_save_one(setting_name, addr, sizeof(bt_addr_le_t)); + + return i; + } + } + + // The peripheral does not match a known peripheral and there is no + // available slot. + return -ENOMEM; } #endif /* IS_ENABLED(CONFIG_ZMK_SPLIT_ROLE_CENTRAL) */ @@ -340,15 +359,20 @@ static int ble_profiles_handle_set(const char *name, size_t len, settings_read_c } } #if IS_ENABLED(CONFIG_ZMK_SPLIT_ROLE_CENTRAL) - else if (settings_name_steq(name, "peripheral_address", &next) && !next) { + else if (settings_name_steq(name, "peripheral_addresses", &next) && next) { if (len != sizeof(bt_addr_le_t)) { return -EINVAL; } - int err = read_cb(cb_arg, &peripheral_addr, sizeof(bt_addr_le_t)); - if (err <= 0) { - LOG_ERR("Failed to handle peripheral address from settings (err %d)", err); - return err; + int i = atoi(next); + if (i < 0 || i >= ZMK_SPLIT_BLE_PERIPHERAL_COUNT) { + LOG_ERR("Failed to store peripheral address in memory"); + } else { + int err = read_cb(cb_arg, &peripheral_addrs[i], sizeof(bt_addr_le_t)); + if (err <= 0) { + LOG_ERR("Failed to handle peripheral address from settings (err %d)", err); + return err; + } } } #endif diff --git a/app/src/split/bluetooth/Kconfig b/app/src/split/bluetooth/Kconfig index 005d75fd..e2c8d5c3 100644 --- a/app/src/split/bluetooth/Kconfig +++ b/app/src/split/bluetooth/Kconfig @@ -17,6 +17,10 @@ config ZMK_SPLIT_ROLE_CENTRAL if ZMK_SPLIT_ROLE_CENTRAL +config ZMK_SPLIT_BLE_CENTRAL_PERIPHERALS + int "Number of peripherals that will connect to the central." + default 1 + config ZMK_SPLIT_BLE_CENTRAL_POSITION_QUEUE_SIZE int "Max number of key position state events to queue when received from peripherals" default 5 diff --git a/app/src/split/bluetooth/central.c b/app/src/split/bluetooth/central.c index a7c0d8a9..53e61be6 100644 --- a/app/src/split/bluetooth/central.c +++ b/app/src/split/bluetooth/central.c @@ -26,7 +26,7 @@ LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL); #include #include -static int start_scan(void); +static int start_scanning(void); #define POSITION_STATE_DATA_LEN 16 @@ -49,6 +49,8 @@ struct peripheral_slot { static struct peripheral_slot peripherals[ZMK_SPLIT_BLE_PERIPHERAL_COUNT]; +static bool is_scanning = false; + static const struct bt_uuid_128 split_service_uuid = BT_UUID_INIT_128(ZMK_SPLIT_BT_SERVICE_UUID); K_MSGQ_DEFINE(peripheral_event_msgq, sizeof(struct zmk_position_state_changed), @@ -130,8 +132,9 @@ int release_peripheral_slot(int index) { return 0; } -int reserve_peripheral_slot() { - for (int i = 0; i < ZMK_SPLIT_BLE_PERIPHERAL_COUNT; i++) { +int reserve_peripheral_slot(const bt_addr_le_t *addr) { + int i = zmk_ble_put_peripheral_addr(addr); + if (i >= 0) { if (peripherals[i].state == PERIPHERAL_SLOT_STATE_OPEN) { // Be sure the slot is fully reinitialized. release_peripheral_slot(i); @@ -344,9 +347,54 @@ static void split_central_process_connection(struct bt_conn *conn) { LOG_DBG("New connection params: Interval: %d, Latency: %d, PHY: %d", info.le.interval, info.le.latency, info.le.phy->rx_phy); + + // Restart scanning if necessary. + start_scanning(); } -static bool split_central_eir_found(struct bt_data *data, void *user_data) { +static int stop_scanning() { + LOG_DBG("Stopping peripheral scanning"); + is_scanning = false; + + int err = bt_le_scan_stop(); + if (err < 0) { + LOG_ERR("Stop LE scan failed (err %d)", err); + return err; + } + + return 0; +} + +static bool split_central_eir_found(const bt_addr_le_t *addr) { + LOG_DBG("Found the split service"); + + // Stop scanning so we can connect to the peripheral device. + int err = stop_scanning(); + if (err < 0) { + return false; + } + + int slot_idx = reserve_peripheral_slot(addr); + if (slot_idx < 0) { + LOG_ERR("Failed to reserve peripheral slot (err %d)", slot_idx); + return false; + } + + struct peripheral_slot *slot = &peripherals[slot_idx]; + + LOG_DBG("Initiating new connnection"); + struct bt_le_conn_param *param = BT_LE_CONN_PARAM(0x0006, 0x0006, 30, 400); + err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, param, &slot->conn); + if (err < 0) { + LOG_ERR("Create conn failed (err %d) (create conn? 0x%04x)", err, BT_HCI_OP_LE_CREATE_CONN); + release_peripheral_slot(slot_idx); + start_scanning(); + } + + return false; +} + +static bool split_central_eir_parse(struct bt_data *data, void *user_data) { bt_addr_le_t *addr = user_data; int i; @@ -361,9 +409,7 @@ static bool split_central_eir_found(struct bt_data *data, void *user_data) { } for (i = 0; i < data->data_len; i += 16) { - struct bt_le_conn_param *param; struct bt_uuid_128 uuid; - int err; if (!bt_uuid_create(&uuid.uuid, &data->data[i], 16)) { LOG_ERR("Unable to load UUID"); @@ -381,46 +427,7 @@ static bool split_central_eir_found(struct bt_data *data, void *user_data) { continue; } - LOG_DBG("Found the split service"); - - zmk_ble_set_peripheral_addr(addr); - - err = bt_le_scan_stop(); - if (err) { - LOG_ERR("Stop LE scan failed (err %d)", err); - continue; - } - - uint8_t slot_idx = reserve_peripheral_slot(); - if (slot_idx < 0) { - LOG_ERR("Faild to reserve peripheral slot (err %d)", slot_idx); - continue; - } - - struct peripheral_slot *slot = &peripherals[slot_idx]; - - slot->conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, addr); - if (slot->conn) { - LOG_DBG("Found existing connection"); - split_central_process_connection(slot->conn); - err = bt_conn_le_phy_update(slot->conn, BT_CONN_LE_PHY_PARAM_2M); - if (err) { - LOG_ERR("Update phy conn failed (err %d)", err); - } - } else { - param = BT_LE_CONN_PARAM(0x0006, 0x0006, 30, 400); - - LOG_DBG("Initiating new connnection"); - - err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, param, &slot->conn); - if (err) { - LOG_ERR("Create conn failed (err %d) (create conn? 0x%04x)", err, - BT_HCI_OP_LE_CREATE_CONN); - start_scan(); - } - } - - return false; + return split_central_eir_found(addr); } } @@ -436,15 +443,34 @@ static void split_central_device_found(const bt_addr_le_t *addr, int8_t rssi, ui /* We're only interested in connectable events */ if (type == BT_GAP_ADV_TYPE_ADV_IND || type == BT_GAP_ADV_TYPE_ADV_DIRECT_IND) { - bt_data_parse(ad, split_central_eir_found, (void *)addr); + bt_data_parse(ad, split_central_eir_parse, (void *)addr); } } -static int start_scan(void) { - int err; +static int start_scanning(void) { + // No action is necessary if central is already scanning. + if (is_scanning) { + LOG_DBG("Scanning already running"); + return 0; + } - err = bt_le_scan_start(BT_LE_SCAN_PASSIVE, split_central_device_found); - if (err) { + // If all the devices are connected, there is no need to scan. + bool has_unconnected = false; + for (int i = 0; i < CONFIG_ZMK_SPLIT_BLE_CENTRAL_PERIPHERALS; i++) { + if (peripherals[i].conn == NULL) { + has_unconnected = true; + break; + } + } + if (!has_unconnected) { + LOG_DBG("All devices are connected, scanning is unnecessary"); + return 0; + } + + // Start scanning otherwise. + is_scanning = true; + int err = bt_le_scan_start(BT_LE_SCAN_PASSIVE, split_central_device_found); + if (err < 0) { LOG_ERR("Scanning failed to start (err %d)", err); return err; } @@ -471,7 +497,7 @@ static void split_central_connected(struct bt_conn *conn, uint8_t conn_err) { release_peripheral_slot_for_conn(conn); - start_scan(); + start_scanning(); return; } @@ -495,7 +521,7 @@ static void split_central_disconnected(struct bt_conn *conn, uint8_t reason) { return; } - start_scan(); + start_scanning(); } static struct bt_conn_cb conn_callbacks = { @@ -527,6 +553,10 @@ void split_central_split_run_callback(struct k_work *work) { LOG_ERR("Source not connected"); continue; } + if (!peripherals[payload_wrapper.source].run_behavior_handle) { + LOG_ERR("Run behavior handle not found"); + continue; + } int err = bt_gatt_write_without_response( peripherals[payload_wrapper.source].conn, @@ -590,7 +620,7 @@ int zmk_split_bt_central_init(const struct device *_arg) { CONFIG_ZMK_BLE_THREAD_PRIORITY, NULL); bt_conn_cb_register(&conn_callbacks); - return start_scan(); + return start_scanning(); } SYS_INIT(zmk_split_bt_central_init, APPLICATION, CONFIG_ZMK_BLE_INIT_PRIORITY);