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/left/src/main.c b/left/src/main.c
index 7117ed8..51d23e6 100644
--- a/left/src/main.c
+++ b/left/src/main.c
@@ -78,5 +78,6 @@ int main(void)
     KeyMatrix_Init(&keyMatrix);
     while (1) {
         KeyMatrix_Scan(&keyMatrix);
+        asm("wfi");
     }
 }
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..3b830ba
--- /dev/null
+++ b/right/src/default_layout.h
@@ -0,0 +1,9 @@
+#ifndef SRC_DEFAULT_LAYOUT_H_
+#define SRC_DEFAULT_LAYOUT_H_
+
+#include <stdint.h>
+#include "keyboard_layout.h"
+
+extern KEYBOARD_LAYOUT(defaultKeyboardLayout);
+
+#endif
diff --git a/right/src/keyboard_layout.h b/right/src/keyboard_layout.h
new file mode 100644
index 0000000..881ac0e
--- /dev/null
+++ b/right/src/keyboard_layout.h
@@ -0,0 +1,65 @@
+#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) {
+		uint8_t k = layout[keyId][modifierState];
+		if (k==0) {
+			k = layout[keyId][0];
+		}
+
+		return k;
+	} 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/led_driver.c b/right/src/led_driver.c
index ab65132..e58ef44 100644
--- a/right/src/led_driver.c
+++ b/right/src/led_driver.c
@@ -19,7 +19,7 @@ void LedDriver_WriteRegister(uint8_t i2cAddress, uint8_t reg, uint8_t val)
     LedDriver_WriteBuffer(i2cAddress, buffer, sizeof(buffer));
 }
 
-void LedDriver_EnableAllLeds()
+void LedDriver_InitAllLeds(char isEnabled)
 {
     CLOCK_EnableClock(LED_DRIVER_SDB_CLOCK);
     PORT_SetPinMux(LED_DRIVER_SDB_PORT, LED_DRIVER_SDB_PIN, kPORT_MuxAsGpio);
@@ -35,14 +35,14 @@ void LedDriver_EnableAllLeds()
         LedDriver_WriteRegister(address, LED_DRIVER_REGISTER_FRAME, LED_DRIVER_FRAME_1);
 
         uint8_t i;
+        for (i=FRAME_REGISTER_PWM_FIRST; i<=FRAME_REGISTER_PWM_LAST; i++) {
+		   LedDriver_WriteRegister(address, i, isEnabled ? 0xFF : 0x00);
+	   }
         for (i=FRAME_REGISTER_LED_CONTROL_FIRST; i<=FRAME_REGISTER_LED_CONTROL_LAST; i++) {
             LedDriver_WriteRegister(address, i, 0xff);
         }
         for (i=FRAME_REGISTER_BLINK_CONTROL_FIRST; i<=FRAME_REGISTER_BLINK_CONTROL_LAST; i++) {
             LedDriver_WriteRegister(address, i, 0x00);
         }
-        for (i=FRAME_REGISTER_PWM_FIRST; i<=FRAME_REGISTER_PWM_LAST; i++) {
-            LedDriver_WriteRegister(address, i, 0xff);
-        }
     }
 }
diff --git a/right/src/led_driver.h b/right/src/led_driver.h
index 5bdd532..d795b49 100644
--- a/right/src/led_driver.h
+++ b/right/src/led_driver.h
@@ -43,6 +43,6 @@
 
     extern void LedDriver_WriteBuffer(uint8_t i2cAddress, uint8_t buffer[], uint8_t size);
     extern void LedDriver_WriteRegister(uint8_t i2cAddress, uint8_t reg, uint8_t val);
-    extern void LedDriver_EnableAllLeds();
+    extern void LedDriver_InitAllLeds(char isEnabled);
 
 #endif
diff --git a/right/src/main.c b/right/src/main.c
index c9ba028..06a7b00 100644
--- a/right/src/main.c
+++ b/right/src/main.c
@@ -6,8 +6,12 @@
 void main() {
     InitPeripherials();
     InitClock();
-    LedDriver_EnableAllLeds();
+    LedDriver_InitAllLeds(0);
+    usbKeyboadTask();
     InitUsb();
 
-    while (1);
+    while (1){
+    	usbKeyboadTask();
+    	asm("wfi");
+    }
 }
diff --git a/right/src/usb_interface_keyboard.c b/right/src/usb_interface_keyboard.c
index 673ecd5..a87745f 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),
@@ -39,7 +40,8 @@ usb_device_class_struct_t UsbKeyboardClass = {
     USB_DEVICE_CONFIGURATION_COUNT,
 };
 
-static usb_keyboard_report_t UsbKeyboardReport;
+volatile static int activeLayout=0;
+volatile static usb_keyboard_report_t UsbKeyboardReport[2];
 
 #define KEYBOARD_MATRIX_COLS_NUM 7
 #define KEYBOARD_MATRIX_ROWS_NUM 5
@@ -65,42 +67,79 @@ key_matrix_t keyMatrix = {
     }
 };
 
-#define KEY_STATE_COUNT (5*7)
-uint8_t leftKeyStates[KEY_STATE_COUNT] = {0};
+void readLeftKeys(uint8_t *stateVector){
+	uint8_t data[] = {0};
+	uint8_t success=0;
+
+	if (I2cWrite(I2C_MAIN_BUS_BASEADDR, I2C_ADDRESS_LEFT_KEYBOARD_HALF, data, sizeof(data)) == kStatus_Success) {
+		if (I2cRead(I2C_MAIN_BUS_BASEADDR, I2C_ADDRESS_LEFT_KEYBOARD_HALF, stateVector, KEY_STATE_COUNT) == kStatus_Success) {
+			success = 1;
+		}
+	}
+
+	if (!success) {
+		bzero(stateVector, KEY_STATE_COUNT);
+	}
+}
+
+void usbKeyboadTask(){
+	//Producer task for USB packets. When the USB interrupt is called,
+	//the newest packet is sent out immediately, thus not doing long task
+	//in the interrupt handler.
+	int newLayout = 1-activeLayout;
+
+	static uint8_t leftKeyStates[KEY_STATE_COUNT];
+
+	UsbKeyboardReport[newLayout].modifiers = 0;
+	UsbKeyboardReport[newLayout].reserved = 0;
+
+	KeyMatrix_Init(&keyMatrix);
+	KeyMatrix_Scan(&keyMatrix);
+
+	bzero(&UsbKeyboardReport[newLayout].scancodes, USB_KEYBOARD_MAX_KEYS*sizeof(UsbKeyboardReport[newLayout].scancodes[0]));
+
+	readLeftKeys(leftKeyStates);
+
+	uint8_t modifierState = getModifierState(leftKeyStates, keyMatrix.keyStates);
+
+	int scancodeIdx = 0;
+	for (uint8_t keyId=0; keyId<KEYBOARD_MATRIX_COLS_NUM*KEYBOARD_MATRIX_ROWS_NUM; keyId++) {
+		if (scancodeIdx>=USB_KEYBOARD_MAX_KEYS) {
+			break;
+		}
+
+		if (keyMatrix.keyStates[keyId]) {
+			uint8_t code=getKeycode(defaultKeyboardLayout, keyId, modifierState);
+			if (code) {
+				UsbKeyboardReport[newLayout].scancodes[scancodeIdx++] = code;
+			}
+		}
+	}
+
+	for (uint8_t keyId=0; keyId<KEY_STATE_COUNT; keyId++) {
+		if (scancodeIdx>=USB_KEYBOARD_MAX_KEYS) {
+			break;
+		}
+
+		if (leftKeyStates[keyId]) {
+			uint8_t code=getKeycode(defaultKeyboardLayout, LAYOUT_LEFT_OFFSET+keyId, modifierState);
+			if (code) {
+				UsbKeyboardReport[newLayout].scancodes[scancodeIdx++] = code;
+			}
+		}
+	}
+
+	//Change to the new layout in atomic operation (int copy). Even if
+	//the copy is not atomic itself, only single bit changes. So it can
+	//never be a problem
+	activeLayout = newLayout;
+}
+
 
 static usb_status_t UsbKeyboardAction(void)
 {
-    UsbKeyboardReport.modifiers = 0;
-    UsbKeyboardReport.reserved = 0;
-
-    KeyMatrix_Init(&keyMatrix);
-    KeyMatrix_Scan(&keyMatrix);
-
-    uint8_t scancodeIdx;
-    for (uint8_t scancodeIdx=0; scancodeIdx<USB_KEYBOARD_MAX_KEYS; scancodeIdx++) {
-        UsbKeyboardReport.scancodes[scancodeIdx] = 0;
-    }
-
-    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);
-
-    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) {
-            UsbKeyboardReport.scancodes[scancodeIdx++] = keyId + HID_KEYBOARD_SC_A;
-        }
-    }
-
-    for (uint8_t keyId=0; keyId<KEY_STATE_COUNT; keyId++) {
-        if (leftKeyStates[keyId] && scancodeIdx<USB_KEYBOARD_MAX_KEYS) {
-            UsbKeyboardReport.scancodes[scancodeIdx++] = keyId + HID_KEYBOARD_SC_A;
-        }
-    }
-
     return USB_DeviceHidSend(UsbCompositeDevice.keyboardHandle, USB_KEYBOARD_ENDPOINT_INDEX,
-                             (uint8_t*)&UsbKeyboardReport, USB_KEYBOARD_REPORT_LENGTH);
+                             (uint8_t*)&UsbKeyboardReport[activeLayout], USB_KEYBOARD_REPORT_LENGTH);
 }
 
 usb_status_t UsbKeyboardCallback(class_handle_t handle, uint32_t event, void *param)
diff --git a/right/src/usb_interface_keyboard.h b/right/src/usb_interface_keyboard.h
index b858751..c3fc84a 100644
--- a/right/src/usb_interface_keyboard.h
+++ b/right/src/usb_interface_keyboard.h
@@ -36,4 +36,5 @@
     extern usb_status_t UsbKeyboardSetConfiguration(class_handle_t handle, uint8_t configuration);
     extern usb_status_t UsbKeyboardSetInterface(class_handle_t handle, uint8_t interface, uint8_t alternateSetting);
 
+    extern void usbKeyboadTask();
 #endif
diff --git a/shared/i2c_adddresses.c b/shared/i2c_adddresses.c
index 948d22b..3f3a650 100644
--- a/shared/i2c_adddresses.c
+++ b/shared/i2c_adddresses.c
@@ -1,7 +1,7 @@
 #include "fsl_i2c.h"
 #include "i2c_addresses.h"
 
-void I2cRead(I2C_Type *baseAddress, uint8_t i2cAddress, uint8_t *data, uint8_t size)
+status_t I2cRead(I2C_Type *baseAddress, uint8_t i2cAddress, uint8_t *data, uint8_t size)
 {
     i2c_master_transfer_t masterXfer;
     masterXfer.slaveAddress = i2cAddress;
@@ -11,10 +11,10 @@ void I2cRead(I2C_Type *baseAddress, uint8_t i2cAddress, uint8_t *data, uint8_t s
     masterXfer.data = data;
     masterXfer.dataSize = size;
     masterXfer.flags = kI2C_TransferDefaultFlag;
-    I2C_MasterTransferBlocking(baseAddress, &masterXfer);
+    return I2C_MasterTransferBlocking(baseAddress, &masterXfer);
 }
 
-void I2cWrite(I2C_Type *baseAddress, uint8_t i2cAddress, uint8_t *data, uint8_t size)
+status_t I2cWrite(I2C_Type *baseAddress, uint8_t i2cAddress, uint8_t *data, uint8_t size)
 {
     i2c_master_transfer_t masterXfer;
     masterXfer.slaveAddress = i2cAddress;
@@ -24,5 +24,5 @@ void I2cWrite(I2C_Type *baseAddress, uint8_t i2cAddress, uint8_t *data, uint8_t
     masterXfer.data = data;
     masterXfer.dataSize = size;
     masterXfer.flags = kI2C_TransferDefaultFlag;
-    I2C_MasterTransferBlocking(baseAddress, &masterXfer);
+    return I2C_MasterTransferBlocking(baseAddress, &masterXfer);
 }
diff --git a/shared/i2c_addresses.h b/shared/i2c_addresses.h
index 687db97..5678d77 100644
--- a/shared/i2c_addresses.h
+++ b/shared/i2c_addresses.h
@@ -29,7 +29,7 @@
 
 // Functions:
 
-    void I2cRead(I2C_Type *baseAddress, uint8_t i2cAddress, uint8_t *data, uint8_t size);
-    void I2cWrite(I2C_Type *baseAddress, uint8_t i2cAddress, uint8_t *data, uint8_t size);
+    status_t I2cRead(I2C_Type *baseAddress, uint8_t i2cAddress, uint8_t *data, uint8_t size);
+    status_t I2cWrite(I2C_Type *baseAddress, uint8_t i2cAddress, uint8_t *data, uint8_t size);
 
 #endif