12 files changed, 474 insertions, 129 deletions

diff --git a/app/src/battery.c b/app/src/battery.c
new file mode 100644
index 0000000..9496570
--- /dev/null
+++ b/app/src/battery.c
@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2020 The ZMK Contributors
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#include <device.h>
+#include <init.h>
+#include <kernel.h>
+#include <drivers/sensor.h>
+#include <bluetooth/services/bas.h>
+
+#include <logging/log.h>
+
+LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
+
+struct device *battery;
+
+static int zmk_battery_update(struct device *battery) {
+    struct sensor_value state_of_charge;
+
+    int rc = sensor_sample_fetch_chan(battery, SENSOR_CHAN_GAUGE_STATE_OF_CHARGE);
+
+    if (rc != 0) {
+        LOG_DBG("Failed to fetch battery values: %d", rc);
+        return rc;
+    }
+
+    rc = sensor_channel_get(battery, SENSOR_CHAN_GAUGE_STATE_OF_CHARGE, &state_of_charge);
+
+    if (rc != 0) {
+        LOG_DBG("Failed to get battery state of charge: %d", rc);
+        return rc;
+    }
+
+    LOG_DBG("Setting BAS GATT battery level to %d.", state_of_charge.val1);
+
+    return bt_gatt_bas_set_battery_level(state_of_charge.val1);
+}
+
+static void zmk_battery_work(struct k_work *work) {
+    int rc = zmk_battery_update(battery);
+
+    if (rc != 0) {
+        LOG_DBG("Failed to update battery value: %d.", rc);
+    }
+}
+
+K_WORK_DEFINE(battery_work, zmk_battery_work);
+
+static void zmk_battery_timer(struct k_timer *timer) { k_work_submit(&battery_work); }
+
+K_TIMER_DEFINE(battery_timer, zmk_battery_timer, NULL);
+
+static int zmk_battery_init(struct device *_arg) {
+    battery = device_get_binding("BATTERY");
+
+    if (battery == NULL) {
+        LOG_DBG("No battery device labelled BATTERY found.");
+        return -ENODEV;
+    }
+
+    int rc = zmk_battery_update(battery);
+
+    if (rc != 0) {
+        LOG_DBG("Failed to update battery value: %d.", rc);
+        return rc;
+    }
+
+    k_timer_start(&battery_timer, K_MINUTES(1), K_MINUTES(1));
+
+    return 0;
+}
+
+SYS_INIT(zmk_battery_init, APPLICATION, CONFIG_APPLICATION_INIT_PRIORITY);
diff --git a/app/src/behaviors/behavior_hold_tap.c b/app/src/behaviors/behavior_hold_tap.c
index 8b3620e..ac91e7d 100644
--- a/app/src/behaviors/behavior_hold_tap.c
+++ b/app/src/behaviors/behavior_hold_tap.c
@@ -8,6 +8,7 @@
 
 #include <device.h>
 #include <drivers/behavior.h>
+#include <dt-bindings/zmk/keys.h>
 #include <logging/log.h>
 #include <zmk/behavior.h>
 #include <zmk/matrix.h>
@@ -16,7 +17,6 @@
 #include <zmk/events/position-state-changed.h>
 #include <zmk/events/keycode-state-changed.h>
 #include <zmk/events/modifiers-state-changed.h>
-#include <zmk/hid.h>
 #include <zmk/behavior.h>
 
 LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
@@ -427,7 +427,7 @@ static int position_state_changed_listener(const struct zmk_event_header *eh) {
     return ZMK_EV_EVENT_CAPTURED;
 }
 
-static bool is_mod(struct keycode_state_changed *ev) {
+static inline bool only_mods(struct keycode_state_changed *ev) {
     return ev->usage_page == USAGE_KEYPAD && ev->keycode >= LCTL && ev->keycode <= RGUI;
 }
 
@@ -440,7 +440,7 @@ static int keycode_state_changed_listener(const struct zmk_event_header *eh) {
         return 0;
     }
 
-    if (!is_mod(ev)) {
+    if (!only_mods(ev)) {
         // LOG_DBG("0x%02X bubble (not a mod)", ev->keycode);
         return 0;
     }
diff --git a/app/src/behaviors/behavior_outputs.c b/app/src/behaviors/behavior_outputs.c
new file mode 100644
index 0000000..e5182bd
--- /dev/null
+++ b/app/src/behaviors/behavior_outputs.c
@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) 2020 The ZMK Contributors
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#define DT_DRV_COMPAT zmk_behavior_outputs
+
+#include <device.h>
+#include <devicetree.h>
+#include <drivers/behavior.h>
+
+#include <dt-bindings/zmk/outputs.h>
+
+#include <zmk/behavior.h>
+#include <zmk/endpoints.h>
+
+#include <logging/log.h>
+LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
+
+static int on_keymap_binding_pressed(struct zmk_behavior_binding *binding,
+                                     struct zmk_behavior_binding_event event) {
+    switch (binding->param1) {
+    case OUT_TOG:
+        return zmk_endpoints_toggle();
+    case OUT_USB:
+        return zmk_endpoints_select(ZMK_ENDPOINT_USB);
+    case OUT_BLE:
+        return zmk_endpoints_select(ZMK_ENDPOINT_BLE);
+    default:
+        LOG_ERR("Unknown output command: %d", binding->param1);
+    }
+
+    return -ENOTSUP;
+}
+
+static int behavior_out_init(struct device *dev) { return 0; }
+
+static const struct behavior_driver_api behavior_outputs_driver_api = {
+    .binding_pressed = on_keymap_binding_pressed,
+};
+
+DEVICE_AND_API_INIT(behavior_out, DT_INST_LABEL(0), behavior_out_init, NULL, NULL, APPLICATION,
+                    CONFIG_KERNEL_INIT_PRIORITY_DEFAULT, &behavior_outputs_driver_api);
diff --git a/app/src/ble.c b/app/src/ble.c
index 9090582..f3962ae 100644
--- a/app/src/ble.c
+++ b/app/src/ble.c
@@ -94,6 +94,12 @@ static void raise_profile_changed_event() {
     ZMK_EVENT_RAISE(ev);
 }
 
+static void raise_profile_changed_event_callback(struct k_work *work) {
+    raise_profile_changed_event();
+}
+
+K_WORK_DEFINE(raise_profile_changed_event_work, raise_profile_changed_event_callback);
+
 static bool active_profile_is_open() {
     return !bt_addr_le_cmp(&profiles[active_profile].peer, BT_ADDR_LE_ANY);
 }
@@ -111,7 +117,7 @@ void set_profile_address(u8_t index, const bt_addr_le_t *addr) {
     raise_profile_changed_event();
 }
 
-bool active_profile_is_connected() {
+bool zmk_ble_active_profile_is_connected() {
     struct bt_conn *conn;
     bt_addr_le_t *addr = zmk_ble_active_profile_addr();
     if (!bt_addr_le_cmp(addr, BT_ADDR_LE_ANY)) {
@@ -163,9 +169,9 @@ int update_advertising() {
     struct bt_conn *conn;
     enum advertising_type desired_adv = ZMK_ADV_NONE;
 
-    if (active_profile_is_open() || !active_profile_is_connected()) {
+    if (active_profile_is_open()) {
         desired_adv = ZMK_ADV_CONN;
-    } else if (!active_profile_is_connected()) {
+    } else if (!zmk_ble_active_profile_is_connected()) {
         desired_adv = ZMK_ADV_CONN;
         // Need to fix directed advertising for privacy centrals. See
         // https://github.com/zephyrproject-rtos/zephyr/pull/14984 char
@@ -327,6 +333,10 @@ static int ble_profiles_handle_set(const char *name, size_t len, settings_read_c
 struct settings_handler profiles_handler = {.name = "ble", .h_set = ble_profiles_handle_set};
 #endif /* IS_ENABLED(CONFIG_SETTINGS) */
 
+static bool is_conn_active_profile(const struct bt_conn *conn) {
+    return bt_addr_le_cmp(bt_conn_get_dst(conn), &profiles[active_profile].peer) == 0;
+}
+
 static void connected(struct bt_conn *conn, u8_t err) {
     char addr[BT_ADDR_LE_STR_LEN];
     bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
@@ -352,6 +362,11 @@ static void connected(struct bt_conn *conn, u8_t err) {
     }
 
     update_advertising();
+
+    if (is_conn_active_profile(conn)) {
+        LOG_DBG("Active profile connected");
+        raise_profile_changed_event();
+    }
 }
 
 static void disconnected(struct bt_conn *conn, u8_t reason) {
@@ -364,6 +379,11 @@ static void disconnected(struct bt_conn *conn, u8_t reason) {
     // We need to do this in a work callback, otherwise the advertising update will still see the
     // connection for a profile as active, and not start advertising yet.
     k_work_submit(&update_advertising_work);
+
+    if (is_conn_active_profile(conn)) {
+        LOG_DBG("Active profile disconnected");
+        k_work_submit(&raise_profile_changed_event_work);
+    }
 }
 
 static void security_changed(struct bt_conn *conn, bt_security_t level, enum bt_security_err err) {
diff --git a/app/src/endpoints.c b/app/src/endpoints.c
index 79d294e..0c79589 100644
--- a/app/src/endpoints.c
+++ b/app/src/endpoints.c
@@ -4,58 +4,238 @@
  * SPDX-License-Identifier: MIT
  */
 
+#include <init.h>
+#include <settings/settings.h>
+
+#include <zmk/ble.h>
 #include <zmk/endpoints.h>
 #include <zmk/hid.h>
 #include <zmk/usb.h>
 #include <zmk/hog.h>
+#include <zmk/event-manager.h>
+#include <zmk/events/ble-active-profile-changed.h>
+#include <zmk/events/usb-conn-state-changed.h>
 
 #include <logging/log.h>
 LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
 
-int zmk_endpoints_send_report(u8_t usage_page) {
-    int err;
-    struct zmk_hid_keypad_report *keypad_report;
-    struct zmk_hid_consumer_report *consumer_report;
-    LOG_DBG("usage page 0x%02X", usage_page);
-    switch (usage_page) {
-    case USAGE_KEYPAD:
-        keypad_report = zmk_hid_get_keypad_report();
-#ifdef CONFIG_ZMK_USB
-        if (zmk_usb_hid_send_report((u8_t *)keypad_report, sizeof(struct zmk_hid_keypad_report)) !=
-            0) {
-            LOG_DBG("USB Send Failed");
+#define DEFAULT_ENDPOINT                                                                           \
+    COND_CODE_1(IS_ENABLED(CONFIG_ZMK_BLE), (ZMK_ENDPOINT_BLE), (ZMK_ENDPOINT_USB))
+
+static enum zmk_endpoint current_endpoint = DEFAULT_ENDPOINT;
+static enum zmk_endpoint preferred_endpoint =
+    ZMK_ENDPOINT_USB; /* Used if multiple endpoints are ready */
+
+static void update_current_endpoint();
+
+int zmk_endpoints_select(enum zmk_endpoint endpoint) {
+    LOG_DBG("Selected endpoint %d", endpoint);
+
+    if (preferred_endpoint == endpoint) {
+        return 0;
+    }
+
+    preferred_endpoint = endpoint;
+
+#if IS_ENABLED(CONFIG_SETTINGS)
+    settings_save_one("endpoints/preferred", &preferred_endpoint, sizeof(preferred_endpoint));
+#endif
+
+    update_current_endpoint();
+
+    return 0;
+}
+
+int zmk_endpoints_toggle() {
+    enum zmk_endpoint new_endpoint =
+        (preferred_endpoint == ZMK_ENDPOINT_USB) ? ZMK_ENDPOINT_BLE : ZMK_ENDPOINT_USB;
+    return zmk_endpoints_select(new_endpoint);
+}
+
+static int send_keypad_report() {
+    struct zmk_hid_keypad_report *keypad_report = zmk_hid_get_keypad_report();
+
+    switch (current_endpoint) {
+#if IS_ENABLED(CONFIG_ZMK_USB)
+    case ZMK_ENDPOINT_USB: {
+        int err = zmk_usb_hid_send_report((u8_t *)keypad_report, sizeof(*keypad_report));
+        if (err) {
+            LOG_ERR("FAILED TO SEND OVER USB: %d", err);
         }
-#endif /* CONFIG_ZMK_USB */
+        return err;
+    }
+#endif /* IS_ENABLED(CONFIG_ZMK_USB) */
 
-#ifdef CONFIG_ZMK_BLE
-        err = zmk_hog_send_keypad_report(&keypad_report->body);
+#if IS_ENABLED(CONFIG_ZMK_BLE)
+    case ZMK_ENDPOINT_BLE: {
+        int err = zmk_hog_send_keypad_report(&keypad_report->body);
         if (err) {
             LOG_ERR("FAILED TO SEND OVER HOG: %d", err);
         }
-#endif /* CONFIG_ZMK_BLE */
+        return err;
+    }
+#endif /* IS_ENABLED(CONFIG_ZMK_BLE) */
 
-        break;
-    case USAGE_CONSUMER:
-        consumer_report = zmk_hid_get_consumer_report();
-#ifdef CONFIG_ZMK_USB
-        if (zmk_usb_hid_send_report((u8_t *)consumer_report,
-                                    sizeof(struct zmk_hid_consumer_report)) != 0) {
-            LOG_DBG("USB Send Failed");
+    default:
+        LOG_ERR("Unsupported endpoint %d", current_endpoint);
+        return -ENOTSUP;
+    }
+}
+
+static int send_consumer_report() {
+    struct zmk_hid_consumer_report *consumer_report = zmk_hid_get_consumer_report();
+
+    switch (current_endpoint) {
+#if IS_ENABLED(CONFIG_ZMK_USB)
+    case ZMK_ENDPOINT_USB: {
+        int err = zmk_usb_hid_send_report((u8_t *)consumer_report, sizeof(*consumer_report));
+        if (err) {
+            LOG_ERR("FAILED TO SEND OVER USB: %d", err);
         }
-#endif /* CONFIG_ZMK_USB */
+        return err;
+    }
+#endif /* IS_ENABLED(CONFIG_ZMK_USB) */
 
-#ifdef CONFIG_ZMK_BLE
-        err = zmk_hog_send_consumer_report(&consumer_report->body);
+#if IS_ENABLED(CONFIG_ZMK_BLE)
+    case ZMK_ENDPOINT_BLE: {
+        int err = zmk_hog_send_consumer_report(&consumer_report->body);
         if (err) {
             LOG_ERR("FAILED TO SEND OVER HOG: %d", err);
         }
-#endif /* CONFIG_ZMK_BLE */
+        return err;
+    }
+#endif /* IS_ENABLED(CONFIG_ZMK_BLE) */
 
-        break;
+    default:
+        LOG_ERR("Unsupported endpoint %d", current_endpoint);
+        return -ENOTSUP;
+    }
+}
+
+int zmk_endpoints_send_report(u8_t usage_page) {
+
+    LOG_DBG("usage page 0x%02X", usage_page);
+    switch (usage_page) {
+    case USAGE_KEYPAD:
+        return send_keypad_report();
+    case USAGE_CONSUMER:
+        return send_consumer_report();
     default:
         LOG_ERR("Unsupported usage page %d", usage_page);
         return -ENOTSUP;
     }
+}
+
+#if IS_ENABLED(CONFIG_SETTINGS)
+
+static int endpoints_handle_set(const char *name, size_t len, settings_read_cb read_cb,
+                                void *cb_arg) {
+    LOG_DBG("Setting endpoint value %s", log_strdup(name));
+
+    if (settings_name_steq(name, "preferred", NULL)) {
+        if (len != sizeof(enum zmk_endpoint)) {
+            LOG_ERR("Invalid endpoint size (got %d expected %d)", len, sizeof(enum zmk_endpoint));
+            return -EINVAL;
+        }
+
+        int err = read_cb(cb_arg, &preferred_endpoint, sizeof(enum zmk_endpoint));
+        if (err <= 0) {
+            LOG_ERR("Failed to read preferred endpoint from settings (err %d)", err);
+            return err;
+        }
+
+        update_current_endpoint();
+    }
+
+    return 0;
+}
+
+struct settings_handler endpoints_handler = {.name = "endpoints", .h_set = endpoints_handle_set};
+#endif /* IS_ENABLED(CONFIG_SETTINGS) */
+
+static int zmk_endpoints_init(struct device *_arg) {
+#if IS_ENABLED(CONFIG_SETTINGS)
+    settings_subsys_init();
+
+    int err = settings_register(&endpoints_handler);
+    if (err) {
+        LOG_ERR("Failed to register the endpoints settings handler (err %d)", err);
+        return err;
+    }
+
+    settings_load();
+#endif
 
     return 0;
 }
+
+static bool is_usb_ready() {
+#if IS_ENABLED(CONFIG_ZMK_USB)
+    return zmk_usb_is_hid_ready();
+#else
+    return false;
+#endif
+}
+
+static bool is_ble_ready() {
+#if IS_ENABLED(CONFIG_ZMK_BLE)
+    return zmk_ble_active_profile_is_connected();
+#else
+    return false;
+#endif
+}
+
+static enum zmk_endpoint get_selected_endpoint() {
+    if (is_ble_ready()) {
+        if (is_usb_ready()) {
+            LOG_DBG("Both endpoints are ready. Using %d", preferred_endpoint);
+            return preferred_endpoint;
+        }
+
+        LOG_DBG("Only BLE is ready.");
+        return ZMK_ENDPOINT_BLE;
+    }
+
+    if (is_usb_ready()) {
+        LOG_DBG("Only USB is ready.");
+        return ZMK_ENDPOINT_USB;
+    }
+
+    LOG_DBG("No endpoints are ready.");
+    return DEFAULT_ENDPOINT;
+}
+
+static void disconnect_current_endpoint() {
+    zmk_hid_keypad_clear();
+    zmk_hid_consumer_clear();
+
+    zmk_endpoints_send_report(USAGE_KEYPAD);
+    zmk_endpoints_send_report(USAGE_CONSUMER);
+}
+
+static void update_current_endpoint() {
+    enum zmk_endpoint new_endpoint = get_selected_endpoint();
+
+    if (new_endpoint != current_endpoint) {
+        /* Cancel all current keypresses so keys don't stay held on the old endpoint. */
+        disconnect_current_endpoint();
+
+        current_endpoint = new_endpoint;
+        LOG_INF("Endpoint changed: %d", current_endpoint);
+    }
+}
+
+static int endpoint_listener(const struct zmk_event_header *eh) {
+    update_current_endpoint();
+    return 0;
+}
+
+ZMK_LISTENER(endpoint_listener, endpoint_listener);
+#if IS_ENABLED(CONFIG_ZMK_USB)
+ZMK_SUBSCRIPTION(endpoint_listener, usb_conn_state_changed);
+#endif
+#if IS_ENABLED(CONFIG_ZMK_BLE)
+ZMK_SUBSCRIPTION(endpoint_listener, ble_active_profile_changed);
+#endif
+
+SYS_INIT(zmk_endpoints_init, APPLICATION, CONFIG_APPLICATION_INIT_PRIORITY);
diff --git a/app/src/events/usb_conn_state_changed.c b/app/src/events/usb_conn_state_changed.c
new file mode 100644
index 0000000..d845f6d
--- /dev/null
+++ b/app/src/events/usb_conn_state_changed.c
@@ -0,0 +1,10 @@
+/*
+ * Copyright (c) 2020 The ZMK Contributors
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#include <kernel.h>
+#include <zmk/events/usb-conn-state-changed.h>
+
+ZMK_EVENT_IMPL(usb_conn_state_changed);
\ No newline at end of file
diff --git a/app/src/hid.c b/app/src/hid.c
index f80906c..1925765 100644
--- a/app/src/hid.c
+++ b/app/src/hid.c
@@ -8,53 +8,58 @@
 LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
 
 #include <zmk/hid.h>
+#include <dt-bindings/zmk/modifiers.h>
 
 static struct zmk_hid_keypad_report kp_report = {
-    .report_id = 1, .body = {.modifiers = 0, .keys = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}}};
+    .report_id = 1, .body = {.modifiers = 0, ._reserved = 0, .keys = {0}}};
 
-static struct zmk_hid_consumer_report consumer_report = {.report_id = 2,
-                                                         .body = {.keys = {0, 0, 0, 0, 0, 0}}};
+static struct zmk_hid_consumer_report consumer_report = {.report_id = 2, .body = {.keys = {0}}};
 
-#define _TOGGLE_MOD(mod, state)                                                                    \
-    if (modifier > MOD_RGUI) {                                                                     \
-        return -EINVAL;                                                                            \
-    }                                                                                              \
-    WRITE_BIT(kp_report.body.modifiers, mod, state);                                               \
-    return 0;
+// Keep track of how often a modifier was pressed.
+// Only release the modifier if the count is 0.
+static int explicit_modifier_counts[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+static zmk_mod_flags explicit_modifiers = 0;
 
-int zmk_hid_register_mod(zmk_mod modifier) { _TOGGLE_MOD(modifier, true); }
-int zmk_hid_unregister_mod(zmk_mod modifier) { _TOGGLE_MOD(modifier, false); }
+#define SET_MODIFIERS(mods)                                                                        \
+    {                                                                                              \
+        kp_report.body.modifiers = mods;                                                           \
+        LOG_DBG("Modifiers set to 0x%02X", kp_report.body.modifiers);                              \
+    }
 
-int zmk_hid_register_mods(zmk_mod_flags modifiers) {
-    kp_report.body.modifiers |= modifiers;
+int zmk_hid_register_mod(zmk_mod modifier) {
+    explicit_modifier_counts[modifier]++;
+    LOG_DBG("Modifier %d count %d", modifier, explicit_modifier_counts[modifier]);
+    WRITE_BIT(explicit_modifiers, modifier, true);
+    SET_MODIFIERS(explicit_modifiers);
     return 0;
 }
 
-int zmk_hid_unregister_mods(zmk_mod_flags modifiers) {
-    kp_report.body.modifiers &= ~modifiers;
+int zmk_hid_unregister_mod(zmk_mod modifier) {
+    if (explicit_modifier_counts[modifier] <= 0) {
+        LOG_ERR("Tried to unregister modifier %d too often", modifier);
+        return -EINVAL;
+    }
+    explicit_modifier_counts[modifier]--;
+    LOG_DBG("Modifier %d count: %d", modifier, explicit_modifier_counts[modifier]);
+    if (explicit_modifier_counts[modifier] == 0) {
+        LOG_DBG("Modifier %d released", modifier);
+        WRITE_BIT(explicit_modifiers, modifier, false);
+    }
+    SET_MODIFIERS(explicit_modifiers);
     return 0;
 }
 
-#define KEY_OFFSET 0x02
-#define MAX_KEYS 6
-
-/*
-#define TOGGLE_BOOT_KEY(match, val)                      \
-    for (int idx = 0; idx < MAX_KEYS; idx++)             \
-    {                                                    \
-        if (kp_report.boot.keys[idx + KEY_OFFSET] != match) \
-        {                                                \
-            continue;                                    \
-        }                                                \
-        kp_report.boot.keys[idx + KEY_OFFSET] = val;        \
-        break;                                           \
+#define TOGGLE_KEYPAD(match, val)                                                                  \
+    for (int idx = 0; idx < ZMK_HID_KEYPAD_NKRO_SIZE; idx++) {                                     \
+        if (kp_report.body.keys[idx] != match) {                                                   \
+            continue;                                                                              \
+        }                                                                                          \
+        kp_report.body.keys[idx] = val;                                                            \
+        break;                                                                                     \
     }
-*/
-
-#define TOGGLE_KEY(code, val) WRITE_BIT(kp_report.body.keys[code / 8], code % 8, val)
 
 #define TOGGLE_CONSUMER(match, val)                                                                \
-    for (int idx = 0; idx < MAX_KEYS; idx++) {                                                     \
+    for (int idx = 0; idx < ZMK_HID_CONSUMER_NKRO_SIZE; idx++) {                                   \
         if (consumer_report.body.keys[idx] != match) {                                             \
             continue;                                                                              \
         }                                                                                          \
@@ -62,19 +67,21 @@ int zmk_hid_unregister_mods(zmk_mod_flags modifiers) {
         break;                                                                                     \
     }
 
+int zmk_hid_implicit_modifiers_press(zmk_mod_flags implicit_modifiers) {
+    SET_MODIFIERS(explicit_modifiers | implicit_modifiers);
+    return 0;
+}
+
+int zmk_hid_implicit_modifiers_release() {
+    SET_MODIFIERS(explicit_modifiers);
+    return 0;
+}
+
 int zmk_hid_keypad_press(zmk_key code) {
     if (code >= LCTL && code <= RGUI) {
         return zmk_hid_register_mod(code - LCTL);
     }
-
-    if (code > ZMK_HID_MAX_KEYCODE) {
-        return -EINVAL;
-    }
-
-    // TOGGLE_BOOT_KEY(0U, code);
-
-    TOGGLE_KEY(code, true);
-
+    TOGGLE_KEYPAD(0U, code);
     return 0;
 };
 
@@ -82,18 +89,12 @@ int zmk_hid_keypad_release(zmk_key code) {
     if (code >= LCTL && code <= RGUI) {
         return zmk_hid_unregister_mod(code - LCTL);
     }
-
-    if (code > ZMK_HID_MAX_KEYCODE) {
-        return -EINVAL;
-    }
-
-    // TOGGLE_BOOT_KEY(0U, code);
-
-    TOGGLE_KEY(code, false);
-
+    TOGGLE_KEYPAD(code, 0U);
     return 0;
 };
 
+void zmk_hid_keypad_clear() { memset(&kp_report.body, 0, sizeof(kp_report.body)); }
+
 int zmk_hid_consumer_press(zmk_key code) {
     TOGGLE_CONSUMER(0U, code);
     return 0;
@@ -104,6 +105,8 @@ int zmk_hid_consumer_release(zmk_key code) {
     return 0;
 };
 
+void zmk_hid_consumer_clear() { memset(&consumer_report.body, 0, sizeof(consumer_report.body)); }
+
 struct zmk_hid_keypad_report *zmk_hid_get_keypad_report() {
     return &kp_report;
 }
diff --git a/app/src/hid_listener.c b/app/src/hid_listener.c
index 4467e6d..32e9d97 100644
--- a/app/src/hid_listener.c
+++ b/app/src/hid_listener.c
@@ -15,10 +15,11 @@ LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
 #include <zmk/hid.h>
 #include <zmk/endpoints.h>
 
-static int hid_listener_keycode_pressed(u8_t usage_page, u32_t keycode) {
+static int hid_listener_keycode_pressed(u8_t usage_page, u32_t keycode,
+                                        zmk_mod_flags implicit_modifiers) {
     int err;
-    LOG_DBG("usage_page 0x%02X keycode 0x%02X", usage_page, keycode);
-
+    LOG_DBG("usage_page 0x%02X keycode 0x%02X mods 0x%02X", usage_page, keycode,
+            implicit_modifiers);
     switch (usage_page) {
     case USAGE_KEYPAD:
         err = zmk_hid_keypad_press(keycode);
@@ -35,14 +36,15 @@ static int hid_listener_keycode_pressed(u8_t usage_page, u32_t keycode) {
         }
         break;
     }
-
+    zmk_hid_implicit_modifiers_press(implicit_modifiers);
     return zmk_endpoints_send_report(usage_page);
 }
 
-static int hid_listener_keycode_released(u8_t usage_page, u32_t keycode) {
+static int hid_listener_keycode_released(u8_t usage_page, u32_t keycode,
+                                         zmk_mod_flags implicit_modifiers) {
     int err;
-    LOG_DBG("usage_page 0x%02X keycode 0x%02X", usage_page, keycode);
-
+    LOG_DBG("usage_page 0x%02X keycode 0x%02X mods 0x%02X", usage_page, keycode,
+            implicit_modifiers);
     switch (usage_page) {
     case USAGE_KEYPAD:
         err = zmk_hid_keypad_release(keycode);
@@ -57,44 +59,27 @@ static int hid_listener_keycode_released(u8_t usage_page, u32_t keycode) {
             LOG_ERR("Unable to release keycode");
             return err;
         }
-        break;
     }
+    // There is a minor issue with this code.
+    // If LC(A) is pressed, then LS(B), then LC(A) is released, the shift for B will be released
+    // prematurely. This causes if LS(B) to repeat like Bbbbbbbb when pressed for a long time.
+    // Solving this would require keeping track of which key's implicit modifiers are currently
+    // active and only releasing modifiers at that time.
+    zmk_hid_implicit_modifiers_release();
     return zmk_endpoints_send_report(usage_page);
 }
 
-static int hid_listener_modifiers_pressed(zmk_mod_flags modifiers) {
-    LOG_DBG("modifiers %d", modifiers);
-
-    zmk_hid_register_mods(modifiers);
-    return zmk_endpoints_send_report(USAGE_KEYPAD);
-}
-
-static int hid_listener_modifiers_released(zmk_mod_flags modifiers) {
-    LOG_DBG("modifiers %d", modifiers);
-
-    zmk_hid_unregister_mods(modifiers);
-    return zmk_endpoints_send_report(USAGE_KEYPAD);
-}
-
 int hid_listener(const struct zmk_event_header *eh) {
     if (is_keycode_state_changed(eh)) {
         const struct keycode_state_changed *ev = cast_keycode_state_changed(eh);
         if (ev->state) {
-            hid_listener_keycode_pressed(ev->usage_page, ev->keycode);
-        } else {
-            hid_listener_keycode_released(ev->usage_page, ev->keycode);
-        }
-    } else if (is_modifiers_state_changed(eh)) {
-        const struct modifiers_state_changed *ev = cast_modifiers_state_changed(eh);
-        if (ev->state) {
-            hid_listener_modifiers_pressed(ev->modifiers);
+            hid_listener_keycode_pressed(ev->usage_page, ev->keycode, ev->implicit_modifiers);
         } else {
-            hid_listener_modifiers_released(ev->modifiers);
+            hid_listener_keycode_released(ev->usage_page, ev->keycode, ev->implicit_modifiers);
         }
     }
     return 0;
 }
 
 ZMK_LISTENER(hid_listener, hid_listener);
-ZMK_SUBSCRIPTION(hid_listener, keycode_state_changed);
-ZMK_SUBSCRIPTION(hid_listener, modifiers_state_changed);
\ No newline at end of file
+ZMK_SUBSCRIPTION(hid_listener, keycode_state_changed);
\ No newline at end of file
diff --git a/app/src/keymap.c b/app/src/keymap.c
index 74fe60d..1d289e5 100644
--- a/app/src/keymap.c
+++ b/app/src/keymap.c
@@ -11,7 +11,6 @@ LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
 #include <zmk/matrix.h>
 #include <zmk/sensors.h>
 #include <zmk/keymap.h>
-#include <dt-bindings/zmk/matrix-transform.h>
 #include <drivers/behavior.h>
 #include <zmk/behavior.h>
 
diff --git a/app/src/power.c b/app/src/power.c
index 73b3f12..bad54d2 100644
--- a/app/src/power.c
+++ b/app/src/power.c
@@ -23,14 +23,7 @@ static u32_t power_last_uptime;
 
 bool is_usb_power_present() {
 #ifdef CONFIG_USB
-    enum usb_dc_status_code usb_status = zmk_usb_get_status();
-    switch (usb_status) {
-    case USB_DC_DISCONNECTED:
-    case USB_DC_UNKNOWN:
-        return false;
-    default:
-        return true;
-    }
+    return zmk_usb_is_powered();
 #else
     return false;
 #endif /* CONFIG_USB */
diff --git a/app/src/rgb_underglow.c b/app/src/rgb_underglow.c
index b371c94..084482e 100644
--- a/app/src/rgb_underglow.c
+++ b/app/src/rgb_underglow.c
@@ -227,6 +227,14 @@ static void zmk_rgb_underglow_tick_handler(struct k_timer *timer) {
 
 K_TIMER_DEFINE(underglow_tick, zmk_rgb_underglow_tick_handler, NULL);
 
+#if IS_ENABLED(CONFIG_SETTINGS)
+static void zmk_rgb_underglow_save_state_work() {
+    settings_save_one("rgb/underglow/state", &state, sizeof(state));
+}
+
+static struct k_delayed_work underglow_save_work;
+#endif
+
 static int zmk_rgb_underglow_init(struct device *_arg) {
     led_strip = device_get_binding(STRIP_LABEL);
     if (led_strip) {
@@ -248,6 +256,7 @@ static int zmk_rgb_underglow_init(struct device *_arg) {
 
 #if IS_ENABLED(CONFIG_SETTINGS)
     settings_register(&rgb_conf);
+    k_delayed_work_init(&underglow_save_work, zmk_rgb_underglow_save_state_work);
 #endif
 
     k_timer_start(&underglow_tick, K_NO_WAIT, K_MSEC(50));
@@ -257,7 +266,8 @@ static int zmk_rgb_underglow_init(struct device *_arg) {
 
 int zmk_rgb_underglow_save_state() {
 #if IS_ENABLED(CONFIG_SETTINGS)
-    return settings_save_one("rgb/underglow/state", &state, sizeof(state));
+    k_delayed_work_cancel(&underglow_save_work);
+    return k_delayed_work_submit(&underglow_save_work, K_MINUTES(1));
 #else
     return 0;
 #endif
diff --git a/app/src/usb.c b/app/src/usb.c
index 434b3d4..79d03c7 100644
--- a/app/src/usb.c
+++ b/app/src/usb.c
@@ -9,10 +9,11 @@
 
 #include <usb/usb_device.h>
 #include <usb/class/usb_hid.h>
-#include <dt-bindings/zmk/keys.h>
 
 #include <zmk/hid.h>
 #include <zmk/keymap.h>
+#include <zmk/event-manager.h>
+#include <zmk/events/usb-conn-state-changed.h>
 
 LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
 
@@ -53,9 +54,34 @@ int zmk_usb_hid_send_report(const u8_t *report, size_t len) {
 
 #endif /* CONFIG_ZMK_USB */
 
+static void raise_usb_status_changed_event() {
+    struct usb_conn_state_changed *ev = new_usb_conn_state_changed();
+    ev->conn_state = zmk_usb_get_conn_state();
+
+    ZMK_EVENT_RAISE(ev);
+}
+
 enum usb_dc_status_code zmk_usb_get_status() { return usb_status; }
 
-void usb_status_cb(enum usb_dc_status_code status, const u8_t *params) { usb_status = status; };
+enum zmk_usb_conn_state zmk_usb_get_conn_state() {
+    switch (usb_status) {
+    case USB_DC_DISCONNECTED:
+    case USB_DC_UNKNOWN:
+        return ZMK_USB_CONN_NONE;
+
+    case USB_DC_ERROR:
+    case USB_DC_RESET:
+        return ZMK_USB_CONN_POWERED;
+
+    default:
+        return ZMK_USB_CONN_HID;
+    }
+}
+
+void usb_status_cb(enum usb_dc_status_code status, const u8_t *params) {
+    usb_status = status;
+    raise_usb_status_changed_event();
+};
 
 static int zmk_usb_init(struct device *_arg) {
     int usb_enable_ret;