X-Git-Url: http://git.linex4red.de/pub/USBasp.git/blobdiff_plain/2ee9fc707784e115d744dbc229bdc893f4bb6bc1..864196c884ef3602aee44f1a68aceb5819a07f21:/Bootloaders/TeensyHID/TeensyHID.c?ds=sidebyside

diff --git a/Bootloaders/TeensyHID/TeensyHID.c b/Bootloaders/TeensyHID/TeensyHID.c
index ee6c91a62..67dded545 100644
--- a/Bootloaders/TeensyHID/TeensyHID.c
+++ b/Bootloaders/TeensyHID/TeensyHID.c
@@ -49,6 +49,24 @@ bool RunBootloader = true;
  */
 int main(void)
 {
+	/* Setup hardware required for the bootloader */
+	SetupHardware();
+	
+	while (RunBootloader)
+	  USB_USBTask();
+
+	/* 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 +80,6 @@ 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 (;;);
 }
 
 /** Event handler for the USB_ConfigurationChanged event. This configures the device's endpoints ready
@@ -105,7 +109,7 @@ void EVENT_USB_UnhandledControlPacket(void)
 				
 				/* Wait until the command (report) has been sent by the host */
 				while (!(Endpoint_IsOUTReceived()));
-
+			
 				/* Read in the write destination address */
 				uint16_t PageAddress = Endpoint_Read_Word_LE();
 				
@@ -144,9 +148,7 @@ void EVENT_USB_UnhandledControlPacket(void)
 
 				Endpoint_ClearOUT();
 
-				/* Acknowledge status stage */
-				while (!(Endpoint_IsINReady()));
-				Endpoint_ClearIN();
+				Endpoint_ClearStatusStage();
 			}
 			
 			break;