6 files changed, 70 insertions, 12 deletions

diff --git a/LUFA/Drivers/USB/LowLevel/Device.h b/LUFA/Drivers/USB/LowLevel/Device.h
index 17984c719..300793612 100644
--- a/LUFA/Drivers/USB/LowLevel/Device.h
+++ b/LUFA/Drivers/USB/LowLevel/Device.h
@@ -140,6 +140,14 @@
 			};
 			
 		/* Inline Functions: */
+			/** Returns the current USB frame number, when in device mode. Every millisecond the USB bus is active (i.e. enumerated to a host)
+			 *  the frame number is incremented by one.
+			 */
+			static inline uint16_t USB_Device_GetFrameNumber(void)
+			{
+				return UDFNUM;
+			}
+		
 			/** Enables the device mode Start Of Frame events. When enabled, this causes the
 			 *  \ref EVENT_USB_Device_StartOfFrame() event to fire once per millisecond, synchronized to the USB bus,
 			 *  at the start of each USB frame when enumerated in device mode.
diff --git a/LUFA/Drivers/USB/LowLevel/Endpoint.c b/LUFA/Drivers/USB/LowLevel/Endpoint.c
index 4b2d1c059..fdb6c7469 100644
--- a/LUFA/Drivers/USB/LowLevel/Endpoint.c
+++ b/LUFA/Drivers/USB/LowLevel/Endpoint.c
@@ -142,6 +142,8 @@ uint8_t Endpoint_WaitUntilReady(void)
 	uint16_t TimeoutMSRem = USB_STREAM_TIMEOUT_MS;
 	#endif
 
+	uint16_t PreviousFrameNumber = USB_Device_GetFrameNumber();
+
 	for (;;)
 	{
 		if (Endpoint_GetEndpointDirection() == ENDPOINT_DIR_IN)
@@ -161,10 +163,12 @@ uint8_t Endpoint_WaitUntilReady(void)
 		  return ENDPOINT_READYWAIT_BusSuspended;
 		else if (Endpoint_IsStalled())
 		  return ENDPOINT_READYWAIT_EndpointStalled;
-			  
-		if (USB_INT_HasOccurred(USB_INT_SOFI))
+
+		uint16_t CurrentFrameNumber = USB_Device_GetFrameNumber();
+
+		if (CurrentFrameNumber != PreviousFrameNumber)
 		{
-			USB_INT_Clear(USB_INT_SOFI);
+			PreviousFrameNumber = CurrentFrameNumber;
 
 			if (!(TimeoutMSRem--))
 			  return ENDPOINT_READYWAIT_Timeout;
diff --git a/LUFA/Drivers/USB/LowLevel/Host.c b/LUFA/Drivers/USB/LowLevel/Host.c
index 5b7a714ba..2b66e28b4 100644
--- a/LUFA/Drivers/USB/LowLevel/Host.c
+++ b/LUFA/Drivers/USB/LowLevel/Host.c
@@ -194,16 +194,19 @@ void USB_Host_ProcessNextHostState(void)
 
 uint8_t USB_Host_WaitMS(uint8_t MS)
 {
-	bool    BusSuspended = USB_Host_IsBusSuspended();
-	uint8_t ErrorCode    = HOST_WAITERROR_Successful;
+	bool     BusSuspended        = USB_Host_IsBusSuspended();
+	uint8_t  ErrorCode           = HOST_WAITERROR_Successful;
+	uint16_t PreviousFrameNumber = USB_Host_GetFrameNumber();
 	
 	USB_Host_ResumeBus();
 
 	while (MS)
 	{
-		if (USB_INT_HasOccurred(USB_INT_HSOFI))
+		uint16_t CurrentFrameNumber = USB_Host_GetFrameNumber();
+
+		if (CurrentFrameNumber != PreviousFrameNumber)
 		{
-			USB_INT_Clear(USB_INT_HSOFI);
+			PreviousFrameNumber = CurrentFrameNumber;
 			MS--;
 		}
 					
@@ -245,11 +248,13 @@ static void USB_Host_ResetDevice(void)
 	
 	USB_Host_ResetBus();
 	while (!(USB_Host_IsBusResetComplete()));
-
 	USB_Host_ResumeBus();
 
-	USB_INT_Clear(USB_INT_HSOFI);
+	bool HSOFIEnabled = USB_INT_IsEnabled(USB_INT_HSOFI);
 
+	USB_INT_Disable(USB_INT_HSOFI);
+	USB_INT_Clear(USB_INT_HSOFI);
+	
 	for (uint8_t MSRem = 10; MSRem != 0; MSRem--)
 	{
 		/* Workaround for powerless-pull-up devices. After a USB bus reset,
@@ -267,6 +272,9 @@ static void USB_Host_ResetDevice(void)
 		_delay_ms(1);
 	}
 
+	if (HSOFIEnabled)
+	  USB_INT_Enable(USB_INT_HSOFI);
+
 	if (BusSuspended)
 	  USB_Host_SuspendBus();
 
diff --git a/LUFA/Drivers/USB/LowLevel/Host.h b/LUFA/Drivers/USB/LowLevel/Host.h
index 8623c4ec6..40f630e84 100644
--- a/LUFA/Drivers/USB/LowLevel/Host.h
+++ b/LUFA/Drivers/USB/LowLevel/Host.h
@@ -245,6 +245,33 @@
 			};
 
 		/* Inline Functions: */
+			/** Returns the current USB frame number, when in host mode. Every millisecond the USB bus is active (i.e. not suspended)
+			 *  the frame number is incremented by one.
+			 */
+			static inline uint16_t USB_Host_GetFrameNumber(void)
+			{
+				return UHFNUM;
+			}
+			
+			/** Enables the host mode Start Of Frame events. When enabled, this causes the
+			 *  \ref EVENT_USB_Host_StartOfFrame() event to fire once per millisecond, synchronized to the USB bus,
+			 *  at the start of each USB frame when a device is enumerated while in host mode.
+			 */
+			static inline void USB_Host_EnableSOFEvents(void) ATTR_ALWAYS_INLINE;
+			static inline void USB_Host_EnableSOFEvents(void)
+			{
+				USB_INT_Enable(USB_INT_HSOFI);
+			}
+				
+			/** Disables the host mode Start Of Frame events. When disabled, this stops the firing of the
+			 *  \ref EVENT_USB_Host_StartOfFrame() event when enumerated in host mode.
+			 */
+			static inline void USB_Host_DisableSOFEvents(void) ATTR_ALWAYS_INLINE;
+			static inline void USB_Host_DisableSOFEvents(void)
+			{
+				USB_INT_Disable(USB_INT_HSOFI);
+			}
+
 			/** Resets the USB bus, including the endpoints in any attached device and pipes on the AVR host.
 			 *  USB bus resets leave the default control pipe configured (if already configured).
 			 *
diff --git a/LUFA/Drivers/USB/LowLevel/Pipe.c b/LUFA/Drivers/USB/LowLevel/Pipe.c
index a8eb50f63..5f1090245 100644
--- a/LUFA/Drivers/USB/LowLevel/Pipe.c
+++ b/LUFA/Drivers/USB/LowLevel/Pipe.c
@@ -133,6 +133,8 @@ uint8_t Pipe_WaitUntilReady(void)
 	#else
 	uint16_t TimeoutMSRem = USB_STREAM_TIMEOUT_MS;
 	#endif
+
+	uint16_t PreviousFrameNumber = USB_Host_GetFrameNumber();
 	
 	for (;;)
 	{
@@ -151,10 +153,12 @@ uint8_t Pipe_WaitUntilReady(void)
 		  return PIPE_READYWAIT_PipeStalled;
 		else if (USB_HostState == HOST_STATE_Unattached)
 		  return PIPE_READYWAIT_DeviceDisconnected;
-			  
-		if (USB_INT_HasOccurred(USB_INT_HSOFI))
+
+		uint16_t CurrentFrameNumber = USB_Host_GetFrameNumber();
+
+		if (CurrentFrameNumber != PreviousFrameNumber)
 		{
-			USB_INT_Clear(USB_INT_HSOFI);
+			PreviousFrameNumber = CurrentFrameNumber;
 
 			if (!(TimeoutMSRem--))
 			  return PIPE_READYWAIT_Timeout;
diff --git a/LUFA/Drivers/USB/LowLevel/USBInterrupt.c b/LUFA/Drivers/USB/LowLevel/USBInterrupt.c
index 1c6d6caa7..36541cf7f 100644
--- a/LUFA/Drivers/USB/LowLevel/USBInterrupt.c
+++ b/LUFA/Drivers/USB/LowLevel/USBInterrupt.c
@@ -209,6 +209,13 @@ ISR(USB_GEN_vect, ISR_BLOCK)
 
 		USB_ResetInterface();
 	}
+
+	if (USB_INT_HasOccurred(USB_INT_HSOFI) && USB_INT_IsEnabled(USB_INT_HSOFI))
+	{
+		USB_INT_Clear(USB_INT_HSOFI);
+		
+		EVENT_USB_Host_StartOfFrame();
+	}
 	#endif
 
 	#if defined(USB_CAN_BE_BOTH)