diff --git a/left/build/kds/.settings/language.settings.xml b/left/build/kds/.settings/language.settings.xml
index 8a3e35d..2fd83ce 100644
--- a/left/build/kds/.settings/language.settings.xml
+++ b/left/build/kds/.settings/language.settings.xml
@@ -5,7 +5,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-58927704711430701" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings Cross ARM" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-452732215601217275" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings Cross ARM" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
@@ -16,7 +16,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="2718165793832371" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings Cross ARM" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-460380576276138715" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings Cross ARM" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
diff --git a/right/build/kds/.gitignore b/right/build/kds/.gitignore
new file mode 100644
index 0000000..530774c
--- /dev/null
+++ b/right/build/kds/.gitignore
@@ -0,0 +1 @@
+/debug
diff --git a/right/build/kds/.settings/language.settings.xml b/right/build/kds/.settings/language.settings.xml
index dbd3995..4bdb3e2 100644
--- a/right/build/kds/.settings/language.settings.xml
+++ b/right/build/kds/.settings/language.settings.xml
@@ -5,7 +5,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-54294142085643834" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings Cross ARM" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-478650929576438734" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings Cross ARM" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
@@ -16,7 +16,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-53037283027211482" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings Cross ARM" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="org.eclipse.cdt.managedbuilder.language.settings.providers.GCCBuiltinSpecsDetector" console="false" env-hash="-485013785041391918" id="ilg.gnuarmeclipse.managedbuild.cross.GCCBuiltinSpecsDetector" keep-relative-paths="false" name="CDT GCC Built-in Compiler Settings Cross ARM" parameter="${COMMAND} ${FLAGS} ${cross_toolchain_flags} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
diff --git a/right/build/kds/uhk-right release jlink.launch b/right/build/kds/uhk-right release jlink.launch
index 82c79b5..d223d70 100644
--- a/right/build/kds/uhk-right release jlink.launch	
+++ b/right/build/kds/uhk-right release jlink.launch	
@@ -70,7 +70,7 @@
 <stringAttribute key="org.eclipse.cdt.launch.PROGRAM_NAME" value="release/uhk-right.elf"/>
 <stringAttribute key="org.eclipse.cdt.launch.PROJECT_ATTR" value="uhk-right"/>
 <booleanAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_AUTO_ATTR" value="true"/>
-<stringAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_ID_ATTR" value="ilg.gnuarmeclipse.managedbuild.cross.config.elf.release.1939339834"/>
+<stringAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_ID_ATTR" value=""/>
 <listAttribute key="org.eclipse.debug.core.MAPPED_RESOURCE_PATHS">
 <listEntry value="/uhk-right"/>
 </listAttribute>
diff --git a/right/src/default_layout.c b/right/src/default_layout.c
new file mode 100644
index 0000000..b065827
--- /dev/null
+++ b/right/src/default_layout.c
@@ -0,0 +1,77 @@
+#include "default_layout.h"
+
+KEYBOARD_LAYOUT(defaultKeyboardLayout)={
+		{HID_KEYBOARD_SC_7_AND_AMPERSAND, HID_KEYBOARD_SC_F7},
+		{HID_KEYBOARD_SC_8_AND_ASTERISK, HID_KEYBOARD_SC_F8},
+		{HID_KEYBOARD_SC_9_AND_OPENING_PARENTHESIS, HID_KEYBOARD_SC_F9},
+		{HID_KEYBOARD_SC_0_AND_CLOSING_PARENTHESIS, HID_KEYBOARD_SC_F10},
+		{HID_KEYBOARD_SC_MINUS_AND_UNDERSCORE, HID_KEYBOARD_SC_F11},
+		{HID_KEYBOARD_SC_EQUAL_AND_PLUS, HID_KEYBOARD_SC_F12},
+		{HID_KEYBOARD_SC_BACKSPACE, HID_KEYBOARD_SC_DELETE},
+		{HID_KEYBOARD_SC_U},
+		{HID_KEYBOARD_SC_I, HID_KEYBOARD_SC_UP_ARROW},
+		{HID_KEYBOARD_SC_O},
+		{HID_KEYBOARD_SC_P, HID_KEYBOARD_SC_PRINT_SCREEN},
+		{HID_KEYBOARD_SC_OPENING_BRACKET_AND_OPENING_BRACE, HID_KEYBOARD_SC_SCROLL_LOCK},
+		{HID_KEYBOARD_SC_CLOSING_BRACKET_AND_CLOSING_BRACE, HID_KEYBOARD_SC_PAUSE},
+		{HID_KEYBOARD_SC_BACKSLASH_AND_PIPE},
+		{HID_KEYBOARD_SC_Y, HID_KEYBOARD_SC_PAGE_UP},
+		{HID_KEYBOARD_SC_J, HID_KEYBOARD_SC_LEFT_ARROW},
+		{HID_KEYBOARD_SC_K, HID_KEYBOARD_SC_DOWN_ARROW},
+		{HID_KEYBOARD_SC_L, HID_KEYBOARD_SC_RIGHT_ARROW},
+		{HID_KEYBOARD_SC_SEMICOLON_AND_COLON},
+		{HID_KEYBOARD_SC_APOSTROPHE_AND_QUOTE},
+		{HID_KEYBOARD_SC_ENTER},
+		{HID_KEYBOARD_SC_H, HID_KEYBOARD_SC_PAGE_DOWN},
+		{HID_KEYBOARD_SC_N, HID_KEYBOARD_SC_MEDIA_MUTE},
+		{HID_KEYBOARD_SC_M},
+		{HID_KEYBOARD_SC_COMMA_AND_LESS_THAN_SIGN},
+		{HID_KEYBOARD_SC_DOT_AND_GREATER_THAN_SIGN},
+		{HID_KEYBOARD_SC_SLASH_AND_QUESTION_MARK},
+		{HID_KEYBOARD_SC_RIGHT_SHIFT},
+		{0}, //??
+		{HID_KEYBOARD_SC_SPACE},
+		{0}, //??
+		{0}, //Fn
+		{HID_KEYBOARD_SC_RIGHT_ALT},
+		{HID_KEYBOARD_SC_RIGHT_GUI},
+		{HID_KEYBOARD_SC_RIGHT_CONTROL},
+
+		//Left half
+		{HID_KEYBOARD_SC_GRAVE_ACCENT_AND_TILDE, HID_KEYBOARD_SC_ESCAPE},
+		{HID_KEYBOARD_SC_1_AND_EXCLAMATION, HID_KEYBOARD_SC_F1},
+		{HID_KEYBOARD_SC_2_AND_AT, HID_KEYBOARD_SC_F2},
+		{HID_KEYBOARD_SC_3_AND_HASHMARK, HID_KEYBOARD_SC_F3},
+		{HID_KEYBOARD_SC_4_AND_DOLLAR, HID_KEYBOARD_SC_F4},
+		{HID_KEYBOARD_SC_5_AND_PERCENTAGE, HID_KEYBOARD_SC_F5},
+		{HID_KEYBOARD_SC_6_AND_CARET, HID_KEYBOARD_SC_F6},
+		{HID_KEYBOARD_SC_TAB},
+		{HID_KEYBOARD_SC_Q},
+		{HID_KEYBOARD_SC_W, HID_KEYBOARD_SC_UP_ARROW},
+		{HID_KEYBOARD_SC_E},
+		{HID_KEYBOARD_SC_R},
+		{0}, //??
+		{HID_KEYBOARD_SC_T, HID_KEYBOARD_SC_HOME},
+		{HID_KEYBOARD_SC_CAPS_LOCK},
+		{HID_KEYBOARD_SC_A, HID_KEYBOARD_SC_LEFT_ARROW},
+		{HID_KEYBOARD_SC_S, HID_KEYBOARD_SC_DOWN_ARROW},
+		{HID_KEYBOARD_SC_D, HID_KEYBOARD_SC_RIGHT_ARROW},
+		{HID_KEYBOARD_SC_F, HID_KEYBOARD_SC_DELETE},
+		{0}, //??
+		{HID_KEYBOARD_SC_G, HID_KEYBOARD_SC_END},
+		{HID_KEYBOARD_SC_LEFT_SHIFT},
+		{HID_KEYBOARD_SC_NON_US_BACKSLASH_AND_PIPE}, //no idea if this is the right one},
+		{HID_KEYBOARD_SC_Z, HID_KEYBOARD_SC_MEDIA_BACKWARD},
+		{HID_KEYBOARD_SC_X, HID_KEYBOARD_SC_MEDIA_PLAY},
+		{HID_KEYBOARD_SC_C, HID_KEYBOARD_SC_MEDIA_FORWARD},
+		{HID_KEYBOARD_SC_V, HID_KEYBOARD_SC_MEDIA_VOLUME_DOWN},
+		{HID_KEYBOARD_SC_B, HID_KEYBOARD_SC_MEDIA_VOLUME_UP},
+		{HID_KEYBOARD_SC_LEFT_CONTROL},
+		{HID_KEYBOARD_SC_LEFT_GUI},
+		{HID_KEYBOARD_SC_LEFT_ALT},
+		{0}, //Fn
+		{0}, //??
+		{0}, //Mod
+		{0} //??
+};
+
diff --git a/right/src/default_layout.h b/right/src/default_layout.h
new file mode 100644
index 0000000..f28875f
--- /dev/null
+++ b/right/src/default_layout.h
@@ -0,0 +1,17 @@
+/*
+ * default_layout.h
+ *
+ *  Created on: 2016 nov. 13
+ *      Author: xdever
+ */
+
+#ifndef SRC_DEFAULT_LAYOUT_H_
+#define SRC_DEFAULT_LAYOUT_H_
+
+#include <stdint.h>
+#include "keyboard_layout.h"
+
+extern KEYBOARD_LAYOUT(defaultKeyboardLayout);
+
+
+#endif /* SRC_DEFAULT_LAYOUT_H_ */
diff --git a/right/src/keyboard_layout.h b/right/src/keyboard_layout.h
new file mode 100644
index 0000000..1df1e6e
--- /dev/null
+++ b/right/src/keyboard_layout.h
@@ -0,0 +1,57 @@
+#ifndef KEYBOARD_LAYOUT_H_
+#define KEYBOARD_LAYOUT_H_
+
+#include <stdint.h>
+#include "lufa/HIDClassCommon.h"
+
+/**
+ * Keyboard layout is a 2D array of scan codes.
+ *
+ * First dimension is the Key ID of a given key. Key IDs are the indices of the
+ * of the active keys of the key_matrix_t structure. In case of left half, an
+ * offset of 35 is added.
+ *
+ * For each Key ID, there are 4 different possible scan codes:
+ * 		- default, when no modifiers are pressed
+ * 		- mod layer
+ * 		- fn layer
+ * 		- mod+fn layer
+ *
+ */
+
+#define KEY_STATE_COUNT (5*7)
+
+#define LAYOUT_KEY_COUNT 70
+#define LAYOUT_MOD_COUNT 4
+
+#define LAYOUT_LEFT_OFFSET KEY_STATE_COUNT
+
+#define KEYBOARD_LAYOUT(name) const uint8_t name[LAYOUT_KEY_COUNT][LAYOUT_MOD_COUNT]
+
+#define KEYID_LEFT_MOD 33
+#define KEYID_LEFT_FN 31
+#define KEYID_RIGHT_FN 31
+
+#define MODIFIER_MOD_PRESSED 1
+#define MODIFIER_FN_PRESSED 2
+
+
+static inline __attribute__((always_inline)) uint8_t getKeycode(KEYBOARD_LAYOUT(layout), uint8_t keyId, uint8_t modifierState)
+{
+	if (keyId<LAYOUT_KEY_COUNT)
+		return layout[keyId][modifierState];
+	else
+		return 0;
+}
+
+static inline __attribute__((always_inline)) uint8_t getModifierState(const uint8_t *leftKeyStates, const uint8_t *rightKeyStates){
+	uint8_t mod=0;
+	if (leftKeyStates[KEYID_LEFT_MOD])
+		mod |= MODIFIER_MOD_PRESSED;
+	if (leftKeyStates[KEYID_LEFT_FN] | rightKeyStates[KEYID_RIGHT_FN])
+		mod |= MODIFIER_FN_PRESSED;
+
+	return mod;
+}
+
+#endif
diff --git a/right/src/usb_interface_keyboard.c b/right/src/usb_interface_keyboard.c
index f18f685..b463147 100644
--- a/right/src/usb_interface_keyboard.c
+++ b/right/src/usb_interface_keyboard.c
@@ -6,6 +6,7 @@
 #include "fsl_i2c.h"
 #include "i2c.h"
 #include "i2c_addresses.h"
+#include "default_layout.h"
 
 static usb_device_endpoint_struct_t UsbKeyboardEndpoints[USB_KEYBOARD_ENDPOINT_COUNT] = {{
     USB_KEYBOARD_ENDPOINT_INDEX | (USB_IN << USB_DESCRIPTOR_ENDPOINT_ADDRESS_DIRECTION_SHIFT),
@@ -65,107 +66,12 @@ key_matrix_t keyMatrix = {
     }
 };
 
-#define KEY_STATE_COUNT (5*7)
-uint8_t leftKeyStates[KEY_STATE_COUNT] = {0};
-
-uint8_t defaultLayout[][2]={
-		{HID_KEYBOARD_SC_7_AND_AMPERSAND, HID_KEYBOARD_SC_F7},
-		{HID_KEYBOARD_SC_8_AND_ASTERISK, HID_KEYBOARD_SC_F8},
-		{HID_KEYBOARD_SC_9_AND_OPENING_PARENTHESIS, HID_KEYBOARD_SC_F9},
-		{HID_KEYBOARD_SC_0_AND_CLOSING_PARENTHESIS, HID_KEYBOARD_SC_F10},
-		{HID_KEYBOARD_SC_MINUS_AND_UNDERSCORE, HID_KEYBOARD_SC_F11},
-		{HID_KEYBOARD_SC_EQUAL_AND_PLUS, HID_KEYBOARD_SC_F12},
-		{HID_KEYBOARD_SC_BACKSPACE, HID_KEYBOARD_SC_DELETE},
-		{HID_KEYBOARD_SC_U},
-		{HID_KEYBOARD_SC_I, HID_KEYBOARD_SC_UP_ARROW},
-		{HID_KEYBOARD_SC_O},
-		{HID_KEYBOARD_SC_P, HID_KEYBOARD_SC_PRINT_SCREEN},
-		{HID_KEYBOARD_SC_OPENING_BRACKET_AND_OPENING_BRACE, HID_KEYBOARD_SC_SCROLL_LOCK},
-		{HID_KEYBOARD_SC_CLOSING_BRACKET_AND_CLOSING_BRACE, HID_KEYBOARD_SC_PAUSE},
-		{HID_KEYBOARD_SC_BACKSLASH_AND_PIPE},
-		{HID_KEYBOARD_SC_Y, HID_KEYBOARD_SC_PAGE_UP},
-		{HID_KEYBOARD_SC_J, HID_KEYBOARD_SC_LEFT_ARROW},
-		{HID_KEYBOARD_SC_K, HID_KEYBOARD_SC_DOWN_ARROW},
-		{HID_KEYBOARD_SC_L, HID_KEYBOARD_SC_RIGHT_ARROW},
-		{HID_KEYBOARD_SC_SEMICOLON_AND_COLON},
-		{HID_KEYBOARD_SC_APOSTROPHE_AND_QUOTE},
-		{HID_KEYBOARD_SC_ENTER},
-		{HID_KEYBOARD_SC_H, HID_KEYBOARD_SC_PAGE_DOWN},
-		{HID_KEYBOARD_SC_N, HID_KEYBOARD_SC_MEDIA_MUTE},
-		{HID_KEYBOARD_SC_M},
-		{HID_KEYBOARD_SC_COMMA_AND_LESS_THAN_SIGN},
-		{HID_KEYBOARD_SC_DOT_AND_GREATER_THAN_SIGN},
-		{HID_KEYBOARD_SC_SLASH_AND_QUESTION_MARK},
-		{HID_KEYBOARD_SC_RIGHT_SHIFT},
-		{0}, //??
-		{HID_KEYBOARD_SC_SPACE},
-		{0}, //??
-		{0}, //Fn
-		{HID_KEYBOARD_SC_RIGHT_ALT},
-		{HID_KEYBOARD_SC_RIGHT_GUI},
-		{HID_KEYBOARD_SC_RIGHT_CONTROL},
-		//Left half
-		{HID_KEYBOARD_SC_GRAVE_ACCENT_AND_TILDE, HID_KEYBOARD_SC_ESCAPE},
-		{HID_KEYBOARD_SC_1_AND_EXCLAMATION, HID_KEYBOARD_SC_F1},
-		{HID_KEYBOARD_SC_2_AND_AT, HID_KEYBOARD_SC_F2},
-		{HID_KEYBOARD_SC_3_AND_HASHMARK, HID_KEYBOARD_SC_F3},
-		{HID_KEYBOARD_SC_4_AND_DOLLAR, HID_KEYBOARD_SC_F4},
-		{HID_KEYBOARD_SC_5_AND_PERCENTAGE, HID_KEYBOARD_SC_F5},
-		{HID_KEYBOARD_SC_6_AND_CARET, HID_KEYBOARD_SC_F6},
-		{HID_KEYBOARD_SC_TAB},
-		{HID_KEYBOARD_SC_Q},
-		{HID_KEYBOARD_SC_W, HID_KEYBOARD_SC_UP_ARROW},
-		{HID_KEYBOARD_SC_E},
-		{HID_KEYBOARD_SC_R},
-		{0}, //??
-		{HID_KEYBOARD_SC_T, HID_KEYBOARD_SC_HOME},
-		{HID_KEYBOARD_SC_CAPS_LOCK},
-		{HID_KEYBOARD_SC_A, HID_KEYBOARD_SC_LEFT_ARROW},
-		{HID_KEYBOARD_SC_S, HID_KEYBOARD_SC_DOWN_ARROW},
-		{HID_KEYBOARD_SC_D, HID_KEYBOARD_SC_RIGHT_ARROW},
-		{HID_KEYBOARD_SC_F, HID_KEYBOARD_SC_DELETE},
-		{0}, //??
-		{HID_KEYBOARD_SC_G, HID_KEYBOARD_SC_END},
-		{HID_KEYBOARD_SC_LEFT_SHIFT},
-		{HID_KEYBOARD_SC_NON_US_BACKSLASH_AND_PIPE}, //no idea if this is the right one},
-		{HID_KEYBOARD_SC_Z, HID_KEYBOARD_SC_MEDIA_BACKWARD},
-		{HID_KEYBOARD_SC_X, HID_KEYBOARD_SC_MEDIA_PLAY},
-		{HID_KEYBOARD_SC_C, HID_KEYBOARD_SC_MEDIA_FORWARD},
-		{HID_KEYBOARD_SC_V, HID_KEYBOARD_SC_MEDIA_VOLUME_DOWN},
-		{HID_KEYBOARD_SC_B, HID_KEYBOARD_SC_MEDIA_VOLUME_UP},
-		{HID_KEYBOARD_SC_LEFT_CONTROL},
-		{HID_KEYBOARD_SC_LEFT_GUI},
-		{HID_KEYBOARD_SC_LEFT_ALT},
-		{0}, //Fn
-		{0}, //??
-		{0}, //Mod
-};
-
-
-#define MOD_PRESSED 1
-#define FN_PRESSED 2
-
-#define MOD_KEYID 33
-#define LEFT_FN 30
-#define RIGHT_FN 33
-
-#define LEFT_OFFSET 35
-
-static uint8_t modifierState=0;
-
-static uint8_t getKeycode(uint8_t keyId)
-{
-	int activeIndex = (modifierState & MOD_PRESSED) ? 1 : 0;
-
-	if (keyId<sizeof(defaultLayout))
-		return defaultLayout[keyId][activeIndex];
-	else
-		return 0;
-}
 
 
 static usb_status_t UsbKeyboardAction(void)
 {
+	uint8_t leftKeyStates[KEY_STATE_COUNT] = {0};
+
     UsbKeyboardReport.modifiers = 0;
     UsbKeyboardReport.reserved = 0;
 
@@ -179,24 +85,28 @@ static usb_status_t UsbKeyboardAction(void)
 
     uint8_t data[] = {0};
     I2cWrite(I2C_MAIN_BUS_BASEADDR, I2C_ADDRESS_LEFT_KEYBOARD_HALF, data, sizeof(data));
-
     I2cRead(I2C_MAIN_BUS_BASEADDR, I2C_ADDRESS_LEFT_KEYBOARD_HALF, leftKeyStates, KEY_STATE_COUNT);
 
-    modifierState = (leftKeyStates[MOD_KEYID] ? MOD_PRESSED : 0) |
-    		((leftKeyStates[LEFT_FN] | keyMatrix.keyStates[RIGHT_FN]) ? FN_PRESSED : 0);
+    uint8_t modifierState = getModifierState(leftKeyStates, keyMatrix.keyStates);
 
     scancodeIdx = 0;
     for (uint8_t keyId=0; keyId<KEYBOARD_MATRIX_COLS_NUM*KEYBOARD_MATRIX_ROWS_NUM; keyId++) {
-        if (keyMatrix.keyStates[keyId] && scancodeIdx<USB_KEYBOARD_MAX_KEYS) {
-        	uint8_t code=getKeycode(keyId);
+    	if (scancodeIdx>=USB_KEYBOARD_MAX_KEYS)
+    		break;
+
+        if (keyMatrix.keyStates[keyId]) {
+        	uint8_t code=getKeycode(defaultKeyboardLayout, keyId, modifierState);
         	if (code)
         		UsbKeyboardReport.scancodes[scancodeIdx++] = code;
         }
     }
 
     for (uint8_t keyId=0; keyId<KEY_STATE_COUNT; keyId++) {
-        if (leftKeyStates[keyId] && scancodeIdx<USB_KEYBOARD_MAX_KEYS) {
-        	uint8_t code=getKeycode(LEFT_OFFSET+keyId);
+    	if (scancodeIdx>=USB_KEYBOARD_MAX_KEYS)
+			break;
+
+        if (leftKeyStates[keyId]) {
+        	uint8_t code=getKeycode(defaultKeyboardLayout, LAYOUT_LEFT_OFFSET+keyId, modifierState);
 			if (code)
 				UsbKeyboardReport.scancodes[scancodeIdx++] = code;
         }