26 files changed, 550 insertions, 181 deletions

diff --git a/common.mk b/common.mk
index d22adff51..cb030b331 100644
--- a/common.mk
+++ b/common.mk
@@ -66,7 +66,14 @@ endif
 
 ifdef KEYMAP_SECTION_ENABLE
     OPT_DEFS += -DKEYMAP_SECTION_ENABLE
-    EXTRALDFLAGS = -Wl,-L$(TMK_DIR),-Tldscript_keymap_avr5.x
+
+    ifeq ($(strip $(MCU)),atmega32u2)
+        EXTRALDFLAGS = -Wl,-L$(TMK_DIR),-Tldscript_keymap_avr35.x
+    else ifeq ($(strip $(MCU)),atmega32u4)
+        EXTRALDFLAGS = -Wl,-L$(TMK_DIR),-Tldscript_keymap_avr5.x
+    else
+        EXTRALDFLAGS = $(error no ldscript for keymap section)
+    endif
 endif
 
 # Version string
diff --git a/common/action_code.h b/common/action_code.h
index 32ef07216..4fe9c1d58 100644
--- a/common/action_code.h
+++ b/common/action_code.h
@@ -266,7 +266,7 @@ enum layer_pram_tap_op {
 #define ACTION_LAYER_ON_OFF(layer)                  ACTION_LAYER_TAP((layer), OP_ON_OFF)
 #define ACTION_LAYER_OFF_ON(layer)                  ACTION_LAYER_TAP((layer), OP_OFF_ON)
 #define ACTION_LAYER_SET_CLEAR(layer)               ACTION_LAYER_TAP((layer), OP_SET_CLEAR)
-#define ACTION_LAYER_MODS(layer, mods)              ACTION_LAYER_TAP((layer), 0xe0 | (mods)&0x0f)
+#define ACTION_LAYER_MODS(layer, mods)              ACTION_LAYER_TAP((layer), 0xe0 | ((mods)&0x0f))
 /* With Tapping */
 #define ACTION_LAYER_TAP_KEY(layer, key)            ACTION_LAYER_TAP((layer), (key))
 #define ACTION_LAYER_TAP_TOGGLE(layer)              ACTION_LAYER_TAP((layer), OP_TAP_TOGGLE)
diff --git a/common/action_macro.c b/common/action_macro.c
index ba93fc8b2..ffaf125c0 100644
--- a/common/action_macro.c
+++ b/common/action_macro.c
@@ -42,6 +42,7 @@ void action_macro_play(const macro_t *macro_p)
                 dprintf("KEY_DOWN(%02X)\n", macro);
                 if (IS_MOD(macro)) {
                     add_weak_mods(MOD_BIT(macro));
+                    send_keyboard_report();
                 } else {
                     register_code(macro);
                 }
@@ -51,6 +52,7 @@ void action_macro_play(const macro_t *macro_p)
                 dprintf("KEY_UP(%02X)\n", macro);
                 if (IS_MOD(macro)) {
                     del_weak_mods(MOD_BIT(macro));
+                    send_keyboard_report();
                 } else {
                     unregister_code(macro);
                 }
diff --git a/common/action_util.c b/common/action_util.c
index dbee630d1..f81877dd9 100644
--- a/common/action_util.c
+++ b/common/action_util.c
@@ -76,7 +76,7 @@ void send_keyboard_report(void) {
 void add_key(uint8_t key)
 {
 #ifdef NKRO_ENABLE
-    if (keyboard_nkro) {
+    if (keyboard_protocol && keyboard_nkro) {
         add_key_bit(key);
         return;
     }
@@ -87,7 +87,7 @@ void add_key(uint8_t key)
 void del_key(uint8_t key)
 {
 #ifdef NKRO_ENABLE
-    if (keyboard_nkro) {
+    if (keyboard_protocol && keyboard_nkro) {
         del_key_bit(key);
         return;
     }
@@ -160,7 +160,7 @@ uint8_t has_anymod(void)
 uint8_t get_first_key(void)
 {
 #ifdef NKRO_ENABLE
-    if (keyboard_nkro) {
+    if (keyboard_protocol && keyboard_nkro) {
         uint8_t i = 0;
         for (; i < KEYBOARD_REPORT_BITS && !keyboard_report->nkro.bits[i]; i++)
             ;
diff --git a/common/avr/bootloader.c b/common/avr/bootloader.c
index cda295b18..7c744e8c7 100644
--- a/common/avr/bootloader.c
+++ b/common/avr/bootloader.c
@@ -11,12 +11,49 @@
 #endif
 
 
-/* Boot Section Size in *BYTEs*
- *   Teensy   halfKay    512
- *   Teensy++ halfKay    1024
- *   Atmel DFU loader    4096
- *   LUFA bootloader     4096
- *   USBaspLoader        2048
+/* Bootloader Size in *bytes*
+ *
+ * AVR Boot section size are defined by setting BOOTSZ fuse in fact. Consult with your MCU datasheet.
+ * Note that 'Word'(2 bytes) size and address are used in datasheet while TMK uses 'Byte'.
+ *
+ *
+ * Size of Bootloaders in bytes:
+ *   Atmel DFU loader(ATmega32U4)   4096
+ *   Atmel DFU loader(AT90USB128)   8192
+ *   LUFA bootloader(ATmega32U4)    4096
+ *   Arduino Caterina(ATmega32U4)   4096
+ *   USBaspLoader(ATmega***)        2048
+ *   Teensy   halfKay(ATmega32U4)   512
+ *   Teensy++ halfKay(AT90USB128)   1024
+ *
+ *
+ * AVR Boot section is located at the end of Flash memory like the followings.
+ *
+ *
+ * byte     Atmel/LUFA(ATMega32u4)          byte     Atmel(AT90SUB128)
+ * 0x0000   +---------------+               0x00000  +---------------+
+ *          |               |                        |               |
+ *          |               |                        |               |
+ *          |  Application  |                        |  Application  |
+ *          |               |                        |               |
+ *          =               =                        =               =
+ *          |               | 32KB-4KB               |               | 128KB-8KB
+ * 0x6000   +---------------+               0x1FC00  +---------------+
+ *          |  Bootloader   | 4KB                    |  Bootloader   | 8KB
+ * 0x7FFF   +---------------+               0x1FFFF  +---------------+
+ *
+ *
+ * byte     Teensy(ATMega32u4)              byte     Teensy++(AT90SUB128)
+ * 0x0000   +---------------+               0x00000  +---------------+
+ *          |               |                        |               |
+ *          |               |                        |               |
+ *          |  Application  |                        |  Application  |
+ *          |               |                        |               |
+ *          =               =                        =               =
+ *          |               | 32KB-512B              |               | 128KB-1KB
+ * 0x7E00   +---------------+               0x1FC00  +---------------+
+ *          |  Bootloader   | 512B                   |  Bootloader   | 1KB
+ * 0x7FFF   +---------------+               0x1FFFF  +---------------+
  */
 #ifndef BOOTLOADER_SIZE
 #warning To use bootloader_jump() you need to define BOOTLOADER_SIZE in config.h.
diff --git a/common/avr/suspend.c b/common/avr/suspend.c
index 80243f02b..af99f52b5 100644
--- a/common/avr/suspend.c
+++ b/common/avr/suspend.c
@@ -85,6 +85,8 @@ void suspend_power_down(void)
     power_down(WDTO_15MS);
 }
 
+__attribute__ ((weak)) void matrix_power_up(void) {}
+__attribute__ ((weak)) void matrix_power_down(void) {}
 bool suspend_wakeup_condition(void)
 {
     matrix_power_up();
diff --git a/common/command.c b/common/command.c
index fbaa9f2d7..d59bb01bb 100644
--- a/common/command.c
+++ b/common/command.c
@@ -112,30 +112,33 @@ bool command_console_extra(uint8_t code)
  ***********************************************************/
 static void command_common_help(void)
 {
-    print("\n\n----- Command Help -----\n");
-    print("c:   enter console mode\n");
-    print("d:   toggle debug enable\n");
-    print("x:   toggle matrix debug\n");
-    print("k:   toggle keyboard debug\n");
-    print("m:   toggle mouse debug\n");
-#ifdef SLEEP_LED_ENABLE
-    print("z:   toggle sleep LED test\n");
+    print("\n\t- Magic -\n"
+          "d:   debug\n"
+          "x:   debug matrix\n"
+          "k:   debug keyboard\n"
+          "m:   debug mouse\n"
+          "v:   version\n"
+          "s:   status\n"
+          "c:   console mode\n"
+          "0-4: layer0-4(F10-F4)\n"
+          "Paus:        bootloader\n"
+
+#ifdef KEYBOARD_LOCK_ENABLE
+          "Caps:        Lock\n"
+#endif
+
+#ifdef BOOTMAGIC_ENABLE
+          "e:   eeprom\n"
 #endif
-    print("v:   print device version & info\n");
-    print("t:   print timer count\n");
-    print("s:   print status\n");
-    print("e:   print eeprom config\n");
+
 #ifdef NKRO_ENABLE
-    print("n:   toggle NKRO\n");
+          "n:   NKRO\n"
+#endif
+
+#ifdef SLEEP_LED_ENABLE
+          "z:   sleep LED test\n"
 #endif
-    print("0/F10:       switch to Layer0 \n");
-    print("1/F1:        switch to Layer1 \n");
-    print("2/F2:        switch to Layer2 \n");
-    print("3/F3:        switch to Layer3 \n");
-    print("4/F4:        switch to Layer4 \n");
-    print("PScr:        power down/remote wake-up\n");
-    print("Caps:        Lock Keyboard(Child Proof)\n");
-    print("Paus:        jump to bootloader\n");
+    );
 }
 
 #ifdef BOOTMAGIC_ENABLE
@@ -191,6 +194,7 @@ static bool command_common(uint8_t code)
             print_eeconfig();
             break;
 #endif
+#ifdef KEYBOARD_LOCK_ENABLE
         case KC_CAPSLOCK:
             if (host_get_driver()) {
                 host_driver = host_get_driver();
@@ -202,6 +206,7 @@ static bool command_common(uint8_t code)
                 print("Unlocked.\n");
             }
             break;
+#endif
         case KC_H:
         case KC_SLASH: /* ? */
             command_common_help();
@@ -212,58 +217,56 @@ static bool command_common(uint8_t code)
             debug_mouse    = false;
             debug_enable   = false;
             command_console_help();
-            print("\nEnter Console Mode\n");
             print("C> ");
             command_state = CONSOLE;
             break;
         case KC_PAUSE:
             clear_keyboard();
-            print("\n\nJump to bootloader... ");
+            print("\n\nbootloader... ");
             _delay_ms(1000);
             bootloader_jump(); // not return
-            print("not supported.\n");
             break;
         case KC_D:
             if (debug_enable) {
-                print("\nDEBUG: disabled.\n");
+                print("\ndebug: off\n");
                 debug_matrix   = false;
                 debug_keyboard = false;
                 debug_mouse    = false;
                 debug_enable   = false;
             } else {
-                print("\nDEBUG: enabled.\n");
+                print("\ndebug: on\n");
                 debug_enable   = true;
             }
             break;
         case KC_X: // debug matrix toggle
             debug_matrix = !debug_matrix;
             if (debug_matrix) {
-                print("\nDEBUG: matrix enabled.\n");
+                print("\nmatrix: on\n");
                 debug_enable = true;
             } else {
-                print("\nDEBUG: matrix disabled.\n");
+                print("\nmatrix: off\n");
             }
             break;
         case KC_K: // debug keyboard toggle
             debug_keyboard = !debug_keyboard;
             if (debug_keyboard) {
-                print("\nDEBUG: keyboard enabled.\n");
+                print("\nkeyboard: on\n");
                 debug_enable = true;
             } else {
-                print("\nDEBUG: keyboard disabled.\n");
+                print("\nkeyboard: off\n");
             }
             break;
         case KC_M: // debug mouse toggle
             debug_mouse = !debug_mouse;
             if (debug_mouse) {
-                print("\nDEBUG: mouse enabled.\n");
+                print("\nmouse: on\n");
                 debug_enable = true;
             } else {
-                print("\nDEBUG: mouse disabled.\n");
+                print("\nmouse: off\n");
             }
             break;
         case KC_V: // print version & information
-            print("\n\n----- Version -----\n");
+            print("\n\t- Version -\n");
             print("DESC: " STR(DESCRIPTION) "\n");
             print("VID: " STR(VENDOR_ID) "(" STR(MANUFACTURER) ") "
                   "PID: " STR(PRODUCT_ID) "(" STR(PRODUCT) ") "
@@ -307,14 +310,16 @@ static bool command_common(uint8_t code)
                   " AVR-LIBC: " __AVR_LIBC_VERSION_STRING__
                   " AVR_ARCH: avr" STR(__AVR_ARCH__) "\n");
             break;
-        case KC_T: // print timer
-            print_val_hex32(timer_count);
-            break;
         case KC_S:
-            print("\n\n----- Status -----\n");
+            print("\n\t- Status -\n");
             print_val_hex8(host_keyboard_leds());
             print_val_hex8(keyboard_protocol);
             print_val_hex8(keyboard_idle);
+#ifdef NKRO_ENABLE
+            print_val_hex8(keyboard_nkro);
+#endif
+            print_val_hex32(timer_count);
+
 #ifdef PROTOCOL_PJRC
             print_val_hex8(UDCON);
             print_val_hex8(UDIEN);
@@ -334,39 +339,21 @@ static bool command_common(uint8_t code)
             clear_keyboard(); //Prevents stuck keys.
             keyboard_nkro = !keyboard_nkro;
             if (keyboard_nkro)
-                print("NKRO: enabled\n");
+                print("NKRO: on\n");
             else
-                print("NKRO: disabled\n");
-            break;
-#endif
-#ifdef EXTRAKEY_ENABLE
-        case KC_PSCREEN:
-            // TODO: Power key should take this feature? otherwise any key during suspend.
-#ifdef PROTOCOL_PJRC
-            if (suspend && remote_wakeup) {
-                usb_remote_wakeup();
-            } else {
-                host_system_send(SYSTEM_POWER_DOWN);
-                host_system_send(0);
-                _delay_ms(500);
-            }
-#else
-            host_system_send(SYSTEM_POWER_DOWN);
-            _delay_ms(100);
-            host_system_send(0);
-            _delay_ms(500);
-#endif
+                print("NKRO: off\n");
             break;
 #endif
         case KC_ESC:
         case KC_GRV:
         case KC_0:
+        case KC_F10:
             switch_default_layer(0);
             break;
         case KC_1 ... KC_9:
             switch_default_layer((code - KC_1) + 1);
             break;
-        case KC_F1 ... KC_F12:
+        case KC_F1 ... KC_F9:
             switch_default_layer((code - KC_F1) + 1);
             break;
         default:
@@ -382,11 +369,12 @@ static bool command_common(uint8_t code)
  ***********************************************************/
 static void command_console_help(void)
 {
-    print("\n\n----- Console Help -----\n");
-    print("ESC/q:       quit\n");
+    print("\n\t- Console -\n"
+          "ESC/q:       quit\n"
 #ifdef MOUSEKEY_ENABLE
-    print("m:   mousekey\n");
+          "m:   mousekey\n"
 #endif
+    );
 }
 
 static bool command_console(uint8_t code)
@@ -398,14 +386,12 @@ static bool command_console(uint8_t code)
             break;
         case KC_Q:
         case KC_ESC:
-            print("\nQuit Console Mode\n");
             command_state = ONESHOT;
             return false;
 #ifdef MOUSEKEY_ENABLE
         case KC_M:
             mousekey_console_help();
-            print("\nEnter Mousekey Console\n");
-            print("M0>");
+            print("M> ");
             command_state = MOUSEKEY;
             return true;
 #endif
@@ -426,16 +412,17 @@ static uint8_t mousekey_param = 0;
 
 static void mousekey_param_print(void)
 {
-    print("\n\n----- Mousekey Parameters -----\n");
-    print("1: mk_delay(*10ms): "); pdec(mk_delay); print("\n");
-    print("2: mk_interval(ms): "); pdec(mk_interval); print("\n");
-    print("3: mk_max_speed: "); pdec(mk_max_speed); print("\n");
-    print("4: mk_time_to_max: "); pdec(mk_time_to_max); print("\n");
-    print("5: mk_wheel_max_speed: "); pdec(mk_wheel_max_speed); print("\n");
-    print("6: mk_wheel_time_to_max: "); pdec(mk_wheel_time_to_max); print("\n");
+    print("\n\t- Values -\n");
+    print("1: delay(*10ms): "); pdec(mk_delay); print("\n");
+    print("2: interval(ms): "); pdec(mk_interval); print("\n");
+    print("3: max_speed: "); pdec(mk_max_speed); print("\n");
+    print("4: time_to_max: "); pdec(mk_time_to_max); print("\n");
+    print("5: wheel_max_speed: "); pdec(mk_wheel_max_speed); print("\n");
+    print("6: wheel_time_to_max: "); pdec(mk_wheel_time_to_max); print("\n");
 }
 
-#define PRINT_SET_VAL(v)  print(#v " = "); print_dec(v); print("\n");
+//#define PRINT_SET_VAL(v)  print(#v " = "); print_dec(v); print("\n");
+#define PRINT_SET_VAL(v)  xprintf(#v " = %d\n", (v))
 static void mousekey_param_inc(uint8_t param, uint8_t inc)
 {
     switch (param) {
@@ -534,24 +521,25 @@ static void mousekey_param_dec(uint8_t param, uint8_t dec)
 
 static void mousekey_console_help(void)
 {
-    print("\n\n----- Mousekey Parameters Help -----\n");
-    print("ESC/q:       quit\n");
-    print("1:   select mk_delay(*10ms)\n");
-    print("2:   select mk_interval(ms)\n");
-    print("3:   select mk_max_speed\n");
-    print("4:   select mk_time_to_max\n");
-    print("5:   select mk_wheel_max_speed\n");
-    print("6:   select mk_wheel_time_to_max\n");
-    print("p:   print parameters\n");
-    print("d:   set default values\n");
-    print("up:  increase parameters(+1)\n");
-    print("down:        decrease parameters(-1)\n");
-    print("pgup:        increase parameters(+10)\n");
-    print("pgdown:      decrease parameters(-10)\n");
-    print("\nspeed = delta * max_speed * (repeat / time_to_max)\n");
-    print("where delta: cursor="); pdec(MOUSEKEY_MOVE_DELTA);
-    print(", wheel="); pdec(MOUSEKEY_WHEEL_DELTA); print("\n");
-    print("See http://en.wikipedia.org/wiki/Mouse_keys\n");
+    print("\n\t- Mousekey -\n"
+          "ESC/q:       quit\n"
+          "1:   delay(*10ms)\n"
+          "2:   interval(ms)\n"
+          "3:   max_speed\n"
+          "4:   time_to_max\n"
+          "5:   wheel_max_speed\n"
+          "6:   wheel_time_to_max\n"
+          "\n"
+          "p:   print values\n"
+          "d:   set defaults\n"
+          "up:  +1\n"
+          "down:        -1\n"
+          "pgup:        +10\n"
+          "pgdown:      -10\n"
+          "\n"
+          "speed = delta * max_speed * (repeat / time_to_max)\n");
+    xprintf("where delta: cursor=%d, wheel=%d\n" 
+            "See http://en.wikipedia.org/wiki/Mouse_keys\n", MOUSEKEY_MOVE_DELTA,  MOUSEKEY_WHEEL_DELTA);
 }
 
 static bool mousekey_console(uint8_t code)
@@ -563,11 +551,14 @@ static bool mousekey_console(uint8_t code)
             break;
         case KC_Q:
         case KC_ESC:
-            mousekey_param = 0;
-            print("\nQuit Mousekey Console\n");
-            print("C> ");
-            command_state = CONSOLE;
-            return false;
+            if (mousekey_param) {
+                mousekey_param = 0;
+            } else {
+                print("C> ");
+                command_state = CONSOLE;
+                return false;
+            }
+            break;
         case KC_P:
             mousekey_param_print();
             break;
@@ -577,12 +568,7 @@ static bool mousekey_console(uint8_t code)
         case KC_4:
         case KC_5:
         case KC_6:
-        case KC_7:
-        case KC_8:
-        case KC_9:
-        case KC_0:
             mousekey_param = numkey2num(code);
-            print("selected parameter: "); pdec(mousekey_param); print("\n");
             break;
         case KC_UP:
             mousekey_param_inc(mousekey_param, 1);
@@ -603,13 +589,16 @@ static bool mousekey_console(uint8_t code)
             mk_time_to_max = MOUSEKEY_TIME_TO_MAX;
             mk_wheel_max_speed = MOUSEKEY_WHEEL_MAX_SPEED;
             mk_wheel_time_to_max = MOUSEKEY_WHEEL_TIME_TO_MAX;
-            print("set default values.\n");
+            print("set default\n");
             break;
         default:
             print("?");
             return false;
     }
-    print("M"); pdec(mousekey_param); print("> ");
+    if (mousekey_param)
+        xprintf("M%d> ", mousekey_param);
+    else
+        print("M>" );
     return true;
 }
 #endif
@@ -637,8 +626,7 @@ static uint8_t numkey2num(uint8_t code)
 
 static void switch_default_layer(uint8_t layer)
 {
-    print("switch_default_layer: "); print_dec(biton32(default_layer_state));
-    print(" to "); print_dec(layer); print("\n");
+    xprintf("L%d\n", layer);
     default_layer_set(1UL<<layer);
     clear_keyboard();
 }
diff --git a/common/keyboard.c b/common/keyboard.c
index b03b124d7..eb7b096be 100644
--- a/common/keyboard.c
+++ b/common/keyboard.c
@@ -62,6 +62,12 @@ static bool has_ghost_in_row(uint8_t row)
 #endif
 
 
+__attribute__ ((weak)) void matrix_setup(void) {}
+void keyboard_setup(void)
+{
+    matrix_setup();
+}
+
 void keyboard_init(void)
 {
     timer_init();
diff --git a/common/keyboard.h b/common/keyboard.h
index 6442716fc..7738251b6 100644
--- a/common/keyboard.h
+++ b/common/keyboard.h
@@ -58,13 +58,15 @@ static inline bool IS_RELEASED(keyevent_t event) { return (!IS_NOEVENT(event) &&
 }
 
 
+/* it runs once at early stage of startup before keyboard_init. */
+void keyboard_setup(void);
+/* it runs once after initializing host side protocol, debug and MCU peripherals. */
 void keyboard_init(void);
+/* it runs repeatedly in main loop */
 void keyboard_task(void);
+/* it runs when host LED status is updated */
 void keyboard_set_leds(uint8_t leds);
 
-__attribute__ ((weak)) void matrix_power_up(void) {}
-__attribute__ ((weak)) void matrix_power_down(void) {}
-
 #ifdef __cplusplus
 }
 #endif
diff --git a/common/keycode.h b/common/keycode.h
index ac4ef00db..448195306 100644
--- a/common/keycode.h
+++ b/common/keycode.h
@@ -156,6 +156,8 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define KC_WSTP KC_WWW_STOP
 #define KC_WREF KC_WWW_REFRESH
 #define KC_WFAV KC_WWW_FAVORITES
+/* Jump to bootloader */
+#define KC_BTLD KC_BOOTLOADER
 /* Transparent */
 #define KC_TRANSPARENT  1
 #define KC_TRNS KC_TRANSPARENT
@@ -214,7 +216,7 @@ enum hid_keyboard_keypad_usage {
     KC_LBRACKET,
     KC_RBRACKET,        /* 0x30 */
     KC_BSLASH,          /* \ (and |) */
-    KC_NONUS_HASH,      /* Non-US # and ~ */
+    KC_NONUS_HASH,      /* Non-US # and ~ (Typically near the Enter key) */
     KC_SCOLON,          /* ; (and :) */
     KC_QUOTE,           /* ' and " */
     KC_GRAVE,           /* Grave accent and tilde */
@@ -264,7 +266,7 @@ enum hid_keyboard_keypad_usage {
     KC_KP_9,
     KC_KP_0,
     KC_KP_DOT,
-    KC_NONUS_BSLASH,    /* Non-US \ and | */
+    KC_NONUS_BSLASH,    /* Non-US \ and | (Typically near the Left-Shift key) */
     KC_APPLICATION,
     KC_POWER,
     KC_KP_EQUAL,
@@ -426,6 +428,9 @@ enum internal_special_keycodes {
     KC_MEDIA_FAST_FORWARD,
     KC_MEDIA_REWIND,    /* 0xBC */
 
+    /* Jump to bootloader */
+    KC_BOOTLOADER       = 0xBF,
+
     /* Fn key */
     KC_FN0              = 0xC0,
     KC_FN1,
diff --git a/common/keymap.c b/common/keymap.c
index 9f4fab521..d48923808 100644
--- a/common/keymap.c
+++ b/common/keymap.c
@@ -20,6 +20,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "action_layer.h"
 #include "action.h"
 #include "action_macro.h"
+#include "wait.h"
 #include "debug.h"
 
 
@@ -140,6 +141,11 @@ static action_t keycode_to_action(uint8_t keycode)
         case KC_TRNS:
             action.code = ACTION_TRANSPARENT;
             break;
+        case KC_BOOTLOADER:
+            clear_keyboard();
+            wait_ms(50);
+            bootloader_jump(); // not return
+            break;
         default:
             action.code = ACTION_NO;
             break;
diff --git a/common/led.h b/common/led.h
index 402a247b9..d5fc051bf 100644
--- a/common/led.h
+++ b/common/led.h
@@ -28,6 +28,14 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define USB_LED_KANA                    4
 
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 void led_set(uint8_t usb_led);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif
diff --git a/common/matrix.h b/common/matrix.h
index 107ee7265..ec6f8cd43 100644
--- a/common/matrix.h
+++ b/common/matrix.h
@@ -43,7 +43,9 @@ extern "C" {
 uint8_t matrix_rows(void);
 /* number of matrix columns */
 uint8_t matrix_cols(void);
-/* intialize matrix for scaning. should be called once. */
+/* should be called at early stage of startup before matrix_init.(optional) */
+void matrix_setup(void);
+/* intialize matrix for scaning. */
 void matrix_init(void);
 /* scan all key states on matrix */
 uint8_t matrix_scan(void);
diff --git a/ldscript_keymap_avr35.x b/ldscript_keymap_avr35.x
new file mode 100644
index 000000000..6665020af
--- /dev/null
+++ b/ldscript_keymap_avr35.x
@@ -0,0 +1,268 @@
+/*
+ * linker script for configurable keymap
+ *
+ * This adds keymap section which places keymap at fixed address and
+ * is based on binutils-avr ldscripts(/usr/lib/ldscripts/avr5.x).
+ */
+OUTPUT_FORMAT("elf32-avr","elf32-avr","elf32-avr")
+OUTPUT_ARCH(avr:35)
+MEMORY
+{
+  /* With keymap section
+   *
+   * Flash Map of ATMega32U4(32KB)
+   * +------------+ 0x0000
+   * | .vectors   |
+   * | .progmem   |
+   * | .init0-9   | > text region
+   * | .text      |
+   * | .fini9-0   |
+   * |            |
+   * |------------| _etext
+   * | .data      |
+   * | .bss       | > data region
+   * | .noinit    |
+   * |            |
+   * |------------| 0x6800
+   * | .keymap    | > keymap region(2KB)
+   * |------------| 0x7000
+   * | bootloader | 4KB
+   * +------------+ 0x7FFF
+   */
+  text   (rx)   : ORIGIN = 0, LENGTH = 64K
+  keymap (rw!x) : ORIGIN = 0x6800, LENGTH = 2K
+  data   (rw!x) : ORIGIN = 0x800060, LENGTH = 0xffa0
+  eeprom (rw!x) : ORIGIN = 0x810000, LENGTH = 64K
+  fuse      (rw!x) : ORIGIN = 0x820000, LENGTH = 1K
+  lock      (rw!x) : ORIGIN = 0x830000, LENGTH = 1K
+  signature (rw!x) : ORIGIN = 0x840000, LENGTH = 1K
+}
+SECTIONS
+{
+  /* Read-only sections, merged into text segment: */
+  .hash          : { *(.hash)           }
+  .dynsym        : { *(.dynsym)         }
+  .dynstr        : { *(.dynstr)         }
+  .gnu.version   : { *(.gnu.version)    }
+  .gnu.version_d   : { *(.gnu.version_d)        }
+  .gnu.version_r   : { *(.gnu.version_r)        }
+  .rel.init      : { *(.rel.init)               }
+  .rela.init     : { *(.rela.init)      }
+  .rel.text      :
+    {
+      *(.rel.text)
+      *(.rel.text.*)
+      *(.rel.gnu.linkonce.t*)
+    }
+  .rela.text     :
+    {
+      *(.rela.text)
+      *(.rela.text.*)
+      *(.rela.gnu.linkonce.t*)
+    }
+  .rel.fini      : { *(.rel.fini)               }
+  .rela.fini     : { *(.rela.fini)      }
+  .rel.rodata    :
+    {
+      *(.rel.rodata)
+      *(.rel.rodata.*)
+      *(.rel.gnu.linkonce.r*)
+    }
+  .rela.rodata   :
+    {
+      *(.rela.rodata)
+      *(.rela.rodata.*)
+      *(.rela.gnu.linkonce.r*)
+    }
+  .rel.data      :
+    {
+      *(.rel.data)
+      *(.rel.data.*)
+      *(.rel.gnu.linkonce.d*)
+    }
+  .rela.data     :
+    {
+      *(.rela.data)
+      *(.rela.data.*)
+      *(.rela.gnu.linkonce.d*)
+    }
+  .rel.ctors     : { *(.rel.ctors)      }
+  .rela.ctors    : { *(.rela.ctors)     }
+  .rel.dtors     : { *(.rel.dtors)      }
+  .rela.dtors    : { *(.rela.dtors)     }
+  .rel.got       : { *(.rel.got)                }
+  .rela.got      : { *(.rela.got)               }
+  .rel.bss       : { *(.rel.bss)                }
+  .rela.bss      : { *(.rela.bss)               }
+  .rel.plt       : { *(.rel.plt)                }
+  .rela.plt      : { *(.rela.plt)               }
+  /* Internal text space or external memory.  */
+  .text   :
+  {
+    *(.vectors)
+    KEEP(*(.vectors))
+    /* For data that needs to reside in the lower 64k of progmem.  */
+    *(.progmem.gcc*)
+    *(.progmem*)
+    . = ALIGN(2);
+     __trampolines_start = . ;
+    /* The jump trampolines for the 16-bit limited relocs will reside here.  */
+    *(.trampolines)
+    *(.trampolines*)
+     __trampolines_end = . ;
+    /* For future tablejump instruction arrays for 3 byte pc devices.
+       We don't relax jump/call instructions within these sections.  */
+    *(.jumptables)
+    *(.jumptables*)
+    /* For code that needs to reside in the lower 128k progmem.  */
+    *(.lowtext)
+    *(.lowtext*)
+     __ctors_start = . ;
+     *(.ctors)
+     __ctors_end = . ;
+     __dtors_start = . ;
+     *(.dtors)
+     __dtors_end = . ;
+    KEEP(SORT(*)(.ctors))
+    KEEP(SORT(*)(.dtors))
+    /* From this point on, we don't bother about wether the insns are
+       below or above the 16 bits boundary.  */
+    *(.init0)  /* Start here after reset.  */
+    KEEP (*(.init0))
+    *(.init1)
+    KEEP (*(.init1))
+    *(.init2)  /* Clear __zero_reg__, set up stack pointer.  */
+    KEEP (*(.init2))
+    *(.init3)
+    KEEP (*(.init3))
+    *(.init4)  /* Initialize data and BSS.  */
+    KEEP (*(.init4))
+    *(.init5)
+    KEEP (*(.init5))
+    *(.init6)  /* C++ constructors.  */
+    KEEP (*(.init6))
+    *(.init7)
+    KEEP (*(.init7))
+    *(.init8)
+    KEEP (*(.init8))
+    *(.init9)  /* Call main().  */
+    KEEP (*(.init9))
+    *(.text)
+    . = ALIGN(2);
+    *(.text.*)
+    . = ALIGN(2);
+    *(.fini9)  /* _exit() starts here.  */
+    KEEP (*(.fini9))
+    *(.fini8)
+    KEEP (*(.fini8))
+    *(.fini7)
+    KEEP (*(.fini7))
+    *(.fini6)  /* C++ destructors.  */
+    KEEP (*(.fini6))
+    *(.fini5)
+    KEEP (*(.fini5))
+    *(.fini4)
+    KEEP (*(.fini4))
+    *(.fini3)
+    KEEP (*(.fini3))
+    *(.fini2)
+    KEEP (*(.fini2))
+    *(.fini1)
+    KEEP (*(.fini1))
+    *(.fini0)  /* Infinite loop after program termination.  */
+    KEEP (*(.fini0))
+     _etext = . ;
+  }  > text
+  .data   : AT (ADDR (.text) + SIZEOF (.text))
+  {
+     PROVIDE (__data_start = .) ;
+    *(.data)
+    *(.data*)
+    *(.rodata)  /* We need to include .rodata here if gcc is used */
+    *(.rodata*) /* with -fdata-sections.  */
+    *(.gnu.linkonce.d*)
+    . = ALIGN(2);
+     _edata = . ;
+     PROVIDE (__data_end = .) ;
+  }  > data
+  .bss   : AT (ADDR (.bss))
+  {
+     PROVIDE (__bss_start = .) ;
+    *(.bss)
+    *(.bss*)
+    *(COMMON)
+     PROVIDE (__bss_end = .) ;
+  }  > data
+   __data_load_start = LOADADDR(.data);
+   __data_load_end = __data_load_start + SIZEOF(.data);
+  /* Global data not cleared after reset.  */
+  .noinit  :
+  {
+     PROVIDE (__noinit_start = .) ;
+    *(.noinit*)
+     PROVIDE (__noinit_end = .) ;
+     _end = . ;
+     PROVIDE (__heap_start = .) ;
+  }  > data
+  /* keymap region is located at end of flash
+   * .fn_actions        Fn actions definitions
+   * .keymaps           Mapping layers
+   */
+  .keymap :
+  {
+    PROVIDE(__keymap_start = .) ;
+    *(.keymap.fn_actions)   /* 32*actions = 64bytes */
+    . = ALIGN(0x40);
+    *(.keymap.keymaps)      /* rest of .keymap section */
+    *(.keymap*)
+    /* . = ALIGN(0x800); */ /* keymap section takes 2KB- */
+  } > keymap = 0x00         /* zero fill */
+  .eeprom  :
+  {
+    *(.eeprom*)
+     __eeprom_end = . ;
+  }  > eeprom
+  .fuse  :
+  {
+    KEEP(*(.fuse))
+    KEEP(*(.lfuse))
+    KEEP(*(.hfuse))
+    KEEP(*(.efuse))
+  }  > fuse
+  .lock  :
+  {
+    KEEP(*(.lock*))
+  }  > lock
+  .signature  :
+  {
+    KEEP(*(.signature*))
+  }  > signature
+  /* Stabs debugging sections.  */
+  .stab 0 : { *(.stab) }
+  .stabstr 0 : { *(.stabstr) }
+  .stab.excl 0 : { *(.stab.excl) }
+  .stab.exclstr 0 : { *(.stab.exclstr) }
+  .stab.index 0 : { *(.stab.index) }
+  .stab.indexstr 0 : { *(.stab.indexstr) }
+  .comment 0 : { *(.comment) }
+  /* DWARF debug sections.
+     Symbols in the DWARF debugging sections are relative to the beginning
+     of the section so we begin them at 0.  */
+  /* DWARF 1 */
+  .debug          0 : { *(.debug) }
+  .line           0 : { *(.line) }
+  /* GNU DWARF 1 extensions */
+  .debug_srcinfo  0 : { *(.debug_srcinfo) }
+  .debug_sfnames  0 : { *(.debug_sfnames) }
+  /* DWARF 1.1 and DWARF 2 */
+  .debug_aranges  0 : { *(.debug_aranges) }
+  .debug_pubnames 0 : { *(.debug_pubnames) }
+  /* DWARF 2 */
+  .debug_info     0 : { *(.debug_info) *(.gnu.linkonce.wi.*) }
+  .debug_abbrev   0 : { *(.debug_abbrev) }
+  .debug_line     0 : { *(.debug_line) }
+  .debug_frame    0 : { *(.debug_frame) }
+  .debug_str      0 : { *(.debug_str) }
+  .debug_loc      0 : { *(.debug_loc) }
+  .debug_macinfo  0 : { *(.debug_macinfo) }
+}
diff --git a/ldscript_keymap_avr5.x b/ldscript_keymap_avr5.x
index c09693e51..9b46e6c36 100644
--- a/ldscript_keymap_avr5.x
+++ b/ldscript_keymap_avr5.x
@@ -1,4 +1,4 @@
-/* 
+/*
  * linker script for configurable keymap
  *
  * This adds keymap section which places keymap at fixed address and
@@ -8,7 +8,7 @@ OUTPUT_FORMAT("elf32-avr","elf32-avr","elf32-avr")
 OUTPUT_ARCH(avr:5)
 MEMORY
 {
-  /* With keymap section 
+  /* With keymap section
    *
    * Flash Map of ATMega32U4(32KB)
    * +------------+ 0x0000
@@ -212,7 +212,7 @@ SECTIONS
   {
     PROVIDE(__keymap_start = .) ;
     *(.keymap.fn_actions)   /* 32*actions = 64bytes */
-    . = ALIGN(0x40); 
+    . = ALIGN(0x40);
     *(.keymap.keymaps)      /* rest of .keymap section */
     *(.keymap*)
     /* . = ALIGN(0x800); */ /* keymap section takes 2KB- */
diff --git a/protocol/ibm4704.c b/protocol/ibm4704.c
index a10a5e74d..6a03cd441 100644
--- a/protocol/ibm4704.c
+++ b/protocol/ibm4704.c
@@ -21,9 +21,10 @@ uint8_t ibm4704_error = 0;
 
 void ibm4704_init(void)
 {
+    inhibit();  // keep keyboard from sending
     IBM4704_INT_INIT();
     IBM4704_INT_ON();
-    idle();
+    idle();     // allow keyboard sending
 }
 
 /*
@@ -104,51 +105,44 @@ uint8_t ibm4704_recv_response(void)
     return rbuf_dequeue();
 }
 
+uint8_t ibm4704_recv(void)
+{
+    if (rbuf_has_data()) {
+        return rbuf_dequeue();
+    } else {
+        return -1;
+    }
+}
+
 /*
 Keyboard to Host
 ----------------
 Data bits are LSB first and Parity is odd. Clock has around 60us high and 30us low part.
 
-        ____      __   __   __   __   __   __   __   __   __   ________
-Clock       \____/  \_/  \_/  \_/  \_/  \_/  \_/  \_/  \_/  \_/
+        ____       __   __   __   __   __   __   __   __   __   _______
+Clock       \_____/  \_/  \_/  \_/  \_/  \_/  \_/  \_/  \_/  \_/
              ____ ____ ____ ____ ____ ____ ____ ____ ____ ____    
 Data    ____/    X____X____X____X____X____X____X____X____X____X________
             Start   0    1    2    3    4    5    6    7    P  Stop
 
 Start bit:  can be long as 300-350us.
 Inhibit:    Pull Data line down to inhibit keyboard to send.
-Timing:     Host reads bit while Clock is hi.
+Timing:     Host reads bit while Clock is hi.(rising edge)
 Stop bit:   Keyboard pulls down Data line to lo after 9th clock.
 */
-uint8_t ibm4704_recv(void)
-{
-    if (rbuf_has_data()) {
-        return rbuf_dequeue();
-    } else {
-        return -1;
-    }
-}
-
 ISR(IBM4704_INT_VECT)
 {
     static enum {
-        INIT, START, BIT0, BIT1, BIT2, BIT3, BIT4, BIT5, BIT6, BIT7, PARITY,
-    } state = INIT;
+        BIT0, BIT1, BIT2, BIT3, BIT4, BIT5, BIT6, BIT7, PARITY, STOP
+    } state = BIT0;
     // LSB first
     static uint8_t data = 0;
     // Odd parity
     static uint8_t parity = false;
 
     ibm4704_error = 0;
-    // return unless falling edge
-    if (clock_in()) { goto RETURN; }    // why this occurs?
 
-    state++;
     switch (state) {
-        case START:
-            // Data:Low
-            WAIT(data_hi, 10, state);
-            break;
         case BIT0:
         case BIT1:
         case BIT2:
@@ -169,6 +163,10 @@ ISR(IBM4704_INT_VECT)
             }
             if (!parity)
                 goto ERROR;
+            break;
+        case STOP:
+            // Data:Low
+            WAIT(data_lo, 100, state);
             rbuf_enqueue(data);
             ibm4704_error = IBM4704_ERR_NONE;
             goto DONE;
@@ -176,13 +174,14 @@ ISR(IBM4704_INT_VECT)
         default:
             goto ERROR;
     }
+    state++;
     goto RETURN;
 ERROR:
     ibm4704_error = state;
     while (ibm4704_send(0xFE)) _delay_ms(1); // resend
     xprintf("R:%02X%02X\n", state, data);
 DONE:
-    state = INIT;
+    state = BIT0;
     data = 0;
     parity = false;
 RETURN:
diff --git a/protocol/lufa/lufa.c b/protocol/lufa/lufa.c
index cdfc7bc6a..345630aa9 100644
--- a/protocol/lufa/lufa.c
+++ b/protocol/lufa/lufa.c
@@ -53,6 +53,7 @@
 #include "lufa.h"
 
 uint8_t keyboard_idle = 0;
+/* 0: Boot Protocol, 1: Report Protocol(default) */
 uint8_t keyboard_protocol = 1;
 static uint8_t keyboard_led_stats = 0;
 
@@ -179,7 +180,6 @@ void EVENT_USB_Device_Reset(void)
 void EVENT_USB_Device_Suspend()
 {
     print("[S]");
-    matrix_power_down();
 #ifdef SLEEP_LED_ENABLE
     sleep_led_enable();
 #endif
@@ -197,13 +197,30 @@ void EVENT_USB_Device_WakeUp()
 #endif
 }
 
+#ifdef CONSOLE_ENABLE
+static bool console_flush = false;
+#define CONSOLE_FLUSH_SET(b)   do { \
+    uint8_t sreg = SREG; cli(); console_flush = b; SREG = sreg; \
+} while (0)
+
+// called every 1ms
 void EVENT_USB_Device_StartOfFrame(void)
 {
+    static uint8_t count;
+    if (++count % 50) return;
+    count = 0;
+
+    if (!console_flush) return;
     Console_Task();
+    console_flush = false;
 }
+#endif
 
 /** Event handler for the USB_ConfigurationChanged event.
  * This is fired when the host sets the current configuration of the USB device after enumeration.
+ *
+ * ATMega32u2 supports dual bank(ping-pong mode) only on endpoint 3 and 4,
+ * it is safe to use singl bank for all endpoints.
  */
 void EVENT_USB_Device_ConfigurationChanged(void)
 {
@@ -228,7 +245,7 @@ void EVENT_USB_Device_ConfigurationChanged(void)
 #ifdef CONSOLE_ENABLE
     /* Setup Console HID Report Endpoints */
     ConfigSuccess &= ENDPOINT_CONFIG(CONSOLE_IN_EPNUM, EP_TYPE_INTERRUPT, ENDPOINT_DIR_IN,
-                                     CONSOLE_EPSIZE, ENDPOINT_BANK_DOUBLE);
+                                     CONSOLE_EPSIZE, ENDPOINT_BANK_SINGLE);
 #if 0
     ConfigSuccess &= ENDPOINT_CONFIG(CONSOLE_OUT_EPNUM, EP_TYPE_INTERRUPT, ENDPOINT_DIR_OUT,
                                      CONSOLE_EPSIZE, ENDPOINT_BANK_SINGLE);
@@ -333,10 +350,7 @@ void EVENT_USB_Device_ControlRequest(void)
                     Endpoint_ClearSETUP();
                     Endpoint_ClearStatusStage();
 
-                    keyboard_protocol = ((USB_ControlRequest.wValue & 0xFF) != 0x00);
-#ifdef NKRO_ENABLE
-                    keyboard_nkro = !!keyboard_protocol;
-#endif
+                    keyboard_protocol = (USB_ControlRequest.wValue & 0xFF);
                     clear_keyboard();
                 }
             }
@@ -383,7 +397,7 @@ static void send_keyboard(report_keyboard_t *report)
 
     /* Select the Keyboard Report Endpoint */
 #ifdef NKRO_ENABLE
-    if (keyboard_nkro) {
+    if (keyboard_protocol && keyboard_nkro) {
         /* Report protocol - NKRO */
         Endpoint_SelectEndpoint(NKRO_IN_EPNUM);
 
@@ -491,6 +505,10 @@ int8_t sendchar(uint8_t c)
     // Because sendchar() is called so many times, waiting each call causes big lag.
     static bool timeouted = false;
 
+    // prevents Console_Task() from running during sendchar() runs.
+    // or char will be lost. These two function is mutually exclusive.
+    CONSOLE_FLUSH_SET(false);
+
     if (USB_DeviceState != DEVICE_STATE_Configured)
         return -1;
 
@@ -524,8 +542,12 @@ int8_t sendchar(uint8_t c)
     Endpoint_Write_8(c);
 
     // send when bank is full
-    if (!Endpoint_IsReadWriteAllowed())
+    if (!Endpoint_IsReadWriteAllowed()) {
+        while (!(Endpoint_IsINReady()));
         Endpoint_ClearIN();
+    } else {
+        CONSOLE_FLUSH_SET(true);
+    }
 
     Endpoint_SelectEndpoint(ep);
     return 0;
@@ -544,7 +566,7 @@ int8_t sendchar(uint8_t c)
 /*******************************************************************************
  * main
  ******************************************************************************/
-static void SetupHardware(void)
+static void setup_mcu(void)
 {
     /* Disable watchdog if enabled by bootloader/fuses */
     MCUSR &= ~(1 << WDRF);
@@ -552,7 +574,10 @@ static void SetupHardware(void)
 
     /* Disable clock division */
     clock_prescale_set(clock_div_1);
+}
 
+static void setup_usb(void)
+{
     // Leonardo needs. Without this USB device is not recognized.
     USB_Disable();
 
@@ -566,7 +591,9 @@ static void SetupHardware(void)
 int main(void)  __attribute__ ((weak));
 int main(void)
 {
-    SetupHardware();
+    setup_mcu();
+    keyboard_setup();
+    setup_usb();
     sei();
 
     /* wait for USB startup & debug output */
diff --git a/protocol/next_kbd.c b/protocol/next_kbd.c
index a5a07a7a8..fa3034b3f 100644
--- a/protocol/next_kbd.c
+++ b/protocol/next_kbd.c
@@ -59,10 +59,16 @@ static inline void query(void);
 static inline void reset(void);
 static inline uint32_t response(void);
 
-#define out_hi_delay(intervals)  do { out_hi(); _delay_us(NEXT_KBD_TIMING * intervals); } while (0);
-#define out_lo_delay(intervals)  do { out_lo(); _delay_us(NEXT_KBD_TIMING * intervals); } while (0);
-#define query_delay(intervals)   do { query();  _delay_us(NEXT_KBD_TIMING * intervals); } while (0);
-#define reset_delay(intervals)   do { reset();  _delay_us(NEXT_KBD_TIMING * intervals); } while (0);
+/* The keyboard sends signal with 50us pulse width on OUT line
+ * while it seems to miss the 50us pulse on In line.
+ * next_kbd_set_leds() often fails to sync LED status with 50us
+ * but it works well with 51us(+1us) on TMK converter(ATMeaga32u2) at least.
+ * TODO: test on Teensy and Pro Micro configuration
+ */
+#define out_hi_delay(intervals)  do { out_hi(); _delay_us((NEXT_KBD_TIMING+1) * intervals); } while (0);
+#define out_lo_delay(intervals)  do { out_lo(); _delay_us((NEXT_KBD_TIMING+1) * intervals); } while (0);
+#define query_delay(intervals)   do { query();  _delay_us((NEXT_KBD_TIMING+1) * intervals); } while (0);
+#define reset_delay(intervals)   do { reset();  _delay_us((NEXT_KBD_TIMING+1) * intervals); } while (0);
 
 void next_kbd_init(void)
 {
@@ -79,6 +85,7 @@ void next_kbd_init(void)
 
 void next_kbd_set_leds(bool left, bool right)
 {
+    cli();
     out_lo_delay(9);
     
     out_hi_delay(3);
@@ -98,6 +105,7 @@ void next_kbd_set_leds(bool left, bool right)
     
     out_lo_delay(7);
     out_hi();
+    sei();
 }
 
 #define NEXT_KBD_READ (NEXT_KBD_IN_PIN&(1<<NEXT_KBD_IN_BIT))
diff --git a/protocol/pjrc/main.c b/protocol/pjrc/main.c
index e7bdcc059..45eb17d4c 100644
--- a/protocol/pjrc/main.c
+++ b/protocol/pjrc/main.c
@@ -46,6 +46,8 @@ int main(void)
     // set for 16 MHz clock
     CPU_PRESCALE(0);
 
+    keyboard_setup();
+
     // Initialize the USB, and then wait for the host to set configuration.
     // If the Teensy is powered without a PC connected to the USB port,
     // this will wait forever.
diff --git a/protocol/usb_hid/USB_Host_Shield_2.0/examples/testusbhostFAT/Arduino_Makefile_master b/protocol/usb_hid/USB_Host_Shield_2.0/examples/testusbhostFAT/Arduino_Makefile_master
deleted file mode 160000
-Subproject 94c560c854c7a1dfc35e9de9db05de1b202de6c
diff --git a/protocol/usb_hid/USB_Host_Shield_2.0/examples/testusbhostFAT/RTClib b/protocol/usb_hid/USB_Host_Shield_2.0/examples/testusbhostFAT/RTClib
deleted file mode 160000
-Subproject c30fcdf1f112de581de7b145a97630539e5cff4
diff --git a/protocol/usb_hid/USB_Host_Shield_2.0/examples/testusbhostFAT/generic_storage b/protocol/usb_hid/USB_Host_Shield_2.0/examples/testusbhostFAT/generic_storage
deleted file mode 160000
-Subproject 77762338286535dabb9c94b87060e33e487ff0f
diff --git a/protocol/usb_hid/USB_Host_Shield_2.0/examples/testusbhostFAT/xmem2 b/protocol/usb_hid/USB_Host_Shield_2.0/examples/testusbhostFAT/xmem2
deleted file mode 160000
-Subproject 77b033420485f7d3d35430c0e8d4d844aa89483
diff --git a/protocol/usb_hid/leonardo_led.h b/protocol/usb_hid/leonardo_led.h
deleted file mode 100644
index 6f67a88f5..000000000
--- a/protocol/usb_hid/leonardo_led.h
+++ /dev/null
@@ -1,10 +0,0 @@
-#ifndef LEONARDO_LED_H
-#define LEONARDO_LED_H
-
-// Leonardo "TX" LED for debug
-#define LED_TX_INIT    (DDRD  |=  (1<<5))
-#define LED_TX_ON      (PORTD &= ~(1<<5))
-#define LED_TX_OFF     (PORTD |=  (1<<5))
-#define LED_TX_TOGGLE  (PORTD ^=  (1<<5))
-
-#endif
diff --git a/protocol/usb_hid/parser.cpp b/protocol/usb_hid/parser.cpp
index 28151f9d5..1a152ff3f 100644
--- a/protocol/usb_hid/parser.cpp
+++ b/protocol/usb_hid/parser.cpp
@@ -10,15 +10,24 @@ uint16_t usb_hid_time_stamp;
 
 void KBDReportParser::Parse(HID *hid, bool is_rpt_id, uint8_t len, uint8_t *buf)
 {
-    ::memcpy(&usb_hid_keyboard_report, buf, sizeof(report_keyboard_t));
-    usb_hid_time_stamp = millis();
+    bool is_error = false;
+    report_keyboard_t *report = (report_keyboard_t *)buf;
+
+    dprintf("KBDReport: %02X %02X", report->mods, report->reserved);
+    for (uint8_t i = 0; i < KEYBOARD_REPORT_KEYS; i++) {
+        if (IS_ERROR(report->keys[i])) {
+            is_error = true;
+        }
+        dprintf(" %02X", report->keys[i]);
+    }
+    dprint("\r\n");
 
-    debug("KBDReport: ");
-    debug_hex(usb_hid_keyboard_report.mods);
-    debug(" --");
-    for (uint8_t i = 0; i < 6; i++) {
-        debug(" ");
-        debug_hex(usb_hid_keyboard_report.keys[i]);
+    // ignore error and not send report to computer
+    if (is_error) {
+        dprint("Error usage! \r\n");
+        return;
     }
-    debug("\r\n");
+
+    ::memcpy(&usb_hid_keyboard_report, buf, sizeof(report_keyboard_t));
+    usb_hid_time_stamp = millis();
 }
diff --git a/rules.mk b/rules.mk
index a790f874b..860fc1a93 100644
--- a/rules.mk
+++ b/rules.mk
@@ -124,6 +124,7 @@ CFLAGS += -O$(OPT)
 CFLAGS += -funsigned-char
 CFLAGS += -funsigned-bitfields
 CFLAGS += -ffunction-sections
+CFLAGS += -fdata-sections
 CFLAGS += -fno-inline-small-functions
 CFLAGS += -fpack-struct
 CFLAGS += -fshort-enums