From a5c39dfa76eeebd09568ce959cd3dd088498ad3f Mon Sep 17 00:00:00 2001 From: Pete Johanson Date: Thu, 3 Dec 2020 16:04:48 -0500 Subject: [PATCH] fix(ble): Perform GATT notifies from dedicated queue. * Zephyr BT stack frees TX buffers from system workqueue, and to avoid blocking waiting to allocate, perform notify from a dedicated queue. --- app/Kconfig | 18 ++++- app/src/hog.c | 118 ++++++++++++++++++++++++++---- app/src/split/bluetooth/central.c | 5 +- 3 files changed, 123 insertions(+), 18 deletions(-) diff --git a/app/Kconfig b/app/Kconfig index 65abf7a5..7602b9bd 100644 --- a/app/Kconfig +++ b/app/Kconfig @@ -41,7 +41,7 @@ config USB_NUMOF_EP_WRITE_RETRIES #ZMK_USB endif -config ZMK_BLE +menuconfig ZMK_BLE bool "BLE (HID over GATT)" select BT select BT_SMP @@ -58,6 +58,22 @@ if ZMK_BLE config SYSTEM_WORKQUEUE_STACK_SIZE default 2048 +config ZMK_BLE_THREAD_STACK_SIZE + int "BLE notify thread stack size" + default 512 + +config ZMK_BLE_THREAD_PRIORITY + int "BLE notify thread priority" + default 5 + +config ZMK_BLE_KEYBOARD_REPORT_QUEUE_SIZE + int "Max number of keyboard HID reports to queue for sending over BLE" + default 20 + +config ZMK_BLE_CONSUMER_REPORT_QUEUE_SIZE + int "Max number of consumer HID reports to queue for sending over BLE" + default 5 + config ZMK_BLE_CLEAR_BONDS_ON_START bool "Configuration that clears all bond information from the keyboard on startup." default n diff --git a/app/src/hog.c b/app/src/hog.c index 8d10b8f7..07343097 100644 --- a/app/src/hog.c +++ b/app/src/hog.c @@ -5,6 +5,7 @@ */ #include +#include #include @@ -156,28 +157,115 @@ struct bt_conn *destination_connection() { return conn; } +K_THREAD_STACK_DEFINE(hog_q_stack, CONFIG_ZMK_BLE_THREAD_STACK_SIZE); + +struct k_work_q hog_work_q; + +K_MSGQ_DEFINE(zmk_hog_keyboard_msgq, sizeof(struct zmk_hid_keyboard_report_body), + CONFIG_ZMK_BLE_KEYBOARD_REPORT_QUEUE_SIZE, 4); + +void send_keyboard_report_callback(struct k_work *work) { + struct zmk_hid_keyboard_report_body report; + + while (k_msgq_get(&zmk_hog_keyboard_msgq, &report, K_NO_WAIT) == 0) { + struct bt_conn *conn = destination_connection(); + if (conn == NULL) { + return; + } + + struct bt_gatt_notify_params notify_params = { + .attr = &hog_svc.attrs[5], + .data = &report, + .len = sizeof(report), + }; + + int err = bt_gatt_notify_cb(conn, ¬ify_params); + if (err) { + LOG_ERR("Error notifying %d", err); + } + + bt_conn_unref(conn); + } +} + +K_WORK_DEFINE(hog_keyboard_work, send_keyboard_report_callback); + int zmk_hog_send_keyboard_report(struct zmk_hid_keyboard_report_body *report) { - struct bt_conn *conn = destination_connection(); - if (conn == NULL) { - return -ENOTCONN; + int err = k_msgq_put(&zmk_hog_keyboard_msgq, report, K_MSEC(100)); + if (err) { + switch (err) { + case -EAGAIN: { + LOG_WRN("Keyboard message queue full, popping first message and queueing again"); + struct zmk_hid_keyboard_report_body discarded_report; + k_msgq_get(&zmk_hog_keyboard_msgq, &discarded_report, K_NO_WAIT); + return zmk_hog_send_keyboard_report(report); + } + default: + LOG_WRN("Failed to queue keyboard report to send (%d)", err); + return err; + } } - LOG_DBG("Sending to NULL? %s", conn == NULL ? "yes" : "no"); + k_work_submit_to_queue(&hog_work_q, &hog_keyboard_work); - int err = bt_gatt_notify(conn, &hog_svc.attrs[5], report, - sizeof(struct zmk_hid_keyboard_report_body)); - bt_conn_unref(conn); - return err; + return 0; }; +K_MSGQ_DEFINE(zmk_hog_consumer_msgq, sizeof(struct zmk_hid_consumer_report_body), + CONFIG_ZMK_BLE_CONSUMER_REPORT_QUEUE_SIZE, 4); + +void send_consumer_report_callback(struct k_work *work) { + struct zmk_hid_consumer_report_body report; + + while (k_msgq_get(&zmk_hog_consumer_msgq, &report, K_NO_WAIT) == 0) { + struct bt_conn *conn = destination_connection(); + if (conn == NULL) { + return; + } + + struct bt_gatt_notify_params notify_params = { + .attr = &hog_svc.attrs[10], + .data = &report, + .len = sizeof(report), + }; + + int err = bt_gatt_notify_cb(conn, ¬ify_params); + if (err) { + LOG_DBG("Error notifying %d", err); + } + + bt_conn_unref(conn); + } +}; + +K_WORK_DEFINE(hog_consumer_work, send_consumer_report_callback); + int zmk_hog_send_consumer_report(struct zmk_hid_consumer_report_body *report) { - struct bt_conn *conn = destination_connection(); - if (conn == NULL) { - return -ENOTCONN; + int err = k_msgq_put(&zmk_hog_consumer_msgq, report, K_MSEC(100)); + if (err) { + switch (err) { + case -EAGAIN: { + LOG_WRN("Consumer message queue full, popping first message and queueing again"); + struct zmk_hid_consumer_report_body discarded_report; + k_msgq_get(&zmk_hog_consumer_msgq, &discarded_report, K_NO_WAIT); + return zmk_hog_send_consumer_report(report); + } + default: + LOG_WRN("Failed to queue consumer report to send (%d)", err); + return err; + } } - int err = bt_gatt_notify(conn, &hog_svc.attrs[10], report, - sizeof(struct zmk_hid_consumer_report_body)); - bt_conn_unref(conn); - return err; + k_work_submit_to_queue(&hog_work_q, &hog_consumer_work); + + return 0; }; + +int zmk_hog_init(const struct device *_arg) { + k_work_q_start(&hog_work_q, hog_q_stack, K_THREAD_STACK_SIZEOF(hog_q_stack), + CONFIG_ZMK_BLE_THREAD_PRIORITY); + + return 0; +} + +SYS_INIT(zmk_hog_init, APPLICATION, CONFIG_ZMK_BLE_INIT_PRIORITY); diff --git a/app/src/split/bluetooth/central.c b/app/src/split/bluetooth/central.c index 9cce7870..e9dfbac5 100644 --- a/app/src/split/bluetooth/central.c +++ b/app/src/split/bluetooth/central.c @@ -57,8 +57,9 @@ void peripheral_event_work_callback(struct k_work *work) { 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 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]; uint8_t changed_positions[POSITION_STATE_DATA_LEN];