From c5fde6de5587c9f30991754b990538cde5becc07 Mon Sep 17 00:00:00 2001 From: Kim Streich Date: Sun, 3 Apr 2022 12:50:51 +0400 Subject: [PATCH] feature(split): add support for sensors from peripheral MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This commit adds a new GATT characteristics on the peripheral side and wires it up to read sensor values. The central side subscribes to this new characteristics and replays sensor values on its side. — This commit was originally made by Stephen Wan. I just adjusted it so that it rebases on top of later changes on the zmk main branch. --- app/include/zmk/split/bluetooth/service.h | 6 +- app/include/zmk/split/bluetooth/uuid.h | 1 + app/src/split/bluetooth/central.c | 95 +++++++++++++++++++++++ app/src/split/bluetooth/service.c | 72 ++++++++++++++++- app/src/split/bluetooth/split_listener.c | 32 ++++++-- 5 files changed, 197 insertions(+), 9 deletions(-) diff --git a/app/include/zmk/split/bluetooth/service.h b/app/include/zmk/split/bluetooth/service.h index f0c1d79ff7d..9acf120c80e 100644 --- a/app/include/zmk/split/bluetooth/service.h +++ b/app/include/zmk/split/bluetooth/service.h @@ -6,6 +6,8 @@ #pragma once +#include + #define ZMK_SPLIT_RUN_BEHAVIOR_DEV_LEN 9 struct zmk_split_run_behavior_data { @@ -20,5 +22,7 @@ struct zmk_split_run_behavior_payload { char behavior_dev[ZMK_SPLIT_RUN_BEHAVIOR_DEV_LEN]; } __packed; + int zmk_split_bt_position_pressed(uint8_t position); -int zmk_split_bt_position_released(uint8_t position); \ No newline at end of file +int zmk_split_bt_position_released(uint8_t position); +int zmk_split_bt_sensor_triggered(uint8_t sensor_number, struct sensor_value value); diff --git a/app/include/zmk/split/bluetooth/uuid.h b/app/include/zmk/split/bluetooth/uuid.h index 735f5751d0a..7dac743ffe7 100644 --- a/app/include/zmk/split/bluetooth/uuid.h +++ b/app/include/zmk/split/bluetooth/uuid.h @@ -16,3 +16,4 @@ #define ZMK_SPLIT_BT_SERVICE_UUID ZMK_BT_SPLIT_UUID(0x00000000) #define ZMK_SPLIT_BT_CHAR_POSITION_STATE_UUID ZMK_BT_SPLIT_UUID(0x00000001) #define ZMK_SPLIT_BT_CHAR_RUN_BEHAVIOR_UUID ZMK_BT_SPLIT_UUID(0x00000002) +#define ZMK_SPLIT_BT_CHAR_SENSOR_STATE_UUID ZMK_BT_SPLIT_UUID(0x00000002) diff --git a/app/src/split/bluetooth/central.c b/app/src/split/bluetooth/central.c index 76fe8955266..b6c8b57060b 100644 --- a/app/src/split/bluetooth/central.c +++ b/app/src/split/bluetooth/central.c @@ -20,10 +20,12 @@ LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL); #include #include #include +#include #include #include #include #include +#include #include static int start_scan(void); @@ -162,6 +164,50 @@ int confirm_peripheral_slot_conn(struct bt_conn *conn) { return 0; } +#if ZMK_KEYMAP_HAS_SENSORS +K_MSGQ_DEFINE(peripheral_sensor_event_msgq, sizeof(struct zmk_sensor_event), + CONFIG_ZMK_SPLIT_BLE_CENTRAL_POSITION_QUEUE_SIZE, 4); + +void peripheral_sensor_event_work_callback(struct k_work *work) { + struct zmk_sensor_event ev; + while (k_msgq_get(&peripheral_sensor_event_msgq, &ev, K_NO_WAIT) == 0) { + LOG_DBG("Trigger sensor change for %d", ev.sensor_number); + ZMK_EVENT_RAISE(new_zmk_sensor_event(ev)); + } +} + +K_WORK_DEFINE(peripheral_sensor_event_work, peripheral_sensor_event_work_callback); + +struct sensor_event { + uint8_t sensor_number; + struct sensor_value value; +}; + +static uint8_t split_central_sensor_notify_func(struct bt_conn *conn, + struct bt_gatt_subscribe_params *params, + const void *data, uint16_t length) { + + const struct sensor_event *sensor_event = data; + + if (!data) { + LOG_DBG("[UNSUBSCRIBED]"); + params->value_handle = 0U; + return BT_GATT_ITER_STOP; + } + LOG_DBG("[SENSOR NOTIFICATION] data %p length %u", data, length); + + struct zmk_sensor_event ev = { + .sensor_number = sensor_event->sensor_number, + .value = {.val1 = (sensor_event->value).val1, .val2 = (sensor_event->value).val2}, + .timestamp = k_uptime_get()}; + + k_msgq_put(&peripheral_sensor_event_msgq, &ev, K_NO_WAIT); + k_work_submit(&peripheral_sensor_event_work); + + return BT_GATT_ITER_CONTINUE; +} +#endif /* ZMK_KEYMAP_HAS_SENSORS */ + static uint8_t split_central_notify_func(struct bt_conn *conn, struct bt_gatt_subscribe_params *params, const void *data, uint16_t length) { @@ -221,6 +267,39 @@ static void split_central_subscribe(struct bt_conn *conn, struct bt_gatt_subscri } } +#if ZMK_KEYMAP_HAS_SENSORS +static struct bt_uuid_128 sensor_uuid = BT_UUID_INIT_128(ZMK_SPLIT_BT_SERVICE_UUID); +static struct bt_gatt_discover_params sensor_discover_params; +static struct bt_gatt_subscribe_params sensor_subscribe_params; +static uint8_t split_central_sensor_desc_discovery_func(struct bt_conn *conn, + const struct bt_gatt_attr *attr, + struct bt_gatt_discover_params *params) { + int err; + + if (!bt_uuid_cmp(sensor_discover_params.uuid, + BT_UUID_DECLARE_128(ZMK_SPLIT_BT_CHAR_SENSOR_STATE_UUID))) { + memcpy(&sensor_uuid, BT_UUID_GATT_CCC, sizeof(sensor_uuid)); + sensor_discover_params.uuid = &sensor_uuid.uuid; + sensor_discover_params.start_handle = attr->handle; + sensor_discover_params.type = BT_GATT_DISCOVER_DESCRIPTOR; + + sensor_subscribe_params.value_handle = bt_gatt_attr_value_handle(attr); + + err = bt_gatt_discover(conn, &sensor_discover_params); + if (err) { + LOG_ERR("Discover failed (err %d)", err); + } + } else { + sensor_subscribe_params.notify = split_central_sensor_notify_func; + sensor_subscribe_params.value = BT_GATT_CCC_NOTIFY; + sensor_subscribe_params.ccc_handle = attr->handle; + split_central_subscribe(conn, &sensor_subscribe_params); + } + + return BT_GATT_ITER_STOP; +} +#endif /* ZMK_KEYMAP_HAS_SENSORS */ + static uint8_t split_central_chrc_discovery_func(struct bt_conn *conn, const struct bt_gatt_attr *attr, struct bt_gatt_discover_params *params) { @@ -298,6 +377,22 @@ static uint8_t split_central_service_discovery_func(struct bt_conn *conn, if (err) { LOG_ERR("Failed to start discovering split service characteristics (err %d)", err); } + +#if ZMK_KEYMAP_HAS_SENSORS + memcpy(&sensor_uuid, BT_UUID_DECLARE_128(ZMK_SPLIT_BT_CHAR_SENSOR_STATE_UUID), + sizeof(sensor_uuid)); + sensor_discover_params.uuid = &sensor_uuid.uuid; + sensor_discover_params.start_handle = attr->handle; + sensor_discover_params.end_handle = 0xffff; + sensor_discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; + sensor_discover_params.func = split_central_sensor_desc_discovery_func; + + err = bt_gatt_discover(conn, &sensor_discover_params); + if (err) { + LOG_ERR("Discover failed (err %d)", err); + } +#endif /* ZMK_KEYMAP_HAS_SENSORS */ + return BT_GATT_ITER_STOP; } diff --git a/app/src/split/bluetooth/service.c b/app/src/split/bluetooth/service.c index 5da5401d682..730e260333b 100644 --- a/app/src/split/bluetooth/service.c +++ b/app/src/split/bluetooth/service.c @@ -4,6 +4,7 @@ * SPDX-License-Identifier: MIT */ +#include #include #include #include @@ -20,6 +21,23 @@ LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL); #include #include #include +#include + +#if ZMK_KEYMAP_HAS_SENSORS +struct sensor_event { + uint8_t sensor_number; + struct sensor_value value; +} sensor_event; + +static ssize_t split_svc_sensor_state(struct bt_conn *conn, const struct bt_gatt_attr *attrs, + void *buf, uint16_t len, uint16_t offset) { + return bt_gatt_attr_read(conn, attrs, buf, len, offset, &sensor_event, sizeof(sensor_event)); +} + +static void split_svc_sensor_state_ccc(const struct bt_gatt_attr *attr, uint16_t value) { + LOG_DBG("value %d", value); +} +#endif /* ZMK_KEYMAP_HAS_SENSORS */ #define POS_STATE_LEN 16 @@ -98,7 +116,14 @@ BT_GATT_SERVICE_DEFINE( BT_GATT_CHRC_WRITE_WITHOUT_RESP, BT_GATT_PERM_WRITE_ENCRYPT, NULL, split_svc_run_behavior, &behavior_run_payload), BT_GATT_DESCRIPTOR(BT_UUID_NUM_OF_DIGITALS, BT_GATT_PERM_READ, split_svc_num_of_positions, NULL, - &num_of_positions), ); + &num_of_positions), +#if ZMK_KEYMAP_HAS_SENSORS + BT_GATT_CHARACTERISTIC(BT_UUID_DECLARE_128(ZMK_SPLIT_BT_CHAR_SENSOR_STATE_UUID), + BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY, BT_GATT_PERM_READ_ENCRYPT, + split_svc_sensor_state, NULL, &sensor_event), + BT_GATT_CCC(split_svc_sensor_state_ccc, BT_GATT_PERM_READ_ENCRYPT | BT_GATT_PERM_WRITE_ENCRYPT), +#endif /* ZMK_KEYMAP_HAS_SENSORS */ +); K_THREAD_STACK_DEFINE(service_q_stack, CONFIG_ZMK_SPLIT_BLE_PERIPHERAL_STACK_SIZE); @@ -151,6 +176,51 @@ int zmk_split_bt_position_released(uint8_t position) { return send_position_state(); } +#if ZMK_KEYMAP_HAS_SENSORS +K_MSGQ_DEFINE(sensor_state_msgq, sizeof(sensor_event), + CONFIG_ZMK_SPLIT_BLE_PERIPHERAL_POSITION_QUEUE_SIZE, 4); + +void send_sensor_state_callback(struct k_work *work) { + struct sensor_event ev; + + while (k_msgq_get(&sensor_state_msgq, &ev, K_NO_WAIT) == 0) { + int err = bt_gatt_notify(NULL, &split_svc.attrs[5], &ev, sizeof(ev)); + if (err) { + LOG_DBG("Error notifying %d", err); + } + } +}; + +K_WORK_DEFINE(service_sensor_notify_work, send_sensor_state_callback); + +int send_sensor_state() { + int err = k_msgq_put(&sensor_state_msgq, &sensor_event, K_MSEC(100)); + if (err) { + // retry... + switch (err) { + case -EAGAIN: { + LOG_WRN("Sensor state message queue full, popping first message and queueing again"); + struct sensor_event discarded_state; + k_msgq_get(&sensor_state_msgq, &discarded_state, K_NO_WAIT); + return send_sensor_state(); + } + default: + LOG_WRN("Failed to queue sensor state to send (%d)", err); + return err; + } + } + + k_work_submit_to_queue(&service_work_q, &service_sensor_notify_work); + return 0; +} + +int zmk_split_bt_sensor_triggered(uint8_t sensor_number, struct sensor_value value) { + sensor_event.sensor_number = sensor_number; + sensor_event.value = value; + return send_sensor_state(); +} +#endif /* ZMK_KEYMAP_HAS_SENSORS */ + int service_init(const struct device *_arg) { static const struct k_work_queue_config queue_config = { .name = "Split Peripheral Notification Queue"}; diff --git a/app/src/split/bluetooth/split_listener.c b/app/src/split/bluetooth/split_listener.c index 3f3763ae04a..93e58743037 100644 --- a/app/src/split/bluetooth/split_listener.c +++ b/app/src/split/bluetooth/split_listener.c @@ -5,6 +5,7 @@ */ #include +#include #include #include @@ -13,21 +14,38 @@ LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL); #include #include +#include #include +#include #include int split_listener(const zmk_event_t *eh) { LOG_DBG(""); - const struct zmk_position_state_changed *ev = as_zmk_position_state_changed(eh); - if (ev != NULL) { - if (ev->state) { - return zmk_split_bt_position_pressed(ev->position); - } else { - return zmk_split_bt_position_released(ev->position); + const struct zmk_position_state_changed *pos_ev; + if ((pos_ev = as_zmk_position_state_changed(eh)) != NULL) { + if (pos_ev != NULL) { + if (pos_ev->state) { + return zmk_split_bt_position_pressed(pos_ev->position); + } else { + return zmk_split_bt_position_released(pos_ev->position); + } } } + +#if ZMK_KEYMAP_HAS_SENSORS + const struct zmk_sensor_event *sensor_ev; + if ((sensor_ev = as_zmk_sensor_event(eh)) != NULL) { + if (sensor_ev != NULL) { + return zmk_split_bt_sensor_triggered(sensor_ev->sensor_number, sensor_ev->value); + } + } +#endif /* ZMK_KEYMAP_HAS_SENSORS */ return ZMK_EV_EVENT_BUBBLE; } ZMK_LISTENER(split_listener, split_listener); -ZMK_SUBSCRIPTION(split_listener, zmk_position_state_changed); \ No newline at end of file +ZMK_SUBSCRIPTION(split_listener, zmk_position_state_changed); + +#if ZMK_KEYMAP_HAS_SENSORS +ZMK_SUBSCRIPTION(split_listener, zmk_sensor_event); +#endif /* ZMK_KEYMAP_HAS_SENSORS */