14 files changed, 943 insertions, 233 deletions

diff --git a/app/src/behaviors/behavior_hold_tap.c b/app/src/behaviors/behavior_hold_tap.c
new file mode 100644
index 0000000..2c6d996
--- /dev/null
+++ b/app/src/behaviors/behavior_hold_tap.c
@@ -0,0 +1,502 @@
+/*
+ * Copyright (c) 2020 Cody McGinnis, Okke Formsma
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#define DT_DRV_COMPAT zmk_behavior_hold_tap
+
+#include <device.h>
+#include <drivers/behavior.h>
+#include <logging/log.h>
+#include <zmk/behavior.h>
+
+#include <zmk/matrix.h>
+#include <zmk/endpoints.h>
+#include <zmk/event-manager.h>
+#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>
+
+LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
+
+#if DT_NODE_EXISTS(DT_DRV_INST(0))
+
+#define ZMK_BHV_HOLD_TAP_MAX_HELD 10
+#define ZMK_BHV_HOLD_TAP_MAX_CAPTURED_EVENTS 40
+
+// increase if you have keyboard with more keys.
+#define ZMK_BHV_HOLD_TAP_POSITION_NOT_USED 9999
+
+
+enum flavor {
+	ZMK_BHV_HOLD_TAP_FLAVOR_HOLD_PREFERRED = 0,
+	ZMK_BHV_HOLD_TAP_FLAVOR_BALANCED = 1,
+	ZMK_BHV_HOLD_TAP_FLAVOR_TAP_PREFERRED = 2,
+};
+
+struct behavior_hold_tap_behaviors {
+	struct zmk_behavior_binding tap;
+	struct zmk_behavior_binding hold;
+};
+
+typedef k_timeout_t (*timer_func)();
+
+struct behavior_hold_tap_config {
+	timer_func tapping_term_ms;
+	struct behavior_hold_tap_behaviors *behaviors;
+	enum flavor flavor;
+};
+
+// this data is specific for each hold-tap
+struct active_hold_tap {
+	s32_t position;
+	u32_t param_hold;
+	u32_t param_tap;
+	bool is_decided;
+	bool is_hold;
+	const struct behavior_hold_tap_config *config;
+	struct k_delayed_work work;
+	bool work_is_cancelled;
+};
+
+// The undecided hold tap is the hold tap that needs to be decided before
+// other keypress events can be released. While the undecided_hold_tap is
+// not NULL, most events are captured in captured_events.
+// After the hold_tap is decided, it will stay in the active_hold_taps until
+// its key-up has been processed and the delayed work is cleaned up.
+struct active_hold_tap *undecided_hold_tap = NULL;
+struct active_hold_tap active_hold_taps[ZMK_BHV_HOLD_TAP_MAX_HELD] = {};
+// We capture most position_state_changed events and some modifiers_state_changed events.
+const struct zmk_event_header *captured_events[ZMK_BHV_HOLD_TAP_MAX_CAPTURED_EVENTS] = {};
+
+static int capture_event(const struct zmk_event_header *event)
+{
+	for (int i = 0; i < ZMK_BHV_HOLD_TAP_MAX_CAPTURED_EVENTS; i++) {
+		if (captured_events[i] == NULL) {
+			captured_events[i] = event;
+			return 0;
+		}
+	}
+	return -ENOMEM;
+}
+
+static struct position_state_changed *find_captured_keydown_event(u32_t position)
+{
+	struct position_state_changed *last_match = NULL;
+	for (int i = 0; i < ZMK_BHV_HOLD_TAP_MAX_CAPTURED_EVENTS; i++) {
+		const struct zmk_event_header *eh = captured_events[i];
+		if (eh == NULL) {
+			return last_match;
+		}
+		if (!is_position_state_changed(eh)) {
+			continue;
+		}
+		struct position_state_changed *position_event = cast_position_state_changed(eh);
+		if (position_event->position == position && position_event->state) {
+			last_match = position_event;
+		}
+	}
+	return last_match;
+}
+
+const struct zmk_listener zmk_listener_behavior_hold_tap;
+
+static void release_captured_events()
+{
+	if (undecided_hold_tap != NULL) {
+		return;
+	}
+
+	// We use a trick to prevent copying the captured_events array.
+	//
+	// Events for different mod-tap instances are separated by a NULL pointer.
+	//
+	// The first event popped will never be catched by the next active hold-tap
+	// because to start capturing a mod-tap-key-down event must first completely
+	// go through the events queue.
+	//
+	// Example of this release process;
+	// [mt2_down, k1_down, k1_up, mt2_up, null, ...]
+	//  ^
+	// mt2_down position event isn't captured because no hold-tap is active.
+	// mt2_down behavior event is handled, now we have an undecided hold-tap
+	// [null, k1_down, k1_up, mt2_up, null, ...]
+	//        ^
+	// k1_down  is captured by the mt2 mod-tap
+	// !note that searches for find_captured_keydown_event by the mt2 behavior will stop at the first null encountered
+	// [mt1_down, null, k1_up, mt2_up, null, ...]
+	//                  ^
+	// k1_up event is captured by the new hold-tap:
+	// [k1_down, k1_up, null, mt2_up, null, ...]
+	//                        ^
+	// mt2_up event is not captured but causes release of mt2 behavior
+	// [k1_down, k1_up, null, null, null, ...]
+	// now mt2 will start releasing it's own captured positions.
+	for (int i = 0; i < ZMK_BHV_HOLD_TAP_MAX_CAPTURED_EVENTS; i++) {
+		const struct zmk_event_header *captured_event = captured_events[i];
+		if (captured_event == NULL) {
+			return;
+		}
+		captured_events[i] = NULL;
+		if (undecided_hold_tap != NULL) {
+			k_msleep(10);
+		}
+		if (is_position_state_changed(captured_event)) {
+			struct position_state_changed *position_event = cast_position_state_changed(captured_event);
+			LOG_DBG("Releasing key position event for position %d %s", position_event->position, (position_event->state ? "pressed" : "released"));
+		} else {
+			struct keycode_state_changed *modifier_event = cast_keycode_state_changed(captured_event);
+			LOG_DBG("Releasing mods changed event 0x%02X %s", modifier_event->keycode, (modifier_event->state ? "pressed" : "released"));
+		}
+		ZMK_EVENT_RAISE_AT(captured_event, behavior_hold_tap);
+	}
+}
+
+static struct active_hold_tap *find_hold_tap(u32_t position)
+{
+	for (int i = 0; i < ZMK_BHV_HOLD_TAP_MAX_HELD; i++) {
+		if (active_hold_taps[i].position == position) {
+			return &active_hold_taps[i];
+		}
+	}
+	return NULL;
+}
+
+static struct active_hold_tap *store_hold_tap(u32_t position, u32_t param_hold, u32_t param_tap, const struct behavior_hold_tap_config *config)
+{
+	for (int i = 0; i < ZMK_BHV_HOLD_TAP_MAX_HELD; i++) {
+		if (active_hold_taps[i].position != ZMK_BHV_HOLD_TAP_POSITION_NOT_USED) {
+			continue;
+		}
+		active_hold_taps[i].position = position;
+		active_hold_taps[i].is_decided = false;
+		active_hold_taps[i].is_hold = false;
+		active_hold_taps[i].config = config;
+		active_hold_taps[i].param_hold = param_hold;
+		active_hold_taps[i].param_tap = param_tap;
+		return &active_hold_taps[i];
+	}
+	return NULL;
+}
+
+static void clear_hold_tap(struct active_hold_tap *hold_tap)
+{
+	hold_tap->position = ZMK_BHV_HOLD_TAP_POSITION_NOT_USED;
+	hold_tap->is_decided = false;
+	hold_tap->is_hold = false;
+	hold_tap->work_is_cancelled = false;
+}
+
+enum decision_moment {
+	HT_KEY_UP               = 0,
+	HT_OTHER_KEY_DOWN       = 1,
+	HT_OTHER_KEY_UP         = 2,
+	HT_TIMER_EVENT          = 3,
+};
+
+static void decide_balanced(struct active_hold_tap *hold_tap, enum decision_moment event)
+{
+	switch (event) {
+	case HT_KEY_UP:
+		hold_tap->is_hold = 0;
+		hold_tap->is_decided = true;
+		break;
+	case HT_OTHER_KEY_UP:
+	case HT_TIMER_EVENT:
+		hold_tap->is_hold = 1;
+		hold_tap->is_decided = true;
+		break;
+	default: return;
+	}
+}
+
+static void decide_tap_preferred(struct active_hold_tap *hold_tap, enum decision_moment event)
+{
+	switch (event) {
+	case HT_KEY_UP:
+		hold_tap->is_hold = 0;
+		hold_tap->is_decided = true;
+		break;
+	case HT_TIMER_EVENT:
+		hold_tap->is_hold = 1;
+		hold_tap->is_decided = true;
+		break;
+	default: return;
+	}
+}
+
+static void decide_hold_preferred(struct active_hold_tap *hold_tap, enum decision_moment event)
+{
+	switch (event) {
+	case HT_KEY_UP:
+		hold_tap->is_hold = 0;
+		hold_tap->is_decided = true;
+		break;
+	case HT_OTHER_KEY_DOWN:
+	case HT_TIMER_EVENT:
+		hold_tap->is_hold = 1;
+		hold_tap->is_decided = true;
+		break;
+	default: return;
+	}
+}
+
+static inline char* flavor_str(enum flavor flavor) {
+	switch(flavor) {
+	case ZMK_BHV_HOLD_TAP_FLAVOR_HOLD_PREFERRED:
+		return "hold-preferred";
+	case ZMK_BHV_HOLD_TAP_FLAVOR_BALANCED:
+		return "balanced";
+	case ZMK_BHV_HOLD_TAP_FLAVOR_TAP_PREFERRED:
+		return "tap-preferred";
+	}
+	return "UNKNOWN FLAVOR";
+}
+
+static void decide_hold_tap(struct active_hold_tap *hold_tap, enum decision_moment event)
+{
+	if (hold_tap->is_decided) {
+		return;
+	}
+
+	if (hold_tap != undecided_hold_tap) {
+		LOG_DBG("ERROR found undecided tap hold that is not the active tap hold");
+		return;
+	}
+
+	switch(hold_tap->config->flavor) {
+	case ZMK_BHV_HOLD_TAP_FLAVOR_HOLD_PREFERRED:
+		decide_hold_preferred(hold_tap, event);
+	case ZMK_BHV_HOLD_TAP_FLAVOR_BALANCED:
+		decide_balanced(hold_tap, event);
+	case ZMK_BHV_HOLD_TAP_FLAVOR_TAP_PREFERRED:
+		decide_tap_preferred(hold_tap, event);
+	}
+
+	if (!hold_tap->is_decided) {
+		return;
+	}
+
+	LOG_DBG("%d decided %s (%s event %d)", 
+		hold_tap->position, 
+		hold_tap->is_hold ? "hold" : "tap", 
+		flavor_str(hold_tap->config->flavor),
+		event);
+	undecided_hold_tap = NULL;
+
+	struct zmk_behavior_binding *behavior;
+	if (hold_tap->is_hold) {
+		behavior = &hold_tap->config->behaviors->hold;
+		struct device *behavior_device = device_get_binding(behavior->behavior_dev);
+		behavior_keymap_binding_pressed(behavior_device, hold_tap->position, hold_tap->param_hold, 0);
+	} else {
+		behavior = &hold_tap->config->behaviors->tap;
+		struct device *behavior_device = device_get_binding(behavior->behavior_dev);
+		behavior_keymap_binding_pressed(behavior_device, hold_tap->position, hold_tap->param_tap, 0);
+	}
+	release_captured_events();
+}
+
+static int on_hold_tap_binding_pressed(struct device *dev, u32_t position, u32_t param_hold, u32_t param_tap)
+{
+	const struct behavior_hold_tap_config *cfg = dev->config_info;
+
+	if (undecided_hold_tap != NULL) {
+		LOG_DBG("ERROR another hold-tap behavior is undecided.");
+		// if this happens, make sure the behavior events occur AFTER other position events.
+		return 0;
+	}
+
+	struct active_hold_tap *hold_tap = store_hold_tap(position, param_hold, param_tap, cfg);
+	if (hold_tap == NULL) {
+		LOG_ERR("unable to store hold-tap info, did you press more than %d hold-taps?", ZMK_BHV_HOLD_TAP_MAX_HELD);
+		return 0;
+	}
+
+	LOG_DBG("%d new undecided hold_tap", position);
+	undecided_hold_tap = hold_tap;
+	k_delayed_work_submit(&hold_tap->work, cfg->tapping_term_ms());
+
+	// todo: once we get timing info for keypresses, start the timer relative to the original keypress
+	// don't forget to simulate a timer-event before the event after that time was handled.
+
+	return 0;
+}
+
+static int on_hold_tap_binding_released(struct device *dev, u32_t position, u32_t _, u32_t __)
+{
+	struct active_hold_tap *hold_tap = find_hold_tap(position);
+
+	if (hold_tap == NULL) {
+		LOG_ERR("ACTIVE_HOLD_TAP_CLEANED_UP_TOO_EARLY");
+		return 0;
+	}
+
+	int work_cancel_result = k_delayed_work_cancel(&hold_tap->work);
+	decide_hold_tap(hold_tap, HT_KEY_UP);
+
+	struct zmk_behavior_binding *behavior;
+	if (hold_tap->is_hold) {
+		behavior = &hold_tap->config->behaviors->hold;
+		struct device *behavior_device = device_get_binding(behavior->behavior_dev);
+		behavior_keymap_binding_released(behavior_device, hold_tap->position, hold_tap->param_hold, 0);
+	} else {
+		behavior = &hold_tap->config->behaviors->tap;
+		struct device *behavior_device = device_get_binding(behavior->behavior_dev);
+		behavior_keymap_binding_released(behavior_device, hold_tap->position, hold_tap->param_tap, 0);
+	}
+
+
+	if (work_cancel_result == -EINPROGRESS) {
+		// let the timer handler clean up
+		// if we'd clear now, the timer may call back for an uninitialized active_hold_tap.
+		LOG_DBG("%d hold-tap timer work in event queue", position);
+		hold_tap->work_is_cancelled = true;
+	} else {
+		LOG_DBG("%d cleaning up hold-tap", position);
+		clear_hold_tap(hold_tap);
+	}
+
+	return 0;
+}
+
+static const struct behavior_driver_api behavior_hold_tap_driver_api = {
+	.binding_pressed = on_hold_tap_binding_pressed,
+	.binding_released = on_hold_tap_binding_released,
+};
+
+
+static int position_state_changed_listener(const struct zmk_event_header *eh)
+{
+	struct position_state_changed *ev = cast_position_state_changed(eh);
+
+	if (undecided_hold_tap == NULL) {
+		LOG_DBG("%d bubble (no undecided hold_tap active)", ev->position);
+		return 0;
+	}
+
+	if (undecided_hold_tap->position == ev->position) {
+		if (ev->state) { // keydown
+			LOG_ERR("hold-tap listener should be called before before most other listeners!");
+			return 0;
+		} else { // keyup
+			LOG_DBG("%d bubble undecided hold-tap keyrelease event", undecided_hold_tap->position);
+			return 0;
+		}
+	}
+
+	if (!ev->state && find_captured_keydown_event(ev->position) == NULL) {
+		// no keydown event has been captured, let it bubble.
+		// we'll catch modifiers later in modifier_state_changed_listener
+		LOG_DBG("%d bubbling %d %s event", undecided_hold_tap->position, ev->position, ev->state ? "down" : "up");
+		return 0;
+	}
+
+	LOG_DBG("%d capturing %d %s event", undecided_hold_tap->position, ev->position, ev->state ? "down" : "up");
+	capture_event(eh);
+	decide_hold_tap(undecided_hold_tap, ev->state ? HT_OTHER_KEY_DOWN : HT_OTHER_KEY_UP);
+	return ZMK_EV_EVENT_CAPTURED;
+}
+
+static bool is_mod(struct keycode_state_changed *ev)
+{
+	return ev->usage_page == USAGE_KEYPAD && ev->keycode >= LCTL && ev->keycode <= RGUI;
+}
+
+static int keycode_state_changed_listener(const struct zmk_event_header *eh)
+{
+	// we want to catch layer-up events too... how?
+	struct keycode_state_changed *ev = cast_keycode_state_changed(eh);
+
+	if (undecided_hold_tap == NULL) {
+		// LOG_DBG("0x%02X bubble (no undecided hold_tap active)", ev->keycode);
+		return 0;
+	}
+
+	if (!is_mod(ev)) {
+		// LOG_DBG("0x%02X bubble (not a mod)", ev->keycode);
+		return 0;
+	}
+
+	// only key-up events will bubble through position_state_changed_listener
+	// if a undecided_hold_tap is active.
+	LOG_DBG("%d capturing 0x%02X %s event", undecided_hold_tap->position, ev->keycode, ev->state ? "down" : "up");
+	capture_event(eh);
+	return ZMK_EV_EVENT_CAPTURED;
+}
+
+
+int behavior_hold_tap_listener(const struct zmk_event_header *eh)
+{
+	if (is_position_state_changed(eh)) {
+		return position_state_changed_listener(eh);
+	} else if (is_keycode_state_changed(eh)) {
+		return keycode_state_changed_listener(eh);
+	}
+	return 0;
+}
+
+ZMK_LISTENER(behavior_hold_tap, behavior_hold_tap_listener);
+ZMK_SUBSCRIPTION(behavior_hold_tap, position_state_changed);
+// this should be modifiers_state_changed, but unfrotunately that's not implemented yet.
+ZMK_SUBSCRIPTION(behavior_hold_tap, keycode_state_changed);
+
+void behavior_hold_tap_timer_work_handler(struct k_work *item)
+{
+	struct active_hold_tap *hold_tap = CONTAINER_OF(item, struct active_hold_tap, work);
+
+	if (hold_tap->work_is_cancelled) {
+		clear_hold_tap(hold_tap);
+	} else {
+		decide_hold_tap(hold_tap, HT_TIMER_EVENT);
+	}
+}
+
+static int behavior_hold_tap_init(struct device *dev)
+{
+	static bool init_first_run = true;
+
+	if (init_first_run) {
+		for (int i = 0; i < ZMK_BHV_HOLD_TAP_MAX_HELD; i++) {
+			k_delayed_work_init(&active_hold_taps[i].work, behavior_hold_tap_timer_work_handler);
+			active_hold_taps[i].position = ZMK_BHV_HOLD_TAP_POSITION_NOT_USED;
+		}
+	}
+	init_first_run = false;
+	return 0;
+}
+
+struct behavior_hold_tap_data {};
+static struct behavior_hold_tap_data behavior_hold_tap_data;
+
+#define _TRANSFORM_ENTRY(idx, node)														   \
+	{  \
+		.behavior_dev = DT_LABEL(DT_INST_PHANDLE_BY_IDX(node, bindings, idx)),								   \
+	    .param1 = COND_CODE_0(DT_INST_PHA_HAS_CELL_AT_IDX(node, bindings, idx, param1), (0), (DT_INST_PHA_BY_IDX(node, bindings, idx, param1))), \
+	    .param2 = COND_CODE_0(DT_INST_PHA_HAS_CELL_AT_IDX(node, bindings, idx, param2), (0), (DT_INST_PHA_BY_IDX(node, bindings, idx, param2))), \
+	},
+
+#define KP_INST(n)													 \
+	static k_timeout_t behavior_hold_tap_config_##n##_gettime() { return K_MSEC(DT_INST_PROP(n, tapping_term_ms)); } \
+	static struct behavior_hold_tap_behaviors behavior_hold_tap_behaviors_##n = {					 \
+		.hold = _TRANSFORM_ENTRY(0, n)										 \
+	  	.tap = _TRANSFORM_ENTRY(1, n)									 \
+	};														 \
+	static struct behavior_hold_tap_config behavior_hold_tap_config_##n = {						 \
+		.behaviors = &behavior_hold_tap_behaviors_##n,								 \
+		.tapping_term_ms = &behavior_hold_tap_config_##n##_gettime,						 \
+		.flavor = DT_ENUM_IDX(DT_DRV_INST(n), flavor),									 \
+	};														 \
+	DEVICE_AND_API_INIT( \
+		behavior_hold_tap_##n, DT_INST_LABEL(n), behavior_hold_tap_init,				 \
+			    &behavior_hold_tap_data,									 \
+			    &behavior_hold_tap_config_##n,								 \
+			    APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT,						 \
+			    &behavior_hold_tap_driver_api);
+
+DT_INST_FOREACH_STATUS_OKAY(KP_INST)
+
+
+#endif
\ No newline at end of file
diff --git a/app/src/behaviors/behavior_key_press.c b/app/src/behaviors/behavior_key_press.c
index 34df1c0..7404c79 100644
--- a/app/src/behaviors/behavior_key_press.c
+++ b/app/src/behaviors/behavior_key_press.c
@@ -28,27 +28,17 @@ static int behavior_key_press_init(struct device *dev)
 static int on_keymap_binding_pressed(struct device *dev, u32_t position, u32_t keycode, u32_t _)
 {
   const struct behavior_key_press_config *cfg = dev->config_info;
-  struct keycode_state_changed *ev;
   LOG_DBG("position %d usage_page 0x%02X keycode 0x%02X", position, cfg->usage_page, keycode);
 
-  ev = new_keycode_state_changed();
-  ev->usage_page = cfg->usage_page;
-  ev->keycode = keycode;
-  ev->state = true;
-  return ZMK_EVENT_RAISE(ev);
+  return ZMK_EVENT_RAISE(create_keycode_state_changed(cfg->usage_page, keycode, true));
 }
 
 static int on_keymap_binding_released(struct device *dev, u32_t position, u32_t keycode, u32_t _)
 {
   const struct behavior_key_press_config *cfg = dev->config_info;
-  struct keycode_state_changed *ev;
   LOG_DBG("position %d usage_page 0x%02X keycode 0x%02X", position, cfg->usage_page, keycode);
 
-  ev = new_keycode_state_changed();
-  ev->usage_page = cfg->usage_page;
-  ev->keycode = keycode;
-  ev->state = false;
-  return ZMK_EVENT_RAISE(ev);
+  return ZMK_EVENT_RAISE(create_keycode_state_changed(cfg->usage_page, keycode, false));
 }
 
 static const struct behavior_driver_api behavior_key_press_driver_api = {
diff --git a/app/src/behaviors/behavior_mod_tap.c b/app/src/behaviors/behavior_mod_tap.c
deleted file mode 100644
index 62604eb..0000000
--- a/app/src/behaviors/behavior_mod_tap.c
+++ /dev/null
@@ -1,83 +0,0 @@
-/*
- * Copyright (c) 2020 Peter Johanson <peter@peterjohanson.com>
- *
- * SPDX-License-Identifier: MIT
- */
-
-#define DT_DRV_COMPAT zmk_behavior_mod_tap
-
-#include <device.h>
-#include <drivers/behavior.h>
-#include <logging/log.h>
-
-#include <zmk/event-manager.h>
-#include <zmk/events/keycode-state-changed.h>
-#include <zmk/events/modifiers-state-changed.h>
-
-LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
-
-struct behavior_mod_tap_config { };
-struct behavior_mod_tap_data {
-  u16_t pending_press_positions;
-};
-
-int behavior_mod_tap_listener(const struct zmk_event_header *eh)
-{
-  if (is_keycode_state_changed(eh)) {
-    struct device *dev = device_get_binding(DT_INST_LABEL(0));
-    const struct keycode_state_changed *ev = cast_keycode_state_changed(eh);
-    if (ev->state) {
-      struct behavior_mod_tap_data *data = dev->driver_data;
-      data->pending_press_positions = 0;
-    }
-  }
-  return 0;
-}
-
-ZMK_LISTENER(behavior_mod_tap, behavior_mod_tap_listener);
-ZMK_SUBSCRIPTION(behavior_mod_tap, keycode_state_changed);
-
-static int behavior_mod_tap_init(struct device *dev)
-{
-	return 0;
-};
-
-
-static int on_keymap_binding_pressed(struct device *dev, u32_t position, u32_t mods, u32_t keycode)
-{
-  struct behavior_mod_tap_data *data = dev->driver_data;
-  LOG_DBG("mods: %d, keycode: %d", mods, keycode);
-  WRITE_BIT(data->pending_press_positions, position, true);
-  return ZMK_EVENT_RAISE(create_modifiers_state_changed(mods, true));
-}
-
-static int on_keymap_binding_released(struct device *dev, u32_t position, u32_t mods, u32_t keycode)
-{
-  struct behavior_mod_tap_data *data = dev->driver_data;
-  LOG_DBG("mods: %d, keycode: %d", mods, keycode);
-  
-  ZMK_EVENT_RAISE(create_modifiers_state_changed(mods, false));
-  k_msleep(10); // TODO: Better approach than k_msleep to avoid USB send failures? Retries in the USB endpoint layer?
-  if (data->pending_press_positions & BIT(position)) {
-    ZMK_EVENT_RAISE(create_keycode_state_changed(USAGE_KEYPAD, keycode, true));
-    k_msleep(10);
-    ZMK_EVENT_RAISE(create_keycode_state_changed(USAGE_KEYPAD, keycode, false));
-  }
-
-  return 0;
-}
-
-static const struct behavior_driver_api behavior_mod_tap_driver_api = {
-  .binding_pressed = on_keymap_binding_pressed,
-  .binding_released = on_keymap_binding_released,
-};
-
-static const struct behavior_mod_tap_config behavior_mod_tap_config = {};
-
-static struct behavior_mod_tap_data behavior_mod_tap_data;
-
-DEVICE_AND_API_INIT(behavior_mod_tap, DT_INST_LABEL(0), behavior_mod_tap_init,
-                    &behavior_mod_tap_data,
-                    &behavior_mod_tap_config,
-                    APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT,
-                    &behavior_mod_tap_driver_api);
diff --git a/app/src/behaviors/behavior_none.c b/app/src/behaviors/behavior_none.c
new file mode 100644
index 0000000..e822d5e
--- /dev/null
+++ b/app/src/behaviors/behavior_none.c
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2020 Peter Johanson <peter@peterjohanson.com>
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#define DT_DRV_COMPAT zmk_behavior_none
+
+#include <device.h>
+#include <power/reboot.h>
+#include <drivers/behavior.h>
+#include <logging/log.h>
+
+LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
+
+struct behavior_none_config { };
+struct behavior_none_data { };
+
+static int behavior_none_init(struct device *dev)
+{
+	return 0;
+};
+
+static int on_keymap_binding_pressed(struct device *dev, u32_t position, u32_t _param1, u32_t _param2)
+{
+  return 0;
+}
+
+static int on_keymap_binding_released(struct device *dev, u32_t position, u32_t _param1, u32_t _param2)
+{
+  return 0;
+}
+
+static const struct behavior_driver_api behavior_none_driver_api = {
+  .binding_pressed = on_keymap_binding_pressed,
+  .binding_released = on_keymap_binding_released,
+};
+
+
+static const struct behavior_none_config behavior_none_config = {};
+
+static struct behavior_none_data behavior_none_data;
+
+DEVICE_AND_API_INIT(behavior_none, DT_INST_LABEL(0), behavior_none_init,
+                    &behavior_none_data,
+                    &behavior_none_config,
+                    APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT,
+                    &behavior_none_driver_api);
\ No newline at end of file
diff --git a/app/src/behaviors/behavior_reset.c b/app/src/behaviors/behavior_reset.c
index 44cbc21..30a96ea 100644
--- a/app/src/behaviors/behavior_reset.c
+++ b/app/src/behaviors/behavior_reset.c
@@ -13,8 +13,9 @@
 
 LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
 
-struct behavior_reset_config { };
-struct behavior_reset_data { };
+struct behavior_reset_config {
+  int type;
+};
 
 static int behavior_reset_init(struct device *dev)
 {
@@ -23,9 +24,11 @@ static int behavior_reset_init(struct device *dev)
 
 static int on_keymap_binding_pressed(struct device *dev, u32_t position, u32_t _param1, u32_t _param2)
 {
+  const struct behavior_reset_config *cfg = dev->config_info;
+
   // TODO: Correct magic code for going into DFU?
   // See https://github.com/adafruit/Adafruit_nRF52_Bootloader/blob/d6b28e66053eea467166f44875e3c7ec741cb471/src/main.c#L107
-  sys_reboot(0);
+  sys_reboot(cfg->type);
   return 0;
 }
 
@@ -34,12 +37,14 @@ static const struct behavior_driver_api behavior_reset_driver_api = {
 };
 
 
-static const struct behavior_reset_config behavior_reset_config = {};
-
-static struct behavior_reset_data behavior_reset_data;
-
-DEVICE_AND_API_INIT(behavior_reset, DT_INST_LABEL(0), behavior_reset_init,
-                    &behavior_reset_data,
-                    &behavior_reset_config,
-                    APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT,
-                    &behavior_reset_driver_api);
\ No newline at end of file
+#define RST_INST(n) \
+  static const struct behavior_reset_config behavior_reset_config_##n = { \
+    .type = DT_INST_PROP(n, type) \
+  }; \
+  DEVICE_AND_API_INIT(behavior_reset_##n, DT_INST_LABEL(n), behavior_reset_init, \
+                    NULL, \
+                    &behavior_reset_config_##n, \
+                    APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT, \
+                    &behavior_reset_driver_api);
+  
+DT_INST_FOREACH_STATUS_OKAY(RST_INST)
\ No newline at end of file
diff --git a/app/src/behaviors/behavior_toggle_layer.c b/app/src/behaviors/behavior_toggle_layer.c
new file mode 100644
index 0000000..02a76d2
--- /dev/null
+++ b/app/src/behaviors/behavior_toggle_layer.c
@@ -0,0 +1,56 @@
+/*
+ * Copyright (c) 2020 Cody McGinnis <brainwart@gmail.com>
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#define DT_DRV_COMPAT zmk_behavior_toggle_layer
+
+#include <device.h>
+#include <drivers/behavior.h>
+#include <logging/log.h>
+
+#include <zmk/keymap.h>
+
+LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
+
+struct behavior_tog_config
+{
+};
+struct behavior_tog_data
+{
+};
+
+static int behavior_tog_init(struct device *dev)
+{
+  return 0;
+};
+
+static int tog_keymap_binding_pressed(struct device *dev, u32_t position, u32_t layer, u32_t _)
+{
+  LOG_DBG("position %d layer %d", position, layer);
+
+  return zmk_keymap_layer_toggle(layer);
+}
+
+static int tog_keymap_binding_released(struct device *dev, u32_t position, u32_t layer, u32_t _)
+{
+  LOG_DBG("position %d layer %d", position, layer);
+
+  return 0;
+}
+
+static const struct behavior_driver_api behavior_tog_driver_api = {
+    .binding_pressed = tog_keymap_binding_pressed,
+    .binding_released = tog_keymap_binding_released,
+};
+
+static const struct behavior_tog_config behavior_tog_config = {};
+
+static struct behavior_tog_data behavior_tog_data;
+
+DEVICE_AND_API_INIT(behavior_tog, DT_INST_LABEL(0), behavior_tog_init,
+                    &behavior_tog_data,
+                    &behavior_tog_config,
+                    APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT,
+                    &behavior_tog_driver_api);
diff --git a/app/src/ble.c b/app/src/ble.c
index 559b04f..0e96d16 100644
--- a/app/src/ble.c
+++ b/app/src/ble.c
@@ -1,3 +1,8 @@
+/*
+ * Copyright (c) 2020 Peter Johanson
+ *
+ * SPDX-License-Identifier: MIT
+ */
 
 #include <device.h>
 #include <init.h>
@@ -23,6 +28,16 @@ static struct bt_conn *auth_passkey_entry_conn;
 static u8_t passkey_entries[6] = {0, 0, 0, 0, 0, 0};
 static u8_t passkey_digit = 0;
 
+#if IS_ENABLED(CONFIG_ZMK_SPLIT_BLE_ROLE_PERIPHERAL)
+#define ZMK_ADV_PARAMS BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONNECTABLE | \
+                                        BT_LE_ADV_OPT_USE_NAME | \
+                                        BT_LE_ADV_OPT_ONE_TIME, \
+                                        BT_GAP_ADV_FAST_INT_MIN_2, \
+                                        BT_GAP_ADV_FAST_INT_MAX_2, NULL)
+#else
+#define ZMK_ADV_PARAMS BT_LE_ADV_CONN_NAME
+#endif
+
 static void connected(struct bt_conn *conn, u8_t err)
 {
     char addr[BT_ADDR_LE_STR_LEN];
@@ -31,17 +46,21 @@ static void connected(struct bt_conn *conn, u8_t err)
 
     if (err)
     {
-        printk("Failed to connect to %s (%u)\n", addr, err);
+        LOG_WRN("Failed to connect to %s (%u)", log_strdup(addr), err);
         return;
     }
 
-    printk("Connected %s\n", addr);
+    LOG_DBG("Connected %s", log_strdup(addr));
 
-    bt_conn_le_param_update(conn, BT_LE_CONN_PARAM(0x0006, 0x000c, 5, 400));
+    bt_conn_le_param_update(conn, BT_LE_CONN_PARAM(0x0006, 0x000c, 30, 400));
+
+#if IS_ENABLED(CONFIG_ZMK_SPLIT_BLE_ROLE_PERIPHERAL)
+    bt_conn_le_phy_update(conn, BT_CONN_LE_PHY_PARAM_2M);
+#endif
 
     if (bt_conn_set_security(conn, BT_SECURITY_L2))
     {
-        printk("Failed to set security\n");
+        LOG_ERR("Failed to set security");
     }
 }
 
@@ -51,7 +70,7 @@ static void disconnected(struct bt_conn *conn, u8_t reason)
 
     bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
 
-    printk("Disconnected from %s (reason 0x%02x)\n", addr, reason);
+    LOG_DBG("Disconnected from %s (reason 0x%02x)", log_strdup(addr), reason);
 }
 
 static void security_changed(struct bt_conn *conn, bt_security_t level,
@@ -63,11 +82,11 @@ static void security_changed(struct bt_conn *conn, bt_security_t level,
 
     if (!err)
     {
-        printk("Security changed: %s level %u\n", addr, level);
+        LOG_DBG("Security changed: %s level %u", log_strdup(addr), level);
     }
     else
     {
-        printk("Security failed: %s level %u err %d\n", addr, level,
+        LOG_ERR("Security failed: %s level %u err %d", log_strdup(addr), level,
                err);
     }
 }
@@ -84,7 +103,7 @@ static void auth_passkey_display(struct bt_conn *conn, unsigned int passkey)
 
     bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
 
-    printk("Passkey for %s: %06u\n", addr, passkey);
+    LOG_DBG("Passkey for %s: %06u", log_strdup(addr), passkey);
 }
 
 #ifdef CONFIG_ZMK_BLE_PASSKEY_ENTRY
@@ -95,7 +114,7 @@ static void auth_passkey_entry(struct bt_conn *conn)
 
     bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
 
-    printk("Passkey entry requested for %s\n", addr);
+    LOG_DBG("Passkey entry requested for %s", log_strdup(addr));
     auth_passkey_entry_conn = bt_conn_ref(conn);
 }
 
@@ -115,7 +134,7 @@ static void auth_cancel(struct bt_conn *conn)
 
     passkey_digit = 0;
 
-    printk("Pairing cancelled: %s\n", addr);
+    LOG_DBG("Pairing cancelled: %s", log_strdup(addr));
 }
 
 static struct bt_conn_auth_cb zmk_ble_auth_cb_display = {
@@ -146,14 +165,14 @@ static void zmk_ble_ready(int err)
     LOG_DBG("ready? %d", err);
     if (err)
     {
-        printk("Bluetooth init failed (err %d)\n", err);
+        LOG_ERR("Bluetooth init failed (err %d)", err);
         return;
     }
 
-    err = bt_le_adv_start(BT_LE_ADV_CONN_NAME, zmk_ble_ad, ARRAY_SIZE(zmk_ble_ad), NULL, 0);
+    err = bt_le_adv_start(ZMK_ADV_PARAMS, zmk_ble_ad, ARRAY_SIZE(zmk_ble_ad), NULL, 0);
     if (err)
     {
-        printk("Advertising failed to start (err %d)\n", err);
+        LOG_ERR("Advertising failed to start (err %d)", err);
         return;
     }
 }
@@ -164,7 +183,7 @@ static int zmk_ble_init(struct device *_arg)
 
     if (err)
     {
-        printk("BLUETOOTH FAILED");
+        LOG_ERR("BLUETOOTH FAILED (%d)", err);
         return err;
     }
 
@@ -181,6 +200,12 @@ static int zmk_ble_init(struct device *_arg)
     return 0;
 }
 
+int zmk_ble_unpair_all()
+{
+    LOG_DBG("");
+    return bt_unpair(BT_ID_DEFAULT, NULL);
+};
+
 bool zmk_ble_handle_key_user(struct zmk_key_event *key_event)
 {
     zmk_key key = key_event->key;
diff --git a/app/src/ble_unpair_combo.c b/app/src/ble_unpair_combo.c
new file mode 100644
index 0000000..82fa834
--- /dev/null
+++ b/app/src/ble_unpair_combo.c
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2020 Peter Johanson
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#include <device.h>
+#include <init.h>
+
+#define DT_DRV_COMPAT zmk_bt_unpair_combo
+
+#if DT_HAS_COMPAT_STATUS_OKAY(DT_DRV_COMPAT)
+
+#include <logging/log.h>
+LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
+
+#include <zmk/ble.h>
+#include <zmk/event-manager.h>
+#include <zmk/events/position-state-changed.h>
+
+
+static u8_t combo_state;
+
+const u32_t key_positions[] = DT_INST_PROP(0, key_positions);
+#define KP_LEN DT_INST_PROP_LEN(0, key_positions)
+
+int index_for_key_position(u32_t kp)
+{
+    for (int i = 0; i < KP_LEN; i++) {
+        if (key_positions[i] == kp) {
+            return i;
+        }
+    }
+
+    return -1;
+}
+
+int unpair_combo_listener(const struct zmk_event_header *eh)
+{
+    if (is_position_state_changed(eh)) {
+        const struct position_state_changed *psc = cast_position_state_changed(eh);
+
+        int kp_index = index_for_key_position(psc->position);
+        if (kp_index < 0) {
+            return 0;
+        }
+
+        WRITE_BIT(combo_state, kp_index, psc->state);
+    }
+
+    return 0;
+};
+
+void unpair_combo_work_handler(struct k_work *work)
+{
+    for (int i = 0; i < KP_LEN; i++) {
+        if (!(combo_state & BIT(i))) {
+            LOG_DBG("Key position %d not held, skipping unpair combo", key_positions[i]);
+            return;
+        }
+    }
+
+    zmk_ble_unpair_all();
+};
+
+struct k_delayed_work unpair_combo_work;
+
+int zmk_ble_unpair_combo_init(struct device *_unused)
+{
+    k_delayed_work_init(&unpair_combo_work, unpair_combo_work_handler);
+    k_delayed_work_submit(&unpair_combo_work, K_SECONDS(2));
+
+    return 0;
+};
+
+ZMK_LISTENER(zmk_ble_unpair_combo, unpair_combo_listener);
+ZMK_SUBSCRIPTION(zmk_ble_unpair_combo, position_state_changed);
+
+SYS_INIT(zmk_ble_unpair_combo_init,
+        APPLICATION,
+        CONFIG_APPLICATION_INIT_PRIORITY);
+
+#endif /* DT_HAS_COMPAT_STATUS_OKAY(DT_DRV_COMPAT) */
diff --git a/app/src/event_manager.c b/app/src/event_manager.c
index 567cdf0..47ad6b7 100644
--- a/app/src/event_manager.c
+++ b/app/src/event_manager.c
@@ -17,16 +17,30 @@ extern struct zmk_event_type* __event_type_end[];
 extern struct zmk_event_subscription __event_subscriptions_start[];
 extern struct zmk_event_subscription __event_subscriptions_end[];
 
-int zmk_event_manager_raise(struct zmk_event_header *event)
+
+int zmk_event_manager_handle_from(struct zmk_event_header *event, u8_t start_index)
 {
-    int ret;
-    struct zmk_event_subscription *ev_sub;
-    for (ev_sub = __event_subscriptions_start; ev_sub != __event_subscriptions_end; ev_sub++) {
+    int ret = 0;
+    u8_t len = __event_subscriptions_end - __event_subscriptions_start;
+    for (int i = start_index; i < len; i++) {
+        struct zmk_event_subscription *ev_sub = __event_subscriptions_start + i;
         if (ev_sub->event_type == event->event) {
             ret = ev_sub->listener->callback(event);
-            if (ret) {
+            if (ret < 0) {
                 LOG_DBG("Listener returned an error: %d", ret);
                 goto release;
+            } else if (ret > 0) {
+                switch (ret) {
+                    case ZMK_EV_EVENT_HANDLED:
+                        LOG_DBG("Listener handled the event");
+                        ret = 0;
+                        goto release;
+                    case ZMK_EV_EVENT_CAPTURED:
+                        LOG_DBG("Listener captured the event");
+                        event->last_listener_index = i;
+                        // Listeners are expected to free events they capture
+                        return 0;
+                }
             }
         }
     }
@@ -34,4 +48,46 @@ int zmk_event_manager_raise(struct zmk_event_header *event)
 release:
     k_free(event);
     return ret;
-}
\ No newline at end of file
+}
+
+int zmk_event_manager_raise(struct zmk_event_header *event)
+{
+    return zmk_event_manager_handle_from(event, 0);
+}
+
+int zmk_event_manager_raise_after(struct zmk_event_header *event, const struct zmk_listener *listener)
+{
+    u8_t len = __event_subscriptions_end - __event_subscriptions_start;
+    for (int i = 0; i < len; i++) {
+        struct zmk_event_subscription *ev_sub = __event_subscriptions_start + i;
+
+        if (ev_sub->event_type == event->event && ev_sub->listener == listener) {
+            return zmk_event_manager_handle_from(event, i+1);
+        }
+    }
+
+    LOG_WRN("Unable to find where to raise this after event");
+
+    return -EINVAL;
+}
+
+int zmk_event_manager_raise_at(struct zmk_event_header *event, const struct zmk_listener *listener)
+{
+    u8_t len = __event_subscriptions_end - __event_subscriptions_start;
+    for (int i = 0; i < len; i++) {
+        struct zmk_event_subscription *ev_sub = __event_subscriptions_start + i;
+
+        if (ev_sub->event_type == event->event && ev_sub->listener == listener) {
+            return zmk_event_manager_handle_from(event, i);
+        }
+    }
+
+    LOG_WRN("Unable to find where to raise this event");
+
+    return -EINVAL;
+}
+
+int zmk_event_manager_release(struct zmk_event_header *event)
+{
+    return zmk_event_manager_handle_from(event, event->last_listener_index + 1);
+}
diff --git a/app/src/events/keycode_state_changed.c b/app/src/events/keycode_state_changed.c
index 964b24a..73508e1 100644
--- a/app/src/events/keycode_state_changed.c
+++ b/app/src/events/keycode_state_changed.c
@@ -7,4 +7,4 @@
 #include <kernel.h>
 #include <zmk/events/keycode-state-changed.h>
 
-ZMK_EVENT_IMPL(keycode_state_changed);
\ No newline at end of file
+ZMK_EVENT_IMPL(keycode_state_changed);
diff --git a/app/src/keymap.c b/app/src/keymap.c
index 24e249d..57cdad6 100644
--- a/app/src/keymap.c
+++ b/app/src/keymap.c
@@ -17,96 +17,53 @@ LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
 static u32_t zmk_keymap_layer_state = 0;
 static u8_t zmk_keymap_layer_default = 0;
 
-#define ZMK_KEYMAP_NODE DT_CHOSEN(zmk_keymap)
-#define ZMK_KEYMAP_LAYERS_LEN DT_PROP_LEN(ZMK_KEYMAP_NODE, layers)
+#define DT_DRV_COMPAT zmk_keymap
+
+#define LAYER_CHILD_LEN(node) 1+
+#define ZMK_KEYMAP_NODE DT_DRV_INST(0)
+#define ZMK_KEYMAP_LAYERS_LEN (DT_INST_FOREACH_CHILD(0, LAYER_CHILD_LEN) 0) 
 
 #define LAYER_NODE(l) DT_PHANDLE_BY_IDX(ZMK_KEYMAP_NODE, layers, l)
 
 #define _TRANSFORM_ENTRY(idx, layer) \
-	{ .behavior_dev = DT_LABEL(DT_PHANDLE_BY_IDX(DT_PHANDLE_BY_IDX(ZMK_KEYMAP_NODE, layers, layer), bindings, idx)), \
-	  .param1 = COND_CODE_0(DT_PHA_HAS_CELL_AT_IDX(LAYER_NODE(layer), bindings, idx, param1), (0), (DT_PHA_BY_IDX(LAYER_NODE(layer), bindings, idx, param1))), \
-	  .param2 = COND_CODE_0(DT_PHA_HAS_CELL_AT_IDX(LAYER_NODE(layer), bindings, idx, param2), (0), (DT_PHA_BY_IDX(LAYER_NODE(layer), bindings, idx, param2))), \
+	{ .behavior_dev = DT_LABEL(DT_PHANDLE_BY_IDX(layer, bindings, idx)), \
+	  .param1 = COND_CODE_0(DT_PHA_HAS_CELL_AT_IDX(layer, bindings, idx, param1), (0), (DT_PHA_BY_IDX(layer, bindings, idx, param1))), \
+	  .param2 = COND_CODE_0(DT_PHA_HAS_CELL_AT_IDX(layer, bindings, idx, param2), (0), (DT_PHA_BY_IDX(layer, bindings, idx, param2))), \
 	},
 
-#define TRANSFORMED_LAYER(idx) \
-  { UTIL_LISTIFY(DT_PROP_LEN(DT_PHANDLE_BY_IDX(ZMK_KEYMAP_NODE, layers, idx), bindings), _TRANSFORM_ENTRY, idx) }
+#define TRANSFORMED_LAYER(node) \
+  { UTIL_LISTIFY(DT_PROP_LEN(node, bindings), _TRANSFORM_ENTRY, node) },
 
-static struct zmk_behavior_binding zmk_keymap[ZMK_KEYMAP_LAYERS_LEN][ZMK_KEYMAP_LEN] = {
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 0)
-	TRANSFORMED_LAYER(0),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 1)
-	TRANSFORMED_LAYER(1),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 2)
-	TRANSFORMED_LAYER(2),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 3)
-	TRANSFORMED_LAYER(3),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 4)
-	TRANSFORMED_LAYER(4),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 5)
-	TRANSFORMED_LAYER(5),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 6)
-	TRANSFORMED_LAYER(6),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 7)
-	TRANSFORMED_LAYER(7),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 8)
-	TRANSFORMED_LAYER(8),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 9)
-	TRANSFORMED_LAYER(9),
-#endif
-};
 
 #if ZMK_KEYMAP_HAS_SENSORS
 #define _TRANSFORM_SENSOR_ENTRY(idx, layer) \
-	{ .behavior_dev = DT_LABEL(DT_PHANDLE_BY_IDX(DT_PHANDLE_BY_IDX(ZMK_KEYMAP_NODE, layers, layer), sensor_bindings, idx)), \
-	  .param1 = COND_CODE_0(DT_PHA_HAS_CELL_AT_IDX(LAYER_NODE(layer), sensor_bindings, idx, param1), (0), (DT_PHA_BY_IDX(LAYER_NODE(layer), sensor_bindings, idx, param1))), \
-	  .param2 = COND_CODE_0(DT_PHA_HAS_CELL_AT_IDX(LAYER_NODE(layer), sensor_bindings, idx, param2), (0), (DT_PHA_BY_IDX(LAYER_NODE(layer), sensor_bindings, idx, param2))), \
+	{ .behavior_dev = DT_LABEL(DT_PHANDLE_BY_IDX(layer, sensor_bindings, idx)), \
+	  .param1 = COND_CODE_0(DT_PHA_HAS_CELL_AT_IDX(layer, sensor_bindings, idx, param1), (0), (DT_PHA_BY_IDX(layer, sensor_bindings, idx, param1))), \
+	  .param2 = COND_CODE_0(DT_PHA_HAS_CELL_AT_IDX(layer, sensor_bindings, idx, param2), (0), (DT_PHA_BY_IDX(layer, sensor_bindings, idx, param2))), \
 	},
 
-#define SENSOR_LAYER(idx) \
-	COND_CODE_1(DT_NODE_HAS_PROP(DT_PHANDLE_BY_IDX(ZMK_KEYMAP_NODE, layers, idx), sensor_bindings), \
-		({ UTIL_LISTIFY(DT_PROP_LEN(DT_PHANDLE_BY_IDX(ZMK_KEYMAP_NODE, layers, idx), sensor_bindings), _TRANSFORM_SENSOR_ENTRY, idx) }), \
-		(NULL))
+#define SENSOR_LAYER(node) \
+	COND_CODE_1(DT_NODE_HAS_PROP(node, sensor_bindings), \
+		({ UTIL_LISTIFY(DT_PROP_LEN(node, sensor_bindings), _TRANSFORM_SENSOR_ENTRY, node) }), \
+		({})),
+
+#endif /* ZMK_KEYMAP_HAS_SENSORS */
+
+// State
+
+// When a behavior handles a key position "down" event, we record the layer state
+// here so that even if that layer is deactivated before the "up", event, we
+// still send the release event to the behavior in that layer also.
+static u32_t zmk_keymap_active_behavior_layer[ZMK_KEYMAP_LEN];
+
+static struct zmk_behavior_binding zmk_keymap[ZMK_KEYMAP_LAYERS_LEN][ZMK_KEYMAP_LEN] = {
+	DT_INST_FOREACH_CHILD(0, TRANSFORMED_LAYER)
+};
+
+#if ZMK_KEYMAP_HAS_SENSORS
 
 static struct zmk_behavior_binding zmk_sensor_keymap[ZMK_KEYMAP_LAYERS_LEN][ZMK_KEYMAP_SENSORS_LEN] = {
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 0)
-	SENSOR_LAYER(0),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 1)
-	SENSOR_LAYER(1),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 2)
-	SENSOR_LAYER(2),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 3)
-	SENSOR_LAYER(3),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 4)
-	SENSOR_LAYER(4),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 5)
-	SENSOR_LAYER(5),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 6)
-	SENSOR_LAYER(6),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 7)
-	SENSOR_LAYER(7),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 8)
-	SENSOR_LAYER(8),
-#endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 9)
-	SENSOR_LAYER(9),
-#endif
+	DT_INST_FOREACH_CHILD(0, SENSOR_LAYER)
 };
 
 #endif /* ZMK_KEYMAP_HAS_SENSORS */
@@ -119,6 +76,11 @@ static struct zmk_behavior_binding zmk_sensor_keymap[ZMK_KEYMAP_LAYERS_LEN][ZMK_
 	WRITE_BIT(zmk_keymap_layer_state, layer, state); \
 	return 0;
 
+bool zmk_keymap_layer_active(u8_t layer)
+{
+	return (zmk_keymap_layer_state & (BIT(layer))) == (BIT(layer));
+};
+
 int zmk_keymap_layer_activate(u8_t layer)
 {
 	SET_LAYER_STATE(layer, true);
@@ -129,30 +91,53 @@ int zmk_keymap_layer_deactivate(u8_t layer)
 	SET_LAYER_STATE(layer, false);
 };
 
-int zmk_keymap_position_state_changed(u32_t position, bool pressed)
+int zmk_keymap_layer_toggle(u8_t layer)
 {
-	for (int layer = ZMK_KEYMAP_LAYERS_LEN - 1; layer >= zmk_keymap_layer_default; layer--)
+	if (zmk_keymap_layer_active(layer))
 	{
-		if ((zmk_keymap_layer_state & BIT(layer)) == BIT(layer) || layer == zmk_keymap_layer_default)
-		{
-			struct zmk_behavior_binding *binding = &zmk_keymap[layer][position];
-			struct device *behavior;
-			int ret;
+		return zmk_keymap_layer_deactivate(layer);
+	}
+
+  	return zmk_keymap_layer_activate(layer);
+};
 
-			LOG_DBG("layer: %d position: %d, binding name: %s", layer, position, log_strdup(binding->behavior_dev));
+bool is_active_layer(u8_t layer, u32_t layer_state)
+{
+	return (layer_state & BIT(layer)) == BIT(layer)
+		|| layer == zmk_keymap_layer_default;
+}
 
-			behavior = device_get_binding(binding->behavior_dev);
+int zmk_keymap_apply_position_state(int layer, u32_t position, bool pressed)
+{
+	struct zmk_behavior_binding *binding = &zmk_keymap[layer][position];
+	struct device *behavior;
 
-			if (!behavior) {
-				LOG_DBG("No behavior assigned to %d on layer %d", position, layer);
-				continue;
-			}
-			if (pressed) {
-				ret = behavior_keymap_binding_pressed(behavior, position, binding->param1, binding->param2);
-			} else {
-				ret = behavior_keymap_binding_released(behavior, position, binding->param1, binding->param2);
-			}
+	LOG_DBG("layer: %d position: %d, binding name: %s", layer, position, log_strdup(binding->behavior_dev));
+
+	behavior = device_get_binding(binding->behavior_dev);
+
+	if (!behavior) {
+		LOG_DBG("No behavior assigned to %d on layer %d", position, layer);
+		return 1;
+	}
+
+	if (pressed) {
+		return behavior_keymap_binding_pressed(behavior, position, binding->param1, binding->param2);
+	} else {
+		return behavior_keymap_binding_released(behavior, position, binding->param1, binding->param2);
+	}
+}
 			
+int zmk_keymap_position_state_changed(u32_t position, bool pressed)
+{
+	for (int layer = ZMK_KEYMAP_LAYERS_LEN - 1; layer >= zmk_keymap_layer_default; layer--)
+	{
+		u32_t layer_state = pressed ? zmk_keymap_layer_state : zmk_keymap_active_behavior_layer[position];
+		if (is_active_layer(layer, layer_state))
+		{
+			int ret = zmk_keymap_apply_position_state(layer, position, pressed);
+
+			zmk_keymap_active_behavior_layer[position] = zmk_keymap_layer_state;
 
 			if (ret > 0) {
 				LOG_DBG("behavior processing to continue to next layer");
diff --git a/app/src/kscan_mock.c b/app/src/kscan_mock.c
index b0ba90b..d626c71 100644
--- a/app/src/kscan_mock.c
+++ b/app/src/kscan_mock.c
@@ -6,13 +6,14 @@
 
 #define DT_DRV_COMPAT zmk_kscan_mock
 
+#include <stdlib.h>
 #include <device.h>
 #include <drivers/kscan.h>
 #include <logging/log.h>
 
 LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
 
-#include <zmk/kscan-mock.h>
+#include <dt-bindings/zmk/kscan-mock.h>
 
 struct kscan_mock_data
 {
@@ -50,6 +51,7 @@ static int kscan_mock_configure(struct device *dev, kscan_callback_t callback)
     struct kscan_mock_config_##n                                                   \
     {                                                                              \
         u32_t events[DT_INST_PROP_LEN(n, events)];                                 \
+        bool exit_after;                                                           \
     };                                                                             \
     static void kscan_mock_schedule_next_event_##n(struct device *dev)             \
     {                                                                              \
@@ -60,6 +62,9 @@ static int kscan_mock_configure(struct device *dev, kscan_callback_t callback)
             u32_t ev = cfg->events[data->event_index];                             \
             LOG_DBG("delaying next keypress: %d", ZMK_MOCK_MSEC(ev));              \
             k_delayed_work_submit(&data->work, K_MSEC(ZMK_MOCK_MSEC(ev)));         \
+        } else if (cfg->exit_after) {                                              \
+            LOG_DBG("Exiting");                                                    \
+            exit(0);                                                               \
         }                                                                          \
     }                                                                              \
     static void kscan_mock_work_handler_##n(struct k_work *work)                   \
@@ -67,12 +72,13 @@ static int kscan_mock_configure(struct device *dev, kscan_callback_t callback)
         struct kscan_mock_data *data =                                             \
             CONTAINER_OF(work, struct kscan_mock_data, work);                      \
         const struct kscan_mock_config_##n *cfg = data->dev->config_info;          \
-        u32_t ev = cfg->events[data->event_index++];                               \
+        u32_t ev = cfg->events[data->event_index];                                 \
         LOG_DBG("ev %u row %d column %d state %d\n", ev,                           \
                 ZMK_MOCK_ROW(ev), ZMK_MOCK_COL(ev), ZMK_MOCK_IS_PRESS(ev));        \
         data->callback(data->dev,                                                  \
                        ZMK_MOCK_ROW(ev), ZMK_MOCK_COL(ev), ZMK_MOCK_IS_PRESS(ev)); \
         kscan_mock_schedule_next_event_##n(data->dev);                             \
+        data->event_index++;                                                       \
     }                                                                              \
     static int kscan_mock_init_##n(struct device *dev)                             \
     {                                                                              \
@@ -93,11 +99,12 @@ static int kscan_mock_configure(struct device *dev, kscan_callback_t callback)
     };                                                                             \
     static struct kscan_mock_data kscan_mock_data_##n;                             \
     static const struct kscan_mock_config_##n kscan_mock_config_##n = {            \
-        .events = DT_INST_PROP(n, events)};                                        \
+        .events = DT_INST_PROP(n, events),                                         \
+        .exit_after = DT_INST_PROP(n, exit_after) };                               \
     DEVICE_AND_API_INIT(kscan_mock_##n, DT_INST_LABEL(n), kscan_mock_init_##n,     \
                         &kscan_mock_data_##n,                                      \
                         &kscan_mock_config_##n,                                    \
                         APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT,          \
                         &mock_driver_api_##n);
 
-DT_INST_FOREACH_STATUS_OKAY(MOCK_INST_INIT)
\ No newline at end of file
+DT_INST_FOREACH_STATUS_OKAY(MOCK_INST_INIT)
diff --git a/app/src/split/bluetooth/central.c b/app/src/split/bluetooth/central.c
index b6d7222..237096f 100644
--- a/app/src/split/bluetooth/central.c
+++ b/app/src/split/bluetooth/central.c
@@ -149,6 +149,12 @@ static void split_central_process_connection(struct bt_conn *conn) {
 			return;
 		}
 	}
+
+	struct bt_conn_info info;
+
+	bt_conn_get_info(conn, &info);
+
+	LOG_DBG("New connection params: Interval: %d, Latency: %d, PHY: %d", info.le.interval, info.le.latency, info.le.phy->rx_phy);
 }
 
 static bool split_central_eir_found(struct bt_data *data, void *user_data)
@@ -199,13 +205,19 @@ static bool split_central_eir_found(struct bt_data *data, void *user_data)
 				LOG_DBG("Found existing connection");
 				split_central_process_connection(default_conn);
 			} else {
-				param = BT_LE_CONN_PARAM(0x0006, 0x000c, 5, 400);
+				param = BT_LE_CONN_PARAM(0x0006, 0x0006, 30, 400);
 				err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN,
 							param, &default_conn);
 				if (err) {
 					LOG_ERR("Create conn failed (err %d)", err);
 					start_scan();
 				}
+
+				err = bt_conn_le_phy_update(default_conn, BT_CONN_LE_PHY_PARAM_2M);
+				if (err) {
+					LOG_ERR("Update phy conn failed (err %d)", err);
+					start_scan();
+				}
 			}
 
 			return false;
diff --git a/app/src/usb_hid.c b/app/src/usb_hid.c
index 4c6dd4b..784fc25 100644
--- a/app/src/usb_hid.c
+++ b/app/src/usb_hid.c
@@ -11,18 +11,42 @@
 
 LOG_MODULE_DECLARE(zmk, CONFIG_ZMK_LOG_LEVEL);
 
-static enum usb_dc_status_code usb_status;
+static enum usb_dc_status_code usb_status = USB_DC_UNKNOWN;
 
 static struct device *hid_dev;
 
+static K_SEM_DEFINE(hid_sem, 1, 1);
+
+static void in_ready_cb(void)
+{
+	k_sem_give(&hid_sem);
+}
+
+static const struct hid_ops ops =
+{
+	.int_in_ready = in_ready_cb,
+};
+
 int zmk_usb_hid_send_report(const u8_t *report, size_t len)
 {
-	if (usb_status == USB_DC_SUSPEND)
-	{
+	switch(usb_status) {
+	case USB_DC_SUSPEND:
 		return usb_wakeup_request();
+	case USB_DC_ERROR:
+	case USB_DC_RESET:
+	case USB_DC_DISCONNECTED:
+	case USB_DC_UNKNOWN:
+		return -ENODEV;
+	default:
+		k_sem_take(&hid_sem, K_MSEC(30));
+		int err = hid_int_ep_write(hid_dev, report, len, NULL);
+
+		if (err) {
+			k_sem_give(&hid_sem);
+		}
+
+		return err;
 	}
-
-	return hid_int_ep_write(hid_dev, report, len, NULL);
 }
 
 void usb_hid_status_cb(enum usb_dc_status_code status, const u8_t *params)
@@ -43,7 +67,7 @@ static int zmk_usb_hid_init(struct device *_arg)
 
 	usb_hid_register_device(hid_dev,
 							zmk_hid_report_desc, sizeof(zmk_hid_report_desc),
-							NULL);
+							&ops);
 
 	usb_hid_init(hid_dev);