2 files changed, 75 insertions, 17 deletions

diff --git a/LUFA/Drivers/USB/LowLevel/Endpoint.c b/LUFA/Drivers/USB/LowLevel/Endpoint.c
index ee3f9d640..57c613a1f 100644
--- a/LUFA/Drivers/USB/LowLevel/Endpoint.c
+++ b/LUFA/Drivers/USB/LowLevel/Endpoint.c
@@ -44,21 +44,49 @@ bool Endpoint_ConfigureEndpoint_Prv(const uint8_t Number,
                                     const uint8_t UECFG0XData,
                                     const uint8_t UECFG1XData)
 {
-	Endpoint_SelectEndpoint(Number);
-	Endpoint_EnableEndpoint();
-
-	UECFG1X = 0;
+	uint8_t UECFG0XTemp[ENDPOINT_TOTAL_ENDPOINTS];
+	uint8_t UECFG1XTemp[ENDPOINT_TOTAL_ENDPOINTS];
+	
+	for (uint8_t EPNum = 0; EPNum < ENDPOINT_TOTAL_ENDPOINTS; EPNum++)
+	{
+		Endpoint_SelectEndpoint(EPNum);
+		UECFG0XTemp[EPNum] = UECFG0X;
+		UECFG1XTemp[EPNum] = UECFG1X;
+	}
+	
+	UECFG0XTemp[Number] = UECFG0XData;
+	UECFG1XTemp[Number] = UECFG1XData;
+	
+	for (uint8_t EPNum = 1; EPNum < ENDPOINT_TOTAL_ENDPOINTS; EPNum++)
+	{
+		Endpoint_SelectEndpoint(EPNum);	
+		UEIENX  = 0;
+		UEINTX  = 0;
+		UECFG1X = 0;
+		Endpoint_DisableEndpoint();
+	}
 
-	UECFG0X = UECFG0XData;
-	UECFG1X = UECFG1XData;
+	for (uint8_t EPNum = 0; EPNum < ENDPOINT_TOTAL_ENDPOINTS; EPNum++)
+	{
+		if (!(UECFG1XTemp[EPNum] & (1 << ALLOC)))
+		  continue;
+		
+		Endpoint_SelectEndpoint(EPNum);		
+		Endpoint_EnableEndpoint();
 
-	return Endpoint_IsConfigured();
+		UECFG0X = UECFG0XTemp[EPNum];
+		UECFG1X = UECFG1XTemp[EPNum];
+		
+		if (!(Endpoint_IsConfigured()))
+		  return false;
+	}
+	
+	Endpoint_SelectEndpoint(Number);
+	return true;
 }
 
 void Endpoint_ClearEndpoints(void)
 {
-	UEINT = 0;
-
 	for (uint8_t EPNum = 0; EPNum < ENDPOINT_TOTAL_ENDPOINTS; EPNum++)
 	{
 		Endpoint_SelectEndpoint(EPNum);	
diff --git a/LUFA/Drivers/USB/LowLevel/Pipe.c b/LUFA/Drivers/USB/LowLevel/Pipe.c
index f70f0184f..f8cf4f9ba 100644
--- a/LUFA/Drivers/USB/LowLevel/Pipe.c
+++ b/LUFA/Drivers/USB/LowLevel/Pipe.c
@@ -45,17 +45,47 @@ bool Pipe_ConfigurePipe(const uint8_t Number,
                         const uint16_t Size,
                         const uint8_t Banks)
 {
-	Pipe_SelectPipe(Number);
-	Pipe_EnablePipe();
-
-	UPCFG1X = 0;
+	uint8_t UPCFG0XTemp[PIPE_TOTAL_PIPES];
+	uint8_t UPCFG1XTemp[PIPE_TOTAL_PIPES];
+	
+	for (uint8_t PNum = 0; PNum < PIPE_TOTAL_PIPES; PNum++)
+	{
+		Pipe_SelectPipe(PNum);
+		UPCFG0XTemp[PNum] = UPCFG0X;
+		UPCFG1XTemp[PNum] = UPCFG1X;
+	}
 	
-	UPCFG0X = ((Type << EPTYPE0) | Token | ((EndpointNumber & PIPE_EPNUM_MASK) << PEPNUM0));
-	UPCFG1X = ((1 << ALLOC) | Banks | Pipe_BytesToEPSizeMask(Size));
+	UPCFG0XTemp[Number] = ((Type << EPTYPE0) | Token | ((EndpointNumber & PIPE_EPNUM_MASK) << PEPNUM0));
+	UPCFG1XTemp[Number] = ((1 << ALLOC) | Banks | Pipe_BytesToEPSizeMask(Size));
+	
+	for (uint8_t PNum = 0; PNum < PIPE_TOTAL_PIPES; PNum++)
+	{
+		Pipe_SelectPipe(PNum);
+		UPIENX  = 0;
+		UPINTX  = 0;
+		UPCFG1X = 0;
+		Pipe_DisablePipe();
+	}
 
-	Pipe_SetInfiniteINRequests();
+	for (uint8_t PNum = 0; PNum < PIPE_TOTAL_PIPES; PNum++)
+	{
+		if (!(UPCFG1XTemp[PNum] & (1 << ALLOC)))
+		  continue;
+		
+		Pipe_SelectPipe(PNum);		
+		Pipe_EnablePipe();
 
-	return Pipe_IsConfigured();
+		UPCFG0X = UPCFG0XTemp[PNum];
+		UPCFG1X = UPCFG1XTemp[PNum];
+		
+		if (!(Pipe_IsConfigured()))
+		  return false;
+	}
+	
+	Pipe_SelectPipe(Number);
+	Pipe_SetInfiniteINRequests();
+	
+	return true;
 }
 
 void Pipe_ClearPipes(void)