X-Git-Url: http://git.linex4red.de/pub/USBasp.git/blobdiff_plain/6a10d6b465be27db090d760dc0fbe722c94e4344..b462f2d457ec2f0cfa22a1c3db198cb22f6809a1:/LUFA/Drivers/USB/LowLevel/Endpoint.c

diff --git a/LUFA/Drivers/USB/LowLevel/Endpoint.c b/LUFA/Drivers/USB/LowLevel/Endpoint.c
index 273e1908a..1ff66ddfa 100644
--- a/LUFA/Drivers/USB/LowLevel/Endpoint.c
+++ b/LUFA/Drivers/USB/LowLevel/Endpoint.c
@@ -28,7 +28,8 @@
   this software.
 */
 
-#include "USBMode.h"
+#include "../HighLevel/USBMode.h"
+
 #if defined(USB_CAN_BE_DEVICE)
 
 #define  INCLUDE_FROM_ENDPOINT_C
@@ -38,34 +39,23 @@
 uint8_t USB_ControlEndpointSize = ENDPOINT_CONTROLEP_DEFAULT_SIZE;
 #endif
 
-#if !defined(STATIC_ENDPOINT_CONFIGURATION)
-bool Endpoint_ConfigureEndpoint(const uint8_t  Number, const uint8_t Type, const uint8_t Direction,
-			                    const uint16_t Size, const uint8_t Banks)
+uint8_t Endpoint_BytesToEPSizeMaskDynamic(const uint16_t Size)
 {
-	Endpoint_SelectEndpoint(Number);
-	Endpoint_EnableEndpoint();
-
-	UECFG1X = 0;	
-
-	UECFG0X = ((Type << EPTYPE0) | Direction);
-	UECFG1X = ((1 << ALLOC) | Banks | Endpoint_BytesToEPSizeMask(Size));
-
-	return Endpoint_IsConfigured();
+	return Endpoint_BytesToEPSizeMask(Size);
 }
-#else
-bool Endpoint_ConfigureEndpointStatic(const uint8_t Number, const uint8_t UECFG0XData, const uint8_t UECFG1XData)
+
+bool Endpoint_ConfigureEndpoint_Prv(const uint8_t Number, const uint8_t UECFG0XData, const uint8_t UECFG1XData)
 {
 	Endpoint_SelectEndpoint(Number);
 	Endpoint_EnableEndpoint();
 
-	UECFG1X = 0;	
+	UECFG1X = 0;
 
 	UECFG0X = UECFG0XData;
 	UECFG1X = UECFG1XData;
 
 	return Endpoint_IsConfigured();
 }
-#endif
 
 void Endpoint_ClearEndpoints(void)
 {
@@ -81,14 +71,28 @@ void Endpoint_ClearEndpoints(void)
 	}
 }
 
+#if !defined(CONTROL_ONLY_DEVICE)
 uint8_t Endpoint_WaitUntilReady(void)
 {
-	uint8_t TimeoutMSRem = USB_STREAM_TIMEOUT_MS;
+	#if (USB_STREAM_TIMEOUT_MS < 0xFF)
+	uint8_t  TimeoutMSRem = USB_STREAM_TIMEOUT_MS;	
+	#else
+	uint16_t TimeoutMSRem = USB_STREAM_TIMEOUT_MS;
+	#endif
 
-	USB_INT_Clear(USB_INT_SOFI);
-
-	while (!(Endpoint_ReadWriteAllowed()))
+	for (;;)
 	{
+		if (Endpoint_GetEndpointDirection() == ENDPOINT_DIR_IN)
+		{
+			if (Endpoint_IsINReady())
+			  return ENDPOINT_READYWAIT_NoError;
+		}
+		else
+		{
+			if (Endpoint_IsOUTReceived())
+			  return ENDPOINT_READYWAIT_NoError;		
+		}
+		
 		if (!(USB_IsConnected))
 		  return ENDPOINT_READYWAIT_DeviceDisconnected;
 		else if (Endpoint_IsStalled())
@@ -102,13 +106,11 @@ uint8_t Endpoint_WaitUntilReady(void)
 			  return ENDPOINT_READYWAIT_Timeout;
 		}
 	}
-	
-	return ENDPOINT_READYWAIT_NoError;
 }
 
 uint8_t Endpoint_Discard_Stream(uint16_t Length
 #if !defined(NO_STREAM_CALLBACKS)
-                                , uint8_t (* const Callback)(void)
+                                , StreamCallbackPtr_t Callback
 #endif
 								)
 {
@@ -117,30 +119,73 @@ uint8_t Endpoint_Discard_Stream(uint16_t Length
 	if ((ErrorCode = Endpoint_WaitUntilReady()))
 	  return ErrorCode;
 
-	while (Length--)
+	#if defined(FAST_STREAM_TRANSFERS)
+	uint8_t BytesRemToAlignment = (Endpoint_BytesInEndpoint() & 0x07);
+
+	if (Length >= 8)
 	{
-		if (!(Endpoint_ReadWriteAllowed()))
+		Length -= BytesRemToAlignment;
+
+		switch (BytesRemToAlignment)
 		{
-			Endpoint_ClearCurrentBank();
+			default:
+				do
+				{
+					if (!(Endpoint_IsReadWriteAllowed()))
+					{
+						Endpoint_ClearOUT();
+
+						#if !defined(NO_STREAM_CALLBACKS)
+						if ((Callback != NULL) && (Callback() == STREAMCALLBACK_Abort))
+						  return ENDPOINT_RWSTREAM_CallbackAborted;
+						#endif
+
+						if ((ErrorCode = Endpoint_WaitUntilReady()))
+						  return ErrorCode;
+					}
+
+					Length -= 8;
+					
+					Endpoint_Discard_Byte();
+			case 7: Endpoint_Discard_Byte();
+			case 6: Endpoint_Discard_Byte();
+			case 5: Endpoint_Discard_Byte();
+			case 4: Endpoint_Discard_Byte();
+			case 3: Endpoint_Discard_Byte();
+			case 2: Endpoint_Discard_Byte();
+			case 1:	Endpoint_Discard_Byte();
+				} while (Length >= 8);	
+		}
+	}
+	#endif
+
+	while (Length)
+	{
+		if (!(Endpoint_IsReadWriteAllowed()))
+		{
+			Endpoint_ClearOUT();
 
 			#if !defined(NO_STREAM_CALLBACKS)
 			if ((Callback != NULL) && (Callback() == STREAMCALLBACK_Abort))
-			  return ENDPOINT_RWSTREAM_ERROR_CallbackAborted;
+			  return ENDPOINT_RWSTREAM_CallbackAborted;
 			#endif
 
 			if ((ErrorCode = Endpoint_WaitUntilReady()))
 			  return ErrorCode;
 		}
-
-		Endpoint_Discard_Byte();
+		else
+		{
+			Endpoint_Discard_Byte();
+			Length--;
+		}
 	}
 	
-	return ENDPOINT_RWSTREAM_ERROR_NoError;
+	return ENDPOINT_RWSTREAM_NoError;
 }
 
 uint8_t Endpoint_Write_Stream_LE(const void* Buffer, uint16_t Length
 #if !defined(NO_STREAM_CALLBACKS)
-                                 , uint8_t (* const Callback)(void)
+                                 , StreamCallbackPtr_t Callback
 #endif
 								 )
 {
@@ -150,30 +195,73 @@ uint8_t Endpoint_Write_Stream_LE(const void* Buffer, uint16_t Length
 	if ((ErrorCode = Endpoint_WaitUntilReady()))
 	  return ErrorCode;
 
-	while (Length--)
+	#if defined(FAST_STREAM_TRANSFERS)
+	uint8_t BytesRemToAlignment = (Endpoint_BytesInEndpoint() & 0x07);
+
+	if (Length >= 8)
+	{
+		Length -= BytesRemToAlignment;
+
+		switch (BytesRemToAlignment)
+		{
+			default:
+				do
+				{
+					if (!(Endpoint_IsReadWriteAllowed()))
+					{
+						Endpoint_ClearIN();
+
+						#if !defined(NO_STREAM_CALLBACKS)
+						if ((Callback != NULL) && (Callback() == STREAMCALLBACK_Abort))
+						  return ENDPOINT_RWSTREAM_CallbackAborted;
+						#endif
+
+						if ((ErrorCode = Endpoint_WaitUntilReady()))
+						  return ErrorCode;
+					}
+
+					Length -= 8;
+					
+					Endpoint_Write_Byte(*(DataStream++));
+			case 7: Endpoint_Write_Byte(*(DataStream++));
+			case 6: Endpoint_Write_Byte(*(DataStream++));
+			case 5: Endpoint_Write_Byte(*(DataStream++));
+			case 4: Endpoint_Write_Byte(*(DataStream++));
+			case 3: Endpoint_Write_Byte(*(DataStream++));
+			case 2: Endpoint_Write_Byte(*(DataStream++));
+			case 1:	Endpoint_Write_Byte(*(DataStream++));
+				} while (Length >= 8);	
+		}
+	}
+	#endif
+
+	while (Length)
 	{
-		if (!(Endpoint_ReadWriteAllowed()))
+		if (!(Endpoint_IsReadWriteAllowed()))
 		{
-			Endpoint_ClearCurrentBank();
+			Endpoint_ClearIN();
 			
 			#if !defined(NO_STREAM_CALLBACKS)
 			if ((Callback != NULL) && (Callback() == STREAMCALLBACK_Abort))
-			  return ENDPOINT_RWSTREAM_ERROR_CallbackAborted;
+			  return ENDPOINT_RWSTREAM_CallbackAborted;
 			#endif
 
 			if ((ErrorCode = Endpoint_WaitUntilReady()))
 			  return ErrorCode;
 		}
-
-		Endpoint_Write_Byte(*(DataStream++));
+		else
+		{
+			Endpoint_Write_Byte(*(DataStream++));
+			Length--;
+		}
 	}
-	
-	return ENDPOINT_RWSTREAM_ERROR_NoError;
+
+	return ENDPOINT_RWSTREAM_NoError;
 }
 
 uint8_t Endpoint_Write_Stream_BE(const void* Buffer, uint16_t Length
 #if !defined(NO_STREAM_CALLBACKS)
-                                 , uint8_t (* const Callback)(void)
+                                 , StreamCallbackPtr_t Callback
 #endif
 								 )
 {
@@ -183,30 +271,73 @@ uint8_t Endpoint_Write_Stream_BE(const void* Buffer, uint16_t Length
 	if ((ErrorCode = Endpoint_WaitUntilReady()))
 	  return ErrorCode;
 
-	while (Length--)
+	#if defined(FAST_STREAM_TRANSFERS)
+	uint8_t BytesRemToAlignment = (Endpoint_BytesInEndpoint() & 0x07);
+
+	if (Length >= 8)
 	{
-		if (!(Endpoint_ReadWriteAllowed()))
+		Length -= BytesRemToAlignment;
+
+		switch (BytesRemToAlignment)
 		{
-			Endpoint_ClearCurrentBank();
+			default:
+				do
+				{
+					if (!(Endpoint_IsReadWriteAllowed()))
+					{
+						Endpoint_ClearIN();
+
+						#if !defined(NO_STREAM_CALLBACKS)
+						if ((Callback != NULL) && (Callback() == STREAMCALLBACK_Abort))
+						  return ENDPOINT_RWSTREAM_CallbackAborted;
+						#endif
+
+						if ((ErrorCode = Endpoint_WaitUntilReady()))
+						  return ErrorCode;
+					}
+
+					Length -= 8;
+					
+					Endpoint_Write_Byte(*(DataStream--));
+			case 7: Endpoint_Write_Byte(*(DataStream--));
+			case 6: Endpoint_Write_Byte(*(DataStream--));
+			case 5: Endpoint_Write_Byte(*(DataStream--));
+			case 4: Endpoint_Write_Byte(*(DataStream--));
+			case 3: Endpoint_Write_Byte(*(DataStream--));
+			case 2: Endpoint_Write_Byte(*(DataStream--));
+			case 1:	Endpoint_Write_Byte(*(DataStream--));
+				} while (Length >= 8);	
+		}
+	}
+	#endif
+
+	while (Length)
+	{
+		if (!(Endpoint_IsReadWriteAllowed()))
+		{
+			Endpoint_ClearIN();
 
 			#if !defined(NO_STREAM_CALLBACKS)
 			if ((Callback != NULL) && (Callback() == STREAMCALLBACK_Abort))
-			  return ENDPOINT_RWSTREAM_ERROR_CallbackAborted;
+			  return ENDPOINT_RWSTREAM_CallbackAborted;
 			#endif
 
 			if ((ErrorCode = Endpoint_WaitUntilReady()))
 			  return ErrorCode;
 		}
-
-		Endpoint_Write_Byte(*(DataStream--));
+		else
+		{
+			Endpoint_Write_Byte(*(DataStream--));
+			Length--;
+		}
 	}
-	
-	return ENDPOINT_RWSTREAM_ERROR_NoError;
+
+	return ENDPOINT_RWSTREAM_NoError;
 }
 
 uint8_t Endpoint_Read_Stream_LE(void* Buffer, uint16_t Length
 #if !defined(NO_STREAM_CALLBACKS)
-                                 , uint8_t (* const Callback)(void)
+                                 , StreamCallbackPtr_t Callback
 #endif
 								 )
 {
@@ -216,30 +347,73 @@ uint8_t Endpoint_Read_Stream_LE(void* Buffer, uint16_t Length
 	if ((ErrorCode = Endpoint_WaitUntilReady()))
 	  return ErrorCode;
 
-	while (Length--)
+	#if defined(FAST_STREAM_TRANSFERS)
+	uint8_t BytesRemToAlignment = (Endpoint_BytesInEndpoint() & 0x07);
+
+	if (Length >= 8)
+	{
+		Length -= BytesRemToAlignment;
+
+		switch (BytesRemToAlignment)
+		{
+			default:
+				do
+				{
+					if (!(Endpoint_IsReadWriteAllowed()))
+					{
+						Endpoint_ClearOUT();
+
+						#if !defined(NO_STREAM_CALLBACKS)
+						if ((Callback != NULL) && (Callback() == STREAMCALLBACK_Abort))
+						  return ENDPOINT_RWSTREAM_CallbackAborted;
+						#endif
+
+						if ((ErrorCode = Endpoint_WaitUntilReady()))
+						  return ErrorCode;
+					}
+
+					Length -= 8;
+					
+					*(DataStream++) = Endpoint_Read_Byte();
+			case 7: *(DataStream++) = Endpoint_Read_Byte();
+			case 6: *(DataStream++) = Endpoint_Read_Byte();
+			case 5: *(DataStream++) = Endpoint_Read_Byte();
+			case 4: *(DataStream++) = Endpoint_Read_Byte();
+			case 3: *(DataStream++) = Endpoint_Read_Byte();
+			case 2: *(DataStream++) = Endpoint_Read_Byte();
+			case 1:	*(DataStream++) = Endpoint_Read_Byte();
+				} while (Length >= 8);	
+		}
+	}
+	#endif
+
+	while (Length)
 	{
-		if (!(Endpoint_ReadWriteAllowed()))
+		if (!(Endpoint_IsReadWriteAllowed()))
 		{
-			Endpoint_ClearCurrentBank();
+			Endpoint_ClearOUT();
 
 			#if !defined(NO_STREAM_CALLBACKS)
 			if ((Callback != NULL) && (Callback() == STREAMCALLBACK_Abort))
-			  return ENDPOINT_RWSTREAM_ERROR_CallbackAborted;
+			  return ENDPOINT_RWSTREAM_CallbackAborted;
 			#endif
 
 			if ((ErrorCode = Endpoint_WaitUntilReady()))
 			  return ErrorCode;
 		}
-		
-		*(DataStream++) = Endpoint_Read_Byte();
+		else
+		{
+			*(DataStream++) = Endpoint_Read_Byte();
+			Length--;
+		}
 	}
-	
-	return ENDPOINT_RWSTREAM_ERROR_NoError;
+
+	return ENDPOINT_RWSTREAM_NoError;
 }
 
 uint8_t Endpoint_Read_Stream_BE(void* Buffer, uint16_t Length
 #if !defined(NO_STREAM_CALLBACKS)
-                                 , uint8_t (* const Callback)(void)
+                                 , StreamCallbackPtr_t Callback
 #endif
 								 )
 {
@@ -249,93 +423,142 @@ uint8_t Endpoint_Read_Stream_BE(void* Buffer, uint16_t Length
 	if ((ErrorCode = Endpoint_WaitUntilReady()))
 	  return ErrorCode;
 
-	while (Length--)
+	#if defined(FAST_STREAM_TRANSFERS)
+	uint8_t BytesRemToAlignment = (Endpoint_BytesInEndpoint() & 0x07);
+
+	if (Length >= 8)
+	{
+		Length -= BytesRemToAlignment;
+
+		switch (BytesRemToAlignment)
+		{
+			default:
+				do
+				{
+					if (!(Endpoint_IsReadWriteAllowed()))
+					{
+						Endpoint_ClearOUT();
+
+						#if !defined(NO_STREAM_CALLBACKS)
+						if ((Callback != NULL) && (Callback() == STREAMCALLBACK_Abort))
+						  return ENDPOINT_RWSTREAM_CallbackAborted;
+						#endif
+
+						if ((ErrorCode = Endpoint_WaitUntilReady()))
+						  return ErrorCode;
+					}
+
+					Length -= 8;
+					
+					*(DataStream--) = Endpoint_Read_Byte();
+			case 7: *(DataStream--) = Endpoint_Read_Byte();
+			case 6: *(DataStream--) = Endpoint_Read_Byte();
+			case 5: *(DataStream--) = Endpoint_Read_Byte();
+			case 4: *(DataStream--) = Endpoint_Read_Byte();
+			case 3: *(DataStream--) = Endpoint_Read_Byte();
+			case 2: *(DataStream--) = Endpoint_Read_Byte();
+			case 1:	*(DataStream--) = Endpoint_Read_Byte();
+				} while (Length >= 8);	
+		}
+	}
+	#endif
+
+	while (Length)
 	{
-		if (!(Endpoint_ReadWriteAllowed()))
+		if (!(Endpoint_IsReadWriteAllowed()))
 		{
-			Endpoint_ClearCurrentBank();
+			Endpoint_ClearOUT();
 
 			#if !defined(NO_STREAM_CALLBACKS)
 			if ((Callback != NULL) && (Callback() == STREAMCALLBACK_Abort))
-			  return ENDPOINT_RWSTREAM_ERROR_CallbackAborted;
+			  return ENDPOINT_RWSTREAM_CallbackAborted;
 			#endif
 
 			if ((ErrorCode = Endpoint_WaitUntilReady()))
 			  return ErrorCode;
 		}
-		
-		*(DataStream--) = Endpoint_Read_Byte();
+		else
+		{
+			*(DataStream--) = Endpoint_Read_Byte();
+			Length--;
+		}
 	}
-	
-	return ENDPOINT_RWSTREAM_ERROR_NoError;
+
+	return ENDPOINT_RWSTREAM_NoError;
 }
+#endif
 
 uint8_t Endpoint_Write_Control_Stream_LE(const void* Buffer, uint16_t Length)
 {
-	uint8_t* DataStream = (uint8_t*)Buffer;
-	bool     SendZLP    = true;
+	uint8_t* DataStream     = (uint8_t*)Buffer;
+	bool     LastPacketFull = false;
+	
+	if (Length > USB_ControlRequest.wLength)
+	  Length = USB_ControlRequest.wLength;
 	
-	while (Length && !(Endpoint_IsSetupOUTReceived()))
+	while (Length && !(Endpoint_IsOUTReceived()))
 	{
-		while (!(Endpoint_IsSetupINReady()));
+		while (!(Endpoint_IsINReady()));
 		
 		while (Length && (Endpoint_BytesInEndpoint() < USB_ControlEndpointSize))
 		{
 			Endpoint_Write_Byte(*(DataStream++));
-			
 			Length--;
 		}
 		
-		SendZLP = (Endpoint_BytesInEndpoint() == USB_ControlEndpointSize);
-		Endpoint_ClearSetupIN();
+		LastPacketFull = (Endpoint_BytesInEndpoint() == USB_ControlEndpointSize);
+		Endpoint_ClearIN();
 	}
 	
-	if (Endpoint_IsSetupOUTReceived())
-	  return ENDPOINT_RWCSTREAM_ERROR_HostAborted;
+	if (Endpoint_IsOUTReceived())
+	  return ENDPOINT_RWCSTREAM_HostAborted;
 	
-	if (SendZLP)
+	if (LastPacketFull)
 	{
-		while (!(Endpoint_IsSetupINReady()));
-		Endpoint_ClearSetupIN();
+		while (!(Endpoint_IsINReady()));
+		Endpoint_ClearIN();
 	}
 	
-	while (!(Endpoint_IsSetupOUTReceived()));
+	while (!(Endpoint_IsOUTReceived()));
 
-	return ENDPOINT_RWCSTREAM_ERROR_NoError;
+	return ENDPOINT_RWCSTREAM_NoError;
 }
 
 uint8_t Endpoint_Write_Control_Stream_BE(const void* Buffer, uint16_t Length)
 {
-	uint8_t* DataStream = (uint8_t*)(Buffer + Length - 1);
-	bool     SendZLP    = true;
+	uint8_t* DataStream     = (uint8_t*)(Buffer + Length - 1);
+	bool     LastPacketFull = false;
 	
-	while (Length && !(Endpoint_IsSetupOUTReceived()))
+	if (Length > USB_ControlRequest.wLength)
+	  Length = USB_ControlRequest.wLength;
+
+	while (Length && !(Endpoint_IsOUTReceived()))
 	{
-		while (!(Endpoint_IsSetupINReady()));
-		
-		while (Length && (Endpoint_BytesInEndpoint() < USB_ControlEndpointSize))
+		if (Endpoint_IsINReady())
 		{
-			Endpoint_Write_Byte(*(DataStream--));
+			while (Length && (Endpoint_BytesInEndpoint() < USB_ControlEndpointSize))
+			{
+				Endpoint_Write_Byte(*(DataStream--));
+				Length--;
+			}
 			
-			Length--;
+			LastPacketFull = (Endpoint_BytesInEndpoint() == USB_ControlEndpointSize);
+			Endpoint_ClearIN();
 		}
-		
-		SendZLP = (Endpoint_BytesInEndpoint() == USB_ControlEndpointSize);
-		Endpoint_ClearSetupIN();
 	}
 	
-	if (Endpoint_IsSetupOUTReceived())
-	  return ENDPOINT_RWCSTREAM_ERROR_HostAborted;
+	if (Endpoint_IsOUTReceived())
+	  return ENDPOINT_RWCSTREAM_HostAborted;
 	
-	if (SendZLP)
+	if (LastPacketFull)
 	{
-		while (!(Endpoint_IsSetupINReady()));
-		Endpoint_ClearSetupIN();
+		while (!(Endpoint_IsINReady()));
+		Endpoint_ClearIN();
 	}
 	
-	while (!(Endpoint_IsSetupOUTReceived()));
+	while (!(Endpoint_IsOUTReceived()));
 
-	return ENDPOINT_RWCSTREAM_ERROR_NoError;
+	return ENDPOINT_RWCSTREAM_NoError;
 }
 
 uint8_t Endpoint_Read_Control_Stream_LE(void* Buffer, uint16_t Length)
@@ -344,21 +567,21 @@ uint8_t Endpoint_Read_Control_Stream_LE(void* Buffer, uint16_t Length)
 	
 	while (Length)
 	{
-		while (!(Endpoint_IsSetupOUTReceived()));
-		
-		while (Length && Endpoint_BytesInEndpoint())
+		if (Endpoint_IsOUTReceived())
 		{
-			*(DataStream++) = Endpoint_Read_Byte();
+			while (Length && Endpoint_BytesInEndpoint())
+			{
+				*(DataStream++) = Endpoint_Read_Byte();
+				Length--;
+			}
 			
-			Length--;
+			Endpoint_ClearOUT();
 		}
-		
-		Endpoint_ClearSetupOUT();
 	}
 	
-	while (!(Endpoint_IsSetupINReady()));
+	while (!(Endpoint_IsINReady()));
 	
-	return ENDPOINT_RWCSTREAM_ERROR_NoError;
+	return ENDPOINT_RWCSTREAM_NoError;
 }
 
 uint8_t Endpoint_Read_Control_Stream_BE(void* Buffer, uint16_t Length)
@@ -367,21 +590,21 @@ uint8_t Endpoint_Read_Control_Stream_BE(void* Buffer, uint16_t Length)
 	
 	while (Length)
 	{
-		while (!(Endpoint_IsSetupOUTReceived()));
-		
-		while (Length && Endpoint_BytesInEndpoint())
+		if (Endpoint_IsOUTReceived())
 		{
-			*(DataStream--) = Endpoint_Read_Byte();
+			while (Length && Endpoint_BytesInEndpoint())
+			{
+				*(DataStream--) = Endpoint_Read_Byte();
+				Length--;
+			}
 			
-			Length--;
+			Endpoint_ClearOUT();
 		}
-		
-		Endpoint_ClearSetupOUT();
 	}
 	
-	while (!(Endpoint_IsSetupINReady()));
+	while (!(Endpoint_IsINReady()));
 
-	return ENDPOINT_RWCSTREAM_ERROR_NoError;
+	return ENDPOINT_RWCSTREAM_NoError;
 }
 
 #endif