diff --git a/app/include/zmk/ble.h b/app/include/zmk/ble.h index 0bdbab38..f813ddc8 100644 --- a/app/include/zmk/ble.h +++ b/app/include/zmk/ble.h @@ -9,6 +9,17 @@ #include #include +#define ZMK_BLE_IS_CENTRAL \ + (IS_ENABLED(CONFIG_ZMK_SPLIT) && IS_ENABLED(CONFIG_ZMK_BLE) && \ + IS_ENABLED(CONFIG_ZMK_SPLIT_BLE_ROLE_CENTRAL)) + +#if ZMK_BLE_IS_CENTRAL +#define ZMK_BLE_PROFILE_COUNT (CONFIG_BT_MAX_PAIRED - 1) +#define ZMK_BLE_SPLIT_PERIPHERAL_COUNT 1 +#else +#define ZMK_BLE_PROFILE_COUNT CONFIG_BT_MAX_PAIRED +#endif + int zmk_ble_clear_bonds(); int zmk_ble_prof_next(); int zmk_ble_prof_prev(); diff --git a/app/include/zmk/events/position_state_changed.h b/app/include/zmk/events/position_state_changed.h index 59619db6..7685fe0f 100644 --- a/app/include/zmk/events/position_state_changed.h +++ b/app/include/zmk/events/position_state_changed.h @@ -8,16 +8,9 @@ #include #include -#include - -#if IS_ENABLED(CONFIG_ZMK_BLE) -typedef const bt_addr_le_t *zmk_position_state_changed_source_t; -#else -typedef void *zmk_position_state_changed_source_t; -#endif struct zmk_position_state_changed { - zmk_position_state_changed_source_t source; + uint8_t source; uint32_t position; bool state; int64_t timestamp; diff --git a/app/include/zmk/keymap.h b/app/include/zmk/keymap.h index 0771542c..1195b943 100644 --- a/app/include/zmk/keymap.h +++ b/app/include/zmk/keymap.h @@ -20,8 +20,8 @@ int zmk_keymap_layer_toggle(uint8_t layer); int zmk_keymap_layer_to(uint8_t layer); const char *zmk_keymap_layer_label(uint8_t layer); -int zmk_keymap_position_state_changed(zmk_position_state_changed_source_t source, uint32_t position, - bool pressed, int64_t timestamp); +int zmk_keymap_position_state_changed(uint8_t source, uint32_t position, bool pressed, + int64_t timestamp); #define ZMK_KEYMAP_EXTRACT_BINDING(idx, drv_inst) \ { \ diff --git a/app/include/zmk/split/bluetooth/central.h b/app/include/zmk/split/bluetooth/central.h index ab46a8f5..07240860 100644 --- a/app/include/zmk/split/bluetooth/central.h +++ b/app/include/zmk/split/bluetooth/central.h @@ -4,5 +4,5 @@ #include #include -int zmk_split_bt_invoke_behavior(const bt_addr_le_t *source, struct zmk_behavior_binding *binding, +int zmk_split_bt_invoke_behavior(uint8_t source, struct zmk_behavior_binding *binding, struct zmk_behavior_binding_event event, bool state); \ No newline at end of file diff --git a/app/src/ble.c b/app/src/ble.c index 68061129..afc2e47f 100644 --- a/app/src/ble.c +++ b/app/src/ble.c @@ -72,7 +72,7 @@ enum advertising_type { BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONNECTABLE | BT_LE_ADV_OPT_ONE_TIME, BT_GAP_ADV_FAST_INT_MIN_2, \ BT_GAP_ADV_FAST_INT_MAX_2, NULL) -static struct zmk_ble_profile profiles[PROFILE_COUNT]; +static struct zmk_ble_profile profiles[ZMK_BLE_PROFILE_COUNT]; static uint8_t active_profile; #define DEVICE_NAME CONFIG_BT_DEVICE_NAME @@ -260,7 +260,7 @@ static int ble_save_profile() { } int zmk_ble_prof_select(uint8_t index) { - if (index >= PROFILE_COUNT) { + if (index >= ZMK_BLE_PROFILE_COUNT) { return -ERANGE; } @@ -281,12 +281,13 @@ int zmk_ble_prof_select(uint8_t index) { int zmk_ble_prof_next() { LOG_DBG(""); - return zmk_ble_prof_select((active_profile + 1) % PROFILE_COUNT); + return zmk_ble_prof_select((active_profile + 1) % ZMK_BLE_PROFILE_COUNT); }; int zmk_ble_prof_prev() { LOG_DBG(""); - return zmk_ble_prof_select((active_profile + PROFILE_COUNT - 1) % PROFILE_COUNT); + return zmk_ble_prof_select((active_profile + ZMK_BLE_PROFILE_COUNT - 1) % + ZMK_BLE_PROFILE_COUNT); }; bt_addr_le_t *zmk_ble_active_profile_addr() { return &profiles[active_profile].peer; } @@ -324,8 +325,9 @@ static int ble_profiles_handle_set(const char *name, size_t len, settings_read_c return -EINVAL; } - if (idx >= PROFILE_COUNT) { - LOG_WRN("Profile address for index %d is larger than max of %d", idx, PROFILE_COUNT); + if (idx >= ZMK_BLE_PROFILE_COUNT) { + LOG_WRN("Profile address for index %d is larger than max of %d", idx, + ZMK_BLE_PROFILE_COUNT); return -EINVAL; } @@ -591,7 +593,7 @@ static int zmk_ble_init(const struct device *_arg) { bt_unpair(i, NULL); } - for (int i = 0; i < PROFILE_COUNT; i++) { + for (int i = 0; i < ZMK_BLE_PROFILE_COUNT; i++) { char setting_name[15]; sprintf(setting_name, "ble/profiles/%d", i); diff --git a/app/src/keymap.c b/app/src/keymap.c index 16ec3169..8acc5ec1 100644 --- a/app/src/keymap.c +++ b/app/src/keymap.c @@ -4,10 +4,6 @@ * SPDX-License-Identifier: MIT */ -#define IS_BLE_CENTRAL \ - (IS_ENABLED(CONFIG_ZMK_SPLIT) && IS_ENABLED(CONFIG_ZMK_BLE) && \ - IS_ENABLED(CONFIG_ZMK_SPLIT_BLE_ROLE_CENTRAL)) - #include #include #include @@ -19,7 +15,8 @@ LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL); #include #include -#if IS_BLE_CENTRAL +#include +#if ZMK_BLE_IS_CENTRAL #include #endif @@ -170,8 +167,8 @@ int invoke_locally(struct zmk_behavior_binding *binding, struct zmk_behavior_bin } } -int zmk_keymap_apply_position_state(zmk_position_state_changed_source_t source, int layer, - uint32_t position, bool pressed, int64_t timestamp) { +int zmk_keymap_apply_position_state(uint8_t source, int layer, uint32_t position, bool pressed, + int64_t timestamp) { // We want to make a copy of this, since it may be converted from // relative to absolute before being invoked struct zmk_behavior_binding binding = zmk_keymap[layer][position]; @@ -209,8 +206,8 @@ int zmk_keymap_apply_position_state(zmk_position_state_changed_source_t source, case BEHAVIOR_LOCALITY_CENTRAL: return invoke_locally(&binding, event, pressed); case BEHAVIOR_LOCALITY_EVENT_SOURCE: -#if IS_BLE_CENTRAL - if (!bt_addr_le_cmp(source, BT_ADDR_LE_NONE)) { +#if ZMK_BLE_IS_CENTRAL + if (source == UINT_MAX) { return invoke_locally(&binding, event, pressed); } else { return zmk_split_bt_invoke_behavior(source, &binding, event, pressed); @@ -219,8 +216,10 @@ int zmk_keymap_apply_position_state(zmk_position_state_changed_source_t source, return invoke_locally(&binding, event, pressed); #endif case BEHAVIOR_LOCALITY_GLOBAL: -#if IS_BLE_CENTRAL - zmk_split_bt_invoke_behavior(BT_ADDR_LE_ANY, &binding, event, pressed); +#if ZMK_BLE_IS_CENTRAL + for (int i = 0; i < ZMK_BLE_SPLIT_PERIPHERAL_COUNT; i++) { + zmk_split_bt_invoke_behavior(i, &binding, event, pressed); + } #endif return invoke_locally(&binding, event, pressed); } @@ -228,8 +227,8 @@ int zmk_keymap_apply_position_state(zmk_position_state_changed_source_t source, return -ENOTSUP; } -int zmk_keymap_position_state_changed(zmk_position_state_changed_source_t source, uint32_t position, - bool pressed, int64_t timestamp) { +int zmk_keymap_position_state_changed(uint8_t source, uint32_t position, bool pressed, + int64_t timestamp) { if (pressed) { zmk_keymap_active_behavior_layer[position] = _zmk_keymap_layer_state; } diff --git a/app/src/kscan.c b/app/src/kscan.c index 13ad2ccb..3425fabe 100644 --- a/app/src/kscan.c +++ b/app/src/kscan.c @@ -50,12 +50,11 @@ void zmk_kscan_process_msgq(struct k_work *item) { uint32_t position = zmk_matrix_transform_row_column_to_position(ev.row, ev.column); LOG_DBG("Row: %d, col: %d, position: %d, pressed: %s", ev.row, ev.column, position, (pressed ? "true" : "false")); - ZMK_EVENT_RAISE(new_zmk_position_state_changed((struct zmk_position_state_changed) { -#if IS_ENABLED(CONFIG_ZMK_BLE) - .source = BT_ADDR_LE_NONE, -#endif - .state = pressed, .position = position, .timestamp = k_uptime_get() - })); + ZMK_EVENT_RAISE(new_zmk_position_state_changed( + (struct zmk_position_state_changed){.source = UINT8_MAX, + .state = pressed, + .position = position, + .timestamp = k_uptime_get()})); } } diff --git a/app/src/split/bluetooth/central.c b/app/src/split/bluetooth/central.c index c28941f1..0d52d051 100644 --- a/app/src/split/bluetooth/central.c +++ b/app/src/split/bluetooth/central.c @@ -29,17 +29,112 @@ static int start_scan(void); #define POSITION_STATE_DATA_LEN 16 -static struct bt_conn *default_conn; +enum peripheral_slot_state { + PERIPHERAL_SLOT_STATE_OPEN, + PERIPHERAL_SLOT_STATE_CONNECTING, + PERIPHERAL_SLOT_STATE_CONNECTED, +}; + +struct peripheral_slot { + enum peripheral_slot_state state; + struct bt_conn *conn; + struct bt_gatt_discover_params discover_params; + struct bt_gatt_subscribe_params subscribe_params; + struct bt_gatt_discover_params sub_discover_params; + uint16_t run_behavior_handle; + uint8_t position_state[POSITION_STATE_DATA_LEN]; + uint8_t changed_positions[POSITION_STATE_DATA_LEN]; +}; + +static struct peripheral_slot peripherals[ZMK_BLE_SPLIT_PERIPHERAL_COUNT]; static const struct bt_uuid_128 split_service_uuid = BT_UUID_INIT_128(ZMK_SPLIT_BT_SERVICE_UUID); -static struct bt_gatt_discover_params discover_params; -static struct bt_gatt_subscribe_params subscribe_params; -static struct bt_gatt_discover_params sub_discover_params; -static uint16_t run_behavior_handle; K_MSGQ_DEFINE(peripheral_event_msgq, sizeof(struct zmk_position_state_changed), CONFIG_ZMK_SPLIT_BLE_CENTRAL_POSITION_QUEUE_SIZE, 4); +int peripheral_slot_index_for_conn(struct bt_conn *conn) { + for (int i = 0; i < ZMK_BLE_SPLIT_PERIPHERAL_COUNT; i++) { + if (peripherals[i].conn == conn) { + return i; + } + } + + return -EINVAL; +} + +struct peripheral_slot *peripheral_slot_for_conn(struct bt_conn *conn) { + int idx = peripheral_slot_index_for_conn(conn); + if (idx < 0) { + return NULL; + } + + return &peripherals[idx]; +} + +int release_peripheral_slot(int index) { + if (index < 0 || index >= ZMK_BLE_SPLIT_PERIPHERAL_COUNT) { + return -EINVAL; + } + + struct peripheral_slot *slot = &peripherals[index]; + + if (slot->state == PERIPHERAL_SLOT_STATE_OPEN) { + return -EINVAL; + } + + LOG_DBG("Releasing peripheral slot at %d", index); + + if (slot->conn != NULL) { + bt_conn_unref(slot->conn); + slot->conn = NULL; + } + slot->state = PERIPHERAL_SLOT_STATE_OPEN; + + for (int i = 0; i < POSITION_STATE_DATA_LEN; i++) { + slot->position_state[i] = 0U; + slot->changed_positions[i] = 0U; + } + + // Clean up previously discovered handles; + slot->subscribe_params.value_handle = 0; + slot->run_behavior_handle = 0; + + return 0; +} + +int reserve_peripheral_slot() { + for (int i = 0; i < ZMK_BLE_SPLIT_PERIPHERAL_COUNT; i++) { + if (peripherals[i].state == PERIPHERAL_SLOT_STATE_OPEN) { + // Be sure the slot is fully reinitialized. + release_peripheral_slot(i); + peripherals[i].state = PERIPHERAL_SLOT_STATE_CONNECTING; + return i; + } + } + + return -ENOMEM; +} + +int release_peripheral_slot_for_conn(struct bt_conn *conn) { + int idx = peripheral_slot_index_for_conn(conn); + if (idx < 0) { + return idx; + } + + return release_peripheral_slot(idx); +} + +int confirm_peripheral_slot_conn(struct bt_conn *conn) { + int idx = peripheral_slot_index_for_conn(conn); + if (idx < 0) { + return idx; + } + + peripherals[idx].state = PERIPHERAL_SLOT_STATE_CONNECTED; + return 0; +} + void peripheral_event_work_callback(struct k_work *work) { struct zmk_position_state_changed ev; while (k_msgq_get(&peripheral_event_msgq, &ev, K_NO_WAIT) == 0) { @@ -53,9 +148,12 @@ K_WORK_DEFINE(peripheral_event_work, peripheral_event_work_callback); static uint8_t split_central_notify_func(struct bt_conn *conn, struct bt_gatt_subscribe_params *params, const void *data, uint16_t length) { - static uint8_t position_state[POSITION_STATE_DATA_LEN]; + struct peripheral_slot *slot = peripheral_slot_for_conn(conn); - uint8_t changed_positions[POSITION_STATE_DATA_LEN]; + if (slot == NULL) { + LOG_ERR("No peripheral state found for connection"); + return BT_GATT_ITER_CONTINUE; + } if (!data) { LOG_DBG("[UNSUBSCRIBED]"); @@ -66,16 +164,18 @@ static uint8_t split_central_notify_func(struct bt_conn *conn, LOG_DBG("[NOTIFICATION] data %p length %u", data, length); for (int i = 0; i < POSITION_STATE_DATA_LEN; i++) { - changed_positions[i] = ((uint8_t *)data)[i] ^ position_state[i]; - position_state[i] = ((uint8_t *)data)[i]; + slot->changed_positions[i] = ((uint8_t *)data)[i] ^ slot->position_state[i]; + slot->position_state[i] = ((uint8_t *)data)[i]; + LOG_DBG("data: %d", slot->position_state[i]); } for (int i = 0; i < POSITION_STATE_DATA_LEN; i++) { for (int j = 0; j < 8; j++) { - if (changed_positions[i] & BIT(j)) { + if (slot->changed_positions[i] & BIT(j)) { uint32_t position = (i * 8) + j; - bool pressed = position_state[i] & BIT(j); - struct zmk_position_state_changed ev = {.source = bt_conn_get_dst(conn), + bool pressed = slot->position_state[i] & BIT(j); + struct zmk_position_state_changed ev = {.source = + peripheral_slot_index_for_conn(conn), .position = position, .state = pressed, .timestamp = k_uptime_get()}; @@ -90,7 +190,13 @@ static uint8_t split_central_notify_func(struct bt_conn *conn, } static void split_central_subscribe(struct bt_conn *conn) { - int err = bt_gatt_subscribe(conn, &subscribe_params); + struct peripheral_slot *slot = peripheral_slot_for_conn(conn); + if (slot == NULL) { + LOG_ERR("No peripheral state found for connection"); + return; + } + + int err = bt_gatt_subscribe(conn, &slot->subscribe_params); switch (err) { case -EALREADY: LOG_DBG("[ALREADY SUBSCRIBED]"); @@ -117,28 +223,34 @@ static uint8_t split_central_chrc_discovery_func(struct bt_conn *conn, return BT_GATT_ITER_STOP; } + struct peripheral_slot *slot = peripheral_slot_for_conn(conn); + if (slot == NULL) { + LOG_ERR("No peripheral state found for connection"); + return BT_GATT_ITER_STOP; + } + LOG_DBG("[ATTRIBUTE] handle %u", attr->handle); if (!bt_uuid_cmp(((struct bt_gatt_chrc *)attr->user_data)->uuid, BT_UUID_DECLARE_128(ZMK_SPLIT_BT_CHAR_POSITION_STATE_UUID))) { LOG_DBG("Found position state characteristic"); - discover_params.uuid = NULL; - discover_params.start_handle = attr->handle + 2; - discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; + slot->discover_params.uuid = NULL; + slot->discover_params.start_handle = attr->handle + 2; + slot->discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; - subscribe_params.disc_params = &sub_discover_params; - subscribe_params.end_handle = discover_params.end_handle; - subscribe_params.value_handle = bt_gatt_attr_value_handle(attr); - subscribe_params.notify = split_central_notify_func; - subscribe_params.value = BT_GATT_CCC_NOTIFY; + slot->subscribe_params.disc_params = &slot->sub_discover_params; + slot->subscribe_params.end_handle = slot->discover_params.end_handle; + slot->subscribe_params.value_handle = bt_gatt_attr_value_handle(attr); + slot->subscribe_params.notify = split_central_notify_func; + slot->subscribe_params.value = BT_GATT_CCC_NOTIFY; split_central_subscribe(conn); } else if (!bt_uuid_cmp(((struct bt_gatt_chrc *)attr->user_data)->uuid, BT_UUID_DECLARE_128(ZMK_SPLIT_BT_CHAR_RUN_BEHAVIOR_UUID))) { LOG_DBG("Found run behavior handle"); - run_behavior_handle = bt_gatt_attr_value_handle(attr); + slot->run_behavior_handle = bt_gatt_attr_value_handle(attr); } - bool subscribed = (run_behavior_handle && subscribe_params.value_handle); + bool subscribed = (slot->run_behavior_handle && slot->subscribe_params.value_handle); return subscribed ? BT_GATT_ITER_STOP : BT_GATT_ITER_CONTINUE; } @@ -154,18 +266,24 @@ static uint8_t split_central_service_discovery_func(struct bt_conn *conn, LOG_DBG("[ATTRIBUTE] handle %u", attr->handle); - if (bt_uuid_cmp(discover_params.uuid, BT_UUID_DECLARE_128(ZMK_SPLIT_BT_SERVICE_UUID))) { + struct peripheral_slot *slot = peripheral_slot_for_conn(conn); + if (slot == NULL) { + LOG_ERR("No peripheral state found for connection"); + return BT_GATT_ITER_STOP; + } + + if (bt_uuid_cmp(slot->discover_params.uuid, BT_UUID_DECLARE_128(ZMK_SPLIT_BT_SERVICE_UUID))) { LOG_DBG("Found other service"); return BT_GATT_ITER_CONTINUE; } LOG_DBG("Found split service"); - discover_params.uuid = NULL; - discover_params.func = split_central_chrc_discovery_func; - discover_params.start_handle = attr->handle + 1; - discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; + slot->discover_params.uuid = NULL; + slot->discover_params.func = split_central_chrc_discovery_func; + slot->discover_params.start_handle = attr->handle + 1; + slot->discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; - int err = bt_gatt_discover(conn, &discover_params); + int err = bt_gatt_discover(conn, &slot->discover_params); if (err) { LOG_ERR("Failed to start discovering split service characteristics (err %d)", err); } @@ -183,14 +301,20 @@ static void split_central_process_connection(struct bt_conn *conn) { return; } - if (conn == default_conn && !subscribe_params.value_handle) { - discover_params.uuid = &split_service_uuid.uuid; - discover_params.func = split_central_service_discovery_func; - discover_params.start_handle = 0x0001; - discover_params.end_handle = 0xffff; - discover_params.type = BT_GATT_DISCOVER_PRIMARY; + struct peripheral_slot *slot = peripheral_slot_for_conn(conn); + if (slot == NULL) { + LOG_ERR("No peripheral state found for connection"); + return; + } - err = bt_gatt_discover(default_conn, &discover_params); + if (!slot->subscribe_params.value_handle) { + slot->discover_params.uuid = &split_service_uuid.uuid; + slot->discover_params.func = split_central_service_discovery_func; + slot->discover_params.start_handle = 0x0001; + slot->discover_params.end_handle = 0xffff; + slot->discover_params.type = BT_GATT_DISCOVER_PRIMARY; + + err = bt_gatt_discover(slot->conn, &slot->discover_params); if (err) { LOG_ERR("Discover failed(err %d)", err); return; @@ -251,21 +375,33 @@ static bool split_central_eir_found(struct bt_data *data, void *user_data) { continue; } - default_conn = bt_conn_lookup_addr_le(BT_ID_DEFAULT, addr); - if (default_conn) { + 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(default_conn); + 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); - err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, param, &default_conn); + 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(); } - err = bt_conn_le_phy_update(default_conn, BT_CONN_LE_PHY_PARAM_2M); + 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); start_scan(); @@ -314,8 +450,7 @@ static void split_central_connected(struct bt_conn *conn, uint8_t conn_err) { if (conn_err) { LOG_ERR("Failed to connect to %s (%u)", log_strdup(addr), conn_err); - bt_conn_unref(default_conn); - default_conn = NULL; + release_peripheral_slot_for_conn(conn); start_scan(); return; @@ -323,6 +458,7 @@ static void split_central_connected(struct bt_conn *conn, uint8_t conn_err) { LOG_DBG("Connected: %s", log_strdup(addr)); + confirm_peripheral_slot_conn(conn); split_central_process_connection(conn); } @@ -333,16 +469,7 @@ static void split_central_disconnected(struct bt_conn *conn, uint8_t reason) { LOG_DBG("Disconnected: %s (reason %d)", log_strdup(addr), reason); - if (default_conn != conn) { - return; - } - - bt_conn_unref(default_conn); - default_conn = NULL; - - // Clean up previously discovered handles; - subscribe_params.value_handle = 0; - run_behavior_handle = 0; + release_peripheral_slot_for_conn(conn); start_scan(); } @@ -357,16 +484,30 @@ K_THREAD_STACK_DEFINE(split_central_split_run_q_stack, struct k_work_q split_central_split_run_q; -K_MSGQ_DEFINE(zmk_split_central_split_run_msgq, sizeof(struct zmk_split_run_behavior_payload), +struct zmk_split_run_behavior_payload_wrapper { + uint8_t source; + struct zmk_split_run_behavior_payload payload; +}; + +K_MSGQ_DEFINE(zmk_split_central_split_run_msgq, + sizeof(struct zmk_split_run_behavior_payload_wrapper), CONFIG_ZMK_BLE_SPLIT_CENTRAL_SPLIT_RUN_QUEUE_SIZE, 4); void split_central_split_run_callback(struct k_work *work) { - struct zmk_split_run_behavior_payload payload; + struct zmk_split_run_behavior_payload_wrapper payload_wrapper; - while (k_msgq_get(&zmk_split_central_split_run_msgq, &payload, K_NO_WAIT) == 0) { - int err = - bt_gatt_write_without_response(default_conn, run_behavior_handle, &payload, - sizeof(struct zmk_split_run_behavior_payload), true); + LOG_DBG(""); + + while (k_msgq_get(&zmk_split_central_split_run_msgq, &payload_wrapper, K_NO_WAIT) == 0) { + if (peripherals[payload_wrapper.source].state != PERIPHERAL_SLOT_STATE_CONNECTED) { + LOG_ERR("Source not connected"); + continue; + } + + int err = bt_gatt_write_without_response( + peripherals[payload_wrapper.source].conn, + peripherals[payload_wrapper.source].run_behavior_handle, &payload_wrapper.payload, + sizeof(struct zmk_split_run_behavior_payload), true); if (err) { LOG_ERR("Failed to write the behavior characteristic (err %d)", err); @@ -376,15 +517,18 @@ void split_central_split_run_callback(struct k_work *work) { K_WORK_DEFINE(split_central_split_run_work, split_central_split_run_callback); -static int split_bt_invoke_behavior_payload(struct zmk_split_run_behavior_payload payload) { - int err = k_msgq_put(&zmk_split_central_split_run_msgq, &payload, K_MSEC(100)); +static int +split_bt_invoke_behavior_payload(struct zmk_split_run_behavior_payload_wrapper payload_wrapper) { + LOG_DBG(""); + + int err = k_msgq_put(&zmk_split_central_split_run_msgq, &payload_wrapper, K_MSEC(100)); if (err) { switch (err) { case -EAGAIN: { LOG_WRN("Consumer message queue full, popping first message and queueing again"); - struct zmk_split_run_behavior_payload discarded_report; + struct zmk_split_run_behavior_payload_wrapper discarded_report; k_msgq_get(&zmk_split_central_split_run_msgq, &discarded_report, K_NO_WAIT); - return split_bt_invoke_behavior_payload(payload); + return split_bt_invoke_behavior_payload(payload_wrapper); } default: LOG_WRN("Failed to queue behavior to send (%d)", err); @@ -397,18 +541,19 @@ static int split_bt_invoke_behavior_payload(struct zmk_split_run_behavior_payloa return 0; }; -int zmk_split_bt_invoke_behavior(const bt_addr_le_t *source, struct zmk_behavior_binding *binding, +int zmk_split_bt_invoke_behavior(uint8_t source, struct zmk_behavior_binding *binding, struct zmk_behavior_binding_event event, bool state) { struct zmk_split_run_behavior_payload payload = {.data = { .param1 = binding->param1, .param2 = binding->param2, .position = event.position, - .state = state, + .state = state ? 1 : 0, }}; strncpy(payload.behavior_dev, binding->behavior_dev, ZMK_SPLIT_RUN_BEHAVIOR_DEV_LEN - 1); payload.behavior_dev[ZMK_SPLIT_RUN_BEHAVIOR_DEV_LEN - 1] = '\0'; - return split_bt_invoke_behavior_payload(payload); + struct zmk_split_run_behavior_payload_wrapper wrapper = {.source = source, .payload = payload}; + return split_bt_invoke_behavior_payload(wrapper); } int zmk_split_bt_central_init(const struct device *_arg) {