diff --git a/cores/arduino/USB/HID.cpp b/cores/arduino/USB/HID.cpp
index f5d561e0e7a941025dc7d411027a1eeebb4deca7..6f7f5f5c043d0a0583a03a80ac2258d7908a4f4b 100644
--- a/cores/arduino/USB/HID.cpp
+++ b/cores/arduino/USB/HID.cpp
@@ -198,7 +198,7 @@ bool WEAK HID_Setup(Setup& setup)
 		if (HID_GET_IDLE == r)
 		{
 			UDD_Send(0, &_hid_idle, 1);
-			UDD_ClearIN();
+			UDD_ReleaseIN();
 			return true;
 		}
 	}
diff --git a/cores/arduino/USB/USBCore.cpp b/cores/arduino/USB/USBCore.cpp
index 38f7404f709aedd0cf15597e33285e90d4dc6828..abd40bbd12b3d9c053baee95a0d8af5e038aa372 100644
--- a/cores/arduino/USB/USBCore.cpp
+++ b/cores/arduino/USB/USBCore.cpp
@@ -205,9 +205,18 @@ static bool USB_SendStringDescriptor(const uint8_t *string, int wLength)
 
 uint32_t USBD_RecvControl(void* d, uint32_t len)
 {
-	udd_ack_out_received(0);
-
-	return len;
+	uint8_t *buffer;
+	uint8_t *data = (uint8_t *)d;
+	uint32_t read = UDD_Recv_data(EP0, len);
+	if (read > len)
+		read = len;
+	UDD_Recv(EP0, &buffer);
+	UDD_WaitOUT();
+	for (int i=0; i<read; i++) {
+		data[i] = buffer[i];
+	}
+	UDD_ReleaseOUT();
+	return read;
 }
 
 //	Handle CLASS_INTERFACE requests
@@ -631,7 +640,7 @@ void USB_Handler(void)
 			if (ok)
 			{
 				TRACE_CORE(puts(">>> EP0 Int: Send packet\r\n");)
-				UDD_ClearIN();
+				UDD_ReleaseIN();
 			}
 			else
 			{
diff --git a/cores/arduino/USB/USB_device.h b/cores/arduino/USB/USB_device.h
index dddda66ecc59c2ae50d5480b598542da1f8bc5f1..f7338fb701c17cf33147583325252b1b3baadd9a 100644
--- a/cores/arduino/USB/USB_device.h
+++ b/cores/arduino/USB/USB_device.h
@@ -39,13 +39,17 @@ extern "C" {
 #define USB_ENDPOINT_TYPE_INTERRUPT            0x03
 
 
-extern void UDD_ClearIN(void);
+extern void UDD_ReleaseIN(void);
+extern void UDD_ReleaseOUT(void);
+extern void UDD_WaitIN(void);
+extern void UDD_WaitOUT(void);
 extern uint32_t UDD_FifoByteCount(uint32_t ep);
 extern void UDD_ReleaseRX(uint32_t ep);
 extern void UDD_ReleaseTX(uint32_t ep);
 extern uint32_t UDD_Send(uint32_t ep, const void* data, uint32_t len);
 extern uint8_t UDD_Recv8(uint32_t ep);
 extern void UDD_Recv(uint32_t ep, uint8_t** data);
+extern uint8_t UDD_Recv_data(uint32_t ep, uint32_t len);
 extern void UDD_Init(void);
 extern void UDD_InitEP( uint32_t ul_ep, uint32_t ul_ep_cfg );
 extern void send_zlp (void);
diff --git a/cores/arduino/USB/samd21_device.c b/cores/arduino/USB/samd21_device.c
index 6e750b06d26c8700906d34c34dbd278822006c08..e99e172d879603132f87e853fb54ffa0082110e6 100644
--- a/cores/arduino/USB/samd21_device.c
+++ b/cores/arduino/USB/samd21_device.c
@@ -238,11 +238,25 @@ void UDD_InitEP( uint32_t ul_ep_nb, uint32_t ul_ep_cfg )
 
 
 // Send packet.
-void UDD_ClearIN(void)
+void UDD_ReleaseIN(void)
 {
 	USB->DEVICE.DeviceEndpoint[EP0].EPSTATUSSET.reg = USB_DEVICE_EPSTATUSSET_BK1RDY;
 }
 
+void UDD_ReleaseOUT(void)
+{
+	USB->DEVICE.DeviceEndpoint[EP0].EPSTATUSCLR.reg = USB_DEVICE_EPSTATUSCLR_BK0RDY;
+}
+
+void UDD_WaitIN()
+{
+	while (!( USB->DEVICE.DeviceEndpoint[0].EPSTATUS.bit.BK1RDY )) {}
+}
+
+void UDD_WaitOUT()
+{
+	while (!( USB->DEVICE.DeviceEndpoint[0].EPSTATUS.bit.BK0RDY )) {}
+}
 
 
 uint32_t UDD_Send(uint32_t ep, const void* data, uint32_t len)
@@ -275,6 +289,24 @@ uint8_t UDD_Recv8(uint32_t ep)
 	return udd_ep_out_cache_buffer[ep][0];
 }
 
+uint8_t UDD_Recv_data(uint32_t ep, uint32_t len)
+{
+	TRACE_DEVICE(printf("=> UDD_Recvdata : ep=%d\r\n", (char)ep);)
+
+	usb_endpoint_table[ep].DeviceDescBank[0].ADDR.reg = (uint32_t)&udd_ep_out_cache_buffer[ep];
+	usb_endpoint_table[ep].DeviceDescBank[0].PCKSIZE.bit.MULTI_PACKET_SIZE = len;
+	usb_endpoint_table[ep].DeviceDescBank[0].PCKSIZE.bit.BYTE_COUNT = 0;
+	USB->DEVICE.DeviceEndpoint[ep].EPSTATUSCLR.reg = USB_DEVICE_EPSTATUSCLR_BK0RDY;
+	TRACE_DEVICE(printf("=> UDD_Recv8 : data=%lu\r\n", (unsigned long)data);)
+
+	/* Wait for transfer to complete */
+	while (!( USB->DEVICE.DeviceEndpoint[ep].EPINTFLAG.reg & USB_DEVICE_EPINTFLAG_TRCPT0 ));
+	/* Clear Transfer complete 0 flag */
+	//USB->DEVICE.DeviceEndpoint[ep].EPINTFLAG.bit.TRCPT0 = 1;
+
+	return udd_ep_out_cache_buffer[ep][0];
+}
+
 void UDD_Recv(uint32_t ep, uint8_t** ppData)
 {
 	*ppData = udd_ep_out_cache_buffer[ep];