diff --git a/hardware/arduino/sam/cores/sam/USB/USBCore.cpp b/hardware/arduino/sam/cores/sam/USB/USBCore.cpp
index 191a45e2d..44b5926de 100644
--- a/hardware/arduino/sam/cores/sam/USB/USBCore.cpp
+++ b/hardware/arduino/sam/cores/sam/USB/USBCore.cpp
@@ -101,7 +101,7 @@ class LockEP
 public:
 	LockEP(uint32_t ep) : flags(cpu_irq_save())
 	{
-		UDD_SetEP(ep & 0xF);
+		//UDD_SetEP(ep & 0xF);
 	}
 	~LockEP()
 	{
@@ -113,7 +113,7 @@ public:
 uint32_t USBD_Available(uint32_t ep)
 {
 	LockEP lock(ep);
-	return UDD_FifoByteCount();
+	return UDD_FifoByteCount(ep & 0xF);
 }
 
 //	Non Blocking receive
@@ -124,14 +124,14 @@ uint32_t USBD_Recv(uint32_t ep, void* d, uint32_t len)
 		return -1;
 
 	LockEP lock(ep);
-	uint32_t n = UDD_FifoByteCount();
+	uint32_t n = UDD_FifoByteCount(ep & 0xF);
 	len = min(n,len);
 	n = len;
 	uint8_t* dst = (uint8_t*)d;
 	while (n--)
-		*dst++ = UDD_Recv8();
-	if (len && !UDD_FifoByteCount())	// release empty buffer
-		UDD_ReleaseRX();
+		*dst++ = UDD_Recv8(ep & 0xF);
+	if (len && !UDD_FifoByteCount(ep & 0xF))	// release empty buffer
+		UDD_ReleaseRX(ep & 0xF);
 
 	return len;
 }
@@ -140,7 +140,7 @@ uint32_t USBD_Recv(uint32_t ep, void* d, uint32_t len)
 uint32_t USBD_Recv(uint32_t ep)
 {
 	uint8_t c;
-	if (USBD_Recv(ep, &c, 1) != 1)
+	if (USBD_Recv(ep & 0xF, &c, 1) != 1)
 		return -1;
 	else
 		return c;
@@ -150,9 +150,9 @@ uint32_t USBD_Recv(uint32_t ep)
 uint32_t USBD_SendSpace(uint32_t ep)
 {
 	LockEP lock(ep);
-	if (!UDD_ReadWriteAllowed())
+	if (!UDD_ReadWriteAllowed(ep & 0xF))
 		return 0;
-	return 64 - UDD_FifoByteCount();
+	return 64 - UDD_FifoByteCount(ep & 0xF);
 }
 
 //	Blocking Send of data to an endpoint
@@ -164,6 +164,7 @@ uint32_t USBD_Send(uint32_t ep, const void* d, uint32_t len)
 	int r = len;
 	const uint8_t* data = (const uint8_t*)d;
 	uint8_t timeout = 250;		// 250ms timeout on send? TODO
+
 	while (len)
 	{
 		uint8_t n = USBD_SendSpace(ep);
@@ -178,22 +179,11 @@ uint32_t USBD_Send(uint32_t ep, const void* d, uint32_t len)
 		if (n > len)
 			n = len;
 		len -= n;
-		{
-			LockEP lock(ep);
-			if (ep & TRANSFER_ZERO)
-			{
-				while (n--)
-					UDD_Send8(0);
-			}
-			else
-			{
-				while (n--)
-					UDD_Send8(*data++);
-			}
 
-			if (!UDD_ReadWriteAllowed() || ((len == 0) && (ep & TRANSFER_RELEASE)))	// Release full buffer
-				UDD_ReleaseTX();
-		}
+		UDD_Send(ep & 0xF, data, n);
+
+		if (!UDD_ReadWriteAllowed(ep & 0xF) || ((len == 0) && (ep & TRANSFER_RELEASE)))	// Release full buffer
+			UDD_ReleaseTX(ep & 0xF);
 	}
 	//TXLED1;					// light the TX LED
 	//TxLEDPulse = TX_RX_LED_PULSE_MS;
@@ -205,46 +195,36 @@ int _cend;
 
 void USBD_InitControl(int end)
 {
-	UDD_SetEP(0);
 	_cmark = 0;
 	_cend = end;
 }
 
-static bool USBD_SendControl(uint8_t d)
-{
-	if (_cmark < _cend)
-	{
-		// /!\ NE DEVRAIT THEORIQUEMENT PAS ETRE COMMENTE... mais ca marche mieux sans... pourquoi?!!!
-		//if (!UDD_WaitForINOrOUT())
-		//	return false;
-
-		UDD_Send8(d);
-
-		if (!((_cmark + 1) & 0x3F))
-		{
-			puts("Sent!\r\n");
-			UDD_ClearIN();	// Fifo is full, release this packet
-		}
-	}
-	_cmark++;
-	return true;
-};
-
 //	Clipped by _cmark/_cend
 int USBD_SendControl(uint8_t flags, const void* d, uint32_t len)
 {
 	int sent = len;
+	uint32_t i = 0;
 	const uint8_t* data = (const uint8_t*)d;
 
-	while (len--)
+	printf("=> USBD_SendControl TOTAL len=%d\r\n", len);
+
+	for (i = 0; len > 64; ++i, len -= 64, _cmark += 64)
 	{
-		uint8_t c = *data++;
-		if (!USBD_SendControl(c))
+		if (_cmark < _cend)
 		{
-			printf("=> USBD_SendControl : return -1\r\n");
-			return -1;
+			UDD_Send(EP0, data + (i * 64), 64);
+			UDD_ClearIN();	// Fifo is full, release this packet
+			UDD_WaitIN(); // Wait for new FIFO buffer to be ready
 		}
 	}
+
+	if (len > 0)
+	{
+		if (_cmark < _cend)
+			UDD_Send(EP0, data + (i * 64), len);
+		_cmark += len;
+	}
+
 	return sent;
 }
 
@@ -254,7 +234,7 @@ int USBD_SendControl(uint8_t flags, const void* d, uint32_t len)
 int USBD_RecvControl(void* d, uint32_t len)
 {
 	UDD_WaitOUT() ;
-	UDD_Recv( (uint8_t*)d, len ) ;
+	UDD_Recv(EP0, (uint8_t*)d, len ) ; // WILL NOT WORK WITH CDC
 	UDD_ClearOUT() ;
 
 	return len ;
@@ -398,7 +378,7 @@ static void USB_ISR(void)
 		udd_enable_address();
 
 		// Configure EP 0
-		UDD_SetEP(0);
+		//UDD_SetEP(0);
         UDD_InitEP(0, EP_TYPE_CONTROL);
 		udd_enable_setup_received_interrupt(0);
 		udd_enable_endpoint_interrupt(0);
@@ -429,7 +409,7 @@ static void USB_ISR(void)
 	// EP 0 Interrupt
 	if (Is_udd_endpoint_interrupt(0))
 	{
-		UDD_SetEP(0);
+		//UDD_SetEP(0);
 
 		//printf(">>> EP0 Int: 0x%x\r\n", UOTGHS->UOTGHS_DEVEPTISR[0]);
 
@@ -439,7 +419,7 @@ static void USB_ISR(void)
 		}
 
 		Setup setup ;
-		UDD_Recv((uint8_t*)&setup,8);
+		UDD_Recv(EP0, (uint8_t*)&setup, 8);
 		UDD_ClearSetupInt();
 
 		//printf(">>> EP0 Int: AP clear: 0x%x\r\n", UOTGHS->UOTGHS_DEVEPTISR[0]);
@@ -466,8 +446,8 @@ static void USB_ISR(void)
 			if (GET_STATUS == r)
 			{
 				puts(">>> EP0 Int: GET_STATUS\r\n");
-				UDD_Send8(0);		// TODO
-				UDD_Send8(0);
+				UDD_Send8(EP0, 0);		// TODO
+				UDD_Send8(EP0, 0);
 			}
 			else if (CLEAR_FEATURE == r)
 			{
@@ -494,7 +474,7 @@ static void USB_ISR(void)
 			else if (GET_CONFIGURATION == r)
 			{
 				puts(">>> EP0 Int: GET_CONFIGURATION\r\n");
-				UDD_Send8(1);
+				UDD_Send8(EP0, 1);
 			}
 			else if (SET_CONFIGURATION == r)
 			{
@@ -543,9 +523,9 @@ static void USB_ISR(void)
 
 void USBD_Flush(uint32_t ep)
 {
-	UDD_SetEP(ep);
-	if (UDD_FifoByteCount())
-		UDD_ReleaseTX();
+	//UDD_SetEP(ep);
+	if (UDD_FifoByteCount(ep))
+		UDD_ReleaseTX(ep);
 }
 
 //	VBUS or counting frames
diff --git a/hardware/arduino/sam/cores/sam/libsam_sam3x8e_gcc_dbg.a b/hardware/arduino/sam/cores/sam/libsam_sam3x8e_gcc_dbg.a
index de25c8623..e3a232488 100644
Binary files a/hardware/arduino/sam/cores/sam/libsam_sam3x8e_gcc_dbg.a and b/hardware/arduino/sam/cores/sam/libsam_sam3x8e_gcc_dbg.a differ
diff --git a/hardware/arduino/sam/cores/sam/libsam_sam3x8e_gcc_dbg.a.txt b/hardware/arduino/sam/cores/sam/libsam_sam3x8e_gcc_dbg.a.txt
index 23232e39c..d5c0cf942 100644
--- a/hardware/arduino/sam/cores/sam/libsam_sam3x8e_gcc_dbg.a.txt
+++ b/hardware/arduino/sam/cores/sam/libsam_sam3x8e_gcc_dbg.a.txt
@@ -116,7 +116,21 @@ pmc.o:
 00000000 T pmc_switch_udpck_to_upllck
 
 pwmc.o:
-00000000 r C.19.7195
+00000024 r .LC0
+00000000 r .LC1
+0000016c r .LC10
+0000019c r .LC11
+000001cc r .LC12
+000001fc r .LC13
+00000204 r .LC14
+00000014 r .LC2
+00000050 r .LC3
+0000007c r .LC4
+000000a8 r .LC5
+000000dc r .LC6
+00000108 r .LC7
+00000134 r .LC8
+00000160 r .LC9
 00000000 t FindClockConfiguration
 00000000 T PWMC_ConfigureChannel
 00000000 T PWMC_ConfigureChannelExt
@@ -144,16 +158,19 @@ pwmc.o:
 00000000 T PWMC_SetSyncChannelUpdateUnlock
 00000000 T PWMC_WriteBuffer
          U __assert_func
-00000000 r __func__.5848
-00000000 r __func__.5859
-00000000 r __func__.5874
-00000000 r __func__.5885
-00000000 r __func__.5896
-00000000 r __func__.5903
-00000000 r __func__.5987
-00000000 r __func__.5993
+00000000 r __func__.3192
+00000000 r __func__.3203
+00000000 r __func__.3218
+00000000 r __func__.3229
+00000000 r __func__.3240
+00000000 r __func__.3247
+00000000 r __func__.3331
+00000000 r __func__.3337
 
 rtc.o:
+00000000 r .LC0
+00000010 r .LC1
+0000002c r .LC2
 00000000 T RTC_ClearSCCR
 00000000 T RTC_DisableIt
 00000000 T RTC_EnableIt
@@ -167,19 +184,22 @@ rtc.o:
 00000000 T RTC_SetTime
 00000000 T RTC_SetTimeAlarm
          U __assert_func
-00000000 r __func__.5845
-00000000 r __func__.5854
-00000000 r __func__.5859
+00000000 r __func__.3189
+00000000 r __func__.3198
+00000000 r __func__.3203
 
 rtt.o:
+00000000 r .LC0
+00000010 r .LC1
+0000002c r .LC2
 00000000 T RTT_EnableIT
 00000000 T RTT_GetStatus
 00000000 T RTT_GetTime
 00000000 T RTT_SetAlarm
 00000000 T RTT_SetPrescaler
          U __assert_func
-00000000 r __func__.5852
-00000000 r __func__.5860
+00000000 r __func__.3196
+00000000 r __func__.3204
 
 spi.o:
 00000000 T SPI_Configure
@@ -195,14 +215,16 @@ spi.o:
          U pmc_enable_periph_clk
 
 tc.o:
+00000000 r .LC0
+00000010 r .LC1
 00000000 T TC_Configure
 00000000 T TC_FindMckDivisor
 00000000 T TC_Start
 00000000 T TC_Stop
          U __assert_func
-00000000 r __func__.5847
-00000000 r __func__.5853
-00000000 r __func__.5859
+00000000 r __func__.3191
+00000000 r __func__.3197
+00000000 r __func__.3203
 
 timetick.o:
 00000000 T GetTickCount
@@ -215,6 +237,15 @@ timetick.o:
 00000000 b _dwTickCount
 
 twi.o:
+00000000 r .LC0
+00000010 r .LC1
+00000018 r .LC2
+00000024 r .LC3
+00000054 r .LC4
+00000064 r .LC5
+0000007c r .LC6
+0000009c r .LC7
+000000a8 r .LC8
 00000000 T TWI_ByteReceived
 00000000 T TWI_ByteSent
 00000000 T TWI_ConfigureMaster
@@ -231,18 +262,18 @@ twi.o:
 00000000 T TWI_TransferComplete
 00000000 T TWI_WriteByte
          U __assert_func
-00000000 r __func__.6229
-00000000 r __func__.6244
-00000000 r __func__.6248
-00000000 r __func__.6255
-00000000 r __func__.6259
-00000000 r __func__.6264
-00000000 r __func__.6272
-00000000 r __func__.6286
-00000000 r __func__.6291
-00000000 r __func__.6295
-00000000 r __func__.6300
-00000000 r __func__.6304
+00000000 r __func__.3556
+00000000 r __func__.3571
+00000000 r __func__.3575
+00000000 r __func__.3582
+00000000 r __func__.3586
+00000000 r __func__.3591
+00000000 r __func__.3599
+00000000 r __func__.3613
+00000000 r __func__.3618
+00000000 r __func__.3622
+00000000 r __func__.3627
+00000000 r __func__.3631
 
 udp.o:
 
@@ -267,9 +298,9 @@ uotghs.o:
 00000000 T UDD_Recv8
 00000000 T UDD_ReleaseRX
 00000000 T UDD_ReleaseTX
+00000000 T UDD_Send
 00000000 T UDD_Send8
 00000000 T UDD_SetAddress
-00000000 T UDD_SetEP
 00000000 T UDD_SetStack
 00000000 T UDD_Stall
 00000000 T UDD_WaitForINOrOUT
@@ -281,17 +312,16 @@ uotghs.o:
 00000000 t cpu_irq_save
          U g_interrupt_enabled
 00000000 b gpf_isr
-         U iprintf
          U pmc_enable_periph_clk
          U pmc_enable_udpck
          U pmc_enable_upll_clock
          U pmc_switch_udpck_to_upllck
-         U puts
-00000000 B ul_ep
-00000000 B ul_rcv_index
-00000000 B ul_send_index
+00000000 b ul_recv_fifo_ptr
+00000000 b ul_send_fifo_ptr
 
 usart.o:
+00000000 r .LC0
+00000014 r .LC1
 00000000 T USART_Configure
 00000000 T USART_DisableIt
 00000000 T USART_EnableIt
@@ -308,7 +338,7 @@ usart.o:
 00000000 T USART_Write
 00000000 T USART_WriteBuffer
          U __assert_func
-00000000 r __func__.6150
+00000000 r __func__.3477
 
 wdt.o:
 00000000 T WDT_Disable
diff --git a/hardware/arduino/sam/system/libsam/include/USB_device.h b/hardware/arduino/sam/system/libsam/include/USB_device.h
index cd03bce94..a02ea07b5 100644
--- a/hardware/arduino/sam/system/libsam/include/USB_device.h
+++ b/hardware/arduino/sam/system/libsam/include/USB_device.h
@@ -33,22 +33,25 @@ extern void UDD_ClearRxFlag( unsigned char bEndpoint ) ;
 extern uint32_t UDD_ReceivedSetupInt(void);
 extern void UDD_ClearSetupInt(void);
 
-extern uint32_t UDD_ReadWriteAllowed(void) ;
+extern uint32_t UDD_ReadWriteAllowed(uint32_t ep) ;
 
-extern void UDD_SetEP( uint32_t ep ) ;
-extern uint32_t UDD_FifoByteCount(void) ;
+
+extern uint32_t UDD_FifoByteCount(uint32_t ep) ;
 extern uint8_t UDD_FifoFree(void) ;
 
-extern void UDD_ReleaseRX(void) ;
-extern void UDD_ReleaseTX(void) ;
+extern void UDD_ReleaseRX(uint32_t ep) ;
+extern void UDD_ReleaseTX(uint32_t ep) ;
 extern uint8_t UDD_FrameNumber(void) ;
 
 extern uint8_t UDD_GetConfiguration(void) ;
 
 
-extern void UDD_Send8( uint8_t d ) ;
-extern uint8_t UDD_Recv8(void);
-extern void UDD_Recv(volatile uint8_t* data, uint32_t count);
+
+
+extern void UDD_Send(uint32_t ep, const void* data, uint32_t len);
+extern void UDD_Send8(uint32_t ep,  uint8_t data );
+extern uint8_t UDD_Recv8(uint32_t ep);
+extern void UDD_Recv(uint32_t ep, uint8_t* data, uint32_t len);
 
 
 
@@ -66,7 +69,6 @@ extern void UDD_SetAddress(uint32_t addr);
 extern void UDD_Stall(void);
 extern uint32_t UDD_GetFrameNumber(void);
 
-
 /*! \name Usual Types
  */
 
diff --git a/hardware/arduino/sam/system/libsam/include/uotghs.h b/hardware/arduino/sam/system/libsam/include/uotghs.h
index 427b743ec..a081aa65e 100644
--- a/hardware/arduino/sam/system/libsam/include/uotghs.h
+++ b/hardware/arduino/sam/system/libsam/include/uotghs.h
@@ -19,6 +19,11 @@
 #ifndef UOTGHS_H_INCLUDED
 #define UOTGHS_H_INCLUDED
 
+
+#define MAX_ENDPOINTS	10
+
+#define EP0				0
+
 #define EP_SINGLE_64 (0x32UL) // EP0
 #define EP_DOUBLE_64 (0x36UL) // Other endpoints
 
diff --git a/hardware/arduino/sam/system/libsam/source/uotghs.c b/hardware/arduino/sam/system/libsam/source/uotghs.c
index 123c2f615..a6c46779e 100644
--- a/hardware/arduino/sam/system/libsam/source/uotghs.c
+++ b/hardware/arduino/sam/system/libsam/source/uotghs.c
@@ -23,9 +23,10 @@
 
 static void (*gpf_isr)(void)	= (0UL);
 
-static volatile uint32_t ul_ep			= (0UL);
-static volatile uint32_t ul_send_index	= (0UL);
-static volatile uint32_t ul_recv_index	= (0UL);
+//static volatile uint32_t ul_ep			= (0UL);
+
+static volatile uint32_t ul_send_fifo_ptr[MAX_ENDPOINTS];
+static volatile uint32_t ul_recv_fifo_ptr[MAX_ENDPOINTS];
 
 void UDD_SetStack(void (*pf_isr)(void))
 {
@@ -40,7 +41,13 @@ void UOTGHS_Handler( void )
 
 uint32_t UDD_Init(void)
 {
-	uint32_t ul ;
+	uint32_t i;
+
+	for (i = 0; i < MAX_ENDPOINTS; ++i)
+	{
+		ul_send_fifo_ptr[i] = 0;
+		ul_recv_fifo_ptr[i] = 0;
+	}
 
 	// Enables the USB Clock
 	pmc_enable_periph_clk(ID_UOTGHS);
@@ -212,23 +219,18 @@ udd_enable_endpoint(ul_ep_nb);
 
 }
 
-void UDD_SetEP( uint32_t ep )
-{
-	ul_ep = ep & 0xF; // EP range is 0..9, hence mask is 0xF.
-}
-
 // Wait until ready to accept IN packet.
 void UDD_WaitIN(void)
 {
 	//while (!(UEINTX & (1<<TXINI)));
-	while (!(UOTGHS->UOTGHS_DEVEPTISR[ul_ep] & UOTGHS_DEVEPTISR_TXINI))
+	while (!(UOTGHS->UOTGHS_DEVEPTISR[EP0] & UOTGHS_DEVEPTISR_TXINI))
 		;
 }
 
 void UDD_WaitOUT(void)
 {
 	//while (!(UEINTX & (1<<RXOUTI)));
-	while (!(UOTGHS->UOTGHS_DEVEPTISR[ul_ep] & UOTGHS_DEVEPTISR_RXOUTI))
+	while (!(UOTGHS->UOTGHS_DEVEPTISR[EP0] & UOTGHS_DEVEPTISR_RXOUTI))
 		;
 }
 
@@ -237,15 +239,15 @@ void UDD_ClearIN(void)
 {
 	//printf("=> UDD_ClearIN: sent %d bytes\r\n", ul_send_index);
 	// UEINTX = ~(1<<TXINI);
-	UOTGHS->UOTGHS_DEVEPTICR[ul_ep] = UOTGHS_DEVEPTICR_TXINIC;
-	ul_send_index = 0;
+	UOTGHS->UOTGHS_DEVEPTICR[EP0] = UOTGHS_DEVEPTICR_TXINIC;
+	ul_send_fifo_ptr[EP0] = 0;
 }
 
 void UDD_ClearOUT(void)
 {
 	// UEINTX = ~(1<<RXOUTI);
-	UOTGHS->UOTGHS_DEVEPTICR[ul_ep] = UOTGHS_DEVEPTICR_RXOUTIC;
-	ul_recv_index = 0;
+	UOTGHS->UOTGHS_DEVEPTICR[EP0] = UOTGHS_DEVEPTICR_RXOUTIC;
+	ul_recv_fifo_ptr[EP0] = 0;
 }
 
 // Wait for IN FIFO to be ready to accept data or OUT FIFO to receive data.
@@ -254,61 +256,80 @@ uint32_t UDD_WaitForINOrOUT(void)
 {
 	//while (!(UEINTX & ((1<<TXINI)|(1<<RXOUTI))));
 	//return (UEINTX & (1<<RXOUTI)) == 0;
-	while (!(UOTGHS->UOTGHS_DEVEPTISR[ul_ep] & (UOTGHS_DEVEPTISR_TXINI | UOTGHS_DEVEPTISR_RXOUTI)))
+	while (!(UOTGHS->UOTGHS_DEVEPTISR[EP0] & (UOTGHS_DEVEPTISR_TXINI | UOTGHS_DEVEPTISR_RXOUTI)))
 		;
-	return ((UOTGHS->UOTGHS_DEVEPTISR[ul_ep] & UOTGHS_DEVEPTISR_RXOUTI) == 0);
+	return ((UOTGHS->UOTGHS_DEVEPTISR[EP0] & UOTGHS_DEVEPTISR_RXOUTI) == 0);
 }
 
 uint32_t UDD_ReceivedSetupInt(void)
 {
-	return UOTGHS->UOTGHS_DEVEPTISR[ul_ep] & UOTGHS_DEVEPTISR_RXSTPI;
+	return UOTGHS->UOTGHS_DEVEPTISR[EP0] & UOTGHS_DEVEPTISR_RXSTPI;
 }
 
 void UDD_ClearSetupInt(void)
 {
 	//UEINTX = ~((1<<RXSTPI) | (1<<RXOUTI) | (1<<TXINI));
 	//UOTGHS->UOTGHS_DEVEPTICR[ul_ep] = (UOTGHS_DEVEPTICR_RXSTPIC | UOTGHS_DEVEPTICR_RXOUTIC | UOTGHS_DEVEPTICR_TXINIC);
-	UOTGHS->UOTGHS_DEVEPTICR[ul_ep] = (UOTGHS_DEVEPTICR_RXSTPIC);
+	UOTGHS->UOTGHS_DEVEPTICR[EP0] = (UOTGHS_DEVEPTICR_RXSTPIC);
 }
 
-void UDD_Send8( uint8_t data )
+void UDD_Send(uint32_t ep, const void* data, uint32_t len)
 {
-	uint8_t *ptr_dest = (uint8_t *) &udd_get_endpoint_fifo_access8(ul_ep);
+	const uint8_t *ptr_src = data;
+	uint8_t *ptr_dest = (uint8_t *) &udd_get_endpoint_fifo_access8(ep);
+	uint32_t i;
 
-	printf("=> UDD_Send8 : ul_send_index=%d data=0x%x\r\n", ul_send_index, data);
-	ptr_dest[ul_send_index++] = data;
+	//printf("=> UDD_Send : ep=%d ptr_dest=%d len=%d\r\n", ep, ul_send_fifo_ptr[ep], len);
+
+	for (i = 0, ptr_dest += ul_send_fifo_ptr[ep]; i < len; ++i)
+		*ptr_dest++ = *ptr_src++;
+
+	ul_send_fifo_ptr[ep] += i;
 }
 
-uint8_t UDD_Recv8(void)
+void UDD_Send8(uint32_t ep,  uint8_t data )
 {
-	uint8_t *ptr_dest = (uint8_t *) &udd_get_endpoint_fifo_access8(ul_ep);
+	uint8_t *ptr_dest = (uint8_t *) &udd_get_endpoint_fifo_access8(ep);
+	//printf("=> UDD_Send8 : ul_send_index=%d data=0x%x\r\n", ul_send_index, data);
+	ptr_dest[ul_send_fifo_ptr[ep]] = data;
+	ul_send_fifo_ptr[ep] += 1;
+}
 
+uint8_t UDD_Recv8(uint32_t ep)
+{
+	uint8_t *ptr_dest = (uint8_t *) &udd_get_endpoint_fifo_access8(ep);
+	uint8_t data = ptr_dest[ul_recv_fifo_ptr[ep]];
 	////printf("=> UDD_Recv8 : ul_recv_index=%d\r\n", ul_recv_index);
-	return ptr_dest[ul_recv_index++];
+	ul_recv_fifo_ptr[ep] += 1;
+	return data;
 }
 
-void UDD_Recv(volatile uint8_t* data, uint32_t count)
+void UDD_Recv(uint32_t ep, uint8_t* data, uint32_t len)
 {
-	uint8_t *ptr_dest = (uint8_t *) &udd_get_endpoint_fifo_access8(ul_ep);
+	uint8_t *ptr_src = (uint8_t *) &udd_get_endpoint_fifo_access8(ep);
+	uint8_t *ptr_dest = data;
+	uint32_t i;
 
-	while (count--)
-		*data++ = ptr_dest[ul_recv_index++];
+	for (i = 0, ptr_src += ul_recv_fifo_ptr[ep]; i < len; ++i)
+		*ptr_dest++ = *ptr_src++;
+
+	ul_recv_fifo_ptr[ep] += i;
 }
 
 void UDD_Stall(void)
 {
 	//UECONX = (1<<STALLRQ) | (1<<EPEN);
-	UOTGHS->UOTGHS_DEVEPT = (UOTGHS_DEVEPT_EPEN0 << ul_ep);
-	UOTGHS->UOTGHS_DEVEPTIER[ul_ep] = UOTGHS_DEVEPTIER_STALLRQS;
+	UOTGHS->UOTGHS_DEVEPT = (UOTGHS_DEVEPT_EPEN0 << EP0);
+	UOTGHS->UOTGHS_DEVEPTIER[EP0] = UOTGHS_DEVEPTIER_STALLRQS;
 }
 
 
-uint32_t UDD_FifoByteCount(void)
+uint32_t UDD_FifoByteCount(uint32_t ep)
 {
-	return ((UOTGHS->UOTGHS_DEVEPTISR[ul_ep] & UOTGHS_DEVEPTISR_BYCT_Msk) >> UOTGHS_DEVEPTISR_BYCT_Pos);
+	return ((UOTGHS->UOTGHS_DEVEPTISR[ep] & UOTGHS_DEVEPTISR_BYCT_Msk) >> UOTGHS_DEVEPTISR_BYCT_Pos);
 }
 
-void UDD_ReleaseRX(void)
+void UDD_ReleaseRX(uint32_t ep)
 {
 /*	UEINTX = 0x6B;	// FIFOCON=0 NAKINI=1 RWAL=1 NAKOUTI=0 RXSTPI=1 RXOUTI=0 STALLEDI=1 TXINI=1
 	clear fifocon = send and switch bank
@@ -316,12 +337,12 @@ void UDD_ReleaseRX(void)
 	rxouti/killbank a clearer*/
 
 	//puts("=> UDD_ReleaseRX\r\n");
-	UOTGHS->UOTGHS_DEVEPTICR[ul_ep] = (UOTGHS_DEVEPTICR_NAKOUTIC | UOTGHS_DEVEPTICR_RXOUTIC);
-	UOTGHS->UOTGHS_DEVEPTIDR[ul_ep] = UOTGHS_DEVEPTIDR_FIFOCONC;
-	ul_recv_index = 0;
+	UOTGHS->UOTGHS_DEVEPTICR[ep] = (UOTGHS_DEVEPTICR_NAKOUTIC | UOTGHS_DEVEPTICR_RXOUTIC);
+	UOTGHS->UOTGHS_DEVEPTIDR[ep] = UOTGHS_DEVEPTIDR_FIFOCONC;
+	ul_recv_fifo_ptr[ep] = 0;
 }
 
-void UDD_ReleaseTX(void)
+void UDD_ReleaseTX(uint32_t ep)
 {
 /*	UEINTX = 0x3A;	// FIFOCON=0 NAKINI=0 RWAL=1 NAKOUTI=1 RXSTPI=1 RXOUTI=0 STALLEDI=1 TXINI=0
 	clear fifocon = send and switch bank
@@ -330,15 +351,15 @@ void UDD_ReleaseTX(void)
 	txini a clearer*/
 
 	//puts("=> UDD_ReleaseTX\r\n");
-	UOTGHS->UOTGHS_DEVEPTICR[ul_ep] = (UOTGHS_DEVEPTICR_NAKINIC | UOTGHS_DEVEPTICR_RXOUTIC | UOTGHS_DEVEPTICR_TXINIC);
-	UOTGHS->UOTGHS_DEVEPTIDR[ul_ep] = UOTGHS_DEVEPTIDR_FIFOCONC;
-	ul_send_index = 0;
+	UOTGHS->UOTGHS_DEVEPTICR[ep] = (UOTGHS_DEVEPTICR_NAKINIC | UOTGHS_DEVEPTICR_RXOUTIC | UOTGHS_DEVEPTICR_TXINIC);
+	UOTGHS->UOTGHS_DEVEPTIDR[ep] = UOTGHS_DEVEPTIDR_FIFOCONC;
+	ul_send_fifo_ptr[ep] = 0;
 }
 
 // Return true if the current bank is not full.
-uint32_t UDD_ReadWriteAllowed(void)
+uint32_t UDD_ReadWriteAllowed(uint32_t ep)
 {
-	return (UOTGHS->UOTGHS_DEVEPTISR[ul_ep] & UOTGHS_DEVEPTISR_RWALL);
+	return (UOTGHS->UOTGHS_DEVEPTISR[ep] & UOTGHS_DEVEPTISR_RWALL);
 }
 
 void UDD_SetAddress(uint32_t addr)