1 files changed, 42 insertions, 5 deletions

diff --git a/tmk_core/protocol/vusb/main.c b/tmk_core/protocol/vusb/main.c
index 610638e7d4..7dc16926d2 100644
--- a/tmk_core/protocol/vusb/main.c
+++ b/tmk_core/protocol/vusb/main.c
@@ -21,12 +21,23 @@
 #include "uart.h"
 #include "debug.h"
 #include "suspend.h"
+#include "wait.h"
+#include "sendchar.h"
+
 #ifdef SLEEP_LED_ENABLE
 #    include "sleep_led.h"
 #endif
 
 #define UART_BAUD_RATE 115200
 
+#ifdef CONSOLE_ENABLE
+void console_task(void);
+#endif
+
+#ifdef RAW_ENABLE
+void raw_hid_task(void);
+#endif
+
 /* This is from main.c of USBaspLoader */
 static void initForUsbConnectivity(void) {
     uint8_t i = 0;
@@ -39,10 +50,9 @@ static void initForUsbConnectivity(void) {
         _delay_ms(1);
     }
     usbDeviceConnect();
-    sei();
 }
 
-void usb_remote_wakeup(void) {
+static void usb_remote_wakeup(void) {
     cli();
 
     int8_t ddr_orig = USBDDR;
@@ -59,6 +69,23 @@ void usb_remote_wakeup(void) {
     sei();
 }
 
+/** \brief Setup USB
+ *
+ * FIXME: Needs doc
+ */
+static void setup_usb(void) {
+    // debug("initForUsbConnectivity()\n");
+    initForUsbConnectivity();
+
+    // for Console_Task
+    print_set_sendchar(sendchar);
+}
+
+/** \brief Main
+ *
+ * FIXME: Needs doc
+ */
+int main(void) __attribute__((weak));
 int main(void) {
     bool suspended = false;
 #if USB_COUNT_SOF
@@ -76,8 +103,10 @@ int main(void) {
     keyboard_setup();
 
     host_set_driver(vusb_driver());
-    debug("initForUsbConnectivity()\n");
-    initForUsbConnectivity();
+    setup_usb();
+    sei();
+
+    wait_ms(50);
 
     keyboard_init();
 #ifdef SLEEP_LED_ENABLE
@@ -120,12 +149,13 @@ int main(void) {
         if (!suspended) {
             usbPoll();
 
-            // TODO: configuration process is incosistent. it sometime fails.
+            // TODO: configuration process is inconsistent. it sometime fails.
             // To prevent failing to configure NOT scan keyboard during configuration
             if (usbConfiguration && usbInterruptIsReady()) {
                 keyboard_task();
             }
             vusb_transfer_keyboard();
+
 #ifdef RAW_ENABLE
             usbPoll();
 
@@ -133,6 +163,13 @@ int main(void) {
                 raw_hid_task();
             }
 #endif
+#ifdef CONSOLE_ENABLE
+            usbPoll();
+
+            if (usbConfiguration && usbInterruptIsReady3()) {
+                console_task();
+            }
+#endif
         } else if (suspend_wakeup_condition()) {
             usb_remote_wakeup();
         }