2 files changed, 31 insertions, 13 deletions
diff --git a/Bootloaders/TeensyHID/TeensyHID.c b/Bootloaders/TeensyHID/TeensyHID.c
index ee6c91a62..12d5c0de8 100644
--- a/Bootloaders/TeensyHID/TeensyHID.c
+++ b/Bootloaders/TeensyHID/TeensyHID.c
@@ -49,6 +49,27 @@ bool RunBootloader = true;
  */
 int main(void)
 {
+	/* Setup hardware required for the bootloader */
+	SetupHardware();
+	
+	while (RunBootloader)
+	  USB_USBTask();
+	
+	/* Reset all configured hardware to their default states for the user app */
+	ResetHardware();
+
+	/* Wait 100ms to give the host time to register the disconnection */
+	_delay_ms(100);
+
+	/* Enable the watchdog and force a timeout to reset the AVR */
+	wdt_enable(WDTO_250MS);
+					
+	for (;;);
+}
+
+/** Configures all hardware required for the bootloader. */
+void SetupHardware(void)
+{
 	/* Disable watchdog if enabled by bootloader/fuses */
 	MCUSR &= ~(1 << WDRF);
 	wdt_disable();
@@ -62,20 +83,13 @@ int main(void)
 
 	/* Initialize USB subsystem */
 	USB_Init();
-	
-	while (RunBootloader)
-	  USB_USBTask();
-	  
-	/* Shut down the USB interface, so that the host will register the disconnection */
-	USB_ShutDown();
-
-	/* Wait 100ms to give the host time to register the disconnection */
-	_delay_ms(100);
+}
 
-	/* Enable the watchdog and force a timeout to reset the AVR */
-	wdt_enable(WDTO_250MS);
-					
-	for (;;);
+/** Resets all configured hardware required for the bootloader back to their original states. */
+void ResetHardware(void)
+{
+	/* Shut down the USB subsystem */
+	USB_ShutDown();
 }
 
 /** Event handler for the USB_ConfigurationChanged event. This configures the device's endpoints ready
diff --git a/Bootloaders/TeensyHID/TeensyHID.h b/Bootloaders/TeensyHID/TeensyHID.h
index 0dd58524b..9414eec77 100644
--- a/Bootloaders/TeensyHID/TeensyHID.h
+++ b/Bootloaders/TeensyHID/TeensyHID.h
@@ -59,9 +59,13 @@
 		/** HID Class specific request to send the next HID report to the device. */
 		#define REQ_SetReport             0x09
 		
+		/** Teensy Bootloader special address to start the user application */
 		#define TEENSY_STARTAPPLICATION   0xFFFF
 		
 	/* Function Prototypes: */
+		void SetupHardware(void);
+		void ResetHardware(void);
+
 		void EVENT_USB_ConfigurationChanged(void);
 		void EVENT_USB_UnhandledControlPacket(void);