diff --git a/right/src/usb_interface_keyboard.c b/right/src/usb_interface_keyboard.c
index f158c96..dd3489c 100644
--- a/right/src/usb_interface_keyboard.c
+++ b/right/src/usb_interface_keyboard.c
@@ -62,29 +62,48 @@ key_matrix_t keyMatrix = {
     }
 };
 
+uint8_t keystates[100];
+
 static usb_status_t UsbKeyboardAction(void)
 {
-    uint8_t scancode_i = 0;
     UsbKeyboardReport.modifiers = 0;
     UsbKeyboardReport.reserved = 0;
 
-    KeyMatrix_Init(keyMatrix);
+    KeyMatrix_Init(&keyMatrix);
+    //KeyMatrix_Scan(&keyMatrix);
 
     for (uint8_t scancode_idx=0; scancode_idx<USB_KEYBOARD_MAX_KEYS; scancode_idx++) {
         UsbKeyboardReport.scancodes[scancode_idx] = 0;
     }
 
+    uint8_t scancode_idx = 0;
     for (uint8_t col=0; col<KEYBOARD_MATRIX_COLS_NUM; col++) {
         GPIO_WritePinOutput(keyMatrix.cols[col].gpio, keyMatrix.cols[col].pin, 1);
         for (uint8_t row=0; row<KEYBOARD_MATRIX_ROWS_NUM; row++) {
             if (GPIO_ReadPinInput(keyMatrix.rows[row].gpio, keyMatrix.rows[row].pin)) {
                 GPIO_SetPinsOutput(TEST_LED_GPIO, 1 << TEST_LED_GPIO_PIN);
-                UsbKeyboardReport.scancodes[scancode_i] = HID_KEYBOARD_SC_A + row*KEYBOARD_MATRIX_COLS_NUM + col;
+                UsbKeyboardReport.scancodes[scancode_idx++] = HID_KEYBOARD_SC_A + row*KEYBOARD_MATRIX_COLS_NUM + col;
             }
         }
         GPIO_WritePinOutput(keyMatrix.cols[col].gpio, keyMatrix.cols[col].pin, 0);
     }
 
+/*
+    uint8_t keyId = 0;
+    for (uint8_t col=0; col<keyMatrix.colNum; col++) {
+        GPIO_WritePinOutput(keyMatrix.cols[col].gpio, keyMatrix.cols[col].pin, 1);
+        for (uint8_t row=0; row<keyMatrix.rowNum; row++) {
+            keystates[row+col*keyMatrix.rowNum] = GPIO_ReadPinInput(keyMatrix.rows[row].gpio, keyMatrix.rows[row].pin);
+        }
+        GPIO_WritePinOutput(keyMatrix.cols[col].gpio, keyMatrix.cols[col].pin, 0);
+    }
+
+    for (uint8_t keyId=0; keyId<KEYBOARD_MATRIX_COLS_NUM*KEYBOARD_MATRIX_ROWS_NUM; keyId++) {
+        if (keystates[keyId] && scancode_idx<USB_KEYBOARD_MAX_KEYS) {
+            UsbKeyboardReport.scancodes[scancode_idx++] = keyId + HID_KEYBOARD_SC_A;
+        }
+    }
+*/
     return USB_DeviceHidSend(UsbCompositeDevice.keyboardHandle, USB_KEYBOARD_ENDPOINT_INDEX,
                              (uint8_t*)&UsbKeyboardReport, USB_KEYBOARD_REPORT_LENGTH);
 }
diff --git a/shared/key_matrix.c b/shared/key_matrix.c
index d0fde81..aa9e8eb 100644
--- a/shared/key_matrix.c
+++ b/shared/key_matrix.c
@@ -1,21 +1,33 @@
 #include "fsl_gpio.h"
 #include "key_matrix.h"
 
-void KeyMatrix_Init(key_matrix_t keyMatrix)
+void KeyMatrix_Init(key_matrix_t *keyMatrix)
 {
-    for (uint8_t col_i=0; col_i<keyMatrix.colNum; col_i++) {
-        key_matrix_pin_t col = keyMatrix.cols[col_i];
+    for (uint8_t col_i=0; col_i<keyMatrix->colNum; col_i++) {
+        key_matrix_pin_t col = keyMatrix->cols[col_i];
         CLOCK_EnableClock(col.clock);
         PORT_SetPinConfig(col.port, col.pin,
                           &(port_pin_config_t){.pullSelect=kPORT_PullDisable, .mux=kPORT_MuxAsGpio});
         GPIO_PinInit(col.gpio, col.pin, &(gpio_pin_config_t){kGPIO_DigitalOutput, 1});
     }
 
-    for (uint8_t row_i=0; row_i<keyMatrix.rowNum; row_i++) {
-        key_matrix_pin_t row = keyMatrix.rows[row_i];
+    for (uint8_t row_i=0; row_i<keyMatrix->rowNum; row_i++) {
+        key_matrix_pin_t row = keyMatrix->rows[row_i];
         CLOCK_EnableClock(row.clock);
         PORT_SetPinConfig(row.port, row.pin,
                           &(port_pin_config_t){.pullSelect=kPORT_PullDown, .mux=kPORT_MuxAsGpio});
         GPIO_PinInit(row.gpio, row.pin, &(gpio_pin_config_t){kGPIO_DigitalInput});
     }
 }
+
+void KeyMatrix_Scan(key_matrix_t *keyMatrix)
+{
+    uint8_t keyId = 0;
+    for (uint8_t col=0; col<keyMatrix->colNum; col++) {
+        GPIO_WritePinOutput(keyMatrix->cols[col].gpio, keyMatrix->cols[col].pin, 1);
+        for (uint8_t row=0; row<keyMatrix->rowNum; row++) {
+            keyMatrix->keyStates[keyId++] = GPIO_ReadPinInput(keyMatrix->rows[row].gpio, keyMatrix->rows[row].pin);
+        }
+        GPIO_WritePinOutput(keyMatrix->cols[col].gpio, keyMatrix->cols[col].pin, 0);
+    }
+}
diff --git a/shared/key_matrix.h b/shared/key_matrix.h
index 0c36a13..1665999 100644
--- a/shared/key_matrix.h
+++ b/shared/key_matrix.h
@@ -6,6 +6,10 @@
     #include "fsl_common.h"
     #include "fsl_port.h"
 
+// Macros:
+
+    #define MAX_KEYS_IN_MATRIX 100
+
 // Type definitions:
 
     typedef struct {
@@ -20,10 +24,12 @@
         uint8_t rowNum;
         key_matrix_pin_t *cols;
         key_matrix_pin_t *rows;
+        uint8_t keyStates[MAX_KEYS_IN_MATRIX];
     } key_matrix_t;
 
 // Functions:
 
-    void KeyMatrix_Init(key_matrix_t keyMatrix);
+    void KeyMatrix_Init(key_matrix_t *keyMatrix);
+    void KeyMatrix_Scan(key_matrix_t *keyMatrix);
 
 #endif