diff --git a/keyboards/massdrop/alt/alt.h b/keyboards/massdrop/alt/alt.h
index 387985512ba..8dfed8d2d6d 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 0cbce862932..a5c443ffcde 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 e886290e7e8..d6978fd8012 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 daf67958528..c5539158f58 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 dc7c7eabe5e..c83efca16d1 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 9bfb7fec58d..88c1ac31239 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 a03f891e8c5..6c5dfe19c02 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 daf67958528..c5539158f58 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 3d114f5b7b8..35db619717a 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 ef412d59d65..06823fb629f 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 582c83bf548..3206b40bdbc 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 9cbe67bad67..d9452765725 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 d535b64cd79..04e02790a0d 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 be73beccd7e..2ba0991749c 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 465889c7cbe..7a35f7989f7 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 8cc7767038d..676dac4ea3b 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 449a8bb7d07..00000000000
--- 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 337a904dfee..00000000000
--- 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 b4159d32516..15f0f760cc3 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 6b70e96d0e2..e134cf23608 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 18f69350c0f..1a6f7905e68 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 9a2741534dd..babfdb7a78d 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