diff --git a/Bootloader/main.c b/Bootloader/main.c
index aabaeeb..124e82d 100644
--- a/Bootloader/main.c
+++ b/Bootloader/main.c
@@ -17,12 +17,17 @@
 
 // ----- Includes -----
 
+// Project Includes
+#include <delay.h>
+
 // Local Includes
 #include "mchck.h"
 #include "dfu.desc.h"
 
 #include "debug.h"
 
+#include "usb-internal.h"
+
 
 
 // ----- Variables -----
@@ -173,6 +178,9 @@ static struct dfu_ctx dfu_ctx;
 void init_usb_bootloader( int config )
 {
 	dfu_init( setup_read, setup_write, finish_write, &dfu_ctx );
+
+	// Make sure SysTick counter is disabled
+	SYST_CSR = 0;
 }
 
 void main()
@@ -224,9 +232,43 @@ void main()
 
 	flash_prepare_flashing();
 	usb_init( &dfu_device );
+
+#if defined(_mk20dx256vlh7_) // Kiibohd-dfu
+	// PTA13 - USB Swap
+	// Start, disabled
+	GPIOA_PDDR |= (1<<13);
+	PORTA_PCR13 = PORT_PCR_SRE | PORT_PCR_DSE | PORT_PCR_MUX(1);
+	GPIOA_PCOR |= (1<<13);
+
+	#define USBPortSwapDelay_ms 1000
+	// For keyboards with dual usb ports, doesn't do anything on keyboards without them
+	// If a USB connection is not detected in 2 seconds, switch to the other port to see if it's plugged in there
+	uint32_t last_ms = systick_millis_count;
+	for (;;)
+	{
+		usb_poll();
+
+		// Only check for swapping after delay
+		uint32_t wait_ms = systick_millis_count - last_ms;
+		if ( wait_ms < USBPortSwapDelay_ms )
+		{
+			continue;
+		}
+
+		last_ms = systick_millis_count;
+
+		// USB not initialized, attempt to swap
+		if ( usb.state != USBD_STATE_ADDRESS )
+		{
+			print("USB not initializing, port swapping (if supported)");
+			GPIOA_PTOR |= (1<<13);
+		}
+	}
+#else
 	for (;;)
 	{
 		usb_poll();
 	}
+#endif
 }
 
diff --git a/Lib/mk20dx.c b/Lib/mk20dx.c
index ec6908c..e8b77f8 100644
--- a/Lib/mk20dx.c
+++ b/Lib/mk20dx.c
@@ -527,7 +527,7 @@ void ResetHandler()
 	// Also checking for ARM lock-up signal (invalid firmware image)
 	// RCM_SRS1 & 0x02
 	if (    // PIN  (External Reset Pin/Switch)
-		   RCM_SRS0 & 0x40
+		RCM_SRS0 & 0x40
 		// WDOG (Watchdog timeout)
 		|| RCM_SRS0 & 0x20
 		// LOCKUP (ARM Core LOCKUP event)
@@ -690,11 +690,11 @@ void ResetHandler()
 
 #endif
 
-#if !defined(_bootloader_)
 	// Initialize the SysTick counter
 	SYST_RVR = (F_CPU / 1000) - 1;
 	SYST_CSR = SYST_CSR_CLKSOURCE | SYST_CSR_TICKINT | SYST_CSR_ENABLE;
 
+#if !defined(_bootloader_)
 	__enable_irq();
 #else
 	// Disable Watchdog for bootloader