Massdrop keyboard updates for SEND_STRING, syscalls, stdio, debug prints, Auto Shift (#3973)

* Update for SEND_STRING usage

Update for SEND_STRING usage.
Sending keyboard reports (kbd, nkro) now obey the minimum polling time.
While attempting to send a keyboard report and waiting for a USB poll, other functions of the keyboard, including LED effects and power management, will continue to operate at their intended intervals.

* Updates for send string, syscalls, stdio, debug prints, auto shift

Now properly waiting for previous keys sent over USB to complete before sending new.
Added heap to linker and now compiling with syscalls support.
Removed custom string functions and now using stdio.
dprintf now works as intended through virtser device.

* CTRL and ALT keymap updates

CTRL mac keymap updated
ALT default and mac keymap updated
ALT rules.mk added Auto Shift with default no

* Code cleanup as per discussion with vomindoraan

Code cleanup as per discussion with vomindoraan

22 files changed, 175 insertions, 520 deletions

diff --git a/keyboards/massdrop/alt/alt.h b/keyboards/massdrop/alt/alt.h
index 387985512b..8dfed8d2d6 100644
--- a/keyboards/massdrop/alt/alt.h
+++ b/keyboards/massdrop/alt/alt.h
@@ -22,3 +22,13 @@
     {   K45, KC_NO,   K46,   K47,   K48,   K49,   K50,   K51,   K52,   K53,   K54,   K55,   K56,   K57,   K58, }, \
     {   K59,   K60,   K61, KC_NO, KC_NO, KC_NO,   K62, KC_NO, KC_NO, KC_NO,   K63,   K64,   K65,   K66,   K67, }, \
 }
+
+#define TOGGLE_FLAG_AND_PRINT(var, name) { \
+        if (var) { \
+            dprintf(name " disabled\r\n"); \
+            var = !var; \
+        } else { \
+            var = !var; \
+            dprintf(name " enabled\r\n"); \
+        } \
+    }
diff --git a/keyboards/massdrop/alt/keymaps/default/keymap.c b/keyboards/massdrop/alt/keymaps/default/keymap.c
index 0cbce86293..a5c443ffcd 100644
--- a/keyboards/massdrop/alt/keymaps/default/keymap.c
+++ b/keyboards/massdrop/alt/keymaps/default/keymap.c
@@ -136,8 +136,7 @@ bool process_record_user(uint16_t keycode, keyrecord_t *record) {
         case L_T_BR:
             if (record->event.pressed) {
                 led_animation_breathing = !led_animation_breathing;
-                if (led_animation_breathing)
-                {
+                if (led_animation_breathing) {
                     gcr_breathe = gcr_desired;
                     led_animation_breathe_cur = BREATHE_MIN_STEP;
                     breathe_dir = 1;
@@ -151,50 +150,32 @@ bool process_record_user(uint16_t keycode, keyrecord_t *record) {
             return false;
         case U_T_AUTO:
             if (record->event.pressed && MODS_SHIFT && MODS_CTRL) {
-                usb_extra_manual = !usb_extra_manual;
-                CDC_print("USB extra port manual mode ");
-                CDC_print(usb_extra_manual ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(usb_extra_manual, "USB extra port manual mode");
             }
             return false;
         case U_T_AGCR:
             if (record->event.pressed && MODS_SHIFT && MODS_CTRL) {
-                usb_gcr_auto = !usb_gcr_auto;
-                CDC_print("USB GCR auto mode ");
-                CDC_print(usb_gcr_auto ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(usb_gcr_auto, "USB GCR auto mode");
             }
             return false;
         case DBG_TOG:
             if (record->event.pressed) {
-                debug_enable = !debug_enable;
-                CDC_print("Debug mode ");
-                CDC_print(debug_enable ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(debug_enable, "Debug mode");
             }
             return false;
         case DBG_MTRX:
             if (record->event.pressed) {
-                debug_matrix = !debug_matrix;
-                CDC_print("Debug matrix ");
-                CDC_print(debug_matrix ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(debug_matrix, "Debug matrix");
             }
             return false;
         case DBG_KBD:
             if (record->event.pressed) {
-                debug_keyboard = !debug_keyboard;
-                CDC_print("Debug keyboard ");
-                CDC_print(debug_keyboard ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(debug_keyboard, "Debug keyboard");
             }
             return false;
         case DBG_MOU:
             if (record->event.pressed) {
-                debug_mouse = !debug_mouse;
-                CDC_print("Debug mouse ");
-                CDC_print(debug_mouse ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(debug_mouse, "Debug mouse");
             }
             return false;
         case MD_BOOT:
@@ -209,4 +190,4 @@ bool process_record_user(uint16_t keycode, keyrecord_t *record) {
         default:
             return true; //Process all other keycodes normally
     }
-}
\ No newline at end of file
+}
diff --git a/keyboards/massdrop/alt/keymaps/mac/keymap.c b/keyboards/massdrop/alt/keymaps/mac/keymap.c
index e886290e7e..d6978fd801 100644
--- a/keyboards/massdrop/alt/keymaps/mac/keymap.c
+++ b/keyboards/massdrop/alt/keymaps/mac/keymap.c
@@ -136,8 +136,7 @@ bool process_record_user(uint16_t keycode, keyrecord_t *record) {
         case L_T_BR:
             if (record->event.pressed) {
                 led_animation_breathing = !led_animation_breathing;
-                if (led_animation_breathing)
-                {
+                if (led_animation_breathing) {
                     gcr_breathe = gcr_desired;
                     led_animation_breathe_cur = BREATHE_MIN_STEP;
                     breathe_dir = 1;
@@ -151,50 +150,32 @@ bool process_record_user(uint16_t keycode, keyrecord_t *record) {
             return false;
         case U_T_AUTO:
             if (record->event.pressed && MODS_SHIFT && MODS_CTRL) {
-                usb_extra_manual = !usb_extra_manual;
-                CDC_print("USB extra port manual mode ");
-                CDC_print(usb_extra_manual ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(usb_extra_manual, "USB extra port manual mode");
             }
             return false;
         case U_T_AGCR:
             if (record->event.pressed && MODS_SHIFT && MODS_CTRL) {
-                usb_gcr_auto = !usb_gcr_auto;
-                CDC_print("USB GCR auto mode ");
-                CDC_print(usb_gcr_auto ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(usb_gcr_auto, "USB GCR auto mode");
             }
             return false;
         case DBG_TOG:
             if (record->event.pressed) {
-                debug_enable = !debug_enable;
-                CDC_print("Debug mode ");
-                CDC_print(debug_enable ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(debug_enable, "Debug mode");
             }
             return false;
         case DBG_MTRX:
             if (record->event.pressed) {
-                debug_matrix = !debug_matrix;
-                CDC_print("Debug matrix ");
-                CDC_print(debug_matrix ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(debug_matrix, "Debug matrix");
             }
             return false;
         case DBG_KBD:
             if (record->event.pressed) {
-                debug_keyboard = !debug_keyboard;
-                CDC_print("Debug keyboard ");
-                CDC_print(debug_keyboard ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(debug_keyboard, "Debug keyboard");
             }
             return false;
         case DBG_MOU:
             if (record->event.pressed) {
-                debug_mouse = !debug_mouse;
-                CDC_print("Debug mouse ");
-                CDC_print(debug_mouse ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(debug_mouse, "Debug mouse");
             }
             return false;
         case MD_BOOT:
diff --git a/keyboards/massdrop/alt/rules.mk b/keyboards/massdrop/alt/rules.mk
index daf6795852..c5539158f5 100644
--- a/keyboards/massdrop/alt/rules.mk
+++ b/keyboards/massdrop/alt/rules.mk
@@ -30,3 +30,4 @@ FAUXCLICKY_ENABLE = no      # Use buzzer to emulate clicky switches
 HD44780_ENABLE = no         # Enable support for HD44780 based LCDs (+400)
 VIRTSER_ENABLE = no         # USB Serial Driver
 RAW_ENABLE = no             # Raw device
+AUTO_SHIFT_ENABLE = no      # Auto Shift
diff --git a/keyboards/massdrop/ctrl/ctrl.h b/keyboards/massdrop/ctrl/ctrl.h
index dc7c7eabe5..c83efca16d 100644
--- a/keyboards/massdrop/ctrl/ctrl.h
+++ b/keyboards/massdrop/ctrl/ctrl.h
@@ -30,3 +30,13 @@
     {   K59, K60, K61, K62, K63, K76, K50, K33   }, \
     {   K72, K73, K74, K75, K85, K86, K87,       }, \
 }
+
+#define TOGGLE_FLAG_AND_PRINT(var, name) { \
+        if (var) { \
+            dprintf(name " disabled\r\n"); \
+            var = !var; \
+        } else { \
+            var = !var; \
+            dprintf(name " enabled\r\n"); \
+        } \
+    }
diff --git a/keyboards/massdrop/ctrl/keymaps/default/keymap.c b/keyboards/massdrop/ctrl/keymaps/default/keymap.c
index 9bfb7fec58..88c1ac3123 100644
--- a/keyboards/massdrop/ctrl/keymaps/default/keymap.c
+++ b/keyboards/massdrop/ctrl/keymaps/default/keymap.c
@@ -33,7 +33,7 @@ const uint16_t PROGMEM keymaps[][MATRIX_ROWS][MATRIX_COLS] = {
         KC_TAB,  KC_Q,    KC_W,    KC_E,    KC_R,    KC_T,    KC_Y,    KC_U,    KC_I,    KC_O,    KC_P,    KC_LBRC, KC_RBRC, KC_BSLS,   KC_DEL,  KC_END,  KC_PGDN, \
         KC_CAPS, KC_A,    KC_S,    KC_D,    KC_F,    KC_G,    KC_H,    KC_J,    KC_K,    KC_L,    KC_SCLN, KC_QUOT, KC_ENT, \
         KC_LSFT, KC_Z,    KC_X,    KC_C,    KC_V,    KC_B,    KC_N,    KC_M,    KC_COMM, KC_DOT,  KC_SLSH, KC_RSFT,                              KC_UP, \
-        KC_LCTL, KC_LGUI, KC_LALT,                  KC_SPC,                              KC_RALT, MO(1),   KC_APP,  KC_RCTL,            KC_LEFT, KC_DOWN, KC_RGHT \
+        KC_LCTL, KC_LGUI, KC_LALT,                   KC_SPC,                             KC_RALT, MO(1),   KC_APP,  KC_RCTL,            KC_LEFT, KC_DOWN, KC_RGHT \
     ),
     [1] = LAYOUT(
         KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,            KC_MUTE, KC_TRNS, KC_TRNS, \
@@ -41,7 +41,7 @@ const uint16_t PROGMEM keymaps[][MATRIX_ROWS][MATRIX_COLS] = {
         L_T_BR,  L_PSD,   L_BRI,   L_PSI,   KC_TRNS, KC_TRNS, KC_TRNS, U_T_AUTO,U_T_AGCR,KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,   KC_MPRV, KC_MNXT, KC_VOLD, \
         L_T_PTD, L_PTP,   L_BRD,   L_PTN,   KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, \
         KC_TRNS, L_T_MD,  L_T_ONF, KC_TRNS, KC_TRNS, MD_BOOT, TG_NKRO, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,                              KC_TRNS, \
-        KC_TRNS, KC_TRNS, KC_TRNS,                  KC_TRNS,                             KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,            KC_TRNS, KC_TRNS, KC_TRNS \
+        KC_TRNS, KC_TRNS, KC_TRNS,                   KC_TRNS,                            KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,            KC_TRNS, KC_TRNS, KC_TRNS \
     ),
     /*
     [X] = LAYOUT(
@@ -50,7 +50,7 @@ const uint16_t PROGMEM keymaps[][MATRIX_ROWS][MATRIX_COLS] = {
         KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,   KC_TRNS, KC_TRNS, KC_TRNS, \
         KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, \
         KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, TG_NKRO, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,                              KC_TRNS, \
-        KC_TRNS, KC_TRNS, KC_TRNS,                  KC_TRNS,                             KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,            KC_TRNS, KC_TRNS, KC_TRNS \
+        KC_TRNS, KC_TRNS, KC_TRNS,                   KC_TRNS,                            KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,            KC_TRNS, KC_TRNS, KC_TRNS \
     ),
     */
 };
@@ -139,8 +139,7 @@ bool process_record_user(uint16_t keycode, keyrecord_t *record) {
         case L_T_BR:
             if (record->event.pressed) {
                 led_animation_breathing = !led_animation_breathing;
-                if (led_animation_breathing)
-                {
+                if (led_animation_breathing) {
                     gcr_breathe = gcr_desired;
                     led_animation_breathe_cur = BREATHE_MIN_STEP;
                     breathe_dir = 1;
@@ -154,50 +153,32 @@ bool process_record_user(uint16_t keycode, keyrecord_t *record) {
             return false;
         case U_T_AUTO:
             if (record->event.pressed && MODS_SHIFT && MODS_CTRL) {
-                usb_extra_manual = !usb_extra_manual;
-                CDC_print("USB extra port manual mode ");
-                CDC_print(usb_extra_manual ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(usb_extra_manual, "USB extra port manual mode");
             }
             return false;
         case U_T_AGCR:
             if (record->event.pressed && MODS_SHIFT && MODS_CTRL) {
-                usb_gcr_auto = !usb_gcr_auto;
-                CDC_print("USB GCR auto mode ");
-                CDC_print(usb_gcr_auto ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(usb_gcr_auto, "USB GCR auto mode");
             }
             return false;
         case DBG_TOG:
             if (record->event.pressed) {
-                debug_enable = !debug_enable;
-                CDC_print("Debug mode ");
-                CDC_print(debug_enable ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(debug_enable, "Debug mode");
             }
             return false;
         case DBG_MTRX:
             if (record->event.pressed) {
-                debug_matrix = !debug_matrix;
-                CDC_print("Debug matrix ");
-                CDC_print(debug_matrix ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(debug_matrix, "Debug matrix");
             }
             return false;
         case DBG_KBD:
             if (record->event.pressed) {
-                debug_keyboard = !debug_keyboard;
-                CDC_print("Debug keyboard ");
-                CDC_print(debug_keyboard ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(debug_keyboard, "Debug keyboard");
             }
             return false;
         case DBG_MOU:
             if (record->event.pressed) {
-                debug_mouse = !debug_mouse;
-                CDC_print("Debug mouse ");
-                CDC_print(debug_mouse ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(debug_mouse, "Debug mouse");
             }
             return false;
         case MD_BOOT:
diff --git a/keyboards/massdrop/ctrl/keymaps/mac/keymap.c b/keyboards/massdrop/ctrl/keymaps/mac/keymap.c
index a03f891e8c..6c5dfe19c0 100644
--- a/keyboards/massdrop/ctrl/keymaps/mac/keymap.c
+++ b/keyboards/massdrop/ctrl/keymaps/mac/keymap.c
@@ -33,15 +33,15 @@ const uint16_t PROGMEM keymaps[][MATRIX_ROWS][MATRIX_COLS] = {
         KC_TAB,  KC_Q,    KC_W,    KC_E,    KC_R,    KC_T,    KC_Y,    KC_U,    KC_I,    KC_O,    KC_P,    KC_LBRC, KC_RBRC, KC_BSLS,   KC_DEL,  KC_END,  KC_PGDN, \
         KC_CAPS, KC_A,    KC_S,    KC_D,    KC_F,    KC_G,    KC_H,    KC_J,    KC_K,    KC_L,    KC_SCLN, KC_QUOT, KC_ENT, \
         KC_LSFT, KC_Z,    KC_X,    KC_C,    KC_V,    KC_B,    KC_N,    KC_M,    KC_COMM, KC_DOT,  KC_SLSH, KC_RSFT,                              KC_UP, \
-        KC_LCTL, KC_LALT, KC_LGUI,                  KC_SPC,                              KC_RGUI, MO(1),   KC_APP,  KC_RCTL,            KC_LEFT, KC_DOWN, KC_RGHT \
+        KC_LCTL, KC_LALT, KC_LGUI,                   KC_SPC,                             KC_RGUI, MO(1),   KC_APP,  KC_RCTL,            KC_LEFT, KC_DOWN, KC_RGHT \
     ),
     [1] = LAYOUT(
         KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,            KC_MUTE, KC_TRNS, KC_TRNS, \
         KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,   KC_MPLY, KC_MSTP, KC_VOLU, \
         L_T_BR,  L_PSD,   L_BRI,   L_PSI,   KC_TRNS, KC_TRNS, KC_TRNS, U_T_AUTO,U_T_AGCR,KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,   KC_MPRV, KC_MNXT, KC_VOLD, \
         L_T_PTD, L_PTP,   L_BRD,   L_PTN,   KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, \
-        KC_TRNS, L_T_MD,  L_T_ONF, KC_TRNS, KC_TRNS, KC_TRNS, TG_NKRO, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,                              KC_TRNS, \
-        KC_TRNS, KC_TRNS, KC_TRNS,                  KC_TRNS,                             KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,            KC_TRNS, KC_TRNS, KC_TRNS \
+        KC_TRNS, L_T_MD,  L_T_ONF, KC_TRNS, KC_TRNS, MD_BOOT, TG_NKRO, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,                              KC_TRNS, \
+        KC_TRNS, KC_TRNS, KC_TRNS,                   KC_TRNS,                            KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,            KC_TRNS, KC_TRNS, KC_TRNS \
     ),
     /*
     [X] = LAYOUT(
@@ -50,7 +50,7 @@ const uint16_t PROGMEM keymaps[][MATRIX_ROWS][MATRIX_COLS] = {
         KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,   KC_TRNS, KC_TRNS, KC_TRNS, \
         KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, \
         KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, TG_NKRO, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,                              KC_TRNS, \
-        KC_TRNS, KC_TRNS, KC_TRNS,                  KC_TRNS,                             KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,            KC_TRNS, KC_TRNS, KC_TRNS \
+        KC_TRNS, KC_TRNS, KC_TRNS,                   KC_TRNS,                            KC_TRNS, KC_TRNS, KC_TRNS, KC_TRNS,            KC_TRNS, KC_TRNS, KC_TRNS \
     ),
     */
 };
@@ -139,8 +139,7 @@ bool process_record_user(uint16_t keycode, keyrecord_t *record) {
         case L_T_BR:
             if (record->event.pressed) {
                 led_animation_breathing = !led_animation_breathing;
-                if (led_animation_breathing)
-                {
+                if (led_animation_breathing) {
                     gcr_breathe = gcr_desired;
                     led_animation_breathe_cur = BREATHE_MIN_STEP;
                     breathe_dir = 1;
@@ -154,50 +153,32 @@ bool process_record_user(uint16_t keycode, keyrecord_t *record) {
             return false;
         case U_T_AUTO:
             if (record->event.pressed && MODS_SHIFT && MODS_CTRL) {
-                usb_extra_manual = !usb_extra_manual;
-                CDC_print("USB extra port manual mode ");
-                CDC_print(usb_extra_manual ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(usb_extra_manual, "USB extra port manual mode");
             }
             return false;
         case U_T_AGCR:
             if (record->event.pressed && MODS_SHIFT && MODS_CTRL) {
-                usb_gcr_auto = !usb_gcr_auto;
-                CDC_print("USB GCR auto mode ");
-                CDC_print(usb_gcr_auto ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(usb_gcr_auto, "USB GCR auto mode");
             }
             return false;
         case DBG_TOG:
             if (record->event.pressed) {
-                debug_enable = !debug_enable;
-                CDC_print("Debug mode ");
-                CDC_print(debug_enable ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(debug_enable, "Debug mode");
             }
             return false;
         case DBG_MTRX:
             if (record->event.pressed) {
-                debug_matrix = !debug_matrix;
-                CDC_print("Debug matrix ");
-                CDC_print(debug_matrix ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(debug_matrix, "Debug matrix");
             }
             return false;
         case DBG_KBD:
             if (record->event.pressed) {
-                debug_keyboard = !debug_keyboard;
-                CDC_print("Debug keyboard ");
-                CDC_print(debug_keyboard ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(debug_keyboard, "Debug keyboard");
             }
             return false;
         case DBG_MOU:
             if (record->event.pressed) {
-                debug_mouse = !debug_mouse;
-                CDC_print("Debug mouse ");
-                CDC_print(debug_mouse ? "enabled" : "disabled");
-                CDC_print("\r\n");
+                TOGGLE_FLAG_AND_PRINT(debug_mouse, "Debug mouse");
             }
             return false;
         case MD_BOOT:
diff --git a/keyboards/massdrop/ctrl/rules.mk b/keyboards/massdrop/ctrl/rules.mk
index daf6795852..c5539158f5 100644
--- a/keyboards/massdrop/ctrl/rules.mk
+++ b/keyboards/massdrop/ctrl/rules.mk
@@ -30,3 +30,4 @@ FAUXCLICKY_ENABLE = no      # Use buzzer to emulate clicky switches
 HD44780_ENABLE = no         # Enable support for HD44780 based LCDs (+400)
 VIRTSER_ENABLE = no         # USB Serial Driver
 RAW_ENABLE = no             # Raw device
+AUTO_SHIFT_ENABLE = no      # Auto Shift
diff --git a/lib/arm_atsam/packs/atmel/SAMD51_DFP/1.0.70/gcc/gcc/samd51j18a_flash.ld b/lib/arm_atsam/packs/atmel/SAMD51_DFP/1.0.70/gcc/gcc/samd51j18a_flash.ld
index 3d114f5b7b..35db619717 100644
--- a/lib/arm_atsam/packs/atmel/SAMD51_DFP/1.0.70/gcc/gcc/samd51j18a_flash.ld
+++ b/lib/arm_atsam/packs/atmel/SAMD51_DFP/1.0.70/gcc/gcc/samd51j18a_flash.ld
@@ -35,7 +35,7 @@ SEARCH_DIR(.)
 /* Memory Spaces Definitions */
 MEMORY
 {
-  //rom      (rx)  : ORIGIN = 0x00000000, LENGTH = 0x00040000
+/*rom      (rx)  : ORIGIN = 0x00000000, LENGTH = 0x00040000*/
   rom      (rx)  : ORIGIN = 0x00004000, LENGTH = 0x0003C000
   ram      (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00020000
   bkupram  (rwx) : ORIGIN = 0x47000000, LENGTH = 0x00002000
@@ -45,6 +45,9 @@ MEMORY
 /* The stack size used by the application. NOTE: you need to adjust according to your application. */
 STACK_SIZE = DEFINED(STACK_SIZE) ? STACK_SIZE : DEFINED(__stack_size__) ? __stack_size__ : 0x8000;
 
+/* The heap size used by the application. */
+HEAP_SIZE = DEFINED(HEAP_SIZE) ? HEAP_SIZE : DEFINED(__heap_size__) ? __heap_size__ : 0x800;
+
 _srom = ORIGIN(rom);
 _lrom = LENGTH(rom);
 _erom = ORIGIN(rom) + LENGTH(rom);
@@ -153,6 +156,17 @@ SECTIONS
         _ezero = .;
     } > ram
 
+    /* .heap section for syscalls */
+    .heap (NOLOAD) :
+    {
+        . = ALIGN(4);
+        _end = .;
+        end = .;
+        _heap_start = .;
+        . = . + HEAP_SIZE;
+        _heap_end = .;
+    } > ram
+
     /* stack section */
     .stack (NOLOAD):
     {
diff --git a/tmk_core/arm_atsam.mk b/tmk_core/arm_atsam.mk
index ef412d59d6..06823fb629 100644
--- a/tmk_core/arm_atsam.mk
+++ b/tmk_core/arm_atsam.mk
@@ -36,7 +36,7 @@ LDFLAGS +=-Wl,--gc-sections
 LDFLAGS += -Wl,-Map="%OUT%%PROJ_NAME%.map"
 LDFLAGS += -Wl,--start-group
 LDFLAGS += -Wl,--end-group
-LDFLAGS += -Wl,--gc-sections
+LDFLAGS += --specs=rdimon.specs
 LDFLAGS += -T$(LIB_PATH)/arm_atsam/packs/atmel/SAMD51_DFP/1.0.70/gcc/gcc/samd51j18a_flash.ld
 
 OPT_DEFS += -DPROTOCOL_ARM_ATSAM
diff --git a/tmk_core/common/arm_atsam/printf.h b/tmk_core/common/arm_atsam/printf.h
index 582c83bf54..3206b40bdb 100644
--- a/tmk_core/common/arm_atsam/printf.h
+++ b/tmk_core/common/arm_atsam/printf.h
@@ -1,8 +1,8 @@
 #ifndef _PRINTF_H_
 #define _PRINTF_H_
 
-#define __xprintf dpf
 int dpf(const char *_Format, ...);
+#define __xprintf dpf
 
 #endif //_PRINTF_H_
 
diff --git a/tmk_core/common/print.h b/tmk_core/common/print.h
index 9cbe67bad6..d945276572 100644
--- a/tmk_core/common/print.h
+++ b/tmk_core/common/print.h
@@ -29,7 +29,7 @@
 #include <stdbool.h>
 #include "util.h"
 
-#if defined(PROTOCOL_CHIBIOS)
+#if defined(PROTOCOL_CHIBIOS) || defined(PROTOCOL_ARM_ATSAM)
 #define PSTR(x) x
 #endif
 
diff --git a/tmk_core/protocol/arm_atsam.mk b/tmk_core/protocol/arm_atsam.mk
index d535b64cd7..04e02790a0 100644
--- a/tmk_core/protocol/arm_atsam.mk
+++ b/tmk_core/protocol/arm_atsam.mk
@@ -10,7 +10,6 @@ SRC += $(ARM_ATSAM_DIR)/spi.c
 SRC += $(ARM_ATSAM_DIR)/startup.c
 
 SRC += $(ARM_ATSAM_DIR)/usb/main_usb.c
-SRC += $(ARM_ATSAM_DIR)/usb/spfssf.c
 SRC += $(ARM_ATSAM_DIR)/usb/udc.c
 SRC += $(ARM_ATSAM_DIR)/usb/udi_cdc.c
 SRC += $(ARM_ATSAM_DIR)/usb/udi_hid.c
diff --git a/tmk_core/protocol/arm_atsam/arm_atsam_protocol.h b/tmk_core/protocol/arm_atsam/arm_atsam_protocol.h
index be73beccd7..2ba0991749 100644
--- a/tmk_core/protocol/arm_atsam/arm_atsam_protocol.h
+++ b/tmk_core/protocol/arm_atsam/arm_atsam_protocol.h
@@ -36,7 +36,6 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include "issi3733_driver.h"
 #include "./usb/compiler.h"
 #include "./usb/udc.h"
-#include "./usb/spfssf.h"
 #include "./usb/udi_cdc.h"
 
 #endif //MD_BOOTLOADER
diff --git a/tmk_core/protocol/arm_atsam/d51_util.h b/tmk_core/protocol/arm_atsam/d51_util.h
index 465889c7cb..7a35f7989f 100644
--- a/tmk_core/protocol/arm_atsam/d51_util.h
+++ b/tmk_core/protocol/arm_atsam/d51_util.h
@@ -32,6 +32,16 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define m15_on  REG_PORT_OUTSET1 = 0x40000000 //PB30 High
 #define m15_off REG_PORT_OUTCLR1 = 0x40000000 //PB30 Low
 
+//Debug Port PB23
+#define m27_ena REG_PORT_DIRSET1 = 0x800000 //PB23 Output
+#define m27_on  REG_PORT_OUTSET1 = 0x800000 //PB23 High
+#define m27_off REG_PORT_OUTCLR1 = 0x800000 //PB23 Low
+
+//Debug Port PB31
+#define m28_ena REG_PORT_DIRSET1 = 0x80000000 //PB31 Output
+#define m28_on  REG_PORT_OUTSET1 = 0x80000000 //PB31 High
+#define m28_off REG_PORT_OUTCLR1 = 0x80000000 //PB31 Low
+
 #define m15_loop(M15X) {uint8_t M15L=M15X; while(M15L--){m15_on;CLK_delay_us(1);m15_off;}}
 
 void m15_print(uint32_t x);
diff --git a/tmk_core/protocol/arm_atsam/main_arm_atsam.c b/tmk_core/protocol/arm_atsam/main_arm_atsam.c
index 8cc7767038..676dac4ea3 100644
--- a/tmk_core/protocol/arm_atsam/main_arm_atsam.c
+++ b/tmk_core/protocol/arm_atsam/main_arm_atsam.c
@@ -31,6 +31,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //From keyboard's directory
 #include "config_led.h"
 
+void main_subtasks(void);
 uint8_t keyboard_leds(void);
 void send_keyboard(report_keyboard_t *report);
 void send_mouse(report_mouse_t *report);
@@ -65,7 +66,7 @@ void send_keyboard(report_keyboard_t *report)
     if (!keymap_config.nkro)
     {
 #endif //NKRO_ENABLE
-        dprint("s-kbd\r\n");
+        while (udi_hid_kbd_b_report_trans_ongoing) { main_subtasks(); } //Run other tasks while waiting for USB to be free
 
         irqflags = __get_PRIMASK();
         __disable_irq();
@@ -81,7 +82,7 @@ void send_keyboard(report_keyboard_t *report)
     }
     else
     {
-        dprint("s-nkro\r\n");
+        while (udi_hid_nkro_b_report_trans_ongoing) { main_subtasks(); } //Run other tasks while waiting for USB to be free
 
         irqflags = __get_PRIMASK();
         __disable_irq();
@@ -102,8 +103,6 @@ void send_mouse(report_mouse_t *report)
 #ifdef MOUSEKEY_ENABLE
     uint32_t irqflags;
 
-    dprint("s-mou\r\n");
-
     irqflags = __get_PRIMASK();
     __disable_irq();
     __DMB();
@@ -120,8 +119,6 @@ void send_mouse(report_mouse_t *report)
 void send_system(uint16_t data)
 {
 #ifdef EXTRAKEY_ENABLE
-    dprintf("s-exks %i\r\n", data);
-
     uint32_t irqflags;
 
     irqflags = __get_PRIMASK();
@@ -142,8 +139,6 @@ void send_system(uint16_t data)
 void send_consumer(uint16_t data)
 {
 #ifdef EXTRAKEY_ENABLE
-    dprintf("s-exkc %i\r\n",data);
-
     uint32_t irqflags;
 
     irqflags = __get_PRIMASK();
@@ -160,6 +155,77 @@ void send_consumer(uint16_t data)
 #endif //EXTRAKEY_ENABLE
 }
 
+uint8_t g_drvid;
+
+void main_subtask_usb_state(void)
+{
+    if (usb_state == USB_STATE_POWERDOWN)
+    {
+        uint32_t timer_led = timer_read32();
+
+        led_on;
+        if (led_enabled)
+        {
+            for (g_drvid = 0; g_drvid < ISSI3733_DRIVER_COUNT; g_drvid++)
+            {
+                I2C3733_Control_Set(0);
+            }
+        }
+        while (usb_state == USB_STATE_POWERDOWN)
+        {
+            if (timer_read32() - timer_led > 1000) led_off; //Good to indicate went to sleep, but only for a second
+        }
+        if (led_enabled)
+        {
+            for (g_drvid = 0; g_drvid < ISSI3733_DRIVER_COUNT; g_drvid++)
+            {
+                I2C3733_Control_Set(1);
+            }
+        }
+        led_off;
+    }
+}
+
+void main_subtask_led(void)
+{
+    led_matrix_task();
+}
+
+void main_subtask_power_check(void)
+{
+    static uint64_t next_5v_checkup = 0;
+
+    if (CLK_get_ms() > next_5v_checkup)
+    {
+        next_5v_checkup = CLK_get_ms() + 5;
+
+        v_5v = adc_get(ADC_5V);
+        v_5v_avg = 0.9 * v_5v_avg + 0.1 * v_5v;
+
+        gcr_compute();
+    }
+}
+
+void main_subtask_usb_extra_device(void)
+{
+    static uint64_t next_usb_checkup = 0;
+
+    if (CLK_get_ms() > next_usb_checkup)
+    {
+        next_usb_checkup = CLK_get_ms() + 10;
+
+        USB_HandleExtraDevice();
+    }
+}
+
+void main_subtasks(void)
+{
+    main_subtask_usb_state();
+    main_subtask_led();
+    main_subtask_power_check();
+    main_subtask_usb_extra_device();
+}
+
 int main(void)
 {
     led_ena;
@@ -201,9 +267,8 @@ int main(void)
 
     i2c_led_q_init();
 
-    uint8_t drvid;
-    for (drvid=0;drvid<ISSI3733_DRIVER_COUNT;drvid++)
-        I2C_LED_Q_ONOFF(drvid); //Queue data
+    for (g_drvid = 0; g_drvid < ISSI3733_DRIVER_COUNT; g_drvid++)
+        I2C_LED_Q_ONOFF(g_drvid); //Queue data
 
     keyboard_setup();
 
@@ -214,8 +279,6 @@ int main(void)
 #ifdef VIRTSER_ENABLE
     uint64_t next_print = 0;
 #endif //VIRTSER_ENABLE
-    uint64_t next_usb_checkup = 0;
-    uint64_t next_5v_checkup = 0;
 
     v_5v_avg = adc_get(ADC_5V);
 
@@ -223,58 +286,15 @@ int main(void)
 
     while (1)
     {
-        if (usb_state == USB_STATE_POWERDOWN)
-        {
-            uint32_t timer_led = timer_read32();
-
-            led_on;
-            if (led_enabled)
-            {
-                for (drvid=0;drvid<ISSI3733_DRIVER_COUNT;drvid++)
-                {
-                    I2C3733_Control_Set(0);
-                }
-            }
-            while (usb_state == USB_STATE_POWERDOWN)
-            {
-                if (timer_read32() - timer_led > 1000) led_off; //Good to indicate went to sleep, but only for a second
-            }
-            if (led_enabled)
-            {
-                for (drvid=0;drvid<ISSI3733_DRIVER_COUNT;drvid++)
-                {
-                    I2C3733_Control_Set(1);
-                }
-            }
-            led_off;
-        }
-
         keyboard_task();
 
-        led_matrix_task();
-
-        if (CLK_get_ms() > next_5v_checkup)
-        {
-            next_5v_checkup = CLK_get_ms() + 5;
-
-            v_5v = adc_get(ADC_5V);
-            v_5v_avg = 0.9 * v_5v_avg + 0.1 * v_5v;
-
-            gcr_compute();
-        }
-
-        if (CLK_get_ms() > next_usb_checkup)
-        {
-            next_usb_checkup = CLK_get_ms() + 10;
-
-            USB_HandleExtraDevice();
-        }
+        main_subtasks(); //Note these tasks will also be run while waiting for USB keyboard polling intervals
 
 #ifdef VIRTSER_ENABLE
         if (CLK_get_ms() > next_print)
         {
             next_print = CLK_get_ms() + 250;
-            //dpf("5v=%i 5vu=%i dlow=%i dhi=%i gca=%i gcd=%i\r\n",v_5v,v_5v_avg,v_5v_avg-V5_LOW,v_5v_avg-V5_HIGH,gcr_actual,gcr_desired);
+            dprintf("5v=%u 5vu=%u dlow=%u dhi=%u gca=%u gcd=%u\r\n",v_5v,v_5v_avg,v_5v_avg-V5_LOW,v_5v_avg-V5_HIGH,gcr_actual,gcr_desired);
         }
 #endif //VIRTSER_ENABLE
     }
diff --git a/tmk_core/protocol/arm_atsam/usb/spfssf.c b/tmk_core/protocol/arm_atsam/usb/spfssf.c
deleted file mode 100644
index 449a8bb7d0..0000000000
--- a/tmk_core/protocol/arm_atsam/usb/spfssf.c
+++ /dev/null
@@ -1,268 +0,0 @@
-#include "samd51j18a.h"
-#include "stdarg.h"
-#include "spfssf.h"
-#include "usb_util.h"
-
-int vspf(char *_Dest, const char *_Format, va_list va)
-{
-    //va_list va;                                                   //Variable argument list variable
-    char *d = _Dest;                                               //Pointer to dest
-
-    //va_start(va,_Format);                                         //Initialize the variable argument list
-    while (*_Format)                                                //While not end of format string
-    {
-        if (*_Format == SPF_SPEC_START)                             //If current format string character is the specifier start character
-        {
-            _Format++;                                              //Skip over the character
-            while (*_Format && *_Format <= 64) _Format++;           //Forward past any options
-            if (*_Format == SPF_SPEC_START) *d++ = *_Format;        //If the character is the specifier start character, output the character and advance dest
-            else if (*_Format == SPF_SPEC_LONG)                     //If the character is the long type
-            {
-                _Format++;                                          //Skip over the character
-                if (*_Format == SPF_SPEC_DECIMAL)                   //If the character is the decimal type
-                {
-                    int64_t buf = va_arg(va,int64_t);               //Get the next value from the va list
-                    //if (buf < 0) { *d++ = '-'; buf = -buf; }      //If the given number is negative, add a negative sign to the dest and invert number
-                    //spf_uint2str_32_3t(&d,buf,32);                //Perform the conversion
-                    d += UTIL_ltoa_radix(buf, d, 10);
-                }
-                else if (*_Format == SPF_SPEC_UNSIGNED)             //If the character is the unsigned type
-                {
-                    uint64_t num = va_arg(va,uint64_t);             //Get the next value from the va list
-                    //spf_uint2str_32_3t(&d,num,32);                //Perform the conversion
-                    d += UTIL_ltoa_radix(num, d, 10);
-                }
-                else if (*_Format == SPF_SPEC_UHINT || *_Format == SPF_SPEC_UHINT_UP)   //If the character is the unsigned type
-                {
-                    uint64_t buf = va_arg(va,uint64_t);             //Get the next value from the va list
-                    //spf_uint2hex_32(&d,(unsigned long) buf);
-                    d += UTIL_ltoa_radix(buf, d, 16);
-                }
-                else                                                //If the character was not a known type
-                {
-                    *d++ = SPF_SPEC_START;                          //Output the start specifier
-                    *d++ = SPF_SPEC_LONG;                           //Output the long type
-                    *d++ = *_Format;                                //Output the unknown type
-                }
-            }
-            else if (*_Format == SPF_SPEC_DECIMAL)                  //If the character is the decimal type
-            {
-                int buf = va_arg(va,int);                           //Get the next value from the va list
-                //if (buf < 0) { *d++ = '-'; buf = -buf; }          //If the given number is negative, add a negative sign to the dest and invert number
-                //spf_uint2str_32_3t(&d,buf,16);                    //Perform the conversion
-                d += UTIL_itoa(buf, d);
-            }
-            else if (*_Format == SPF_SPEC_INT)                      //If the character is the integer type
-            {
-                int buf = va_arg(va,int);                           //Get the next value from the va list
-                //if (buf < 0) { *d++ = '-'; buf = -buf; }          //If the given number is negative, add a negative sign to the dest and inverted number
-                //spf_uint2str_32_3t(&d,buf,16);                    //Perform the conversion
-                d += UTIL_itoa(buf, d);
-            }
-            else if (*_Format == SPF_SPEC_UINT)                     //If the character is the unsigned integer type
-            {
-                int buf = va_arg(va,int);                           //Get the next value from the va list
-                //spf_uint2str_32_3t(&d,buf,16);                    //Perform the conversion
-                d += UTIL_utoa(buf, d);
-            }
-            else if (*_Format == SPF_SPEC_STRING)                   //If the character is the string type
-            {
-                char *buf = va_arg(va,char*);                       //Get the next value from the va list
-                while (*buf) *d++ = *buf++;                         //Perform the conversion (simply output characters and adcance pointers)
-            }
-            else if (*_Format == SPF_SPEC_UHINT || *_Format == SPF_SPEC_UHINT_UP)   //If the character is the short type
-            {
-                int buf = va_arg(va,unsigned int);                  //Get the next value from the va list
-                //spf_uint2hex_32(&d,(unsigned long) buf);          //Perform the conversion
-                d += UTIL_utoa(buf, d);
-            }
-            else                                                    //If the character type is unknown
-            {
-                *d++ = SPF_SPEC_START;                              //Output the start specifier
-                *d++ = *_Format;                                    //Output the unknown type
-            }
-        }
-        else *d++ = *_Format;                                       //If the character is unknown, output it to dest and advance dest
-        _Format++;                                                  //Advance the format buffer pointer to next character
-    }
-    //va_end(va);                                                   //End the variable argument list
-
-    *d = '\0';                                                      //Cap off the destination string with a zero
-
-    return d - _Dest;                                               //Return the length of the destintion buffer
-}
-
-int spf(char *_Dest, const char *_Format, ...)
-{
-    va_list va;                                                     //Variable argument list variable
-    int result;
-
-    va_start(va,_Format);                                           //Initialize the variable argument list
-    result = vspf(_Dest, _Format, va);
-    va_end(va);
-    return result;
-}
-
-//sscanf string to number (integer types)
-int64_t ssf_ston(const char **_Src, uint32_t count, uint32_t *conv_count)
-{
-    int64_t value = 0;                                  //Return value accumulator
-    uint32_t counter=count;                             //Counter to keep track of numbers converted
-    const char* p;                                      //Pointer to first non space character
-
-    while (*(*_Src) == SSF_SKIP_SPACE) (*_Src)++;       //Forward through the whitespace to next non whitespace
-
-    p = (*_Src);                                        //Set pointer to first non space character
-    if (*p == '+' || *p == '-') (*_Src)++;              //Skip over sign if any
-    while (*(*_Src) >= ASCII_NUM_START &&
-        *(*_Src) <= ASCII_NUM_END &&
-        counter)                                        //While the source character is a digit and counter is not zero
-    {
-        value *= 10;                                    //Multiply result by 10 to make room for next 1's place number
-        value += *(*_Src)++ - ASCII_NUM_START;          //Add source number to value
-        counter--;                                      //Decrement counter
-    }
-    if (counter - count == 0) return 0;                 //If no number conversion were performed, return 0
-    if (*p == '-') value = -value;                      //If the number given was negative, make the result negative
-
-    if (conv_count) (*conv_count)++;                    //Increment the converted count
-    return value;                                       //Return the value
-}
-
-uint64_t ssf_hton(const char **_Src, uint32_t count,uint32_t *conv_count)
-{
-    int64_t value=0;                                            //Return value accumulator
-    uint32_t counter=count;                                     //Counter to keep track of numbers converted
-    //const char* p;                                            //Pointer to first non space character
-    char c;
-
-    while (*(*_Src) == SSF_SKIP_SPACE) (*_Src)++;               //Forward through the whitespace to next non whitespace
-
-    //p = (*_Src);                                              //Set pointer to first non space character
-
-    while (counter)
-    {
-        c = *(*_Src)++;
-        if (c >= 'a' && c <= 'f') c -= ('a'-'A');               //toupper
-        if (c < '0' || (c > '9' && c < 'A') || c > 'F') break;
-        value *= 16;                                            //Multiply result by 10 to make room for next 1's place number
-        c = c - '0';
-        if (c > 9) c -= 7;
-        value += c;                                             //Add source number to value
-        counter--;                                              //Decrement counter
-    }
-
-    if (counter - count == 0) return 0;                         //If no number conversion were performed, return 0
-    //if (*p == '-') value = -value;                            //If the number given was negative, make the result negative
-
-    if (conv_count) (*conv_count)++;                            //Increment the converted count
-    return value;
-}
-
-//sscanf
-int ssf(const char *_Src, const char *_Format, ...)
-{
-    va_list va;                                                                         //Variable argument list variable
-    unsigned char looking_for=0;                                                        //Static char specified in format to be found in source
-    uint32_t conv_count=0;                                                              //Count of conversions made
-
-    va_start(va,_Format);                                                               //Initialize the variable argument list
-    while (*_Format)                                                                    //While the format string has not been fully read
-    {
-        if (looking_for != 0)                                                           //If we are looking for a matching character in the source string
-        {
-            while (*_Src != looking_for && *_Src) _Src++;                               //While the character is not found in the source string and not the end of the source
-                                                                                        //  string, increment the pointer position
-            if (*_Src == looking_for) _Src++;                                           //If the character was found, step over it
-            else break;                                                                 //Else the end was reached and the scan is now invalid (Could not find static character)
-            looking_for = 0;                                                            //Clear the looking for character
-        }
-        if (*_Format == SSF_SPEC_START)                                                 //If the current format character is the specifier start character
-        {
-            _Format++;                                                                  //Step over the specifier start character
-            if (*_Format == SSF_SPEC_DECIMAL)                                           //If the decimal specifier type is found
-            {
-                int *value=va_arg(va,int*);                                             //User given destination address
-                //*value = (int)ssf_ston(&_Src,5,&conv_count);                          //Run conversion
-                *value = (int)ssf_ston(&_Src,10,&conv_count);                           //Run conversion
-            }
-            else if (*_Format == SSF_SPEC_LONG)                                         //If the long specifier type is found
-            {
-                _Format++;                                                              //Skip over the specifier type
-                if (*_Format == SSF_SPEC_DECIMAL)                                       //If the decimal specifier type is found
-                {
-                    int64_t *value=va_arg(va,int64_t*);                                 //User given destination address
-                    //*value = (int64_t)ssf_ston(&_Src,10,&conv_count);                 //Run conversion
-                    *value = (int64_t)ssf_ston(&_Src,19,&conv_count);                   //Run conversion
-                }
-                else if (*_Format == SSF_SPEC_UHINT)                                    //If the decimal specifier type is found
-                {
-                    uint64_t *value=va_arg(va,uint64_t *);                              //User given destination address
-                    //*value = (uint64_t int)ssf_hton(&_Src,12,&conv_count);            //Run conversion
-                    *value = (uint64_t)ssf_hton(&_Src,16,&conv_count);                  //Run conversion
-                }
-            }
-            else if (*_Format == SSF_SPEC_SHORTINT)                                     //If the short int specifier type is found
-            {
-                _Format++;                                                              //Skip over the specifier type
-                if (*_Format == SSF_SPEC_SHORTINT)                                      //If the short int specifier type is found
-                {
-                    _Format++;                                                          //Skip over the specifier type
-                    if (*_Format == SSF_SPEC_DECIMAL)                                   //If the decimal specifier type is found
-                    {
-                        unsigned char *value=va_arg(va,unsigned char*);                 //User given destination address
-                        //*value = (unsigned char)ssf_ston(&_Src,3,&conv_count);        //Run conversion
-                        *value = (unsigned char)ssf_ston(&_Src,5,&conv_count);          //Run conversion
-                    }
-                }
-            }
-            else if (*_Format == SSF_SPEC_STRING)                                       //If the specifier type is string
-            {
-                char *value=va_arg(va,char*);                                           //User given destination address, max chars read pointer
-                while (*_Src == SSF_SKIP_SPACE) _Src++;                                 //Forward through the whitespace to next non whitespace
-                while (*_Src != SSF_SKIP_SPACE && *_Src) *value++ = *_Src++;            //While any character but space and not end of string and not end location, copy char to dest
-                *value = 0;                                                             //Cap off the string pointer with zero
-                conv_count++;                                                           //Increment the converted count
-            }
-            else if (*_Format == SSF_SPEC_VERSION)                                      //If the specifier type is string
-            {
-                char *value=va_arg(va,char*);                                           //User given destination address, max chars read pointer
-                while (*_Src == SSF_SKIP_SPACE) _Src++;                                 //Forward through the whitespace to next non whitespace
-                while (*_Src != SSF_DELIM_COMMA && *_Src) *value++ = *_Src++;           //While any character but space and not end of string and not end location, copy char to dest
-                *value = 0;                                                             //Cap off the string pointer with zero
-                conv_count++;                                                           //Increment the converted count
-            }
-            else if (*_Format >= ASCII_NUM_START && *_Format <= ASCII_NUM_END)
-            {
-                uint32_t len = (uint32_t)ssf_ston(&_Format,3,NULL);                     //Convert the given length
-                if (*_Format == SSF_SPEC_STRING)                                        //If the specifier type is string
-                {
-                    char *value=va_arg(va,char*),*e;                                    //User given destination address, max chars read pointer
-                    while (*_Src == SSF_SKIP_SPACE) _Src++;                             //Forward through the whitespace to next non whitespace
-                    e = (char*)_Src+len;                                                //Set a maximum length pointer location
-                    while (*_Src != SSF_SKIP_SPACE && *_Src && _Src != e) *value++ = *_Src++;   //While any character but space and not end of string and not end location, copy char to dest
-                    *value = 0;                                                         //Cap off the string pointer with zero
-                    conv_count++;                                                       //Increment the converted count
-                }
-                else if (*_Format == SSF_SPEC_VERSION)                                  //If the specifier type is string
-                {
-                    char *value=va_arg(va,char*),*e;                                    //User given destination address, max chars read pointer
-                    while (*_Src == SSF_SKIP_SPACE) _Src++;                             //Forward through the whitespace to next non whitespace
-                    e = (char*)_Src+len;                                                //Set a maximum length pointer location
-                    while (*_Src != SSF_DELIM_COMMA && *_Src && _Src != e) *value++ = *_Src++;  //While any character but space and not end of string and not end location, copy char to dest
-                    *value = 0;                                                         //Cap off the string pointer with zero
-                    conv_count++;                                                       //Increment the converted count
-                }
-            }
-            else if (*_Format == SSF_SPEC_START) looking_for = *_Format;                //If another start specifier character is found, output a specifier character
-            else break;                                                                 //Scan is now invalid (Uknown type specified)
-        }
-        else if (*_Format == SSF_SKIP_SPACE) { }                                        //If a space is found, ignore it
-        else looking_for = *_Format;                                                    //If any other character is found, it is static and should be found in src as well
-        _Format++;                                                                      //Skip over current format character
-    }
-
-    va_end(va);                                                                         //End the variable argument list
-    return conv_count;                                                                  //Return the number of conversions made
-}
-
diff --git a/tmk_core/protocol/arm_atsam/usb/spfssf.h b/tmk_core/protocol/arm_atsam/usb/spfssf.h
deleted file mode 100644
index 337a904dfe..0000000000
--- a/tmk_core/protocol/arm_atsam/usb/spfssf.h
+++ /dev/null
@@ -1,57 +0,0 @@
-#ifndef ____spfssf_h
-#define ____spfssf_h
-
-#include <stdarg.h>
-
-#define sprintf spf
-#define sscanf  ssf
-
-#define SIZEOF_OFFSET 1
-
-#ifndef NULL
-#define NULL 0
-#endif
-
-#define SPF_NONE    0
-
-#define SPF_SPEC_START      37      //%
-#define SPF_SPEC_DECIMAL    100     //d  16bit dec signed           (-32767 to 32767)               DONE same as i
-#define SPF_SPEC_INT        105     //i  16bit dec signed           (-32767 to 32767)               DONE same as d
-#define SPF_SPEC_UINT       117     //u  16bit dec unsigned         (0 to 65535)                    DONE
-#define SPF_SPEC_STRING     115     //s  variable length            (abcd...)                       DONE
-#define SPF_SPEC_UHINT      120     //x  16bit hex lwrc             (7fa)                           DONE
-#define SPF_SPEC_UHINT_UP   88      //x  16bit hex lwrc             (7fa)                           DONE
-#define SPF_SPEC_LONG       108     //l  start of either ld or lu                                   DONE
-#define SPF_SPEC_DECIMAL    100     //ld 32bit dec signed           (-2147483647 to 2147483647)     DONE
-#define SPF_SPEC_UNSIGNED   117     //lu 32bit dec unsigned         (0 to 4294967295)               DONE
-#define SPF_SPEC_UHINT      120     //lx 32bit hex unsigned         (0 to ffffffff)                 DONE
-
-#define SSF_SPEC_START      37      //%
-#define SSF_SPEC_SHORTINT   104     //h  8bit dec signed            (-127 to 127)                   DONE
-#define SSF_LEN_SHORTINT    3       //hhd
-#define SSF_SPEC_DECIMAL    100     //d  16bit dec signed           (-32767 to 32767)               DONE
-#define SSF_LEN_DECIMAL     5       //32767
-#define SSF_SPEC_INT        105     //i  16bit dec signed           (-32767 to 32767)               DONE
-#define SSF_LEN_INT         5       //32767
-#define SSF_SPEC_LONG       108     //l  start of either ld or lu                                   DONE
-#define SSF_SPEC_DECIMAL    100     //ld 32bit dec signed           (-2147483647 to 2147483647)     DONE
-#define SSF_SPEC_UHINT      120     //lx 32bit hex unsigned                                         DONE
-#define SSF_LEN_LDECIMAL    10      //2147483647
-#define SSF_SPEC_STRING     115     //s  variable length            (abcd...)                       DONE
-#define SSF_SKIP_SPACE      32      //space
-
-#define SSF_SPEC_VERSION    118     //v collect to comma delimiter - special
-#define SSF_DELIM_COMMA     44      //,
-
-#define ASCII_NUM_START     48      //0
-#define ASCII_NUM_END       58      //9
-
-#define T_UINT32_0_LIMIT    14
-#define T_UINT32_1_LIMIT    27
-
-int vspf(char *_Dest, const char *_Format, va_list va);
-int spf(char *_Dest, const char *_Format, ...);
-int ssf(const char *_Src, const char *_Format, ...);
-
-#endif //____spfssf_h
-
diff --git a/tmk_core/protocol/arm_atsam/usb/udi_cdc.c b/tmk_core/protocol/arm_atsam/usb/udi_cdc.c
index b4159d3251..15f0f760cc 100644
--- a/tmk_core/protocol/arm_atsam/usb/udi_cdc.c
+++ b/tmk_core/protocol/arm_atsam/usb/udi_cdc.c
@@ -54,7 +54,6 @@
 #include <string.h>
 #include "udi_cdc_conf.h"
 #include "udi_device_conf.h"
-#include "spfssf.h"
 #include "stdarg.h"
 #include "tmk_core/protocol/arm_atsam/clks.h"
 
@@ -1259,7 +1258,6 @@ uint32_t CDC_print(char *printbuf)
     return 1;
 }
 
-
 char printbuf[CDC_PRINTBUF_SIZE];
 
 int dpf(const char *_Format, ...)
@@ -1267,8 +1265,8 @@ int dpf(const char *_Format, ...)
     va_list va; //Variable argument list variable
     int result;
 
-    va_start(va,_Format); //Initialize the variable argument list
-    result = vspf(printbuf, _Format, va);
+    va_start(va, _Format); //Initialize the variable argument list
+    result = vsnprintf(printbuf, CDC_PRINTBUF_SIZE, _Format, va);
     va_end(va);
 
     CDC_print(printbuf);
@@ -1377,8 +1375,6 @@ void CDC_init(void)
     printbuf[0]=0;
 }
 
-char printbuf[CDC_PRINTBUF_SIZE];
-
 #endif //CDC line 62
 
 //@}
diff --git a/tmk_core/protocol/arm_atsam/usb/udi_cdc.h b/tmk_core/protocol/arm_atsam/usb/udi_cdc.h
index 6b70e96d0e..e134cf2360 100644
--- a/tmk_core/protocol/arm_atsam/usb/udi_cdc.h
+++ b/tmk_core/protocol/arm_atsam/usb/udi_cdc.h
@@ -57,8 +57,8 @@
 #include "udi.h"
 
 // Check the number of port
-#ifndef  UDI_CDC_PORT_NB
-# define  UDI_CDC_PORT_NB 1
+#ifndef UDI_CDC_PORT_NB
+# define UDI_CDC_PORT_NB 1
 #endif
 #if (UDI_CDC_PORT_NB > 1)
 # error UDI_CDC_PORT_NB must be at most 1
@@ -86,9 +86,6 @@ extern UDC_DESC_STORAGE udi_api_t udi_api_cdc_data;
 //! CDC data endpoints size for FS speed (8B, 16B, 32B, 64B)
 #define UDI_CDC_DATA_EPS_FS_SIZE    CDC_RX_SIZE
 
-#define CDC_PRINT_BUF_SIZE  256
-extern char printbuf[CDC_PRINT_BUF_SIZE];
-
 //@}
 
 /**
@@ -371,9 +368,6 @@ uint32_t CDC_print(char *printbuf);
 uint32_t CDC_input(void);
 void CDC_init(void);
 
-#define __xprintf dpf
-int dpf(const char *_Format, ...);
-
 #ifdef __cplusplus
 }
 #endif
diff --git a/tmk_core/protocol/arm_atsam/usb/udi_hid_kbd.c b/tmk_core/protocol/arm_atsam/usb/udi_hid_kbd.c
index 18f69350c0..1a6f7905e6 100644
--- a/tmk_core/protocol/arm_atsam/usb/udi_hid_kbd.c
+++ b/tmk_core/protocol/arm_atsam/usb/udi_hid_kbd.c
@@ -45,6 +45,7 @@
  */
 
 #include "samd51j18a.h"
+#include "d51_util.h"
 #include "conf_usb.h"
 #include "usb_protocol.h"
 #include "udd.h"
@@ -86,7 +87,7 @@ bool udi_hid_kbd_b_report_valid;
 COMPILER_WORD_ALIGNED
 uint8_t udi_hid_kbd_report[UDI_HID_KBD_REPORT_SIZE];
 
-static bool udi_hid_kbd_b_report_trans_ongoing;
+volatile bool udi_hid_kbd_b_report_trans_ongoing;
 
 COMPILER_WORD_ALIGNED
 static uint8_t udi_hid_kbd_report_trans[UDI_HID_KBD_REPORT_SIZE];
@@ -186,8 +187,7 @@ bool udi_hid_kbd_send_report(void)
         return false;
     }
 
-    memcpy(udi_hid_kbd_report_trans, udi_hid_kbd_report,
-            UDI_HID_KBD_REPORT_SIZE);
+    memcpy(udi_hid_kbd_report_trans, udi_hid_kbd_report, UDI_HID_KBD_REPORT_SIZE);
     udi_hid_kbd_b_report_valid = false;
     udi_hid_kbd_b_report_trans_ongoing =
             udd_ep_run(UDI_HID_KBD_EP_IN | USB_EP_DIR_IN,
@@ -249,7 +249,7 @@ bool udi_hid_nkro_b_report_valid;
 COMPILER_WORD_ALIGNED
 uint8_t udi_hid_nkro_report[UDI_HID_NKRO_REPORT_SIZE];
 
-static bool udi_hid_nkro_b_report_trans_ongoing;
+volatile bool udi_hid_nkro_b_report_trans_ongoing;
 
 COMPILER_WORD_ALIGNED
 static uint8_t udi_hid_nkro_report_trans[UDI_HID_NKRO_REPORT_SIZE];
@@ -355,7 +355,7 @@ bool udi_hid_nkro_send_report(void)
         return false;
     }
 
-    memcpy(udi_hid_nkro_report_trans, udi_hid_nkro_report,UDI_HID_NKRO_REPORT_SIZE);
+    memcpy(udi_hid_nkro_report_trans, udi_hid_nkro_report, UDI_HID_NKRO_REPORT_SIZE);
     udi_hid_nkro_b_report_valid = false;
     udi_hid_nkro_b_report_trans_ongoing =
             udd_ep_run(UDI_HID_NKRO_EP_IN | USB_EP_DIR_IN,
diff --git a/tmk_core/protocol/arm_atsam/usb/udi_hid_kbd.h b/tmk_core/protocol/arm_atsam/usb/udi_hid_kbd.h
index 9a2741534d..babfdb7a78 100644
--- a/tmk_core/protocol/arm_atsam/usb/udi_hid_kbd.h
+++ b/tmk_core/protocol/arm_atsam/usb/udi_hid_kbd.h
@@ -60,6 +60,7 @@ extern "C" {
 #ifdef KBD
 extern UDC_DESC_STORAGE udi_api_t udi_api_hid_kbd;
 extern bool udi_hid_kbd_b_report_valid;
+extern volatile bool udi_hid_kbd_b_report_trans_ongoing;
 extern uint8_t udi_hid_kbd_report_set;
 bool udi_hid_kbd_send_report(void);
 #endif //KBD
@@ -70,6 +71,7 @@ bool udi_hid_kbd_send_report(void);
 #ifdef NKRO
 extern UDC_DESC_STORAGE udi_api_t udi_api_hid_nkro;
 extern bool udi_hid_nkro_b_report_valid;
+extern volatile bool udi_hid_nkro_b_report_trans_ongoing;
 bool udi_hid_nkro_send_report(void);
 #endif //NKRO