diff --git a/CMakeLists.txt b/CMakeLists.txt
index da0d9d9b..6b09b3a6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -9,8 +9,10 @@ project(zmk)
 
 # Add your source file to the "app" target. This must come after
 # find_package(Zephyr) which defines the target.
+target_include_directories(app PRIVATE include)
 target_sources(app PRIVATE src/kscan.c)
 target_sources(app PRIVATE src/keymap.c)
 target_sources(app PRIVATE src/usb_hid.c)
 target_sources(app PRIVATE src/main.c)
+target_sources(app PRIVATE src/handlers.c)
 
diff --git a/include/dt-bindings/zmk/keys.h b/include/dt-bindings/zmk/keys.h
index 4ba115ae..1d5beb80 100644
--- a/include/dt-bindings/zmk/keys.h
+++ b/include/dt-bindings/zmk/keys.h
@@ -1,5 +1,72 @@
 
-#define KC_A 4
-#define KC_B 5
-#define KC_C 6
-#define KC_D 7
+#pragma once
+
+#define KC_A 0x04
+#define KC_B 0x05
+#define KC_C 0x06
+#define KC_D 0x07
+#define KC_E 0x08
+#define KC_F 0x09
+#define KC_G 0x0A
+#define KC_H 0x0B
+#define KC_I 0x0C
+#define KC_J 0x0D
+#define KC_K 0x0E
+#define KC_L 0x0F
+#define KC_M 0x10
+#define KC_N 0x11
+#define KC_O 0x12
+#define KC_P 0x13
+#define KC_Q 0x14
+#define KC_R 0x15
+#define KC_S 0x16
+#define KC_T 0x17
+#define KC_U 0x18
+#define KC_V 0x19
+#define KC_W 0x1A
+#define KC_X 0x1B
+#define KC_Y 0x1C
+#define KC_Z 0x1D
+#define KC_1 0x1E
+#define KC_2 0x1F
+#define KC_3 0x20
+#define KC_4 0x21
+#define KC_5 0x22
+#define KC_6 0x23
+#define KC_7 0x24
+#define KC_8 0x25
+#define KC_9 0x26
+#define KC_0 0x27
+#define KC_RET 0x28
+#define KC_ESC 0x29
+#define KC_DEL 0x2A
+#define KC_TAB 0x2B
+#define KC_SPC 0x2C
+#define KC_MIN 0x2D
+#define KC_EQL 0x2E
+#define KC_LBKT 0x2F
+#define KC_RBKT 0x30
+#define KC_FSLH 0x31
+
+#define KC_SCLN 0x33
+#define KC_QUOT 0x34
+#define KC_GRAV 0x35
+#define KC_CMMA 0x36
+#define KC_DOT 0x37
+#define KC_BSLH 0x38
+#define KC_CLCK 0x39
+#define KC_F1 0x3A
+#define KC_F2 0x3B
+
+#define KC_APP 0x65
+
+#define MD_SHFT 0x01
+#define KC_ALT 0x02
+#define KC_CTRL 0x03
+
+#define ZC_RAIS 0xF0
+#define ZC_LOWR 0xF1
+#define ZC_TRNS 0xF2
+#define ZC_NOOP 0xF3
+
+#define ZC_CSTM(n) (0xFF + n)
\ No newline at end of file
diff --git a/src/handlers.c b/src/handlers.c
new file mode 100644
index 00000000..5eb1b512
--- /dev/null
+++ b/src/handlers.c
@@ -0,0 +1,25 @@
+
+#include "handlers.h"
+
+#include "usb_hid.h"
+
+__attribute__((weak)) bool zmk_handle_key_user(struct zmk_key_event key_event)
+{
+	return true;
+};
+
+void zmk_handle_key(struct zmk_key_event key_event)
+{
+	if (!zmk_handle_key_user(key_event))
+	{
+		return;
+	}
+	if (key_event.pressed)
+	{
+		zmk_usb_hid_press_key(key_event.key);
+	}
+	else
+	{
+		zmk_usb_hid_release_key(key_event.key);
+	}
+};
diff --git a/src/handlers.h b/src/handlers.h
new file mode 100644
index 00000000..3a935721
--- /dev/null
+++ b/src/handlers.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include "keymap.h"
+#include <dt-bindings/zmk/keys.h>
+
+struct zmk_key_event
+{
+    zmk_key key;
+    bool pressed;
+};
+
+void zmk_handle_key(struct zmk_key_event key_event);
diff --git a/src/keymap.c b/src/keymap.c
index ecfce59d..1bb624d6 100644
--- a/src/keymap.c
+++ b/src/keymap.c
@@ -1,34 +1,69 @@
 
 #include "keymap.h"
 
-static enum hid_kbd_code zmk_keymap[ZMK_KEYMAP_LAYERS_LEN][ZMK_MATRIX_ROWS * ZMK_MATRIX_COLS] = {
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE,layers,0)
-	DT_PROP_BY_PHANDLE_IDX(ZMK_KEYMAP_NODE,layers,0,keys),
+static u32_t zmk_keymap_layer_state = 0;
+static u32_t zmk_keymap_layer_default = 0;
+
+static zmk_key zmk_keymap[ZMK_KEYMAP_LAYERS_LEN][ZMK_MATRIX_ROWS * ZMK_MATRIX_COLS] = {
+#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 0)
+	DT_PROP_BY_PHANDLE_IDX(ZMK_KEYMAP_NODE, layers, 0, keys),
 #endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE,layers,1)
-	DT_PROP_BY_PHANDLE_IDX(ZMK_KEYMAP_NODE,layers,1,keys),
+#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 1)
+	DT_PROP_BY_PHANDLE_IDX(ZMK_KEYMAP_NODE, layers, 1, keys),
 #endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE,layers,2)
-	DT_PROP_BY_PHANDLE_IDX(ZMK_KEYMAP_NODE,layers,2,keys),
+#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 2)
+	DT_PROP_BY_PHANDLE_IDX(ZMK_KEYMAP_NODE, layers, 2, keys),
 #endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE,layers,3)
-	DT_PROP_BY_PHANDLE_IDX(ZMK_KEYMAP_NODE,layers,3,keys),
+#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 3)
+	DT_PROP_BY_PHANDLE_IDX(ZMK_KEYMAP_NODE, layers, 3, keys),
 #endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE,layers,4)
-	DT_PROP_BY_PHANDLE_IDX(ZMK_KEYMAP_NODE,layers,4,keys),
+#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 4)
+	DT_PROP_BY_PHANDLE_IDX(ZMK_KEYMAP_NODE, layers, 4, keys),
 #endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE,layers,5)
-	DT_PROP_BY_PHANDLE_IDX(ZMK_KEYMAP_NODE,layers,5,keys),
+#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 5)
+	DT_PROP_BY_PHANDLE_IDX(ZMK_KEYMAP_NODE, layers, 5, keys),
 #endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE,layers,6)
-	DT_PROP_BY_PHANDLE_IDX(ZMK_KEYMAP_NODE,layers,6,keys),
+#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 6)
+	DT_PROP_BY_PHANDLE_IDX(ZMK_KEYMAP_NODE, layers, 6, keys),
 #endif
-#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE,layers,7)
-	DT_PROP_BY_PHANDLE_IDX(ZMK_KEYMAP_NODE,layers,7,keys),
+#if DT_PROP_HAS_IDX(ZMK_KEYMAP_NODE, layers, 7)
+	DT_PROP_BY_PHANDLE_IDX(ZMK_KEYMAP_NODE, layers, 7, keys),
 #endif
 };
 
-enum hid_kbd_code zmk_keymap_keycode_from_position(u32_t row, u32_t column)
+#define SET_LAYER_STATE(layer, state)               \
+	if (layer >= 32)                                \
+	{                                               \
+		return false;                               \
+	}                                               \
+	WRITE_BIT(zmk_keymap_layer_state, layer, true); \
+	return true;
+
+bool zmk_keymap_layer_activate(u8_t layer)
 {
-	return zmk_keymap[0][(row * ZMK_MATRIX_ROWS) + column];
+	SET_LAYER_STATE(layer, true);
+};
+
+bool zmk_keymap_layer_deactivate(u8_t layer)
+{
+	SET_LAYER_STATE(layer, false);
+};
+
+zmk_key zmk_keymap_keycode_from_position(u32_t row, u32_t column)
+{
+	for (int layer = ZMK_KEYMAP_LAYERS_LEN - 1; layer >= zmk_keymap_layer_default; layer--)
+	{
+		if ((zmk_keymap_layer_state & BIT(layer)) == BIT(layer))
+		{
+			zmk_key key = zmk_keymap[layer][(row * ZMK_MATRIX_ROWS) + column];
+			if (key == ZC_TRNS)
+			{
+				continue;
+			}
+
+			return key;
+		}
+	}
+
+	return ZC_NOOP;
 }
diff --git a/src/keymap.h b/src/keymap.h
index 1fa606f4..bb758d81 100644
--- a/src/keymap.h
+++ b/src/keymap.h
@@ -1,16 +1,19 @@
-#ifndef ZMK_KEYMAP_H
-#define ZMK_KEYMAP_H
+#pragma once
 
 #include <devicetree.h>
 #include <usb/usb_device.h>
 #include <usb/class/usb_hid.h>
+#include "dt-bindings/zmk/keys.h"
 
 #include "zmk.h"
 
 #define ZMK_KEYMAP_NODE DT_CHOSEN(zmk_keymap)
-#define ZMK_KEYMAP_LAYERS_LEN DT_PROP_LEN(ZMK_KEYMAP_NODE,layers)
+#define ZMK_KEYMAP_LAYERS_LEN DT_PROP_LEN(ZMK_KEYMAP_NODE, layers)
 
+typedef u64_t zmk_key;
 
-enum hid_kbd_code zmk_keymap_keycode_from_position(u32_t row, u32_t column);
+bool zmk_keymap_layer_activate(u8_t layer);
+bool zmk_keymap_layer_deactivate(u8_t layer);
 
-#endif
+zmk_key
+zmk_keymap_keycode_from_position(u32_t row, u32_t column);
diff --git a/src/kscan.c b/src/kscan.c
index 54d08c4f..25bbbfa2 100644
--- a/src/kscan.c
+++ b/src/kscan.c
@@ -13,17 +13,20 @@ LOG_MODULE_REGISTER(zmk_kscan, CONFIG_ZMK_KSCAN_LOG_LEVEL);
 
 #include "keymap.h"
 #include "usb_hid.h"
+#include "handlers.h"
 
 #define ZMK_KSCAN_EVENT_STATE_PRESSED 0
 #define ZMK_KSCAN_EVENT_STATE_RELEASED 1
 
-struct zmk_kscan_event {
+struct zmk_kscan_event
+{
 	u32_t row;
 	u32_t column;
 	u32_t state;
 };
 
-struct zmk_kscan_msg_processor {
+struct zmk_kscan_msg_processor
+{
 	struct k_work work;
 } msg_processor;
 
@@ -34,8 +37,7 @@ static void zmk_kscan_callback(struct device *dev, u32_t row, u32_t column, bool
 	struct zmk_kscan_event ev = {
 		.row = row,
 		.column = column,
-		.state = (pressed ? ZMK_KSCAN_EVENT_STATE_PRESSED : ZMK_KSCAN_EVENT_STATE_RELEASED)
-	};
+		.state = (pressed ? ZMK_KSCAN_EVENT_STATE_PRESSED : ZMK_KSCAN_EVENT_STATE_RELEASED)};
 
 	k_msgq_put(&zmk_kscan_msgq, &ev, K_NO_WAIT);
 	k_work_submit(&msg_processor.work);
@@ -45,23 +47,22 @@ void zmk_kscan_process_msgq(struct k_work *item)
 {
 	struct zmk_kscan_event ev;
 
-	while(k_msgq_get(&zmk_kscan_msgq, &ev, K_NO_WAIT) == 0) {
+	while (k_msgq_get(&zmk_kscan_msgq, &ev, K_NO_WAIT) == 0)
+	{
 		bool pressed = (ev.state == ZMK_KSCAN_EVENT_STATE_PRESSED);
-		// TODO: More than basic mapping, layers, etc.
-		enum hid_kbd_code code = zmk_keymap_keycode_from_position(ev.row, ev.column);
-		LOG_DBG("Row: %d, col: %d, code: %d, pressed: %s\n", ev.row, ev.column, code, (pressed ? "true" : "false"));
-		if (pressed) {
-			zmk_usb_hid_press_key(code);
-		} else {
-			zmk_usb_hid_release_key(code);
-		}
+		zmk_key key = zmk_keymap_keycode_from_position(ev.row, ev.column);
+		struct zmk_key_event kev = (struct zmk_key_event){.key = key, .pressed = pressed};
+
+		LOG_DBG("Row: %d, col: %d, key: %lld, pressed: %s\n", ev.row, ev.column, key, (pressed ? "true" : "false"));
+		zmk_handle_key(kev);
 	}
 }
 
-int zmk_kscan_init(char* name)
+int zmk_kscan_init(char *name)
 {
 	struct device *dev = device_get_binding(name);
-	if (dev == NULL) {
+	if (dev == NULL)
+	{
 		LOG_ERR("Failed to get the KSCAN device");
 		return -EINVAL;
 	}
diff --git a/src/kscan.h b/src/kscan.h
index 7f640316..ebffccda 100644
--- a/src/kscan.h
+++ b/src/kscan.h
@@ -1,6 +1,3 @@
-#ifndef ZMK_KSCAN_H
-#define ZMK_KSCAN_H
+#pragma once
 
 int zmk_kscan_init(char *name);
-
-#endif