1 files changed, 103 insertions, 15 deletions

diff --git a/app/src/hog.c b/app/src/hog.c
index 8d10b8f..0734309 100644
--- a/app/src/hog.c
+++ b/app/src/hog.c
@@ -5,6 +5,7 @@
  */
 
 #include <settings/settings.h>
+#include <init.h>
 
 #include <logging/log.h>
 
@@ -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, &notify_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, &notify_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);