From df087460eb71567fbd162355ff47f0edb878991a Mon Sep 17 00:00:00 2001 From: Stephen Wan Date: Mon, 15 Mar 2021 12:26:18 -0700 Subject: [PATCH] feature(split): add support for sensors from peripheral 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. Co-authored-by: Peter Johanson --- app/include/zmk/sensors.h | 6 +- app/include/zmk/split/bluetooth/service.h | 15 ++++- app/include/zmk/split/bluetooth/uuid.h | 1 + app/src/sensors.c | 6 +- app/src/split/bluetooth/central.c | 79 ++++++++++++++++++++--- app/src/split/bluetooth/service.c | 78 +++++++++++++++++++++- app/src/split/bluetooth/split_listener.c | 26 ++++++-- 7 files changed, 187 insertions(+), 24 deletions(-) diff --git a/app/include/zmk/sensors.h b/app/include/zmk/sensors.h index 8ac1c283..1919d4ce 100644 --- a/app/include/zmk/sensors.h +++ b/app/include/zmk/sensors.h @@ -24,7 +24,9 @@ struct zmk_sensor_config { uint16_t triggers_per_rotation; }; +// This struct is also used for data transfer for splits, so any changes to the size, layout, etc +// is a breaking change for the split GATT service protocol. struct zmk_sensor_channel_data { - enum sensor_channel channel; struct sensor_value value; -}; + enum sensor_channel channel; +} __packed; diff --git a/app/include/zmk/split/bluetooth/service.h b/app/include/zmk/split/bluetooth/service.h index f0c1d79f..7b088535 100644 --- a/app/include/zmk/split/bluetooth/service.h +++ b/app/include/zmk/split/bluetooth/service.h @@ -6,8 +6,18 @@ #pragma once +#include +#include + #define ZMK_SPLIT_RUN_BEHAVIOR_DEV_LEN 9 +struct sensor_event { + uint8_t sensor_index; + + uint8_t channel_data_size; + struct zmk_sensor_channel_data channel_data[ZMK_SENSOR_EVENT_MAX_CHANNELS]; +} __packed; + struct zmk_split_run_behavior_data { uint8_t position; uint8_t state; @@ -21,4 +31,7 @@ struct zmk_split_run_behavior_payload { } __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, + const struct zmk_sensor_channel_data channel_data[], + size_t channel_data_size); diff --git a/app/include/zmk/split/bluetooth/uuid.h b/app/include/zmk/split/bluetooth/uuid.h index cbdb1772..c38131dd 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(0x00000003) diff --git a/app/src/sensors.c b/app/src/sensors.c index e339afe0..60f2bd2a 100644 --- a/app/src/sensors.c +++ b/app/src/sensors.c @@ -29,7 +29,7 @@ struct sensors_item_cfg { { \ .dev = DEVICE_DT_GET_OR_NULL(node), \ .trigger = {.type = SENSOR_TRIG_DATA_READY, .chan = SENSOR_CHAN_ROTATION}, \ - .config = &configs[idx] \ + .config = &configs[idx], .sensor_index = idx \ } #define SENSOR_ITEM(idx, _i) _SENSOR_ITEM(idx, ZMK_KEYMAP_SENSORS_BY_IDX(idx)) @@ -112,7 +112,7 @@ static void zmk_sensors_trigger_handler(const struct device *dev, int sensor_index = test_item - sensors; if (sensor_index < 0 || sensor_index >= ARRAY_SIZE(sensors)) { - LOG_ERR("Invalid sensor item triggered our callback"); + LOG_ERR("Invalid sensor item triggered our callback (%d)", sensor_index); return; } @@ -127,8 +127,6 @@ static void zmk_sensors_trigger_handler(const struct device *dev, static void zmk_sensors_init_item(uint8_t i) { LOG_DBG("Init sensor at index %d", i); - sensors[i].sensor_index = i; - if (!sensors[i].dev) { LOG_DBG("No local device for %d", i); return; diff --git a/app/src/split/bluetooth/central.c b/app/src/split/bluetooth/central.c index 8a5e9d35..2581587b 100644 --- a/app/src/split/bluetooth/central.c +++ b/app/src/split/bluetooth/central.c @@ -21,10 +21,12 @@ LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL); #include #include #include +#include #include #include #include #include +#include static int start_scanning(void); @@ -41,6 +43,7 @@ struct peripheral_slot { struct bt_conn *conn; struct bt_gatt_discover_params discover_params; struct bt_gatt_subscribe_params subscribe_params; + struct bt_gatt_subscribe_params sensor_subscribe_params; struct bt_gatt_discover_params sub_discover_params; uint16_t run_behavior_handle; uint8_t position_state[POSITION_STATE_DATA_LEN]; @@ -165,6 +168,46 @@ 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_index); + ZMK_EVENT_RAISE(new_zmk_sensor_event(ev)); + } +} + +K_WORK_DEFINE(peripheral_sensor_event_work, peripheral_sensor_event_work_callback); + +static uint8_t split_central_sensor_notify_func(struct bt_conn *conn, + struct bt_gatt_subscribe_params *params, + const void *data, uint16_t length) { + 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 sensor_event sensor_event; + memcpy(&sensor_event, data, MIN(length, sizeof(sensor_event))); + struct zmk_sensor_event ev = {.sensor_index = sensor_event.sensor_index, + .channel_data_size = sensor_event.channel_data_size, + .timestamp = k_uptime_get()}; + + memcpy(&ev.channel_data, sensor_event.channel_data, + sizeof(struct zmk_sensor_channel_data) * sensor_event.channel_data_size); + 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) { @@ -209,14 +252,8 @@ static uint8_t split_central_notify_func(struct bt_conn *conn, return BT_GATT_ITER_CONTINUE; } -static void split_central_subscribe(struct bt_conn *conn) { - 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); +static int split_central_subscribe(struct bt_conn *conn, struct bt_gatt_subscribe_params *params) { + int err = bt_gatt_subscribe(conn, params); switch (err) { case -EALREADY: LOG_DBG("[ALREADY SUBSCRIBED]"); @@ -228,6 +265,8 @@ static void split_central_subscribe(struct bt_conn *conn) { LOG_ERR("Subscribe failed (err %d)", err); break; } + + return err; } static uint8_t split_central_chrc_discovery_func(struct bt_conn *conn, @@ -263,14 +302,34 @@ static uint8_t split_central_chrc_discovery_func(struct bt_conn *conn, 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); + split_central_subscribe(conn, &slot->subscribe_params); +#if ZMK_KEYMAP_HAS_SENSORS + } else if (bt_uuid_cmp(((struct bt_gatt_chrc *)attr->user_data)->uuid, + BT_UUID_DECLARE_128(ZMK_SPLIT_BT_CHAR_SENSOR_STATE_UUID)) == 0) { + slot->discover_params.uuid = NULL; + slot->discover_params.start_handle = attr->handle + 2; + slot->discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC; + + slot->sensor_subscribe_params.disc_params = &slot->sub_discover_params; + slot->sensor_subscribe_params.end_handle = slot->discover_params.end_handle; + slot->sensor_subscribe_params.value_handle = bt_gatt_attr_value_handle(attr); + slot->sensor_subscribe_params.notify = split_central_sensor_notify_func; + slot->sensor_subscribe_params.value = BT_GATT_CCC_NOTIFY; + split_central_subscribe(conn, &slot->sensor_subscribe_params); +#endif /* ZMK_KEYMAP_HAS_SENSORS */ } else if (bt_uuid_cmp(((struct bt_gatt_chrc *)attr->user_data)->uuid, BT_UUID_DECLARE_128(ZMK_SPLIT_BT_CHAR_RUN_BEHAVIOR_UUID)) == 0) { LOG_DBG("Found run behavior handle"); + slot->discover_params.uuid = NULL; + slot->discover_params.start_handle = attr->handle + 2; slot->run_behavior_handle = bt_gatt_attr_value_handle(attr); } - bool subscribed = (slot->run_behavior_handle && slot->subscribe_params.value_handle); + bool subscribed = (slot->run_behavior_handle && slot->subscribe_params.value_handle +#if ZMK_KEYMAP_HAS_SENSORS + && slot->sensor_subscribe_params.value_handle +#endif /* ZMK_KEYMAP_HAS_SENSORS */ + ); return subscribed ? BT_GATT_ITER_STOP : BT_GATT_ITER_CONTINUE; } diff --git a/app/src/split/bluetooth/service.c b/app/src/split/bluetooth/service.c index f7b0d587..620df53e 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,22 @@ LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL); #include #include #include +#include +#include + +#if ZMK_KEYMAP_HAS_SENSORS +static struct sensor_event last_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, &last_sensor_event, + sizeof(last_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 +115,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, &last_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 +175,58 @@ 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(struct sensor_event), + CONFIG_ZMK_SPLIT_BLE_PERIPHERAL_POSITION_QUEUE_SIZE, 4); + +void send_sensor_state_callback(struct k_work *work) { + while (k_msgq_get(&sensor_state_msgq, &last_sensor_event, K_NO_WAIT) == 0) { + int err = bt_gatt_notify(NULL, &split_svc.attrs[8], &last_sensor_event, + sizeof(last_sensor_event)); + if (err) { + LOG_DBG("Error notifying %d", err); + } + } +}; + +K_WORK_DEFINE(service_sensor_notify_work, send_sensor_state_callback); + +int send_sensor_state(struct sensor_event ev) { + int err = k_msgq_put(&sensor_state_msgq, &ev, 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(ev); + } + 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_index, + const struct zmk_sensor_channel_data channel_data[], + size_t channel_data_size) { + if (channel_data_size > ZMK_SENSOR_EVENT_MAX_CHANNELS) { + return -EINVAL; + } + + struct sensor_event ev = + (struct sensor_event){.sensor_index = sensor_index, .channel_data_size = channel_data_size}; + memcpy(ev.channel_data, channel_data, + channel_data_size * sizeof(struct zmk_sensor_channel_data)); + return send_sensor_state(ev); +} +#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 eb5398c4..9b680d2c 100644 --- a/app/src/split/bluetooth/split_listener.c +++ b/app/src/split/bluetooth/split_listener.c @@ -13,21 +13,35 @@ 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); + const struct zmk_position_state_changed *pos_ev; + if ((pos_ev = as_zmk_position_state_changed(eh)) != NULL) { + if (pos_ev->state) { + return zmk_split_bt_position_pressed(pos_ev->position); } else { - return zmk_split_bt_position_released(ev->position); + 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) { + return zmk_split_bt_sensor_triggered(sensor_ev->sensor_index, sensor_ev->channel_data, + sensor_ev->channel_data_size); + } +#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 */