Merge branch 'master' into enhance

8 files changed, 438 insertions, 170 deletions

diff --git a/src/class/hid/hid_device.c b/src/class/hid/hid_device.c
index 2ee80750a..55071a5ce 100644
--- a/src/class/hid/hid_device.c
+++ b/src/class/hid/hid_device.c
@@ -282,7 +282,7 @@ bool hidd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t
           uint8_t const report_id   = tu_u16_low(request->wValue);
 
           uint8_t* report_buf = p_hid->epin_buf;
-          uint16_t req_len = request->wLength;
+          uint16_t req_len = tu_min16(request->wLength, CFG_TUD_HID_EP_BUFSIZE);
 
           uint16_t xferlen = 0;
 
@@ -314,7 +314,7 @@ bool hidd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t
           uint8_t const report_id   = tu_u16_low(request->wValue);
 
           uint8_t const* report_buf = p_hid->epout_buf;
-          uint16_t report_len = request->wLength;
+          uint16_t report_len = tu_min16(request->wLength, CFG_TUD_HID_EP_BUFSIZE);
 
           // If host request a specific Report ID, extract report ID in buffer before invoking callback
           if ( (report_id != HID_REPORT_TYPE_INVALID) && (report_len > 1) && (report_id == report_buf[0]) )
diff --git a/src/common/tusb_compiler.h b/src/common/tusb_compiler.h
index d3adbbc2d..3e85939b0 100644
--- a/src/common/tusb_compiler.h
+++ b/src/common/tusb_compiler.h
@@ -32,6 +32,7 @@
 #ifndef _TUSB_COMPILER_H_
 #define _TUSB_COMPILER_H_
 
+#define TU_TOKEN(x)           x
 #define TU_STRING(x)          #x                  ///< stringify without expand
 #define TU_XSTRING(x)         TU_STRING(x)        ///< expand then stringify
 
@@ -41,6 +42,8 @@
 #define TU_XSTRCAT(a, b)      TU_STRCAT(a, b)     ///< expand then concat
 #define TU_XSTRCAT3(a, b, c)  TU_STRCAT3(a, b, c) ///< expand then concat 3 tokens
 
+#define TU_INCLUDE_PATH(_dir,_file) TU_XSTRING( TU_TOKEN(_dir)TU_TOKEN(_file) )
+
 #if defined __COUNTER__ && __COUNTER__ != __COUNTER__
   #define _TU_COUNTER_ __COUNTER__
 #else
diff --git a/src/device/usbd.c b/src/device/usbd.c
index f7846cc0a..fda4a343e 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -986,9 +986,11 @@ static bool process_get_descriptor(uint8_t rhport, tusb_control_request_t const
 
       uint16_t len = sizeof(tusb_desc_device_t);
 
-      // Only send up to EP0 Packet Size if not addressed
+      // Only send up to EP0 Packet Size if not addressed and host requested more data
+      // that device descriptor has.
       // This only happens with the very first get device descriptor and EP0 size = 8 or 16.
-      if ((CFG_TUD_ENDPOINT0_SIZE < sizeof(tusb_desc_device_t)) && !_usbd_dev.addressed)
+      if ((CFG_TUD_ENDPOINT0_SIZE < sizeof(tusb_desc_device_t)) && !_usbd_dev.addressed &&
+          ((tusb_control_request_t*) p_request)->wLength > sizeof(tusb_desc_device_t))
       {
         len = CFG_TUD_ENDPOINT0_SIZE;
 
@@ -1056,6 +1058,7 @@ static bool process_get_descriptor(uint8_t rhport, tusb_control_request_t const
     break;
 
     case TUSB_DESC_DEVICE_QUALIFIER:
+    {
       TU_LOG2(" Device Qualifier\r\n");
 
       TU_VERIFY( tud_descriptor_device_qualifier_cb );
@@ -1065,6 +1068,7 @@ static bool process_get_descriptor(uint8_t rhport, tusb_control_request_t const
 
       // first byte of descriptor is its size
       return tud_control_xfer(rhport, p_request, (void*) desc_qualifier, desc_qualifier[0]);
+    }
     break;
 
     default: return false;
@@ -1392,7 +1396,13 @@ void usbd_edpt_close(uint8_t rhport, uint8_t ep_addr)
   TU_ASSERT(dcd_edpt_close, /**/);
   TU_LOG2("  CLOSING Endpoint: 0x%02X\r\n", ep_addr);
 
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir   = tu_edpt_dir(ep_addr);
+
   dcd_edpt_close(rhport, ep_addr);
+  _usbd_dev.ep_status[epnum][dir].stalled = false;
+  _usbd_dev.ep_status[epnum][dir].busy = false;
+  _usbd_dev.ep_status[epnum][dir].claimed = false;
 
   return;
 }
diff --git a/src/osal/osal_freertos.h b/src/osal/osal_freertos.h
index 66070c273..4573e01f5 100644
--- a/src/osal/osal_freertos.h
+++ b/src/osal/osal_freertos.h
@@ -28,10 +28,10 @@
 #define _TUSB_OSAL_FREERTOS_H_
 
 // FreeRTOS Headers
-#include "FreeRTOS.h"
-#include "semphr.h"
-#include "queue.h"
-#include "task.h"
+#include TU_INCLUDE_PATH(CFG_TUSB_OS_INC_PATH,FreeRTOS.h)
+#include TU_INCLUDE_PATH(CFG_TUSB_OS_INC_PATH,semphr.h)
+#include TU_INCLUDE_PATH(CFG_TUSB_OS_INC_PATH,queue.h)
+#include TU_INCLUDE_PATH(CFG_TUSB_OS_INC_PATH,task.h)
 
 #ifdef __cplusplus
 extern "C" {
diff --git a/src/portable/dialog/da146xx/dcd_da146xx.c b/src/portable/dialog/da146xx/dcd_da146xx.c
index 7b0d8f86c..07e0ddd9b 100644
--- a/src/portable/dialog/da146xx/dcd_da146xx.c
+++ b/src/portable/dialog/da146xx/dcd_da146xx.c
@@ -193,8 +193,14 @@ typedef struct
 #define REG_CLR_BIT(reg, field) USB->reg &= ~USB_ ## reg ## _ ## field ## _Msk
 #define REG_SET_VAL(reg, field, val) USB->reg = (USB->reg & ~USB_ ## reg ## _ ## field ## _Msk) | (val << USB_ ## reg ## _ ## field ## _Pos)
 
+static EPx_REGS * const ep_regs[EP_MAX] = {
+  EP_REGS(USB_EPC0_REG),
+  EP_REGS(USB_EPC1_REG),
+  EP_REGS(USB_EPC3_REG),
+  EP_REGS(USB_EPC5_REG),
+};
+
 typedef struct {
-  EPx_REGS * regs;
   uint8_t * buffer;
   // Total length of current transfer
   uint16_t total_len;
@@ -217,6 +223,7 @@ typedef struct {
 static struct
 {
   bool vbus_present;
+  bool init_called;
   bool in_reset;
   xfer_ctl_t xfer_status[EP_MAX][2];
   // Endpoints that use DMA, one for each direction
@@ -224,15 +231,16 @@ static struct
 } _dcd =
 {
   .vbus_present = false,
-  .xfer_status =
-  {
-    { { .regs = EP_REGS(USB_EPC0_REG) }, { .regs = EP_REGS(USB_EPC0_REG) } },
-    { { .regs = EP_REGS(USB_EPC1_REG) }, { .regs = EP_REGS(USB_EPC1_REG) } },
-    { { .regs = EP_REGS(USB_EPC3_REG) }, { .regs = EP_REGS(USB_EPC3_REG) } },
-    { { .regs = EP_REGS(USB_EPC5_REG) }, { .regs = EP_REGS(USB_EPC5_REG) } },
-  }
+  .init_called = false,
 };
 
+// Converts xfer pointer to epnum (0,1,2,3) regardless of xfer direction
+#define XFER_EPNUM(xfer)      ((xfer - &_dcd.xfer_status[0][0]) >> 1)
+// Converts xfer pinter to EPx_REGS pointer (returns same pointer for IN and OUT with same endpoint number)
+#define XFER_REGS(xfer)       ep_regs[XFER_EPNUM(xfer)]
+// Converts epnum (0,1,2,3) to EPx_REGS pointer
+#define EPNUM_REGS(epnum)     ep_regs[epnum]
+
 // Two endpoint 0 descriptor definition for unified dcd_edpt_open()
 static const tusb_desc_endpoint_t ep0OUT_desc =
 {
@@ -262,8 +270,8 @@ static void fill_tx_fifo(xfer_ctl_t * xfer)
 {
   int left_to_send;
   uint8_t const *src;
-  EPx_REGS *regs = xfer->regs;
   uint8_t const epnum = tu_edpt_number(xfer->ep_addr);
+  EPx_REGS *regs = EPNUM_REGS(epnum);
 
   src = &xfer->buffer[xfer->transferred];
   left_to_send = xfer->total_len - xfer->transferred;
@@ -291,7 +299,7 @@ static void fill_tx_fifo(xfer_ctl_t * xfer)
     }
     else
     {
-      xfer->regs->txc &= ~USB_USB_TXC1_REG_USB_TFWL_Msk;
+      regs->txc &= ~USB_USB_TXC1_REG_USB_TFWL_Msk;
       USB->USB_FWMSK_REG &= ~(1 << (epnum - 1 + USB_USB_FWMSK_REG_USB_M_TXWARN31_Pos));
       // Whole packet already in fifo, no need to refill it later.  Mark last.
       regs->txc |= USB_USB_TXC1_REG_USB_LAST_Msk;
@@ -332,30 +340,31 @@ static void start_rx_packet(xfer_ctl_t *xfer)
   uint8_t const epnum = tu_edpt_number(xfer->ep_addr);
   uint16_t remaining = xfer->total_len - xfer->transferred;
   uint16_t size = tu_min16(remaining, xfer->max_packet_size);
+  EPx_REGS *regs = XFER_REGS(xfer);
 
   xfer->last_packet_size = 0;
   if (xfer->max_packet_size > FIFO_SIZE && remaining > FIFO_SIZE)
   {
     if (try_allocate_dma(epnum, TUSB_DIR_OUT))
     {
-      start_rx_dma(&xfer->regs->rxd, xfer->buffer + xfer->transferred, size);
+      start_rx_dma(&regs->rxd, xfer->buffer + xfer->transferred, size);
     }
     else
     {
       // Other endpoint is using DMA in that direction, fall back to interrupts.
       // For endpoint size greater then FIFO size enable FIFO level warning interrupt
       // when FIFO has less then 17 bytes free.
-      xfer->regs->rxc |= USB_USB_RXC1_REG_USB_RFWL_Msk;
+      regs->rxc |= USB_USB_RXC1_REG_USB_RFWL_Msk;
       USB->USB_FWMSK_REG |= 1 << (epnum - 1 + USB_USB_FWMSK_REG_USB_M_RXWARN31_Pos);
     }
   }
   else if (epnum != 0)
   {
     // If max_packet_size would fit in FIFO no need for FIFO level warning interrupt.
-    xfer->regs->rxc &= ~USB_USB_RXC1_REG_USB_RFWL_Msk;
+    regs->rxc &= ~USB_USB_RXC1_REG_USB_RFWL_Msk;
     USB->USB_FWMSK_REG &= ~(1 << (epnum - 1 + USB_USB_FWMSK_REG_USB_M_RXWARN31_Pos));
   }
-  xfer->regs->rxc |= USB_USB_RXC1_REG_USB_RX_EN_Msk;
+  regs->rxc |= USB_USB_RXC1_REG_USB_RX_EN_Msk;
 }
 
 static void start_tx_dma(void *src, volatile void *dst, uint16_t size)
@@ -374,13 +383,13 @@ static void start_tx_packet(xfer_ctl_t *xfer)
   uint8_t const epnum = tu_edpt_number(xfer->ep_addr);
   uint16_t remaining = xfer->total_len - xfer->transferred;
   uint16_t size = tu_min16(remaining, xfer->max_packet_size);
-  EPx_REGS *regs = xfer->regs;
+  EPx_REGS *regs = EPNUM_REGS(epnum);
 
   xfer->last_packet_size = 0;
 
   regs->txc = USB_USB_TXC1_REG_USB_FLUSH_Msk;
   regs->txc = USB_USB_TXC1_REG_USB_IGN_ISOMSK_Msk;
-  if (xfer->data1) xfer->regs->txc |= USB_USB_TXC1_REG_USB_TOGGLE_TX_Msk;
+  if (xfer->data1) regs->txc |= USB_USB_TXC1_REG_USB_TOGGLE_TX_Msk;
 
   if (xfer->max_packet_size > FIFO_SIZE && remaining > FIFO_SIZE && try_allocate_dma(epnum, TUSB_DIR_IN))
   {
@@ -397,7 +406,7 @@ static void start_tx_packet(xfer_ctl_t *xfer)
 
 static void read_rx_fifo(xfer_ctl_t *xfer, uint16_t bytes_in_fifo)
 {
-  EPx_REGS *regs = xfer->regs;
+  EPx_REGS *regs = XFER_REGS(xfer);
   uint16_t remaining = xfer->total_len - xfer->transferred - xfer->last_packet_size;
   uint16_t receive_this_time = bytes_in_fifo;
 
@@ -467,7 +476,7 @@ static void handle_ep0_tx(void)
 {
   uint32_t txs0;
   xfer_ctl_t *xfer = XFER_CTL_BASE(0, TUSB_DIR_IN);
-  EPx_REGS *regs = xfer->regs;
+  EPx_REGS *regs = XFER_REGS(xfer);
 
   txs0 = regs->USB_TXS0_REG;
 
@@ -501,7 +510,7 @@ static void handle_epx_rx_ev(uint8_t ep)
   int fifo_bytes;
   xfer_ctl_t *xfer = XFER_CTL_BASE(ep, TUSB_DIR_OUT);
 
-  EPx_REGS *regs = xfer->regs;
+  EPx_REGS *regs = EPNUM_REGS(ep);
 
   do
   {
@@ -580,7 +589,7 @@ static void handle_epx_tx_ev(xfer_ctl_t *xfer)
 {
   uint8_t const epnum = tu_edpt_number(xfer->ep_addr);
   uint32_t txs;
-  EPx_REGS *regs = xfer->regs;
+  EPx_REGS *regs = EPNUM_REGS(epnum);
 
   txs = regs->txs;
 
@@ -735,19 +744,13 @@ static void handle_ep0_nak(void)
  *------------------------------------------------------------------*/
 void dcd_init(uint8_t rhport)
 {
-  USB->USB_MCTRL_REG = USB_USB_MCTRL_REG_USBEN_Msk;
-  USB->USB_NFSR_REG = 0;
-  USB->USB_FAR_REG = 0x80;
-  USB->USB_NFSR_REG = NFSR_NODE_RESET;
-  USB->USB_TXMSK_REG = 0;
-  USB->USB_RXMSK_REG = 0;
-
-  USB->USB_MAMSK_REG = USB_USB_MAMSK_REG_USB_M_INTR_Msk |
-                       USB_USB_MAMSK_REG_USB_M_ALT_Msk |
-                       USB_USB_MAMSK_REG_USB_M_WARN_Msk;
-  USB->USB_ALTMSK_REG = USB_USB_ALTMSK_REG_USB_M_RESET_Msk;
+  (void) rhport;
 
-  dcd_connect(rhport);
+  _dcd.init_called = true;
+  if (_dcd.vbus_present)
+  {
+    dcd_connect(rhport);
+  }
 }
 
 void dcd_int_enable(uint8_t rhport)
@@ -783,10 +786,25 @@ void dcd_connect(uint8_t rhport)
 {
   (void)rhport;
 
-  REG_SET_BIT(USB_MCTRL_REG, USB_NAT);
+  if (GET_BIT(USB->USB_MCTRL_REG, USB_USB_MCTRL_REG_USB_NAT) == 0)
+  {
+    USB->USB_MCTRL_REG = USB_USB_MCTRL_REG_USBEN_Msk;
+    USB->USB_NFSR_REG = 0;
+    USB->USB_FAR_REG = 0x80;
+    USB->USB_NFSR_REG = NFSR_NODE_RESET;
+    USB->USB_TXMSK_REG = 0;
+    USB->USB_RXMSK_REG = 0;
+
+    USB->USB_MAMSK_REG = USB_USB_MAMSK_REG_USB_M_INTR_Msk |
+                         USB_USB_MAMSK_REG_USB_M_ALT_Msk |
+                         USB_USB_MAMSK_REG_USB_M_WARN_Msk;
+    USB->USB_ALTMSK_REG = USB_USB_ALTMSK_REG_USB_M_RESET_Msk;
 
-  // Select chosen DMA to be triggered by USB.
-  DMA->DMA_REQ_MUX_REG = (DMA->DMA_REQ_MUX_REG & ~DA146XX_DMA_USB_MUX_MASK) | DA146XX_DMA_USB_MUX;
+    REG_SET_BIT(USB_MCTRL_REG, USB_NAT);
+
+    // Select chosen DMA to be triggered by USB.
+    DMA->DMA_REQ_MUX_REG = (DMA->DMA_REQ_MUX_REG & ~DA146XX_DMA_USB_MUX_MASK) | DA146XX_DMA_USB_MUX;
+  }
 }
 
 void dcd_disconnect(uint8_t rhport)
@@ -796,6 +814,30 @@ void dcd_disconnect(uint8_t rhport)
   REG_CLR_BIT(USB_MCTRL_REG, USB_NAT);
 }
 
+TU_ATTR_ALWAYS_INLINE static inline bool is_in_isr(void)
+{
+  return (SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk) != 0;
+}
+
+void tusb_vbus_changed(bool present)
+{
+  if (present && !_dcd.vbus_present)
+  {
+    _dcd.vbus_present = true;
+    // If power event happened before USB started, delay dcd_connect
+    // until dcd_init is called.
+    if (_dcd.init_called)
+    {
+      dcd_connect(0);
+    }
+  }
+  else if (!present && _dcd.vbus_present)
+  {
+    _dcd.vbus_present = false;
+    USB->USB_MCTRL_REG = 0;
+    dcd_event_bus_signal(0, DCD_EVENT_UNPLUGGED, is_in_isr());
+  }
+}
 
 /*------------------------------------------------------------------*/
 /* DCD Endpoint port
@@ -808,6 +850,7 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
   uint8_t const epnum = tu_edpt_number(desc_edpt->bEndpointAddress);
   uint8_t const dir   = tu_edpt_dir(desc_edpt->bEndpointAddress);
   xfer_ctl_t * xfer = XFER_CTL_BASE(epnum, dir);
+  EPx_REGS *regs = EPNUM_REGS(epnum);
   uint8_t iso_mask = 0;
 
   TU_ASSERT(epnum < EP_MAX);
@@ -832,13 +875,13 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
   {
     if (dir == TUSB_DIR_OUT)
     {
-      xfer->regs->epc_out = epnum | USB_USB_EPC1_REG_USB_EP_EN_Msk | iso_mask;
+      regs->epc_out = epnum | USB_USB_EPC1_REG_USB_EP_EN_Msk | iso_mask;
       USB->USB_RXMSK_REG |= 0x101 << (epnum - 1);
       REG_SET_BIT(USB_MAMSK_REG, USB_M_RX_EV);
     }
     else
     {
-      xfer->regs->epc_in = epnum | USB_USB_EPC1_REG_USB_EP_EN_Msk | iso_mask;
+      regs->epc_in = epnum | USB_USB_EPC1_REG_USB_EP_EN_Msk | iso_mask;
       USB->USB_TXMSK_REG |= 0x101 << (epnum - 1);
       REG_SET_BIT(USB_MAMSK_REG, USB_M_TX_EV);
     }
@@ -850,13 +893,19 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
 void dcd_edpt_close_all (uint8_t rhport)
 {
   (void) rhport;
-  // TODO implement dcd_edpt_close_all()
+
+  for (int epnum = 1; epnum < EP_MAX; ++epnum)
+  {
+    dcd_edpt_close(0, epnum | TUSB_DIR_OUT);
+    dcd_edpt_close(0, epnum | TUSB_DIR_IN);
+  }
 }
 
 void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr)
 {
   uint8_t const epnum = tu_edpt_number(ep_addr);
   uint8_t const dir   = tu_edpt_dir(ep_addr);
+  EPx_REGS *regs = EPNUM_REGS(epnum);
   xfer_ctl_t * xfer = XFER_CTL_BASE(epnum, dir);
 
   (void)rhport;
@@ -872,8 +921,8 @@ void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr)
   {
     if (dir == TUSB_DIR_OUT)
     {
-      xfer->regs->rxc = USB_USB_RXC1_REG_USB_FLUSH_Msk;
-      xfer->regs->epc_out = 0;
+      regs->rxc = USB_USB_RXC1_REG_USB_FLUSH_Msk;
+      regs->epc_out = 0;
       USB->USB_RXMSK_REG &= ~(0x101 << (epnum - 1));
       // Release DMA if needed
       if (_dcd.dma_ep[TUSB_DIR_OUT] == epnum)
@@ -884,8 +933,8 @@ void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr)
     }
     else
     {
-      xfer->regs->txc = USB_USB_TXC1_REG_USB_FLUSH_Msk;
-      xfer->regs->epc_in = 0;
+      regs->txc = USB_USB_TXC1_REG_USB_FLUSH_Msk;
+      regs->epc_in = 0;
       USB->USB_TXMSK_REG &= ~(0x101 << (epnum - 1));
       // Release DMA if needed
       if (_dcd.dma_ep[TUSB_DIR_IN] == epnum)
@@ -895,6 +944,7 @@ void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr)
       }
     }
   }
+  tu_memclr(xfer, sizeof(*xfer));
 }
 
 bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
@@ -930,6 +980,7 @@ void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
   (void)rhport;
 
   xfer_ctl_t * xfer = XFER_CTL_BASE(epnum, dir);
+  EPx_REGS *regs = EPNUM_REGS(epnum);
   xfer->stall = 1;
 
   if (epnum == 0)
@@ -938,11 +989,11 @@ void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
     REG_SET_BIT(USB_EPC0_REG, USB_STALL);
     if (dir == TUSB_DIR_OUT)
     {
-      xfer->regs->USB_RXC0_REG = USB_USB_RXC0_REG_USB_RX_EN_Msk;
+      regs->USB_RXC0_REG = USB_USB_RXC0_REG_USB_RX_EN_Msk;
     }
     else
     {
-      if (xfer->regs->USB_RXC0_REG & USB_USB_RXC0_REG_USB_RX_EN_Msk)
+      if (regs->USB_RXC0_REG & USB_USB_RXC0_REG_USB_RX_EN_Msk)
       {
         // If RX is also enabled TX will not be stalled since RX has
         // higher priority. Enable NAK interrupt to handle stall.
@@ -950,7 +1001,7 @@ void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
       }
       else
       {
-        xfer->regs->USB_TXC0_REG |= USB_USB_TXC0_REG_USB_TX_EN_Msk;
+        regs->USB_TXC0_REG |= USB_USB_TXC0_REG_USB_TX_EN_Msk;
       }
     }
   }
@@ -958,13 +1009,13 @@ void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
   {
     if (dir == TUSB_DIR_OUT)
     {
-      xfer->regs->epc_out |= USB_USB_EPC1_REG_USB_STALL_Msk;
-      xfer->regs->rxc |= USB_USB_RXC1_REG_USB_RX_EN_Msk;
+      regs->epc_out |= USB_USB_EPC1_REG_USB_STALL_Msk;
+      regs->rxc |= USB_USB_RXC1_REG_USB_RX_EN_Msk;
     }
     else
     {
-      xfer->regs->epc_in |= USB_USB_EPC1_REG_USB_STALL_Msk;
-      xfer->regs->txc |= USB_USB_TXC1_REG_USB_TX_EN_Msk | USB_USB_TXC1_REG_USB_LAST_Msk;
+      regs->epc_in |= USB_USB_EPC1_REG_USB_STALL_Msk;
+      regs->txc |= USB_USB_TXC1_REG_USB_TX_EN_Msk | USB_USB_TXC1_REG_USB_LAST_Msk;
     }
   }
 }
@@ -977,6 +1028,7 @@ void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
   (void)rhport;
 
   xfer_ctl_t * xfer = XFER_CTL_BASE(epnum, dir);
+  EPx_REGS *regs = EPNUM_REGS(epnum);
 
   // Clear stall is called in response to Clear Feature ENDPOINT_HALT, reset toggle
   xfer->data1 = 0;
@@ -984,11 +1036,11 @@ void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
 
   if (dir == TUSB_DIR_OUT)
   {
-    xfer->regs->epc_out &= ~USB_USB_EPC1_REG_USB_STALL_Msk;
+    regs->epc_out &= ~USB_USB_EPC1_REG_USB_STALL_Msk;
   }
   else
   {
-    xfer->regs->epc_in &= ~USB_USB_EPC1_REG_USB_STALL_Msk;
+    regs->epc_in &= ~USB_USB_EPC1_REG_USB_STALL_Msk;
   }
   if (epnum == 0)
   {
diff --git a/src/portable/nxp/khci/dcd_khci.c b/src/portable/nxp/khci/dcd_khci.c
index 1f5f4e5e1..ff1997440 100644
--- a/src/portable/nxp/khci/dcd_khci.c
+++ b/src/portable/nxp/khci/dcd_khci.c
@@ -52,23 +52,23 @@ typedef struct TU_ATTR_PACKED
     struct {
       union {
         struct {
-          uint16_t          :  2;
-          uint16_t tok_pid  :  4;
-          uint16_t data     :  1;
-          uint16_t own      :  1;
-          uint16_t          :  8;
+               uint16_t           :  2;
+          __IO uint16_t tok_pid   :  4;
+               uint16_t data      :  1;
+          __IO uint16_t own       :  1;
+               uint16_t           :  8;
         };
         struct {
-          uint16_t          :  2;
-          uint16_t bdt_stall:  1;
-          uint16_t dts      :  1;
-          uint16_t ninc     :  1;
-          uint16_t keep     :  1;
-          uint16_t          : 10;
+               uint16_t           :  2;
+               uint16_t bdt_stall :  1;
+               uint16_t dts       :  1;
+               uint16_t ninc      :  1;
+               uint16_t keep      :  1;
+               uint16_t           : 10;
         };
       };
-      uint16_t bc          : 10;
-      uint16_t             :  6;
+      __IO uint16_t bc : 10;
+           uint16_t    :  6;
     };
   };
   uint8_t *addr;
@@ -120,10 +120,8 @@ static void prepare_next_setup_packet(uint8_t rhport)
 {
   const unsigned out_odd = _dcd.endpoint[0][0].odd;
   const unsigned in_odd  = _dcd.endpoint[0][1].odd;
-  if (_dcd.bdt[0][0][out_odd].own) {
-    TU_LOG1("DCD fail to prepare the next SETUP %d %d\r\n", out_odd, in_odd);
-    return;
-  }
+  TU_ASSERT(0 == _dcd.bdt[0][0][out_odd].own, );
+
   _dcd.bdt[0][0][out_odd].data     = 0;
   _dcd.bdt[0][0][out_odd ^ 1].data = 1;
   _dcd.bdt[0][1][in_odd].data      = 1;
@@ -134,10 +132,16 @@ static void prepare_next_setup_packet(uint8_t rhport)
 
 static void process_stall(uint8_t rhport)
 {
-  if (KHCI->ENDPOINT[0].ENDPT & USB_ENDPT_EPSTALL_MASK) {
-    /* clear stall condition of the control pipe */
-    prepare_next_setup_packet(rhport);
-    KHCI->ENDPOINT[0].ENDPT &= ~USB_ENDPT_EPSTALL_MASK;
+  for (int i = 0; i < 16; ++i) {
+    unsigned const endpt = KHCI->ENDPOINT[i].ENDPT;
+
+    if (endpt & USB_ENDPT_EPSTALL_MASK) {
+      // prepare next setup if endpoint0
+      if ( i == 0 ) prepare_next_setup_packet(rhport);
+
+      // clear stall bit
+      KHCI->ENDPOINT[i].ENDPT = endpt & ~USB_ENDPT_EPSTALL_MASK;
+    }
   }
 }
 
@@ -145,12 +149,17 @@ static void process_tokdne(uint8_t rhport)
 {
   const unsigned s = KHCI->STAT;
   KHCI->ISTAT = USB_ISTAT_TOKDNE_MASK; /* fetch the next token if received */
+
+  uint8_t const epnum = (s >> USB_STAT_ENDP_SHIFT);
+  uint8_t const dir   = (s & USB_STAT_TX_MASK) >> USB_STAT_TX_SHIFT;
+  unsigned const odd  = (s & USB_STAT_ODD_MASK) ? 1 : 0;
+
   buffer_descriptor_t *bd = (buffer_descriptor_t *)&_dcd.bda[s];
   endpoint_state_t    *ep = &_dcd.endpoint_unified[s >> 3];
-  unsigned odd = (s & USB_STAT_ODD_MASK) ? 1 : 0;
 
   /* fetch pid before discarded by the next steps */
   const unsigned pid = bd->tok_pid;
+
   /* reset values for a next transfer */
   bd->bdt_stall = 0;
   bd->dts       = 1;
@@ -163,9 +172,6 @@ static void process_tokdne(uint8_t rhport)
     KHCI->CTL &= ~USB_CTL_TXSUSPENDTOKENBUSY_MASK;
     return;
   }
-  if (s >> 4) {
-    TU_LOG1("TKDNE %x\r\n", s);
-  }
 
   const unsigned bc = bd->bc;
   const unsigned remaining = ep->remaining - bc;
@@ -184,9 +190,9 @@ static void process_tokdne(uint8_t rhport)
   }
   const unsigned length = ep->length;
   dcd_event_xfer_complete(rhport,
-                          ((s & USB_STAT_TX_MASK) << 4) | (s >> USB_STAT_ENDP_SHIFT),
+                          tu_edpt_addr(epnum, dir),
                           length - remaining, XFER_RESULT_SUCCESS, true);
-  if (0 == (s & USB_STAT_ENDP_MASK) && 0 == length) {
+  if (0 == epnum && 0 == length) {
     /* After completion a ZLP of control transfer,
      * it prepares for the next steup transfer. */
     if (_dcd.addr) {
@@ -204,7 +210,8 @@ static void process_bus_reset(uint8_t rhport)
   KHCI->USBCTRL &= ~USB_USBCTRL_SUSP_MASK;
   KHCI->CTL     |= USB_CTL_ODDRST_MASK;
   KHCI->ADDR     = 0;
-  KHCI->INTEN    = (KHCI->INTEN & ~USB_INTEN_RESUMEEN_MASK) | USB_INTEN_SLEEPEN_MASK;
+  KHCI->INTEN    = USB_INTEN_USBRSTEN_MASK | USB_INTEN_TOKDNEEN_MASK | USB_INTEN_SLEEPEN_MASK |
+                   USB_INTEN_ERROREN_MASK  | USB_INTEN_STALLEN_MASK;
 
   KHCI->ENDPOINT[0].ENDPT = USB_ENDPT_EPHSHK_MASK | USB_ENDPT_EPRXEN_MASK | USB_ENDPT_EPTXEN_MASK;
   for (unsigned i = 1; i < 16; ++i) {
@@ -229,21 +236,27 @@ static void process_bus_reset(uint8_t rhport)
   dcd_event_bus_reset(rhport, TUSB_SPEED_FULL, true);
 }
 
-static void process_bus_inactive(uint8_t rhport)
+static void process_bus_sleep(uint8_t rhport)
 {
-  (void) rhport;
+  // Enable resume & disable suspend interrupt
   const unsigned inten = KHCI->INTEN;
+
   KHCI->INTEN    = (inten & ~USB_INTEN_SLEEPEN_MASK) | USB_INTEN_RESUMEEN_MASK;
+  KHCI->USBTRC0 |= USB_USBTRC0_USBRESMEN_MASK;
   KHCI->USBCTRL |= USB_USBCTRL_SUSP_MASK;
+
   dcd_event_bus_signal(rhport, DCD_EVENT_SUSPEND, true);
 }
 
-static void process_bus_active(uint8_t rhport)
+static void process_bus_resume(uint8_t rhport)
 {
-  (void) rhport;
-  KHCI->USBCTRL &= ~USB_USBCTRL_SUSP_MASK;
+  // Enable suspend & disable resume interrupt
   const unsigned inten = KHCI->INTEN;
+
+  KHCI->USBCTRL &= ~USB_USBCTRL_SUSP_MASK; // will also clear USB_USBTRC0_USB_RESUME_INT_MASK
+  KHCI->USBTRC0 &= ~USB_USBTRC0_USBRESMEN_MASK;
   KHCI->INTEN    = (inten & ~USB_INTEN_RESUMEEN_MASK) | USB_INTEN_SLEEPEN_MASK;
+
   dcd_event_bus_signal(rhport, DCD_EVENT_RESUME, true);
 }
 
@@ -256,12 +269,15 @@ void dcd_init(uint8_t rhport)
 
   KHCI->USBTRC0 |= USB_USBTRC0_USBRESET_MASK;
   while (KHCI->USBTRC0 & USB_USBTRC0_USBRESET_MASK);
+
   tu_memclr(&_dcd, sizeof(_dcd));
   KHCI->USBTRC0 |= TU_BIT(6); /* software must set this bit to 1 */
   KHCI->BDTPAGE1 = (uint8_t)((uintptr_t)_dcd.bdt >>  8);
   KHCI->BDTPAGE2 = (uint8_t)((uintptr_t)_dcd.bdt >> 16);
   KHCI->BDTPAGE3 = (uint8_t)((uintptr_t)_dcd.bdt >> 24);
 
+  KHCI->INTEN = USB_INTEN_USBRSTEN_MASK;
+
   dcd_connect(rhport);
   NVIC_ClearPendingIRQ(USB0_IRQn);
 }
@@ -269,8 +285,6 @@ void dcd_init(uint8_t rhport)
 void dcd_int_enable(uint8_t rhport)
 {
   (void) rhport;
-  KHCI->INTEN = USB_INTEN_USBRSTEN_MASK | USB_INTEN_TOKDNEEN_MASK |
-    USB_INTEN_SLEEPEN_MASK | USB_INTEN_ERROREN_MASK | USB_INTEN_STALLEN_MASK;
   NVIC_EnableIRQ(USB0_IRQn);
 }
 
@@ -278,13 +292,11 @@ void dcd_int_disable(uint8_t rhport)
 {
   (void) rhport;
   NVIC_DisableIRQ(USB0_IRQn);
-  KHCI->INTEN = 0;
 }
 
 void dcd_set_address(uint8_t rhport, uint8_t dev_addr)
 {
-  (void) rhport;
-  _dcd.addr = dev_addr & 0x7F;
+  _dcd.addr = dev_addr & 0x7F; 
   /* Response with status first before changing device address */
   dcd_edpt_xfer(rhport, tu_edpt_addr(0, TUSB_DIR_IN), NULL, 0);
 }
@@ -292,9 +304,12 @@ void dcd_set_address(uint8_t rhport, uint8_t dev_addr)
 void dcd_remote_wakeup(uint8_t rhport)
 {
   (void) rhport;
-  unsigned cnt = SystemCoreClock / 100;
+
   KHCI->CTL |= USB_CTL_RESUME_MASK;
+
+  unsigned cnt = SystemCoreClock / 1000;
   while (cnt--) __NOP();
+
   KHCI->CTL &= ~USB_CTL_RESUME_MASK;
 }
 
@@ -321,12 +336,12 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * ep_desc)
   (void) rhport;
 
   const unsigned ep_addr  = ep_desc->bEndpointAddress;
-  const unsigned epn      = ep_addr & 0xFu;
-  const unsigned dir      = (ep_addr & TUSB_DIR_IN_MASK) ? TUSB_DIR_IN : TUSB_DIR_OUT;
+  const unsigned epn      = tu_edpt_number(ep_addr);
+  const unsigned dir      = tu_edpt_dir(ep_addr);
   const unsigned xfer     = ep_desc->bmAttributes.xfer;
   endpoint_state_t *ep    = &_dcd.endpoint[epn][dir];
   const unsigned odd      = ep->odd;
-  buffer_descriptor_t *bd = &_dcd.bdt[epn][dir][0];
+  buffer_descriptor_t *bd = _dcd.bdt[epn][dir];
 
   /* No support for control transfer */
   TU_ASSERT(epn && (xfer != TUSB_XFER_CONTROL));
@@ -347,41 +362,60 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * ep_desc)
   return true;
 }
 
-void dcd_edpt_close_all (uint8_t rhport)
+void dcd_edpt_close_all(uint8_t rhport)
 {
   (void) rhport;
-  // TODO implement dcd_edpt_close_all()
+  const unsigned ie = NVIC_GetEnableIRQ(USB0_IRQn);
+  NVIC_DisableIRQ(USB0_IRQn);
+  for (unsigned i = 1; i < 16; ++i) {
+    KHCI->ENDPOINT[i].ENDPT = 0;
+  }
+  if (ie) NVIC_EnableIRQ(USB0_IRQn);
+  buffer_descriptor_t *bd = _dcd.bdt[1][0];
+  for (unsigned i = 2; i < sizeof(_dcd.bdt)/sizeof(*bd); ++i, ++bd) {
+    bd->head = 0;
+  }
+  endpoint_state_t *ep = &_dcd.endpoint[1][0];
+  for (unsigned i = 2; i < sizeof(_dcd.endpoint)/sizeof(*ep); ++i, ++ep) {
+    /* Clear except the odd */
+    ep->max_packet_size = 0;
+    ep->length          = 0;
+    ep->remaining       = 0;
+  }
 }
 
 void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr)
 {
   (void) rhport;
 
-  const unsigned epn      = ep_addr & 0xFu;
-  const unsigned dir      = (ep_addr & TUSB_DIR_IN_MASK) ? TUSB_DIR_IN : TUSB_DIR_OUT;
+  const unsigned epn      = tu_edpt_number(ep_addr);
+  const unsigned dir      = tu_edpt_dir(ep_addr);
   endpoint_state_t *ep    = &_dcd.endpoint[epn][dir];
-  buffer_descriptor_t *bd = &_dcd.bdt[epn][dir][0];
+  buffer_descriptor_t *bd = _dcd.bdt[epn][dir];
   const unsigned msk      = dir ? USB_ENDPT_EPTXEN_MASK : USB_ENDPT_EPRXEN_MASK;
+  const unsigned ie       = NVIC_GetEnableIRQ(USB0_IRQn);
+  NVIC_DisableIRQ(USB0_IRQn);
   KHCI->ENDPOINT[epn].ENDPT &= ~msk;
   ep->max_packet_size = 0;
   ep->length          = 0;
   ep->remaining       = 0;
-  bd->head            = 0;
+  bd[0].head          = 0;
+  bd[1].head          = 0;
+  if (ie) NVIC_EnableIRQ(USB0_IRQn);
 }
 
 bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t* buffer, uint16_t total_bytes)
 {
   (void) rhport;
-  NVIC_DisableIRQ(USB0_IRQn);
-  const unsigned epn = ep_addr & 0xFu;
-  const unsigned dir = (ep_addr & TUSB_DIR_IN_MASK) ? TUSB_DIR_IN : TUSB_DIR_OUT;
+  const unsigned epn      = tu_edpt_number(ep_addr);
+  const unsigned dir      = tu_edpt_dir(ep_addr);
   endpoint_state_t    *ep = &_dcd.endpoint[epn][dir];
   buffer_descriptor_t *bd = &_dcd.bdt[epn][dir][ep->odd];
+  TU_ASSERT(0 == bd->own);
+
+  const unsigned ie = NVIC_GetEnableIRQ(USB0_IRQn);
+  NVIC_DisableIRQ(USB0_IRQn);
 
-  if (bd->own) {
-    TU_LOG1("DCD XFER fail %x %d %lx %lx\r\n", ep_addr, total_bytes, ep->state, bd->head);
-    return false; /* The last transfer has not completed */
-  }
   ep->length    = total_bytes;
   ep->remaining = total_bytes;
 
@@ -394,42 +428,69 @@ bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t* buffer, uint16_t to
     next->addr = buffer + mps;
     next->own  = 1;
   }
-  bd->bc        = total_bytes >= mps ? mps: total_bytes;
-  bd->addr      = buffer;
+  bd->bc   = total_bytes >= mps ? mps: total_bytes;
+  bd->addr = buffer;
   __DSB();
-  bd->own  = 1; /* the own bit must set after addr */
-  NVIC_EnableIRQ(USB0_IRQn);
+  bd->own  = 1; /* This bit must be set last */
+
+  if (ie) NVIC_EnableIRQ(USB0_IRQn);
   return true;
 }
 
 void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr)
 {
   (void) rhport;
-  const unsigned epn = ep_addr & 0xFu;
+  const unsigned epn = tu_edpt_number(ep_addr);
+
   if (0 == epn) {
     KHCI->ENDPOINT[epn].ENDPT |=  USB_ENDPT_EPSTALL_MASK;
   } else {
-    const unsigned dir      = (ep_addr & TUSB_DIR_IN_MASK) ? TUSB_DIR_IN : TUSB_DIR_OUT;
-    buffer_descriptor_t *bd = _dcd.bdt[epn][dir];
-    bd[0].bdt_stall = 1;
-    bd[1].bdt_stall = 1;
+    const unsigned dir      = tu_edpt_dir(ep_addr);
+    const unsigned odd      = _dcd.endpoint[epn][dir].odd;
+    buffer_descriptor_t *bd = &_dcd.bdt[epn][dir][odd];
+    TU_ASSERT(0 == bd->own,);
+
+    const unsigned ie       = NVIC_GetEnableIRQ(USB0_IRQn);
+    NVIC_DisableIRQ(USB0_IRQn);
+
+    bd->bdt_stall = 1;
+    __DSB();
+    bd->own       = 1; /* This bit must be set last */
+
+    if (ie) NVIC_EnableIRQ(USB0_IRQn);
   }
 }
 
 void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
 {
   (void) rhport;
-  const unsigned epn      = ep_addr & 0xFu;
-  const unsigned dir      = (ep_addr & TUSB_DIR_IN_MASK) ? TUSB_DIR_IN : TUSB_DIR_OUT;
+  const unsigned epn      = tu_edpt_number(ep_addr);
+  TU_VERIFY(epn,);
+  const unsigned dir      = tu_edpt_dir(ep_addr);
   const unsigned odd      = _dcd.endpoint[epn][dir].odd;
   buffer_descriptor_t *bd = _dcd.bdt[epn][dir];
+  TU_VERIFY(bd[odd].own,);
+
+  const unsigned ie       = NVIC_GetEnableIRQ(USB0_IRQn);
+  NVIC_DisableIRQ(USB0_IRQn);
+
+  bd[odd].own = 0;
+  __DSB();
+
+  // clear stall
+  bd[odd].bdt_stall  = 0;
+
+  // Reset data toggle
+  bd[odd    ].data = 0;
+  bd[odd ^ 1].data = 1;
 
-  bd[odd ^ 1].own       = 0;
-  bd[odd ^ 1].data      = 1;
-  bd[odd ^ 1].bdt_stall = 0;
-  bd[odd].own           = 0;
-  bd[odd].data          = 0;
-  bd[odd].bdt_stall     = 0;
+  // We already cleared this in ISR, but just clear it here to be safe
+  const unsigned endpt = KHCI->ENDPOINT[epn].ENDPT;
+  if (endpt & USB_ENDPT_EPSTALL_MASK) {
+    KHCI->ENDPOINT[epn].ENDPT = endpt & ~USB_ENDPT_EPSTALL_MASK;
+  }
+
+  if (ie) NVIC_EnableIRQ(USB0_IRQn);
 }
 
 //--------------------------------------------------------------------+
@@ -437,48 +498,59 @@ void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
 //--------------------------------------------------------------------+
 void dcd_int_handler(uint8_t rhport)
 {
-  (void) rhport;
-
   uint32_t is  = KHCI->ISTAT;
   uint32_t msk = KHCI->INTEN;
+
+  // clear non-enabled interrupts
   KHCI->ISTAT = is & ~msk;
   is &= msk;
+
   if (is & USB_ISTAT_ERROR_MASK) {
     /* TODO: */
     uint32_t es = KHCI->ERRSTAT;
     KHCI->ERRSTAT = es;
     KHCI->ISTAT   = is; /* discard any pending events */
-    return;
   }
 
   if (is & USB_ISTAT_USBRST_MASK) {
     KHCI->ISTAT = is; /* discard any pending events */
     process_bus_reset(rhport);
-    return;
   }
+
   if (is & USB_ISTAT_SLEEP_MASK) {
+    // TU_LOG2("Suspend: "); TU_LOG2_HEX(is);
+
+    // Note Host usually has extra delay after bus reset (without SOF), which could falsely 
+    // detected as Sleep event. Though usbd has debouncing logic so we are good
     KHCI->ISTAT = USB_ISTAT_SLEEP_MASK;
-    process_bus_inactive(rhport);
-    return;
+    process_bus_sleep(rhport);
   }
+
+#if 0 // ISTAT_RESUME never trigger, probably for host mode ?
   if (is & USB_ISTAT_RESUME_MASK) {
+    // TU_LOG2("ISTAT Resume: "); TU_LOG2_HEX(is);
     KHCI->ISTAT = USB_ISTAT_RESUME_MASK;
-    process_bus_active(rhport);
-    return;
+    process_bus_resume(rhport);
   }
+#endif
+
+  if (KHCI->USBTRC0 & USB_USBTRC0_USB_RESUME_INT_MASK) {
+     // TU_LOG2("USBTRC0 Resume: "); TU_LOG2_HEX(is); TU_LOG2_HEX(KHCI->USBTRC0);
+    process_bus_resume(rhport);
+  }
+
   if (is & USB_ISTAT_SOFTOK_MASK) {
     KHCI->ISTAT = USB_ISTAT_SOFTOK_MASK;
     dcd_event_bus_signal(rhport, DCD_EVENT_SOF, true);
-    return;
   }
+
   if (is & USB_ISTAT_STALL_MASK) {
     KHCI->ISTAT = USB_ISTAT_STALL_MASK;
     process_stall(rhport);
-    return;
   }
+
   if (is & USB_ISTAT_TOKDNE_MASK) {
     process_tokdne(rhport);
-    return;
   }
 }
 
diff --git a/src/portable/nxp/transdimension/dcd_transdimension.c b/src/portable/nxp/transdimension/dcd_transdimension.c
index 07009ff50..a7c4545c2 100644
--- a/src/portable/nxp/transdimension/dcd_transdimension.c
+++ b/src/portable/nxp/transdimension/dcd_transdimension.c
@@ -134,7 +134,8 @@ typedef struct
   // QHD is 64 bytes aligned but occupies only 48 bytes
   // Therefore there are 16 bytes padding that we can use.
   //--------------------------------------------------------------------+
-  uint8_t reserved[16];
+  tu_fifo_t * ff;
+  uint8_t reserved[12];
 } dcd_qhd_t;
 
 TU_VERIFY_STATIC( sizeof(dcd_qhd_t) == 64, "size is not correct");
@@ -240,8 +241,9 @@ void dcd_init(uint8_t rhport)
   dcd_reg->USBMODE = USBMODE_CM_DEVICE;
   dcd_reg->OTGSC = OTGSC_VBUS_DISCHARGE | OTGSC_OTG_TERMINATION;
 
-  // TODO Force fullspeed on non-highspeed port
-  // dcd_reg->PORTSC1 = PORTSC1_FORCE_FULL_SPEED;
+#if !TUD_OPT_HIGH_SPEED
+  dcd_reg->PORTSC1 = PORTSC1_FORCE_FULL_SPEED;
+#endif
 
   CleanInvalidateDCache_by_Addr((uint32_t*) &_dcd_data, sizeof(dcd_data_t));
 
@@ -296,18 +298,30 @@ void dcd_disconnect(uint8_t rhport)
 
 static void qtd_init(dcd_qtd_t* p_qtd, void * data_ptr, uint16_t total_bytes)
 {
+  // Force the CPU to flush the buffer. We increase the size by 31 because the call aligns the
+  // address to 32-byte boundaries. Buffer must be word aligned
+  CleanInvalidateDCache_by_Addr((uint32_t*) tu_align((uint32_t) data_ptr, 4), total_bytes + 31);
+
   tu_memclr(p_qtd, sizeof(dcd_qtd_t));
 
-  p_qtd->next        = QTD_NEXT_INVALID;
-  p_qtd->active      = 1;
-  p_qtd->total_bytes = p_qtd->expected_bytes = total_bytes;
+  p_qtd->next            = QTD_NEXT_INVALID;
+  p_qtd->active          = 1;
+  p_qtd->total_bytes     = p_qtd->expected_bytes = total_bytes;
+  p_qtd->int_on_complete = true;
 
   if (data_ptr != NULL)
   {
-    p_qtd->buffer[0]   = (uint32_t) data_ptr;
+    p_qtd->buffer[0] = (uint32_t) data_ptr;
+
+    uint32_t const bufend = p_qtd->buffer[0] + total_bytes;
     for(uint8_t i=1; i<5; i++)
     {
-      p_qtd->buffer[i] |= tu_align4k( p_qtd->buffer[i-1] ) + 4096;
+      uint32_t const next_page = tu_align4k( p_qtd->buffer[i-1] ) + 4096;
+      if ( bufend <= next_page ) break;
+
+      p_qtd->buffer[i] = next_page;
+
+      // TODO page[1] FRAME_N for ISO transfer
     }
   }
 }
@@ -340,9 +354,6 @@ void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr)
 
 bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc)
 {
-  // TODO not support ISO yet
-  TU_VERIFY ( p_endpoint_desc->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS);
-
   uint8_t const epnum = tu_edpt_number(p_endpoint_desc->bEndpointAddress);
   uint8_t const dir   = tu_edpt_dir(p_endpoint_desc->bEndpointAddress);
 
@@ -355,13 +366,27 @@ bool dcd_edpt_open(uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc)
 
   p_qhd->zero_length_termination = 1;
   p_qhd->max_packet_size         = p_endpoint_desc->wMaxPacketSize.size;
+  if (p_endpoint_desc->bmAttributes.xfer == TUSB_XFER_ISOCHRONOUS)
+  {
+    p_qhd->iso_mult = 1;
+  }
+
   p_qhd->qtd_overlay.next        = QTD_NEXT_INVALID;
 
   CleanInvalidateDCache_by_Addr((uint32_t*) &_dcd_data, sizeof(dcd_data_t));
 
   // Enable EP Control
   dcd_registers_t* dcd_reg = _dcd_controller[rhport].regs;
-  dcd_reg->ENDPTCTRL[epnum] |= ((p_endpoint_desc->bmAttributes.xfer << 2) | ENDPTCTRL_ENABLE | ENDPTCTRL_TOGGLE_RESET) << (dir ? 16 : 0);
+
+  uint32_t const epctrl = (p_endpoint_desc->bmAttributes.xfer << ENDPTCTRL_TYPE_POS) | ENDPTCTRL_ENABLE | ENDPTCTRL_TOGGLE_RESET;
+
+  if ( dir == TUSB_DIR_OUT )
+  {
+    dcd_reg->ENDPTCTRL[epnum] = (dcd_reg->ENDPTCTRL[epnum] & 0xFFFF0000u) | epctrl;
+  }else
+  {
+    dcd_reg->ENDPTCTRL[epnum] = (dcd_reg->ENDPTCTRL[epnum] & 0x0000FFFFu) | (epctrl << 16);
+  }
 
   return true;
 }
@@ -381,11 +406,35 @@ void dcd_edpt_close_all (uint8_t rhport)
   }
 }
 
-bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
+void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr)
 {
+  uint8_t const epnum  = tu_edpt_number(ep_addr);
+  uint8_t const dir    = tu_edpt_dir(ep_addr);
+
   dcd_registers_t* dcd_reg = _dcd_controller[rhport].regs;
-  uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir   = tu_edpt_dir(ep_addr);
+
+  _dcd_data.qhd[epnum][dir].qtd_overlay.halted = 1;
+
+  // Flush EP
+  uint32_t const flush_mask = TU_BIT(epnum + (dir ? 16 : 0));
+  dcd_reg->ENDPTFLUSH = flush_mask;
+  while(dcd_reg->ENDPTFLUSH & flush_mask);
+
+  // Clear EP enable
+  dcd_reg->ENDPTCTRL[epnum] &=~(ENDPTCTRL_ENABLE << (dir ? 16 : 0));
+}
+
+static void qhd_start_xfer(uint8_t rhport, uint8_t epnum, uint8_t dir)
+{
+  dcd_registers_t* dcd_reg = _dcd_controller[rhport].regs;
+  dcd_qhd_t* p_qhd = &_dcd_data.qhd[epnum][dir];
+  dcd_qtd_t* p_qtd = &_dcd_data.qtd[epnum][dir];
+
+  p_qhd->qtd_overlay.halted = false;            // clear any previous error
+  p_qhd->qtd_overlay.next   = (uint32_t) p_qtd; // link qtd to qhd
+
+  // flush cache
+  CleanInvalidateDCache_by_Addr((uint32_t*) &_dcd_data, sizeof(dcd_data_t));
 
   if ( epnum == 0 )
   {
@@ -394,25 +443,87 @@ bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t t
     while(dcd_reg->ENDPTSETUPSTAT & TU_BIT(0)) {}
   }
 
+  // start transfer
+  dcd_reg->ENDPTPRIME = TU_BIT(epnum + (dir ? 16 : 0));
+}
+
+bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
+{
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir   = tu_edpt_dir(ep_addr);
+
+  dcd_qhd_t* p_qhd = &_dcd_data.qhd[epnum][dir];
+  dcd_qtd_t* p_qtd = &_dcd_data.qtd[epnum][dir];
+
+  // Prepare qtd
+  qtd_init(p_qtd, buffer, total_bytes);
+
+  // Start qhd transfer
+  p_qhd->ff = NULL;
+  qhd_start_xfer(rhport, epnum, dir);
+
+  return true;
+}
+
+// fifo has to be aligned to 4k boundary
+bool dcd_edpt_xfer_fifo (uint8_t rhport, uint8_t ep_addr, tu_fifo_t * ff, uint16_t total_bytes)
+{
+  uint8_t const epnum = tu_edpt_number(ep_addr);
+  uint8_t const dir   = tu_edpt_dir(ep_addr);
+
   dcd_qhd_t * p_qhd = &_dcd_data.qhd[epnum][dir];
   dcd_qtd_t * p_qtd = &_dcd_data.qtd[epnum][dir];
 
-  // Force the CPU to flush the buffer. We increase the size by 32 because the call aligns the
-  // address to 32-byte boundaries.
-  // void* cast to suppress cast-align warning, buffer must be
-  CleanInvalidateDCache_by_Addr((uint32_t*) tu_align((uint32_t) buffer, 4), total_bytes + 31);
+  tu_fifo_buffer_info_t fifo_info;
 
-  //------------- Prepare qtd -------------//
-  qtd_init(p_qtd, buffer, total_bytes);
-  p_qtd->int_on_complete = true;
+  if (dir)
+  {
+    tu_fifo_get_read_info(ff, &fifo_info);
+  } else
+  {
+    tu_fifo_get_write_info(ff, &fifo_info);
+  }
 
-  p_qhd->qtd_overlay.halted = false;            // clear any previous error
-  p_qhd->qtd_overlay.next   = (uint32_t) p_qtd; // activate by linking qtd to qhd
+  if ( fifo_info.len_lin >= total_bytes )
+  {
+    // Linear length is enough for this transfer
+    qtd_init(p_qtd, fifo_info.ptr_lin, total_bytes);
+  }
+  else
+  {
+    // linear part is not enough
 
-  CleanInvalidateDCache_by_Addr((uint32_t*) &_dcd_data, sizeof(dcd_data_t));
+    // prepare TD up to linear length
+    qtd_init(p_qtd, fifo_info.ptr_lin, fifo_info.len_lin);
 
-  // start transfer
-  dcd_reg->ENDPTPRIME = TU_BIT(epnum + (dir ? 16 : 0));
+    if ( !tu_offset4k((uint32_t) fifo_info.ptr_wrap) && !tu_offset4k(tu_fifo_depth(ff)) )
+    {
+      // If buffer is aligned to 4K & buffer size is multiple of 4K
+      // We can make use of buffer page array to also combine the linear + wrapped length
+      p_qtd->total_bytes = p_qtd->expected_bytes = total_bytes;
+
+      for(uint8_t i = 1, page = 0; i < 5; i++)
+      {
+        // pick up buffer array where linear ends
+        if (p_qtd->buffer[i] == 0)
+        {
+          p_qtd->buffer[i] = (uint32_t) fifo_info.ptr_wrap + 4096 * page;
+          page++;
+        }
+      }
+
+      CleanInvalidateDCache_by_Addr((uint32_t*) tu_align((uint32_t) fifo_info.ptr_wrap, 4), total_bytes - fifo_info.len_wrap + 31);
+    }
+    else
+    {
+      // TODO we may need to carry the wrapped length after the linear part complete
+      // for now only transfer up to linear part
+    }
+  }
+
+  // Start qhd transfer
+  p_qhd->ff = ff;
+  qhd_start_xfer(rhport, epnum, dir);
 
   return true;
 }
@@ -423,6 +534,7 @@ bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t t
 
 static void process_edpt_complete_isr(uint8_t rhport, uint8_t epnum, uint8_t dir)
 {
+  dcd_qhd_t * p_qhd = &_dcd_data.qhd[epnum][dir];
   dcd_qtd_t * p_qtd = &_dcd_data.qtd[epnum][dir];
 
   uint8_t result = p_qtd->halted ? XFER_RESULT_STALLED :
@@ -435,8 +547,21 @@ static void process_edpt_complete_isr(uint8_t rhport, uint8_t epnum, uint8_t dir
     dcd_reg->ENDPTFLUSH = TU_BIT(epnum + (dir ? 16 : 0));
   }
 
+  uint16_t const xferred_bytes = p_qtd->expected_bytes - p_qtd->total_bytes;
+
+  if (p_qhd->ff)
+  {
+    if (dir == TUSB_DIR_IN)
+    {
+      tu_fifo_advance_read_pointer(p_qhd->ff, xferred_bytes);
+    } else
+    {
+      tu_fifo_advance_write_pointer(p_qhd->ff, xferred_bytes);
+    }
+  }
+  
   // only number of bytes in the IOC qtd
-  dcd_event_xfer_complete(rhport, tu_edpt_addr(epnum, dir), p_qtd->expected_bytes - p_qtd->total_bytes, result, true);
+  dcd_event_xfer_complete(rhport, tu_edpt_addr(epnum, dir), xferred_bytes, result, true);
 }
 
 void dcd_int_handler(uint8_t rhport)
diff --git a/src/tusb_option.h b/src/tusb_option.h
index 4d7e1e9d2..dceb2ac2c 100644
--- a/src/tusb_option.h
+++ b/src/tusb_option.h
@@ -203,14 +203,20 @@
   #define CFG_TUSB_MEM_SECTION
 #endif
 
+// alignment requirement of buffer used for endpoint transferring
 #ifndef CFG_TUSB_MEM_ALIGN
   #define CFG_TUSB_MEM_ALIGN      TU_ATTR_ALIGNED(4)
 #endif
 
+// OS selection
 #ifndef CFG_TUSB_OS
   #define CFG_TUSB_OS             OPT_OS_NONE
 #endif
 
+#ifndef CFG_TUSB_OS_INC_PATH
+  #define CFG_TUSB_OS_INC_PATH
+#endif
+
 //--------------------------------------------------------------------
 // DEVICE OPTIONS
 //--------------------------------------------------------------------