1 files changed, 54 insertions, 0 deletions
diff --git a/tmk_core/protocol/arm_atsam/main_arm_atsam.c b/tmk_core/protocol/arm_atsam/main_arm_atsam.c
index ce0f54593c..abea121344 100644
--- a/tmk_core/protocol/arm_atsam/main_arm_atsam.c
+++ b/tmk_core/protocol/arm_atsam/main_arm_atsam.c
@@ -140,6 +140,57 @@ void send_consumer(uint16_t data) {
 #endif  // EXTRAKEY_ENABLE
 }
 
+#ifdef CONSOLE_ENABLE
+#    define CONSOLE_PRINTBUF_SIZE 512
+static char     console_printbuf[CONSOLE_PRINTBUF_SIZE];
+static uint16_t console_printbuf_len = 0;
+
+int8_t sendchar(uint8_t c) {
+    if (console_printbuf_len >= CONSOLE_PRINTBUF_SIZE) return -1;
+
+    console_printbuf[console_printbuf_len++] = c;
+    return 0;
+}
+
+void main_subtask_console_flush(void) {
+    while (udi_hid_con_b_report_trans_ongoing) {
+    }  // Wait for any previous transfers to complete
+
+    uint16_t result = console_printbuf_len;
+    uint32_t irqflags;
+    char *   pconbuf  = console_printbuf;  // Pointer to start send from
+    int      send_out = CONSOLE_EPSIZE;    // Bytes to send per transfer
+
+    while (result > 0) {  // While not error and bytes remain
+        while (udi_hid_con_b_report_trans_ongoing) {
+        }  // Wait for any previous transfers to complete
+
+        irqflags = __get_PRIMASK();
+        __disable_irq();
+        __DMB();
+
+        if (result < CONSOLE_EPSIZE) {                      // If remaining bytes are less than console epsize
+            memset(udi_hid_con_report, 0, CONSOLE_EPSIZE);  // Clear the buffer
+            send_out = result;                              // Send remaining size
+        }
+
+        memcpy(udi_hid_con_report, pconbuf, send_out);  // Copy data into the send buffer
+
+        udi_hid_con_b_report_valid = 1;  // Set report valid
+        udi_hid_con_send_report();       // Send report
+
+        __DMB();
+        __set_PRIMASK(irqflags);
+
+        result -= send_out;   // Decrement result by bytes sent
+        pconbuf += send_out;  // Increment buffer point by bytes sent
+    }
+
+    console_printbuf_len = 0;
+}
+
+#endif  // CONSOLE_ENABLE
+
 void main_subtask_usb_state(void) {
     static uint64_t fsmstate_on_delay = 0;                          // Delay timer to be sure USB is actually operating before bringing up hardware
     uint8_t         fsmstate_now      = USB->DEVICE.FSMSTATUS.reg;  // Current state from hardware register
@@ -214,6 +265,9 @@ void main_subtasks(void) {
     main_subtask_usb_state();
     main_subtask_power_check();
     main_subtask_usb_extra_device();
+#ifdef CONSOLE_ENABLE
+    main_subtask_console_flush();
+#endif
 #ifdef RAW_ENABLE
     main_subtask_raw();
 #endif