Merge remote-tracking branch 'upstream/master' into uac2

26 files changed, 1573 insertions, 353 deletions

diff --git a/src/class/cdc/cdc_device.c b/src/class/cdc/cdc_device.c
index 9180065c4..89326f71d 100644
--- a/src/class/cdc/cdc_device.c
+++ b/src/class/cdc/cdc_device.c
@@ -61,8 +61,8 @@ typedef struct
 #endif
 
   // Endpoint Transfer buffer
-  CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_CDC_EPSIZE];
-  CFG_TUSB_MEM_ALIGN uint8_t epin_buf[CFG_TUD_CDC_EPSIZE];
+  CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_CDC_EP_BUFSIZE];
+  CFG_TUSB_MEM_ALIGN uint8_t epin_buf[CFG_TUD_CDC_EP_BUFSIZE];
 
 }cdcd_interface_t;
 
@@ -82,9 +82,9 @@ static void _prep_out_transaction (uint8_t itf)
 
   // Prepare for incoming data but only allow what we can store in the ring buffer.
   uint16_t max_read = tu_fifo_remaining(&p_cdc->rx_ff);
-  if ( max_read >= TU_ARRAY_SIZE(p_cdc->epout_buf) )
+  if ( max_read >= sizeof(p_cdc->epout_buf) )
   {
-    usbd_edpt_xfer(TUD_OPT_RHPORT, p_cdc->ep_out, p_cdc->epout_buf, TU_ARRAY_SIZE(p_cdc->epout_buf));
+    usbd_edpt_xfer(TUD_OPT_RHPORT, p_cdc->ep_out, p_cdc->epout_buf, sizeof(p_cdc->epout_buf));
   }
 }
 
@@ -148,7 +148,7 @@ uint32_t tud_cdc_n_write(uint8_t itf, void const* buffer, uint32_t bufsize)
 
 #if 0 // TODO issue with circuitpython's REPL
   // flush if queue more than endpoint size
-  if ( tu_fifo_count(&_cdcd_itf[itf].tx_ff) >= CFG_TUD_CDC_EPSIZE )
+  if ( tu_fifo_count(&_cdcd_itf[itf].tx_ff) >= CFG_TUD_CDC_EP_BUFSIZE )
   {
     tud_cdc_n_write_flush(itf);
   }
@@ -164,7 +164,7 @@ uint32_t tud_cdc_n_write_flush (uint8_t itf)
   // skip if previous transfer not complete yet
   TU_VERIFY( !usbd_edpt_busy(TUD_OPT_RHPORT, p_cdc->ep_in), 0 );
 
-  uint16_t count = tu_fifo_read_n(&_cdcd_itf[itf].tx_ff, p_cdc->epin_buf, TU_ARRAY_SIZE(p_cdc->epin_buf));
+  uint16_t count = tu_fifo_read_n(&_cdcd_itf[itf].tx_ff, p_cdc->epin_buf, sizeof(p_cdc->epin_buf));
   if ( count )
   {
     TU_VERIFY( tud_cdc_n_connected(itf), 0 ); // fifo is empty if not connected
@@ -364,7 +364,7 @@ bool cdcd_control_request(uint8_t rhport, tusb_control_request_t const * request
       tud_control_status(rhport, request);
 
       // Invoke callback
-      if ( tud_cdc_line_state_cb) tud_cdc_line_state_cb(itf, dtr, rts);
+      if ( tud_cdc_line_state_cb ) tud_cdc_line_state_cb(itf, dtr, rts);
     }
     break;
 
@@ -420,7 +420,7 @@ bool cdcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_
     {
       // There is no data left, a ZLP should be sent if
       // xferred_bytes is multiple of EP size and not zero
-      if ( xferred_bytes && (0 == (xferred_bytes % CFG_TUD_CDC_EPSIZE)) )
+      if ( xferred_bytes && (0 == (xferred_bytes % CFG_TUD_CDC_EP_BUFSIZE)) )
       {
         usbd_edpt_xfer(TUD_OPT_RHPORT, p_cdc->ep_in, NULL, 0);
       }
diff --git a/src/class/cdc/cdc_device.h b/src/class/cdc/cdc_device.h
index a2523d8d0..9d257bbe9 100644
--- a/src/class/cdc/cdc_device.h
+++ b/src/class/cdc/cdc_device.h
@@ -34,8 +34,13 @@
 //--------------------------------------------------------------------+
 // Class Driver Configuration
 //--------------------------------------------------------------------+
-#ifndef CFG_TUD_CDC_EPSIZE
-#define CFG_TUD_CDC_EPSIZE 64
+#if !defined(CFG_TUD_CDC_EP_BUFSIZE) && defined(CFG_TUD_CDC_EPSIZE)
+  #warning CFG_TUD_CDC_EPSIZE is renamed to CFG_TUD_CDC_EP_BUFSIZE, please update to use the new name
+  #define CFG_TUD_CDC_EP_BUFSIZE    CFG_TUD_CDC_EPSIZE
+#endif
+
+#ifndef CFG_TUD_CDC_EP_BUFSIZE
+  #define CFG_TUD_CDC_EP_BUFSIZE    (TUD_OPT_HIGH_SPEED ? 512 : 64)
 #endif
 
 #ifdef __cplusplus
@@ -95,7 +100,7 @@ uint32_t tud_cdc_n_write_str       (uint8_t itf, char const* str);
 // Force sending data if possible, return number of forced bytes
 uint32_t tud_cdc_n_write_flush     (uint8_t itf);
 
-// Return number of characters available for writing
+// Return the number of bytes (characters) available for writing to TX FIFO buffer in a single n_write operation.
 uint32_t tud_cdc_n_write_available (uint8_t itf);
 
 //--------------------------------------------------------------------+
diff --git a/src/class/dfu/dfu_rt_device.c b/src/class/dfu/dfu_rt_device.c
index 01d4e3490..d4c3ecb62 100644
--- a/src/class/dfu/dfu_rt_device.c
+++ b/src/class/dfu/dfu_rt_device.c
@@ -44,6 +44,14 @@ typedef enum {
   DFU_REQUEST_ABORT       = 6,
 } dfu_requests_t;
 
+typedef struct TU_ATTR_PACKED
+{
+  uint8_t status;
+  uint8_t poll_timeout[3];
+  uint8_t state;
+  uint8_t istring;
+} dfu_status_t;
+
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
@@ -88,10 +96,19 @@ bool dfu_rtd_control_complete(uint8_t rhport, tusb_control_request_t const * req
 
 bool dfu_rtd_control_request(uint8_t rhport, tusb_control_request_t const * request)
 {
-  // Handle class request only
-  TU_VERIFY(request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS);
   TU_VERIFY(request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_INTERFACE);
 
+  // dfu-util will try to claim the interface with SET_INTERFACE request before sending DFU request
+  if ( TUSB_REQ_TYPE_STANDARD == request->bmRequestType_bit.type &&
+       TUSB_REQ_SET_INTERFACE == request->bRequest )
+  {
+    tud_control_status(rhport, request);
+    return true;
+  }
+
+  // Handle class request only from here
+  TU_VERIFY(request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS);
+
   switch ( request->bRequest )
   {
     case DFU_REQUEST_DETACH:
@@ -99,6 +116,14 @@ bool dfu_rtd_control_request(uint8_t rhport, tusb_control_request_t const * requ
       tud_dfu_rt_reboot_to_dfu();
     break;
 
+    case DFU_REQUEST_GETSTATUS:
+    {
+      // status = OK, poll timeout = 0, state = app idle, istring = 0
+      uint8_t status_response[6] = { 0, 0, 0, 0, 0, 0 };
+      tud_control_xfer(rhport, request, status_response, sizeof(status_response));
+    }
+    break;
+
     default: return false; // stall unsupported request
   }
 
diff --git a/src/class/hid/hid_device.c b/src/class/hid/hid_device.c
index 526394d4a..c46fc591a 100644
--- a/src/class/hid/hid_device.c
+++ b/src/class/hid/hid_device.c
@@ -48,8 +48,8 @@ typedef struct
   uint8_t idle_rate;     // up to application to handle idle rate
   uint16_t report_desc_len;
 
-  CFG_TUSB_MEM_ALIGN uint8_t epin_buf[CFG_TUD_HID_BUFSIZE];
-  CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_HID_BUFSIZE];
+  CFG_TUSB_MEM_ALIGN uint8_t epin_buf[CFG_TUD_HID_EP_BUFSIZE];
+  CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_HID_EP_BUFSIZE];
 
   tusb_hid_descriptor_hid_t const * hid_descriptor;
 } hidd_interface_t;
@@ -86,7 +86,7 @@ bool tud_hid_report(uint8_t report_id, void const* report, uint8_t len)
 
   if (report_id)
   {
-    len = tu_min8(len, CFG_TUD_HID_BUFSIZE-1);
+    len = tu_min8(len, CFG_TUD_HID_EP_BUFSIZE-1);
 
     p_hid->epin_buf[0] = report_id;
     memcpy(p_hid->epin_buf+1, report, len);
@@ -94,7 +94,7 @@ bool tud_hid_report(uint8_t report_id, void const* report, uint8_t len)
   }else
   {
     // If report id = 0, skip ID field
-    len = tu_min8(len, CFG_TUD_HID_BUFSIZE);
+    len = tu_min8(len, CFG_TUD_HID_EP_BUFSIZE);
     memcpy(p_hid->epin_buf, report, len);
   }
 
diff --git a/src/class/hid/hid_device.h b/src/class/hid/hid_device.h
index ad8c9ece4..f7ad38bab 100644
--- a/src/class/hid/hid_device.h
+++ b/src/class/hid/hid_device.h
@@ -39,8 +39,14 @@
 // Class Driver Default Configure & Validation
 //--------------------------------------------------------------------+
 
-#ifndef CFG_TUD_HID_BUFSIZE
-#define CFG_TUD_HID_BUFSIZE     16
+#if !defined(CFG_TUD_HID_EP_BUFSIZE) & defined(CFG_TUD_HID_BUFSIZE)
+  // TODO warn user to use new name later on
+  // #warning CFG_TUD_HID_BUFSIZE is renamed to CFG_TUD_HID_EP_BUFSIZE, please update to use the new name
+  #define CFG_TUD_HID_EP_BUFSIZE  CFG_TUD_HID_BUFSIZE
+#endif
+
+#ifndef CFG_TUD_HID_EP_BUFSIZE
+  #define CFG_TUD_HID_EP_BUFSIZE     16
 #endif
 
 //--------------------------------------------------------------------+
diff --git a/src/class/midi/midi_device.c b/src/class/midi/midi_device.c
index 14fb3e50d..d4ed21c78 100644
--- a/src/class/midi/midi_device.c
+++ b/src/class/midi/midi_device.c
@@ -67,8 +67,8 @@ typedef struct
   uint8_t read_target_length;
 
   // Endpoint Transfer buffer
-  CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_MIDI_EPSIZE];
-  CFG_TUSB_MEM_ALIGN uint8_t epin_buf[CFG_TUD_MIDI_EPSIZE];
+  CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_MIDI_EP_BUFSIZE];
+  CFG_TUSB_MEM_ALIGN uint8_t epin_buf[CFG_TUD_MIDI_EP_BUFSIZE];
 
 } midid_interface_t;
 
@@ -160,7 +160,7 @@ static bool maybe_transmit(midid_interface_t* midi, uint8_t itf_index)
   // skip if previous transfer not complete
   TU_VERIFY( !usbd_edpt_busy(TUD_OPT_RHPORT, midi->ep_in) );
 
-  uint16_t count = tu_fifo_read_n(&midi->tx_ff, midi->epin_buf, CFG_TUD_MIDI_EPSIZE);
+  uint16_t count = tu_fifo_read_n(&midi->tx_ff, midi->epin_buf, CFG_TUD_MIDI_EP_BUFSIZE);
   if (count > 0)
   {
     TU_ASSERT( usbd_edpt_xfer(TUD_OPT_RHPORT, midi->ep_in, midi->epin_buf, count) );
@@ -359,7 +359,7 @@ uint16_t midid_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint
   }
 
   // Prepare for incoming data
-  if ( !usbd_edpt_xfer(rhport, p_midi->ep_out, p_midi->epout_buf, CFG_TUD_MIDI_EPSIZE) )
+  if ( !usbd_edpt_xfer(rhport, p_midi->ep_out, p_midi->epout_buf, CFG_TUD_MIDI_EP_BUFSIZE) )
   {
     TU_LOG1_FAILED();
     TU_BREAKPOINT();
@@ -404,7 +404,7 @@ bool midid_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32
     midi_rx_done_cb(p_midi, p_midi->epout_buf, xferred_bytes);
 
     // prepare for next
-    TU_ASSERT( usbd_edpt_xfer(rhport, p_midi->ep_out, p_midi->epout_buf, CFG_TUD_MIDI_EPSIZE), false );
+    TU_ASSERT( usbd_edpt_xfer(rhport, p_midi->ep_out, p_midi->epout_buf, CFG_TUD_MIDI_EP_BUFSIZE), false );
   } else if ( ep_addr == p_midi->ep_in ) {
     maybe_transmit(p_midi, itf);
   }
diff --git a/src/class/midi/midi_device.h b/src/class/midi/midi_device.h
index bec0984f2..1828a21a3 100644
--- a/src/class/midi/midi_device.h
+++ b/src/class/midi/midi_device.h
@@ -36,8 +36,14 @@
 //--------------------------------------------------------------------+
 // Class Driver Configuration
 //--------------------------------------------------------------------+
-#ifndef CFG_TUD_MIDI_EPSIZE
-#define CFG_TUD_MIDI_EPSIZE 64
+
+#if !defined(CFG_TUD_MIDI_EP_BUFSIZE) && defined(CFG_TUD_MIDI_EPSIZE)
+  #warning CFG_TUD_MIDI_EPSIZE is renamed to CFG_TUD_MIDI_EP_BUFSIZE, please update to use the new name
+  #define CFG_TUD_MIDI_EP_BUFSIZE    CFG_TUD_MIDI_EPSIZE
+#endif
+
+#ifndef CFG_TUD_MIDI_EP_BUFSIZE
+  #define CFG_TUD_MIDI_EP_BUFSIZE     (TUD_OPT_HIGH_SPEED ? 512 : 64)
 #endif
 
 #ifdef __cplusplus
diff --git a/src/class/msc/msc_device.c b/src/class/msc/msc_device.c
index 3ddcd4e49..692fd2441 100644
--- a/src/class/msc/msc_device.c
+++ b/src/class/msc/msc_device.c
@@ -65,7 +65,7 @@ typedef struct
 }mscd_interface_t;
 
 CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static mscd_interface_t _mscd_itf;
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static uint8_t _mscd_buf[CFG_TUD_MSC_BUFSIZE];
+CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static uint8_t _mscd_buf[CFG_TUD_MSC_EP_BUFSIZE];
 
 //--------------------------------------------------------------------+
 // INTERNAL OBJECT & FUNCTION DECLARATION
@@ -564,7 +564,7 @@ bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
       else
       {
         // READ10 & WRITE10 Can be executed with large bulk of data e.g write 8K bytes (several flash write)
-        // We break it into multiple smaller command whose data size is up to CFG_TUD_MSC_BUFSIZE
+        // We break it into multiple smaller command whose data size is up to CFG_TUD_MSC_EP_BUFSIZE
         if (SCSI_CMD_READ_10 == p_cbw->command[0])
         {
           proc_read10_cmd(rhport, p_msc);
diff --git a/src/class/msc/msc_device.h b/src/class/msc/msc_device.h
index 30ffd02e1..3aa93ffd7 100644
--- a/src/class/msc/msc_device.h
+++ b/src/class/msc/msc_device.h
@@ -38,12 +38,19 @@
 //--------------------------------------------------------------------+
 // Class Driver Configuration
 //--------------------------------------------------------------------+
-TU_VERIFY_STATIC(CFG_TUD_MSC_BUFSIZE < UINT16_MAX, "Size is not correct");
 
-#ifndef CFG_TUD_MSC_BUFSIZE
-  #error CFG_TUD_MSC_BUFSIZE must be defined, value of a block size should work well, the more the better
+#if !defined(CFG_TUD_MSC_EP_BUFSIZE) & defined(CFG_TUD_MSC_BUFSIZE)
+  // TODO warn user to use new name later on
+  // #warning CFG_TUD_MSC_BUFSIZE is renamed to CFG_TUD_MSC_EP_BUFSIZE, please update to use the new name
+  #define CFG_TUD_MSC_EP_BUFSIZE  CFG_TUD_MSC_BUFSIZE
 #endif
 
+#ifndef CFG_TUD_MSC_EP_BUFSIZE
+  #error CFG_TUD_MSC_EP_BUFSIZE must be defined, value of a block size should work well, the more the better
+#endif
+
+TU_VERIFY_STATIC(CFG_TUD_MSC_EP_BUFSIZE < UINT16_MAX, "Size is not correct");
+
 /** \addtogroup ClassDriver_MSC
  *  @{
  * \defgroup MSC_Device Device
diff --git a/src/class/net/net_device.h b/src/class/net/net_device.h
index fb72146b0..0175ea56b 100644
--- a/src/class/net/net_device.h
+++ b/src/class/net/net_device.h
@@ -37,7 +37,7 @@
 #include "netif/ethernet.h"
 
 /* declared here, NOT in usb_descriptors.c, so that the driver can intelligently ZLP as needed */
-#define CFG_TUD_NET_ENDPOINT_SIZE ((CFG_TUSB_RHPORT0_MODE & OPT_MODE_HIGH_SPEED) ? 512 : 64)
+#define CFG_TUD_NET_ENDPOINT_SIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
 
 /* Maximum Tranmission Unit (in bytes) of the network, including Ethernet header */
 #define CFG_TUD_NET_MTU           (1500 + SIZEOF_ETH_HDR)
diff --git a/src/common/tusb_common.h b/src/common/tusb_common.h
index 4b4183ae6..1dba6c914 100644
--- a/src/common/tusb_common.h
+++ b/src/common/tusb_common.h
@@ -228,7 +228,6 @@ void tu_print_var(uint8_t const* buf, uint32_t bufsize)
   for(uint32_t i=0; i<bufsize; i++) tu_printf("%02X ", buf[i]);
 }
 
-
 // Log with debug level 1
 #define TU_LOG1               tu_printf
 #define TU_LOG1_MEM           tu_print_mem
@@ -243,9 +242,9 @@ void tu_print_var(uint8_t const* buf, uint32_t bufsize)
   #define TU_LOG2             TU_LOG1
   #define TU_LOG2_MEM         TU_LOG1_MEM
   #define TU_LOG2_VAR         TU_LOG1_VAR
-  #define TU_LOG2_LOCATION()  TU_LOG1_LOCATION()
   #define TU_LOG2_INT         TU_LOG1_INT
   #define TU_LOG2_HEX         TU_LOG1_HEX
+  #define TU_LOG2_LOCATION()  TU_LOG1_LOCATION()
 #endif
 
 
@@ -279,6 +278,7 @@ static inline char const* lookup_find(lookup_table_t const* p_table, uint32_t ke
   #define TU_LOG1_VAR(...)
   #define TU_LOG1_INT(...)
   #define TU_LOG1_HEX(...)
+  #define TU_LOG1_LOCATION()
   #define TU_LOG1_FAILED()
 #endif
 
@@ -288,6 +288,7 @@ static inline char const* lookup_find(lookup_table_t const* p_table, uint32_t ke
   #define TU_LOG2_VAR(...)
   #define TU_LOG2_INT(...)
   #define TU_LOG2_HEX(...)
+  #define TU_LOG2_LOCATION()
 #endif
 
 #ifdef __cplusplus
diff --git a/src/device/dcd.h b/src/device/dcd.h
index 68798c259..a4635346b 100644
--- a/src/device/dcd.h
+++ b/src/device/dcd.h
@@ -60,11 +60,17 @@ typedef struct TU_ATTR_ALIGNED(4)
   uint8_t rhport;
   uint8_t event_id;
 
-  union {
-    // USBD_EVT_SETUP_RECEIVED
+  union
+  {
+    // BUS RESET
+    struct {
+      tusb_speed_t speed;
+    } bus_reset;
+
+    // SETUP_RECEIVED
     tusb_control_request_t setup_received;
 
-    // USBD_EVT_XFER_COMPLETE
+    // XFER_COMPLETE
     struct {
       uint8_t  ep_addr;
       uint8_t  result;
@@ -143,6 +149,9 @@ extern void dcd_event_handler(dcd_event_t const * event, bool in_isr);
 // helper to send bus signal event
 extern void dcd_event_bus_signal (uint8_t rhport, dcd_eventid_t eid, bool in_isr);
 
+// helper to send bus reset event
+extern void dcd_event_bus_reset (uint8_t rhport, tusb_speed_t speed, bool in_isr);
+
 // helper to send setup received
 extern void dcd_event_setup_received(uint8_t rhport, uint8_t const * setup, bool in_isr);
 
diff --git a/src/device/usbd.c b/src/device/usbd.c
index b5264bc17..20d4925d2 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -40,7 +40,8 @@
 //--------------------------------------------------------------------+
 // Device Data
 //--------------------------------------------------------------------+
-typedef struct {
+typedef struct
+{
   struct TU_ATTR_PACKED
   {
     volatile uint8_t connected    : 1;
@@ -53,6 +54,8 @@ typedef struct {
     uint8_t self_powered          : 1; // configuration descriptor's attribute
   };
 
+  uint8_t speed;
+
   uint8_t itf2drv[16];     // map interface number to driver (0xff is invalid)
   uint8_t ep2drv[8][2];    // map endpoint to driver ( 0xff is invalid )
 
@@ -304,6 +307,11 @@ void usbd_driver_print_control_complete_name(bool (*control_complete) (uint8_t,
 //--------------------------------------------------------------------+
 // Application API
 //--------------------------------------------------------------------+
+tusb_speed_t tud_speed_get(void)
+{
+  return (tusb_speed_t) _usbd_dev.speed;
+}
+
 bool tud_mounted(void)
 {
   return _usbd_dev.configured;
@@ -403,16 +411,21 @@ void tud_task (void)
 
     if ( !osal_queue_receive(_usbd_q, &event) ) return;
 
-    TU_LOG2("USBD %s", event.event_id < DCD_EVENT_COUNT ? _usbd_event_str[event.event_id] : "CORRUPTED");
-    TU_LOG2("%s", (event.event_id != DCD_EVENT_XFER_COMPLETE && event.event_id != DCD_EVENT_SETUP_RECEIVED) ? "\r\n" : " ");
+#if CFG_TUSB_DEBUG >= 2
+    if (event.event_id == DCD_EVENT_SETUP_RECEIVED) TU_LOG2("\r\n"); // extra line for setup
+    TU_LOG2("USBD %s ", event.event_id < DCD_EVENT_COUNT ? _usbd_event_str[event.event_id] : "CORRUPTED");
+#endif
 
     switch ( event.event_id )
     {
       case DCD_EVENT_BUS_RESET:
+        TU_LOG2("\r\n");
         usbd_reset(event.rhport);
+        _usbd_dev.speed = event.bus_reset.speed;
       break;
 
       case DCD_EVENT_UNPLUGGED:
+        TU_LOG2("\r\n");
         usbd_reset(event.rhport);
 
         // invoke callback
@@ -464,14 +477,17 @@ void tud_task (void)
       break;
 
       case DCD_EVENT_SUSPEND:
+        TU_LOG2("\r\n");
         if (tud_suspend_cb) tud_suspend_cb(_usbd_dev.remote_wakeup_en);
       break;
 
       case DCD_EVENT_RESUME:
+        TU_LOG2("\r\n");
         if (tud_resume_cb) tud_resume_cb();
       break;
 
       case DCD_EVENT_SOF:
+        TU_LOG2("\r\n");
         for ( uint8_t i = 0; i < USBD_CLASS_DRIVER_COUNT; i++ )
         {
           if ( _usbd_driver[i].sof )
@@ -482,6 +498,7 @@ void tud_task (void)
       break;
 
       case USBD_EVENT_FUNC_CALL:
+        TU_LOG2("\r\n");
         if ( event.func_call.func ) event.func_call.func(event.func_call.param);
       break;
 
@@ -887,20 +904,11 @@ static bool process_get_descriptor(uint8_t rhport, tusb_control_request_t const
       TU_LOG2(" String[%u]\r\n", desc_index);
 
       // String Descriptor always uses the desc set from user
-      if ( desc_index == 0xEE )
-      {
-        // The 0xEE index string is a Microsoft OS Descriptors.
-        // https://docs.microsoft.com/en-us/windows-hardware/drivers/usbcon/microsoft-defined-usb-descriptors
-        return false;
-      }
-      else
-      {
-        uint8_t const* desc_str = (uint8_t const*) tud_descriptor_string_cb(desc_index, p_request->wIndex);
-        TU_ASSERT(desc_str);
+      uint8_t const* desc_str = (uint8_t const*) tud_descriptor_string_cb(desc_index, p_request->wIndex);
+      TU_VERIFY(desc_str);
 
-        // first byte of descriptor is its size
-        return tud_control_xfer(rhport, p_request, (void*) desc_str, desc_str[0]);
-      }
+      // first byte of descriptor is its size
+      return tud_control_xfer(rhport, p_request, (void*) desc_str, desc_str[0]);
     break;
 
     case TUSB_DESC_DEVICE_QUALIFIER:
@@ -980,7 +988,14 @@ void dcd_event_handler(dcd_event_t const * event, bool in_isr)
 
 void dcd_event_bus_signal (uint8_t rhport, dcd_eventid_t eid, bool in_isr)
 {
-  dcd_event_t event = { .rhport = rhport, .event_id = eid, };
+  dcd_event_t event = { .rhport = rhport, .event_id = eid };
+  dcd_event_handler(&event, in_isr);
+}
+
+void dcd_event_bus_reset (uint8_t rhport, tusb_speed_t speed, bool in_isr)
+{
+  dcd_event_t event = { .rhport = rhport, .event_id = DCD_EVENT_BUS_RESET };
+  event.bus_reset.speed = speed;
   dcd_event_handler(&event, in_isr);
 }
 
diff --git a/src/device/usbd.h b/src/device/usbd.h
index fa0be803c..0d634d178 100644
--- a/src/device/usbd.h
+++ b/src/device/usbd.h
@@ -42,6 +42,8 @@ extern "C" {
 //--------------------------------------------------------------------+
 
 // Init device stack
+// Note: when using with RTOS, this should be called after scheduler/kernel is started.
+// Otherwise it could cause kernel issue since USB IRQ handler does use RTOS queue API.
 bool tud_init (void);
 
 // Task function should be called in main/rtos loop
@@ -53,6 +55,9 @@ bool tud_task_event_ready(void);
 // Interrupt handler, name alias to DCD
 #define tud_int_handler   dcd_int_handler
 
+// Get current bus speed
+tusb_speed_t tud_speed_get(void);
+
 // Check if device is connected and configured
 bool tud_mounted(void);
 
diff --git a/src/host/usbh.h b/src/host/usbh.h
index 42a2bd095..fe39a19f4 100644
--- a/src/host/usbh.h
+++ b/src/host/usbh.h
@@ -91,6 +91,9 @@ TU_ATTR_WEAK void tuh_umount_cb(uint8_t dev_addr);
 //--------------------------------------------------------------------+
 // CLASS-USBH & INTERNAL API
 //--------------------------------------------------------------------+
+
+// Note: when using with RTOS, this should be called after scheduler/kernel is started.
+// Otherwise it could cause kernel issue since USB IRQ handler does use RTOS queue API.
 bool usbh_init(void);
 bool usbh_control_xfer (uint8_t dev_addr, tusb_control_request_t* request, uint8_t* data);
 
diff --git a/src/osal/osal_freertos.h b/src/osal/osal_freertos.h
index 42ea1fd19..004bd1b6b 100644
--- a/src/osal/osal_freertos.h
+++ b/src/osal/osal_freertos.h
@@ -58,7 +58,23 @@ static inline osal_semaphore_t osal_semaphore_create(osal_semaphore_def_t* semde
 
 static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr)
 {
-  return in_isr ?  xSemaphoreGiveFromISR(sem_hdl, NULL) : xSemaphoreGive(sem_hdl);
+  if ( !in_isr )
+  {
+    return xSemaphoreGive(sem_hdl) != 0;
+  }
+  else
+  {
+    BaseType_t xHigherPriorityTaskWoken;
+    BaseType_t res = xSemaphoreGiveFromISR(sem_hdl, &xHigherPriorityTaskWoken);
+
+#if CFG_TUSB_MCU == OPT_MCU_ESP32S2
+    if ( xHigherPriorityTaskWoken ) portYIELD_FROM_ISR();
+#else
+    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+#endif
+
+    return res != 0;
+  }
 }
 
 static inline bool osal_semaphore_wait (osal_semaphore_t sem_hdl, uint32_t msec)
@@ -125,7 +141,23 @@ static inline bool osal_queue_receive(osal_queue_t qhdl, void* data)
 
 static inline bool osal_queue_send(osal_queue_t qhdl, void const * data, bool in_isr)
 {
-  return in_isr ? xQueueSendToBackFromISR(qhdl, data, NULL) : xQueueSendToBack(qhdl, data, OSAL_TIMEOUT_WAIT_FOREVER);
+  if ( !in_isr )
+  {
+    return xQueueSendToBack(qhdl, data, OSAL_TIMEOUT_WAIT_FOREVER) != 0;
+  }
+  else
+  {
+    BaseType_t xHigherPriorityTaskWoken;
+    BaseType_t res = xQueueSendToBackFromISR(qhdl, data, &xHigherPriorityTaskWoken);
+
+#if CFG_TUSB_MCU == OPT_MCU_ESP32S2
+    if ( xHigherPriorityTaskWoken ) portYIELD_FROM_ISR();
+#else
+    portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
+#endif
+
+    return res != 0;
+  }
 }
 
 static inline bool osal_queue_empty(osal_queue_t qhdl)
diff --git a/src/portable/dialog/da146xx/dcd_da146xx.c b/src/portable/dialog/da146xx/dcd_da146xx.c
new file mode 100644
index 000000000..3a3459b7d
--- /dev/null
+++ b/src/portable/dialog/da146xx/dcd_da146xx.c
@@ -0,0 +1,842 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2020 Jerzy Kasenberg
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * This file is part of the TinyUSB stack.
+ */
+
+#include "tusb_option.h"
+
+#if TUSB_OPT_DEVICE_ENABLED && CFG_TUSB_MCU == OPT_MCU_DA1469X
+
+#include "DA1469xAB.h"
+
+#include "device/dcd.h"
+
+/*------------------------------------------------------------------*/
+/* MACRO TYPEDEF CONSTANT ENUM
+ *------------------------------------------------------------------*/
+
+// Since TinyUSB doesn't use SOF for now, and this interrupt too often (1ms interval)
+// We disable SOF for now until needed later on
+#define USE_SOF           0
+
+#define EP_MAX            4
+
+#define NFSR_NODE_RESET         0
+#define NFSR_NODE_RESUME        1
+#define NFSR_NODE_OPERATIONAL   2
+#define NFSR_NODE_SUSPEND       3
+
+static TU_ATTR_ALIGNED(4) uint8_t _setup_packet[8];
+
+typedef struct
+{
+  union
+  {
+    __IOM uint32_t epc_in;
+    __IOM uint32_t USB_EPC0_REG;                 /*!< (@ 0x00000080) Endpoint Control 0 Register  */
+    __IOM uint32_t USB_EPC1_REG;                 /*!< (@ 0x000000A0) Endpoint Control Register 1  */
+    __IOM uint32_t USB_EPC3_REG;                 /*!< (@ 0x000000C0) Endpoint Control Register 3  */
+    __IOM uint32_t USB_EPC5_REG;                 /*!< (@ 0x000000E0) Endpoint Control Register 5  */
+  };
+  union
+  {
+    __IOM uint32_t txd;
+    __IOM uint32_t USB_TXD0_REG;                 /*!< (@ 0x00000084) Transmit Data 0 Register     */
+    __IOM uint32_t USB_TXD1_REG;                 /*!< (@ 0x000000A4) Transmit Data Register 1     */
+    __IOM uint32_t USB_TXD2_REG;                 /*!< (@ 0x000000C4) Transmit Data Register 2     */
+    __IOM uint32_t USB_TXD3_REG;                 /*!< (@ 0x000000E4) Transmit Data Register 3     */
+  };
+  union
+  {
+    __IOM uint32_t txs;
+    __IOM uint32_t USB_TXS0_REG;                 /*!< (@ 0x00000088) Transmit Status 0 Register   */
+    __IOM uint32_t USB_TXS1_REG;                 /*!< (@ 0x000000A8) Transmit Status Register 1   */
+    __IOM uint32_t USB_TXS2_REG;                 /*!< (@ 0x000000C8) Transmit Status Register 2   */
+    __IOM uint32_t USB_TXS3_REG;                 /*!< (@ 0x000000E8) Transmit Status Register 3   */
+  };
+  union
+  {
+    __IOM uint32_t txc;
+    __IOM uint32_t USB_TXC0_REG;                 /*!< (@ 0x0000008C) Transmit command 0 Register  */
+    __IOM uint32_t USB_TXC1_REG;                 /*!< (@ 0x000000AC) Transmit Command Register 1  */
+    __IOM uint32_t USB_TXC2_REG;                 /*!< (@ 0x000000CC) Transmit Command Register 2  */
+    __IOM uint32_t USB_TXC3_REG;                 /*!< (@ 0x000000EC) Transmit Command Register 3  */
+  };
+  union
+  {
+    __IOM uint32_t epc_out;
+    __IOM uint32_t USB_EP0_NAK_REG;              /*!< (@ 0x00000090) EP0 INNAK and OUTNAK Register */
+    __IOM uint32_t USB_EPC2_REG;                 /*!< (@ 0x000000B0) Endpoint Control Register 2   */
+    __IOM uint32_t USB_EPC4_REG;                 /*!< (@ 0x000000D0) Endpoint Control Register 4   */
+    __IOM uint32_t USB_EPC6_REG;                 /*!< (@ 0x000000F0) Endpoint Control Register 6   */
+  };
+  union
+  {
+    __IOM uint32_t rxd;
+    __IOM uint32_t USB_RXD0_REG;                 /*!< (@ 0x00000094) Receive Data 0 Register       */
+    __IOM uint32_t USB_RXD1_REG;                 /*!< (@ 0x000000B4) Receive Data Register,1       */
+    __IOM uint32_t USB_RXD2_REG;                 /*!< (@ 0x000000D4) Receive Data Register 2       */
+    __IOM uint32_t USB_RXD3_REG;                 /*!< (@ 0x000000F4) Receive Data Register 3       */
+  };
+  union
+  {
+    __IOM uint32_t rxs;
+    __IOM uint32_t USB_RXS0_REG;                 /*!< (@ 0x00000098) Receive Status 0 Register     */
+    __IOM uint32_t USB_RXS1_REG;                 /*!< (@ 0x000000B8) Receive Status Register 1     */
+    __IOM uint32_t USB_RXS2_REG;                 /*!< (@ 0x000000D8) Receive Status Register 2     */
+    __IOM uint32_t USB_RXS3_REG;                 /*!< (@ 0x000000F8) Receive Status Register 3     */
+  };
+  union
+  {
+    __IOM uint32_t rxc;
+    __IOM uint32_t USB_RXC0_REG;                 /*!< (@ 0x0000009C) Receive Command 0 Register    */
+    __IOM uint32_t USB_RXC1_REG;                 /*!< (@ 0x000000BC) Receive Command Register 1    */
+    __IOM uint32_t USB_RXC2_REG;                 /*!< (@ 0x000000DC) Receive Command Register 2    */
+    __IOM uint32_t USB_RXC3_REG;                 /*!< (@ 0x000000FC) Receive Command Register 3    */
+  };
+} EPx_REGS;
+
+#define EP_REGS(first_ep_reg) (EPx_REGS*)(&USB->first_ep_reg)
+
+// Dialog register fields and bit mask are very long. Filed masks repeat register names.
+// Those convenience macros are a way to reduce complexity of register modification lines.
+#define GET_BIT(val, field) (val & field ## _Msk) >> field ## _Pos
+#define REG_GET_BIT(reg, field) (USB->reg & USB_ ## reg ## _ ## field ## _Msk)
+#define REG_SET_BIT(reg, field) USB->reg |= USB_ ## reg ## _ ## field ## _Msk
+#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)
+
+typedef struct {
+  EPx_REGS * regs;
+  uint8_t * buffer;
+  // Total length of current transfer
+  uint16_t total_len;
+  // Bytes transferred so far
+  uint16_t transferred;
+  uint16_t max_packet_size;
+  // Packet size sent or received so far. It is used to modify transferred field
+  // after ACK is received or when filling ISO endpoint with size larger then
+  // FIFO size.
+  uint16_t last_packet_size;
+  uint8_t ep_addr;
+  // DATA0/1 toggle bit 1 DATA1 is expected or transmitted
+  uint8_t data1 : 1;
+  // Endpoint is stalled
+  uint8_t stall : 1;
+} xfer_ctl_t;
+
+static struct
+{
+  bool vbus_present;
+  bool in_reset;
+  xfer_ctl_t xfer_status[EP_MAX][2];
+} _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) } },
+  }
+};
+
+// Two endpoint 0 descriptor definition for unified dcd_edpt_open()
+static const tusb_desc_endpoint_t ep0OUT_desc =
+{
+  .bLength          = sizeof(tusb_desc_endpoint_t),
+  .bDescriptorType  = TUSB_DESC_ENDPOINT,
+
+  .bEndpointAddress = 0x00,
+  .bmAttributes     = { .xfer = TUSB_XFER_CONTROL },
+  .wMaxPacketSize   = { .size = CFG_TUD_ENDPOINT0_SIZE },
+  .bInterval        = 0
+};
+
+static const tusb_desc_endpoint_t ep0IN_desc =
+{
+  .bLength          = sizeof(tusb_desc_endpoint_t),
+  .bDescriptorType  = TUSB_DESC_ENDPOINT,
+
+  .bEndpointAddress = 0x80,
+  .bmAttributes     = { .xfer = TUSB_XFER_CONTROL },
+  .wMaxPacketSize   = { .size = CFG_TUD_ENDPOINT0_SIZE },
+  .bInterval        = 0
+};
+
+#define XFER_CTL_BASE(_ep, _dir) &_dcd.xfer_status[_ep][_dir]
+
+// Function could be called when VBUS change was detected.
+void tusb_vbus_changed(bool present)
+{
+  if (present != _dcd.vbus_present)
+  {
+    _dcd.vbus_present = present;
+    if (present)
+    {
+      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;
+    }
+    else
+    {
+      USB->USB_MCTRL_REG = 0;
+    }
+  }
+}
+
+static void transmit_packet(xfer_ctl_t * xfer)
+{
+  int left_to_send;
+  uint8_t const *src;
+  EPx_REGS *regs = xfer->regs;
+  uint32_t txc;
+
+  txc = USB_USB_TXC1_REG_USB_TX_EN_Msk;
+  if (xfer->data1) txc |= USB_USB_TXC1_REG_USB_TOGGLE_TX_Msk;
+
+  src = &xfer->buffer[xfer->transferred];
+  left_to_send = xfer->total_len - xfer->transferred;
+  if (left_to_send > xfer->max_packet_size - xfer->last_packet_size)
+  {
+    left_to_send = xfer->max_packet_size - xfer->last_packet_size;
+  }
+
+  // Loop checks TCOUNT all the time since this value is saturated to 31
+  // and can't be read just once before.
+  while ((regs->txs & USB_USB_TXS1_REG_USB_TCOUNT_Msk) > 0 && left_to_send > 0)
+  {
+    regs->txd = *src++;
+    xfer->last_packet_size++;
+    left_to_send--;
+  }
+  if (tu_edpt_number(xfer->ep_addr) != 0)
+  {
+    if (left_to_send > 0)
+    {
+      // Max packet size is set to value greater then FIFO. Enable fifo level warning
+      // to handle larger packets.
+      txc |= USB_USB_TXC1_REG_USB_TFWL_Msk;
+    }
+    else
+    {
+      // Whole packet already in fifo, no need to refill it later.  Mark last.
+      txc |= USB_USB_TXC1_REG_USB_LAST_Msk;
+    }
+  }
+  // Enable transfer with correct interrupts enabled
+  regs->txc = txc;
+}
+
+static void receive_packet(xfer_ctl_t *xfer, uint16_t bytes_in_fifo)
+{
+  EPx_REGS *regs = xfer->regs;
+  uint16_t remaining = xfer->total_len - xfer->transferred;
+  uint16_t receive_this_time = bytes_in_fifo;
+
+  if (remaining <= bytes_in_fifo) receive_this_time = remaining;
+
+  uint8_t *buf = xfer->buffer + xfer->transferred + xfer->last_packet_size;
+
+  for (int i = 0; i < receive_this_time; ++i) buf[i] = regs->rxd;
+
+  xfer->transferred += receive_this_time;
+  xfer->last_packet_size += receive_this_time;
+}
+
+static void handle_ep0_rx(void)
+{
+  int packet_size;
+  uint32_t rxs0 = USB->USB_RXS0_REG;
+
+  xfer_ctl_t *xfer = XFER_CTL_BASE(0, TUSB_DIR_OUT);
+
+  packet_size = GET_BIT(rxs0, USB_USB_RXS0_REG_USB_RCOUNT);
+  if (rxs0 & USB_USB_RXS0_REG_USB_SETUP_Msk)
+  {
+    xfer_ctl_t *xfer_in = XFER_CTL_BASE(0, TUSB_DIR_IN);
+    // Setup packet is in
+    for (int i = 0; i < packet_size; ++i) _setup_packet[i] = USB->USB_RXD0_REG;
+
+    xfer->stall = 0;
+    xfer->data1 = 1;
+    xfer_in->stall = 0;
+    xfer_in->data1 = 1;
+    REG_SET_BIT(USB_TXC0_REG, USB_TOGGLE_TX0);
+    REG_CLR_BIT(USB_EPC0_REG, USB_STALL);
+    dcd_event_setup_received(0, _setup_packet,true);
+  }
+  else
+  {
+    if (GET_BIT(rxs0, USB_USB_RXS0_REG_USB_TOGGLE_RX0) != xfer->data1)
+    {
+      // Toggle bit does not match discard packet
+      REG_SET_BIT(USB_RXC0_REG, USB_FLUSH);
+    }
+    else
+    {
+      receive_packet(xfer, packet_size);
+      xfer->data1 ^= 1;
+
+      if (xfer->total_len == xfer->transferred || xfer->last_packet_size < xfer->max_packet_size)
+      {
+        dcd_event_xfer_complete(0, 0, xfer->transferred, XFER_RESULT_SUCCESS, true);
+      }
+      else
+      {
+        xfer->last_packet_size = 0;
+        // Re-enable reception
+        REG_SET_BIT(USB_RXC0_REG, USB_RX_EN);
+      }
+    }
+  }
+}
+
+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;
+
+  txs0 = regs->USB_TXS0_REG;
+
+  if (GET_BIT(txs0, USB_USB_TXS0_REG_USB_TX_DONE))
+  {
+    // ACK received
+    if (GET_BIT(txs0, USB_USB_TXS0_REG_USB_ACK_STAT))
+    {
+      xfer->transferred += xfer->last_packet_size;
+      xfer->last_packet_size = 0;
+      xfer->data1 ^= 1;
+      REG_SET_VAL(USB_TXC0_REG, USB_TOGGLE_TX0, xfer->data1);
+      if (xfer->transferred == xfer->total_len)
+      {
+        dcd_event_xfer_complete(0, 0 | TUSB_DIR_IN_MASK, xfer->total_len, XFER_RESULT_SUCCESS, true);
+        return;
+      }
+    }
+    else
+    {
+      // Start from the beginning
+      xfer->last_packet_size = 0;
+    }
+    transmit_packet(xfer);
+  }
+}
+
+static void handle_epx_rx_ev(uint8_t ep)
+{
+  uint32_t rxs;
+  int packet_size;
+  xfer_ctl_t *xfer = XFER_CTL_BASE(ep, TUSB_DIR_OUT);
+
+  EPx_REGS *regs = xfer->regs;
+
+  rxs = regs->rxs;
+
+  if (GET_BIT(rxs, USB_USB_RXS1_REG_USB_RX_ERR))
+  {
+    regs->rxc |= USB_USB_RXC1_REG_USB_FLUSH_Msk;
+  }
+  else
+  {
+    packet_size = GET_BIT(rxs, USB_USB_RXS1_REG_USB_RXCOUNT);
+    receive_packet(xfer, packet_size);
+    if (GET_BIT(rxs, USB_USB_RXS1_REG_USB_RX_LAST))
+    {
+      if (GET_BIT(rxs, USB_USB_RXS1_REG_USB_TOGGLE_RX) != xfer->data1)
+      {
+        // Toggle bit does not match discard packet
+        regs->rxc |= USB_USB_RXC1_REG_USB_FLUSH_Msk;
+      }
+      else
+      {
+        xfer->data1 ^= 1;
+        if (xfer->total_len == xfer->transferred || xfer->last_packet_size < xfer->max_packet_size)
+        {
+          dcd_event_xfer_complete(0, xfer->ep_addr, xfer->transferred, XFER_RESULT_SUCCESS, true);
+        }
+        else
+        {
+          xfer->last_packet_size = 0;
+          // Re-enable reception
+          regs->rxc |= USB_USB_RXC1_REG_USB_RX_EN_Msk;
+        }
+      }
+    }
+  }
+}
+
+static void handle_rx_ev(void)
+{
+  if (USB->USB_RXEV_REG & 1)
+    handle_epx_rx_ev(1);
+  if (USB->USB_RXEV_REG & 2)
+    handle_epx_rx_ev(2);
+  if (USB->USB_RXEV_REG & 4)
+    handle_epx_rx_ev(3);
+}
+
+static void handle_epx_tx_ev(xfer_ctl_t *xfer)
+{
+  uint32_t usb_txs1_reg;
+  EPx_REGS *regs = xfer->regs;
+
+  usb_txs1_reg = regs->USB_TXS1_REG;
+
+  if (GET_BIT(usb_txs1_reg, USB_USB_TXS1_REG_USB_TX_DONE))
+  {
+    if (GET_BIT(usb_txs1_reg, USB_USB_TXS1_REG_USB_ACK_STAT))
+    {
+      // ACK received, update transfer state and DATA0/1 bit
+      xfer->transferred += xfer->last_packet_size;
+      xfer->last_packet_size = 0;
+      xfer->data1 ^= 1;
+
+      if (xfer->transferred == xfer->total_len)
+      {
+        dcd_event_xfer_complete(0, xfer->ep_addr, xfer->total_len, XFER_RESULT_SUCCESS, true);
+        return;
+      }
+    }
+    else
+    {
+      xfer->last_packet_size = 0;
+    }
+    transmit_packet(xfer);
+  }
+}
+
+static void handle_tx_ev(void)
+{
+  if (USB->USB_TXEV_REG & 1)
+    handle_epx_tx_ev(XFER_CTL_BASE(1, TUSB_DIR_IN));
+  if (USB->USB_TXEV_REG & 2)
+    handle_epx_tx_ev(XFER_CTL_BASE(2, TUSB_DIR_IN));
+  if (USB->USB_TXEV_REG & 4)
+    handle_epx_tx_ev(XFER_CTL_BASE(3, TUSB_DIR_IN));
+}
+
+static void handle_bus_reset(void)
+{
+  USB->USB_NFSR_REG = 0;
+  USB->USB_FAR_REG = 0x80;
+  USB->USB_ALTMSK_REG = 0;
+  USB->USB_NFSR_REG = NFSR_NODE_RESET;
+  USB->USB_TXMSK_REG = 0;
+  USB->USB_RXMSK_REG = 0;
+  (void)USB->USB_ALTEV_REG;
+  _dcd.in_reset = true;
+
+  dcd_event_bus_signal(0, DCD_EVENT_BUS_RESET, true);
+
+  USB->USB_MAMSK_REG = USB_USB_MAMSK_REG_USB_M_INTR_Msk |
+#if USE_SOF
+                       USB_USB_MAMSK_REG_USB_M_FRAME_Msk |
+#endif
+                       USB_USB_MAMSK_REG_USB_M_WARN_Msk |
+                       USB_USB_MAMSK_REG_USB_M_ALT_Msk;
+  USB->USB_NFSR_REG = NFSR_NODE_OPERATIONAL;
+  USB->USB_ALTMSK_REG = USB_USB_ALTMSK_REG_USB_M_SD3_Msk |
+                        USB_USB_ALTMSK_REG_USB_M_RESUME_Msk;
+  // There is no information about end of reset state
+  // USB_FRAME event will be used to enable reset detection again
+  REG_SET_BIT(USB_MAEV_REG, USB_FRAME);
+  dcd_edpt_open (0, &ep0OUT_desc);
+  dcd_edpt_open (0, &ep0IN_desc);
+}
+
+static void handle_alt_ev(void)
+{
+  uint32_t alt_ev = USB->USB_ALTEV_REG;
+
+  if (GET_BIT(alt_ev, USB_USB_ALTEV_REG_USB_RESET))
+  {
+    handle_bus_reset();
+  }
+  else
+  {
+    if (GET_BIT(alt_ev, USB_USB_ALTEV_REG_USB_RESUME))
+    {
+      USB->USB_NFSR_REG = NFSR_NODE_OPERATIONAL;
+      USB->USB_ALTMSK_REG &= ~USB_USB_ALTMSK_REG_USB_M_RESUME_Msk;
+      USB->USB_ALTMSK_REG |= USB_USB_ALTMSK_REG_USB_M_SD3_Msk;
+      dcd_event_bus_signal(0, DCD_EVENT_RESUME, true);
+    }
+    if (GET_BIT(alt_ev, USB_USB_ALTEV_REG_USB_SD3))
+    {
+      USB->USB_NFSR_REG = NFSR_NODE_SUSPEND;
+      USB->USB_ALTMSK_REG |= USB_USB_ALTMSK_REG_USB_M_RESUME_Msk;
+      USB->USB_ALTMSK_REG &= ~USB_USB_ALTMSK_REG_USB_M_SD3_Msk | USB_USB_ALTMSK_REG_USB_M_SD5_Msk;
+      dcd_event_bus_signal(0, DCD_EVENT_SUSPEND, true);
+    }
+  }
+}
+
+static void handle_epx_tx_refill(uint8_t ep)
+{
+  transmit_packet(XFER_CTL_BASE(ep, TUSB_DIR_IN));
+}
+
+static void handle_fifo_warning(void)
+{
+  uint32_t fifo_warning = USB->USB_FWEV_REG;
+
+  if (fifo_warning & 0x01)
+    handle_epx_tx_refill(1);
+  if (fifo_warning & 0x02)
+    handle_epx_tx_refill(2);
+  if (fifo_warning & 0x04)
+    handle_epx_tx_refill(3);
+  if (fifo_warning & 0x10)
+    handle_epx_rx_ev(1);
+  if (fifo_warning & 0x20)
+    handle_epx_rx_ev(2);
+  if (fifo_warning & 0x40)
+    handle_epx_rx_ev(3);
+}
+
+static void handle_ep0_nak(void)
+{
+  uint32_t ep0_nak = USB->USB_EP0_NAK_REG;
+
+  if (REG_GET_BIT(USB_EPC0_REG, USB_STALL))
+  {
+    if (GET_BIT(ep0_nak, USB_USB_EP0_NAK_REG_USB_EP0_INNAK))
+    {
+      // EP0 is stalled and NAK was sent, it means that RX is enabled
+      // Disable RX for now.
+      REG_CLR_BIT(USB_RXC0_REG, USB_RX_EN);
+      REG_SET_BIT(USB_TXC0_REG, USB_TX_EN);
+    }
+    if (GET_BIT(ep0_nak, USB_USB_EP0_NAK_REG_USB_EP0_OUTNAK))
+    {
+      REG_SET_BIT(USB_RXC0_REG, USB_RX_EN);
+    }
+  }
+  else
+  {
+    REG_CLR_BIT(USB_MAMSK_REG, USB_M_EP0_NAK);
+  }
+}
+
+/*------------------------------------------------------------------*/
+/* Controller API
+ *------------------------------------------------------------------*/
+void dcd_init(uint8_t rhport)
+{
+  (void)rhport;
+
+  USB->USB_MCTRL_REG = USB_USB_MCTRL_REG_USBEN_Msk;
+  tusb_vbus_changed((CRG_TOP->ANA_STATUS_REG & CRG_TOP_ANA_STATUS_REG_VBUS_AVAILABLE_Msk) != 0);
+}
+
+void dcd_int_enable(uint8_t rhport)
+{
+  (void)rhport;
+
+  NVIC_EnableIRQ(USB_IRQn);
+}
+
+void dcd_int_disable(uint8_t rhport)
+{
+  (void)rhport;
+
+  NVIC_DisableIRQ(USB_IRQn);
+}
+
+void dcd_set_address(uint8_t rhport, uint8_t dev_addr)
+{
+  (void)rhport;
+
+  // Set default address for one ZLP
+  USB->USB_EPC0_REG = USB_USB_EPC0_REG_USB_DEF_Msk;
+  USB->USB_FAR_REG = (dev_addr & USB_USB_FAR_REG_USB_AD_Msk) | USB_USB_FAR_REG_USB_AD_EN_Msk;
+  dcd_edpt_xfer(rhport, tu_edpt_addr(0, TUSB_DIR_IN), NULL, 0);
+}
+
+void dcd_remote_wakeup(uint8_t rhport)
+{
+  (void)rhport;
+}
+
+void dcd_connect(uint8_t rhport)
+{
+  (void)rhport;
+
+  REG_SET_BIT(USB_MCTRL_REG, USB_NAT);
+}
+
+void dcd_disconnect(uint8_t rhport)
+{
+  (void)rhport;
+
+  REG_CLR_BIT(USB_MCTRL_REG, USB_NAT);
+}
+
+
+/*------------------------------------------------------------------*/
+/* DCD Endpoint port
+ *------------------------------------------------------------------*/
+
+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);
+  uint8_t iso_mask = 0;
+  
+  (void)rhport;
+
+  TU_ASSERT(desc_edpt->wMaxPacketSize.size <= 1023);
+  TU_ASSERT(epnum < EP_MAX);
+
+  xfer->max_packet_size = desc_edpt->wMaxPacketSize.size;
+  xfer->ep_addr = desc_edpt->bEndpointAddress;
+  xfer->data1 = 0;
+
+  if (epnum != 0 && desc_edpt->bmAttributes.xfer == 1) iso_mask = USB_USB_EPC1_REG_USB_ISO_Msk;
+
+  if (epnum == 0)
+  {
+    USB->USB_MAMSK_REG |= USB_USB_MAMSK_REG_USB_M_EP0_RX_Msk |
+                          USB_USB_MAMSK_REG_USB_M_EP0_TX_Msk;
+  }
+  else
+  {
+    if (dir == TUSB_DIR_OUT)
+    {
+      xfer->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;
+      USB->USB_TXMSK_REG |= 0x101 << (epnum - 1);
+      REG_SET_BIT(USB_MAMSK_REG, USB_M_TX_EV);
+    }
+  }
+
+  return true;
+}
+
+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);
+  xfer_ctl_t * xfer = XFER_CTL_BASE(epnum, dir);
+
+  (void)rhport;
+
+  xfer->buffer = buffer;
+  xfer->total_len = total_bytes;
+  xfer->last_packet_size = 0;
+  xfer->transferred = 0;
+
+  if (dir == TUSB_DIR_OUT)
+  {
+    if (epnum != 0)
+    {
+      if (xfer->max_packet_size > 64)
+      {
+        // 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;
+      }
+      else
+      {
+        // 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;
+      }
+    }
+    // USB_RX_EN bit is in same place for all endpoints.
+    xfer->regs->rxc = USB_USB_RXC0_REG_USB_RX_EN_Msk;
+  }
+  else // IN
+  {
+    transmit_packet(xfer);
+  }
+
+  return true;
+}
+
+void dcd_edpt_stall(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);
+
+  (void)rhport;
+
+  xfer_ctl_t * xfer = XFER_CTL_BASE(epnum, dir);
+  xfer->stall = 1;
+
+  if (epnum == 0)
+  {
+    // EP0 has just one registers to control stall for IN and OUT
+    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;
+    }
+    else
+    {
+      if (xfer->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.
+        REG_SET_BIT(USB_MAMSK_REG, USB_M_EP0_NAK);
+      }
+      else
+      {
+        xfer->regs->USB_TXC0_REG |= USB_USB_TXC0_REG_USB_TX_EN_Msk;
+      }
+    }
+  }
+  else
+  {
+    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;
+    }
+    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;
+    }
+  }
+}
+
+void dcd_edpt_clear_stall(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);
+
+  (void)rhport;
+
+  xfer_ctl_t * xfer = XFER_CTL_BASE(epnum, dir);
+
+  // Clear stall is called in response to Clear Feature ENDPOINT_HALT, reset toggle
+  xfer->data1 = 0;
+  xfer->stall = 0;
+
+  if (dir == TUSB_DIR_OUT)
+  {
+    xfer->regs->epc_out &= ~USB_USB_EPC1_REG_USB_STALL_Msk;
+  }
+  else
+  {
+    xfer->regs->epc_in &= ~USB_USB_EPC1_REG_USB_STALL_Msk;
+  }
+  if (epnum == 0)
+  {
+    REG_CLR_BIT(USB_MAMSK_REG, USB_M_EP0_NAK);
+  }
+}
+
+/*------------------------------------------------------------------*/
+/* Interrupt Handler
+ *------------------------------------------------------------------*/
+
+void dcd_int_handler(uint8_t rhport)
+{
+  uint32_t int_status = USB->USB_MAEV_REG;
+
+  (void)rhport;
+
+  if (GET_BIT(int_status, USB_USB_MAEV_REG_USB_WARN))
+  {
+    handle_fifo_warning();
+  }
+
+  if (GET_BIT(int_status, USB_USB_MAEV_REG_USB_CH_EV))
+  {
+    // TODO: for now just clear interrupt
+    (void)USB->USB_CHARGER_STAT_REG;
+  }
+
+  if (GET_BIT(int_status, USB_USB_MAEV_REG_USB_EP0_NAK))
+  {
+    handle_ep0_nak();
+  }
+
+  if (GET_BIT(int_status, USB_USB_MAEV_REG_USB_EP0_RX))
+  {
+    handle_ep0_rx();
+  }
+
+  if (GET_BIT(int_status, USB_USB_MAEV_REG_USB_EP0_TX))
+  {
+    handle_ep0_tx();
+  }
+
+  if (GET_BIT(int_status, USB_USB_MAEV_REG_USB_RX_EV))
+  {
+    handle_rx_ev();
+  }
+
+  if (GET_BIT(int_status, USB_USB_MAEV_REG_USB_NAK))
+  {
+    (void)USB->USB_NAKEV_REG;
+  }
+
+  if (GET_BIT(int_status, USB_USB_MAEV_REG_USB_FRAME))
+  {
+    if (_dcd.in_reset)
+    {
+      // Enable reset detection
+      _dcd.in_reset = false;
+      (void)USB->USB_ALTEV_REG;
+    }
+#if USE_SOF
+    dcd_event_bus_signal(0, DCD_EVENT_SOF, true);
+#else
+    // SOF was used to re-enable reset detection
+    // No need to keep it enabled
+    USB->USB_MAMSK_REG &= ~USB_USB_MAMSK_REG_USB_M_FRAME_Msk;
+#endif
+  }
+
+  if (GET_BIT(int_status, USB_USB_MAEV_REG_USB_TX_EV))
+  {
+    handle_tx_ev();
+  }
+
+  if (GET_BIT(int_status, USB_USB_MAEV_REG_USB_ALT))
+  {
+    handle_alt_ev();
+  }
+}
+
+#endif
diff --git a/src/portable/espressif/esp32s2/dcd_esp32s2.c b/src/portable/espressif/esp32s2/dcd_esp32s2.c
index d49b69a10..3d30b6016 100644
--- a/src/portable/espressif/esp32s2/dcd_esp32s2.c
+++ b/src/portable/espressif/esp32s2/dcd_esp32s2.c
@@ -637,6 +637,13 @@ static void handle_epin_ints(void)
           USB0.dtknqr4_fifoemptymsk &= ~(1 << n);
         }
       }
+
+      // XFER Timeout
+      if (USB0.in_ep_reg[n].diepint & USB_D_TIMEOUT0_M) {
+        // Clear interrupt or enpoint will hang.
+        USB0.in_ep_reg[n].diepint = USB_D_TIMEOUT0_M;
+        // Maybe retry?
+      }
     }
   }
 }
diff --git a/src/portable/microchip/samd/dcd_samd.c b/src/portable/microchip/samd/dcd_samd.c
index a859b72b9..d0cc8560a 100644
--- a/src/portable/microchip/samd/dcd_samd.c
+++ b/src/portable/microchip/samd/dcd_samd.c
@@ -26,7 +26,8 @@
 
 #include "tusb_option.h"
 
-#if TUSB_OPT_DEVICE_ENABLED && (CFG_TUSB_MCU == OPT_MCU_SAMD51 || CFG_TUSB_MCU == OPT_MCU_SAMD21)
+#if TUSB_OPT_DEVICE_ENABLED && \
+    (CFG_TUSB_MCU == OPT_MCU_SAMD21 || CFG_TUSB_MCU == OPT_MCU_SAMD51 || CFG_TUSB_MCU == OPT_MCU_SAME5X)
 
 #include "sam.h"
 #include "device/dcd.h"
@@ -42,6 +43,8 @@ static inline void prepare_setup(void)
 {
   // Only make sure the EP0 OUT buffer is ready
   sram_registers[0][0].ADDR.reg = (uint32_t) _setup_packet;
+  sram_registers[0][0].PCKSIZE.bit.MULTI_PACKET_SIZE = sizeof(_setup_packet);
+  sram_registers[0][0].PCKSIZE.bit.BYTE_COUNT = 0;
 }
 
 // Setup the control endpoint 0.
@@ -90,7 +93,7 @@ void dcd_init (uint8_t rhport)
   USB->DEVICE.INTENSET.reg = /* USB_DEVICE_INTENSET_SOF | */ USB_DEVICE_INTENSET_EORST;
 }
 
-#if CFG_TUSB_MCU == OPT_MCU_SAMD51
+#if CFG_TUSB_MCU == OPT_MCU_SAMD51 || CFG_TUSB_MCU == OPT_MCU_SAME5X
 
 void dcd_int_enable(uint8_t rhport)
 {
@@ -123,6 +126,11 @@ void dcd_int_disable(uint8_t rhport)
   (void) rhport;
   NVIC_DisableIRQ(USB_IRQn);
 }
+
+#else
+
+#error "No implementation available for dcd_int_enable / dcd_int_disable"
+
 #endif
 
 void dcd_set_address (uint8_t rhport, uint8_t dev_addr)
diff --git a/src/portable/nuvoton/nuc505/dcd_nuc505.c b/src/portable/nuvoton/nuc505/dcd_nuc505.c
index e67b69ab2..a7961a687 100644
--- a/src/portable/nuvoton/nuc505/dcd_nuc505.c
+++ b/src/portable/nuvoton/nuc505/dcd_nuc505.c
@@ -452,7 +452,9 @@ void dcd_int_handler(uint8_t rhport)
       USBD->CEPINTEN = USBD_CEPINTEN_SETUPPKIEN_Msk;
       USBD->BUSINTEN = USBD_BUSINTEN_RSTIEN_Msk | USBD_BUSINTEN_RESUMEIEN_Msk | USBD_BUSINTEN_SUSPENDIEN_Msk | USBD_BUSINTEN_DMADONEIEN_Msk;
       USBD->CEPINTSTS = 0x1ffc;
-      dcd_event_bus_signal(0, DCD_EVENT_BUS_RESET, true);
+
+      tusb_speed_t speed = (USBD->OPER & USBD_OPER_CURSPD_Msk) ? TUSB_SPEED_HIGH : TUSB_SPEED_FULL;
+      dcd_event_bus_reset(0, speed, true);
     }
 
     if (bus_state & USBD_BUSINTSTS_RESUMEIF_Msk)
diff --git a/src/portable/nxp/transdimension/dcd_transdimension.c b/src/portable/nxp/transdimension/dcd_transdimension.c
index 7baf43e23..46751cf5d 100644
--- a/src/portable/nxp/transdimension/dcd_transdimension.c
+++ b/src/portable/nxp/transdimension/dcd_transdimension.c
@@ -82,11 +82,14 @@ enum {
 };
 
 // PORTSC1
+#define PORTSC1_PORT_SPEED_POS    26
+
 enum {
   PORTSC1_CURRENT_CONNECT_STATUS = TU_BIT(0),
   PORTSC1_FORCE_PORT_RESUME      = TU_BIT(6),
   PORTSC1_SUSPEND                = TU_BIT(7),
   PORTSC1_FORCE_FULL_SPEED       = TU_BIT(24),
+  PORTSC1_PORT_SPEED             = TU_BIT(26) | TU_BIT(27)
 };
 
 // OTGSC
@@ -236,7 +239,7 @@ typedef struct
 {
   dcd_registers_t* regs;  // registers
   const IRQn_Type irqnum; // IRQ number
-  const uint8_t ep_count;   // Max bi-directional Endpoints
+  const uint8_t ep_count; // Max bi-directional Endpoints
 }dcd_controller_t;
 
 #if CFG_TUSB_MCU == OPT_MCU_MIMXRT10XX
@@ -244,7 +247,7 @@ typedef struct
   // Therefore QHD_MAX is 2 x max endpoint count
   #define QHD_MAX  (8*2)
 
-  dcd_controller_t _dcd_controller[] =
+  static const dcd_controller_t _dcd_controller[] =
   {
     // RT1010 and RT1020 only has 1 USB controller
     #if FSL_FEATURE_SOC_USBHS_COUNT == 1
@@ -258,7 +261,7 @@ typedef struct
 #else
   #define QHD_MAX (6*2)
 
-  dcd_controller_t _dcd_controller[] =
+  static const dcd_controller_t _dcd_controller[] =
   {
     { .regs = (dcd_registers_t*) LPC_USB0_BASE, .irqnum = USB0_IRQn, .ep_count = 6 },
     { .regs = (dcd_registers_t*) LPC_USB1_BASE, .irqnum = USB1_IRQn, .ep_count = 4 }
@@ -512,7 +515,8 @@ void dcd_int_handler(uint8_t rhport)
   if (int_status & INTR_RESET)
   {
     bus_reset(rhport);
-    dcd_event_bus_signal(rhport, DCD_EVENT_BUS_RESET, true);
+    uint32_t speed = (dcd_reg->PORTSC1 & PORTSC1_PORT_SPEED) >> PORTSC1_PORT_SPEED_POS;
+    dcd_event_bus_reset(rhport, (tusb_speed_t) speed, true);
   }
 
   if (int_status & INTR_SUSPEND)
diff --git a/src/portable/sony/cxd56/dcd_cxd56.c b/src/portable/sony/cxd56/dcd_cxd56.c
index bcf94dc21..9269903a5 100644
--- a/src/portable/sony/cxd56/dcd_cxd56.c
+++ b/src/portable/sony/cxd56/dcd_cxd56.c
@@ -134,7 +134,8 @@ static void _dcd_disconnect(FAR struct usbdevclass_driver_s *driver, FAR struct
 {
   (void) driver;
 
-  dcd_event_bus_signal(0, DCD_EVENT_BUS_RESET, true);
+  tusb_speed_t speed = (dev->speed == 3) ? TUSB_SPEED_HIGH : TUSB_SPEED_FULL;
+  dcd_event_bus_reset(0, speed, true);
   DEV_CONNECT(dev);
 }
 
diff --git a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
index b539f4795..da184abe7 100644
--- a/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
+++ b/src/portable/st/stm32_fsdev/dcd_stm32_fsdev.c
@@ -144,6 +144,12 @@
 #  define DCD_STM32_BTABLE_LENGTH (PMA_LENGTH - DCD_STM32_BTABLE_BASE)
 #endif
 
+// Since TinyUSB doesn't use SOF for now, and this interrupt too often (1ms interval)
+// We disable SOF for now until needed later on
+#ifndef USE_SOF
+#  define USE_SOF     0
+#endif
+
 /***************************************************
  * Checks, structs, defines, function definitions, etc.
  */
@@ -235,7 +241,7 @@ void dcd_init (uint8_t rhport)
     pcd_set_endpoint(USB,i,0u);
   }
 
-  USB->CNTR |= USB_CNTR_RESETM | USB_CNTR_SOFM | USB_CNTR_ESOFM | USB_CNTR_CTRM | USB_CNTR_SUSPM | USB_CNTR_WKUPM;
+  USB->CNTR |= USB_CNTR_RESETM | (USE_SOF ? USB_CNTR_SOFM : 0) | USB_CNTR_ESOFM | USB_CNTR_CTRM | USB_CNTR_SUSPM | USB_CNTR_WKUPM;
   dcd_handle_bus_reset();
   
   // Data-line pull-up is left disconnected.
@@ -542,10 +548,12 @@ void dcd_int_handler(uint8_t rhport) {
     dcd_event_bus_signal(0, DCD_EVENT_SUSPEND, true);
   }
 
+#if USE_SOF
   if(int_status & USB_ISTR_SOF) {
     reg16_clear_bits(&USB->ISTR, USB_ISTR_SOF);
     dcd_event_bus_signal(0, DCD_EVENT_SOF, true);
   }
+#endif 
 
   if(int_status & USB_ISTR_ESOF) {
     if(remoteWakeCountdown == 1u)
diff --git a/src/portable/st/synopsys/dcd_synopsys.c b/src/portable/st/synopsys/dcd_synopsys.c
index 717a59db6..7eb63e52f 100644
--- a/src/portable/st/synopsys/dcd_synopsys.c
+++ b/src/portable/st/synopsys/dcd_synopsys.c
@@ -3,6 +3,7 @@
  *
  * Copyright (c) 2018 Scott Shawcroft, 2019 William D. Jones for Adafruit Industries
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
+ * Copyright (c) 2020 Jan Duempelmann
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -27,6 +28,10 @@
 
 #include "tusb_option.h"
 
+// Since TinyUSB doesn't use SOF for now, and this interrupt too often (1ms interval)
+// We disable SOF for now until needed later on
+#define USE_SOF     0
+
 #if defined (STM32F105x8) || defined (STM32F105xB) || defined (STM32F105xC) || \
     defined (STM32F107xB) || defined (STM32F107xC)
 #define STM32F1_SYNOPSYS
@@ -48,62 +53,88 @@
       (CFG_TUSB_MCU == OPT_MCU_STM32L4 && defined(STM32L4_SYNOPSYS)) \
     )
 
-// TODO Support OTG_HS
 // EP_MAX       : Max number of bi-directional endpoints including EP0
 // EP_FIFO_SIZE : Size of dedicated USB SRAM
 #if CFG_TUSB_MCU == OPT_MCU_STM32F1
   #include "stm32f1xx.h"
-  #define EP_MAX          4
-  #define EP_FIFO_SIZE    1280
+  #define EP_MAX_FS       4
+  #define EP_FIFO_SIZE_FS 1280
+
 #elif CFG_TUSB_MCU == OPT_MCU_STM32F2
   #include "stm32f2xx.h"
-  #define EP_MAX          USB_OTG_FS_MAX_IN_ENDPOINTS
-  #define EP_FIFO_SIZE    USB_OTG_FS_TOTAL_FIFO_SIZE
+  #define EP_MAX_FS       USB_OTG_FS_MAX_IN_ENDPOINTS
+  #define EP_FIFO_SIZE_FS USB_OTG_FS_TOTAL_FIFO_SIZE
+
 #elif CFG_TUSB_MCU == OPT_MCU_STM32F4
   #include "stm32f4xx.h"
-  #define EP_MAX          USB_OTG_FS_MAX_IN_ENDPOINTS
-  #define EP_FIFO_SIZE    USB_OTG_FS_TOTAL_FIFO_SIZE
+  #define EP_MAX_FS       USB_OTG_FS_MAX_IN_ENDPOINTS
+  #define EP_FIFO_SIZE_FS USB_OTG_FS_TOTAL_FIFO_SIZE
+  #define EP_MAX_HS       USB_OTG_HS_MAX_IN_ENDPOINTS
+  #define EP_FIFO_SIZE_HS USB_OTG_HS_TOTAL_FIFO_SIZE
+
 #elif CFG_TUSB_MCU == OPT_MCU_STM32H7
   #include "stm32h7xx.h"
-  #define EP_MAX          9
-  #define EP_FIFO_SIZE    4096
-  // TODO The official name of the USB FS peripheral on H7 is "USB2_OTG_FS".
+  #define EP_MAX_FS       9
+  #define EP_FIFO_SIZE_FS 4096
+  #define EP_MAX_HS       9
+  #define EP_FIFO_SIZE_HS 4096
+
 #elif CFG_TUSB_MCU == OPT_MCU_STM32F7
   #include "stm32f7xx.h"
-  #define EP_MAX          6
-  #define EP_FIFO_SIZE    1280
+  #define EP_MAX_FS       6
+  #define EP_FIFO_SIZE_FS 1280
+  #define EP_MAX_HS       9
+  #define EP_FIFO_SIZE_HS 4096
+
 #elif CFG_TUSB_MCU == OPT_MCU_STM32L4
   #include "stm32l4xx.h"
-  #define EP_MAX          6
-  #define EP_FIFO_SIZE    1280
+  #define EP_MAX_FS       6
+  #define EP_FIFO_SIZE_FS 1280
+
 #else
   #error "Unsupported MCUs"
+
 #endif
 
 #include "device/dcd.h"
 
-/*------------------------------------------------------------------*/
-/* MACRO TYPEDEF CONSTANT ENUM
- *------------------------------------------------------------------*/
+//--------------------------------------------------------------------+
+// MACRO TYPEDEF CONSTANT ENUM
+//--------------------------------------------------------------------+
 
-// Since TinyUSB doesn't use SOF for now, and this interrupt too often (1ms interval)
-// We disable SOF for now until needed later on
-#define USE_SOF     0
+// On STM32 we associate Port0 to OTG_FS, and Port1 to OTG_HS
+#if TUD_OPT_RHPORT == 0
+  #define EP_MAX            EP_MAX_FS
+  #define EP_FIFO_SIZE      EP_FIFO_SIZE_FS
+  #define RHPORT_REGS_BASE  USB_OTG_FS_PERIPH_BASE
+  #define RHPORT_IRQn       OTG_FS_IRQn
 
-#define DEVICE_BASE     (USB_OTG_DeviceTypeDef *) (USB_OTG_FS_PERIPH_BASE + USB_OTG_DEVICE_BASE)
-#define OUT_EP_BASE     (USB_OTG_OUTEndpointTypeDef *) (USB_OTG_FS_PERIPH_BASE + USB_OTG_OUT_ENDPOINT_BASE)
-#define IN_EP_BASE      (USB_OTG_INEndpointTypeDef *) (USB_OTG_FS_PERIPH_BASE + USB_OTG_IN_ENDPOINT_BASE)
-#define FIFO_BASE(_x)   ((volatile uint32_t *) (USB_OTG_FS_PERIPH_BASE + USB_OTG_FIFO_BASE + (_x) * USB_OTG_FIFO_SIZE))
+#else
+  #define EP_MAX            EP_MAX_HS
+  #define EP_FIFO_SIZE      EP_FIFO_SIZE_HS
+  #define RHPORT_REGS_BASE  USB_OTG_HS_PERIPH_BASE
+  #define RHPORT_IRQn       OTG_HS_IRQn
+#endif
+
+#define GLOBAL_BASE(_port)     ((USB_OTG_GlobalTypeDef*) RHPORT_REGS_BASE)
+#define DEVICE_BASE(_port)     (USB_OTG_DeviceTypeDef *) (RHPORT_REGS_BASE + USB_OTG_DEVICE_BASE)
+#define OUT_EP_BASE(_port)     (USB_OTG_OUTEndpointTypeDef *) (RHPORT_REGS_BASE + USB_OTG_OUT_ENDPOINT_BASE)
+#define IN_EP_BASE(_port)      (USB_OTG_INEndpointTypeDef *) (RHPORT_REGS_BASE + USB_OTG_IN_ENDPOINT_BASE)
+#define FIFO_BASE(_port, _x)   ((volatile uint32_t *) (RHPORT_REGS_BASE + USB_OTG_FIFO_BASE + (_x) * USB_OTG_FIFO_SIZE))
 
-static TU_ATTR_ALIGNED(4) uint32_t _setup_packet[6];
-static uint8_t _setup_offs; // We store up to 3 setup packets.
+enum
+{
+  DCD_HIGH_SPEED        = 0, // Highspeed mode
+  DCD_FULL_SPEED_USE_HS = 1, // Full speed in Highspeed port (probably with internal PHY)
+  DCD_FULL_SPEED        = 3, // Full speed with internal PHY
+};
+
+static TU_ATTR_ALIGNED(4) uint32_t _setup_packet[2];
 
 typedef struct {
   uint8_t * buffer;
   uint16_t total_len;
-  uint16_t queued_len;
   uint16_t max_size;
-  bool short_packet;
 } xfer_ctl_t;
 
 typedef volatile uint32_t * usb_fifo_t;
@@ -111,11 +142,23 @@ typedef volatile uint32_t * usb_fifo_t;
 xfer_ctl_t xfer_status[EP_MAX][2];
 #define XFER_CTL_BASE(_ep, _dir) &xfer_status[_ep][_dir]
 
+// EP0 transfers are limited to 1 packet - larger sizes has to be split
+static uint16_t ep0_pending[2];     // Index determines direction as tusb_dir_t type
+
+// FIFO RAM allocation so far in words
+static uint16_t _allocated_fifo_words;
 
 // Setup the control endpoint 0.
-static void bus_reset(void) {
-  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE;
-  USB_OTG_OUTEndpointTypeDef * out_ep = OUT_EP_BASE;
+static void bus_reset(uint8_t rhport)
+{
+  (void) rhport;
+
+  USB_OTG_GlobalTypeDef * usb_otg = GLOBAL_BASE(rhport);
+  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE(rhport);
+  USB_OTG_OUTEndpointTypeDef * out_ep = OUT_EP_BASE(rhport);
+  USB_OTG_INEndpointTypeDef * in_ep = IN_EP_BASE(rhport);
+
+  tu_memclr(xfer_status, sizeof(xfer_status));
 
   for(uint8_t n = 0; n < EP_MAX; n++) {
     out_ep[n].DOEPCTL |= USB_OTG_DOEPCTL_SNAK;
@@ -147,100 +190,286 @@ static void bus_reset(void) {
   // - Each EP IN needs at least max packet size, 16 words is sufficient for EP0 IN
   //
   // - All EP OUT shared a unique OUT FIFO which uses
-  //   * 10 locations in hardware for setup packets + setup control words (up to 3 setup packets).
-  //   * 2 locations for OUT endpoint control words.
-  //   * 16 for largest packet size of 64 bytes. ( TODO Highspeed is 512 bytes)
-  //   * 1 location for global NAK (not required/used here).
-  //   * It is recommended to allocate 2 times the largest packet size, therefore
-  //   Recommended value = 10 + 1 + 2 x (16+2) = 47 --> Let's make it 52
-  USB_OTG_FS->GRXFSIZ = 52;
+  //   - 13 for setup packets + control words (up to 3 setup packets).
+  //   - 1 for global NAK (not required/used here).
+  //   - Largest-EPsize / 4 + 1. ( FS: 64 bytes, HS: 512 bytes). Recommended is  "2 x (Largest-EPsize/4) + 1"
+  //   - 2 for each used OUT endpoint
+  //
+  //   Therefore GRXFSIZ = 13 + 1 + 1 + 2 x (Largest-EPsize/4) + 2 x EPOUTnum
+  //   - FullSpeed (64 Bytes ): GRXFSIZ = 15 + 2 x  16 + 2 x EP_MAX = 47  + 2 x EP_MAX
+  //   - Highspeed (512 bytes): GRXFSIZ = 15 + 2 x 128 + 2 x EP_MAX = 271 + 2 x EP_MAX
+  //
+  //   NOTE: Largest-EPsize & EPOUTnum is actual used endpoints in configuration. Since DCD has no knowledge
+  //   of the overall picture yet. We will use the worst scenario: largest possible + EP_MAX
+  //
+  //   FIXME: for Isochronous, largest EP size can be 1023/1024 for FS/HS respectively. In addition if multiple ISO
+  //   are enabled at least "2 x (Largest-EPsize/4) + 1" are recommended.  Maybe provide a macro for application to
+  //   overwrite this.
+
+#if TUD_OPT_HIGH_SPEED
+  _allocated_fifo_words = 271 + 2*EP_MAX;
+#else
+  _allocated_fifo_words =  47 + 2*EP_MAX;
+#endif
+
+  usb_otg->GRXFSIZ = _allocated_fifo_words;
 
   // Control IN uses FIFO 0 with 64 bytes ( 16 32-bit word )
-  USB_OTG_FS->DIEPTXF0_HNPTXFSIZ = (16 << USB_OTG_TX0FD_Pos) | (USB_OTG_FS->GRXFSIZ & 0x0000ffffUL);
+  usb_otg->DIEPTXF0_HNPTXFSIZ = (16 << USB_OTG_TX0FD_Pos) | _allocated_fifo_words;
+
+  _allocated_fifo_words += 16;
+
+  // TU_LOG2_INT(_allocated_fifo_words);
+
+  // Fixed control EP0 size to 64 bytes
+  in_ep[0].DIEPCTL &= ~(0x03 << USB_OTG_DIEPCTL_MPSIZ_Pos);
+  xfer_status[0][TUSB_DIR_OUT].max_size = xfer_status[0][TUSB_DIR_IN].max_size = 64;
 
   out_ep[0].DOEPTSIZ |= (3 << USB_OTG_DOEPTSIZ_STUPCNT_Pos);
 
-  USB_OTG_FS->GINTMSK |= USB_OTG_GINTMSK_OEPINT | USB_OTG_GINTMSK_IEPINT;
+  usb_otg->GINTMSK |= USB_OTG_GINTMSK_OEPINT | USB_OTG_GINTMSK_IEPINT;
 }
 
-static void end_of_reset(void) {
-  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE;
-  USB_OTG_INEndpointTypeDef * in_ep = IN_EP_BASE;
-  // On current silicon on the Full Speed core, speed is fixed to Full Speed.
-  // However, keep for debugging and in case Low Speed is ever supported.
-  uint32_t enum_spd = (dev->DSTS & USB_OTG_DSTS_ENUMSPD_Msk) >> USB_OTG_DSTS_ENUMSPD_Pos;
+// Set turn-around timeout according to link speed
+static void set_turnaround(USB_OTG_GlobalTypeDef * usb_otg, tusb_speed_t speed)
+{
+  usb_otg->GUSBCFG &= ~USB_OTG_GUSBCFG_TRDT;
 
-  // Maximum packet size for EP 0 is set for both directions by writing
-  // DIEPCTL.
-  if(enum_spd == 0x03) {
-    // 64 bytes
-    in_ep[0].DIEPCTL &= ~(0x03 << USB_OTG_DIEPCTL_MPSIZ_Pos);
-    xfer_status[0][TUSB_DIR_OUT].max_size = 64;
-    xfer_status[0][TUSB_DIR_IN].max_size = 64;
-  } else {
-    // 8 bytes
-    in_ep[0].DIEPCTL |= (0x03 << USB_OTG_DIEPCTL_MPSIZ_Pos);
-    xfer_status[0][TUSB_DIR_OUT].max_size = 8;
-    xfer_status[0][TUSB_DIR_IN].max_size = 8;
+  if ( speed == TUSB_SPEED_HIGH )
+  {
+    // Use fixed 0x09 for Highspeed
+    usb_otg->GUSBCFG |= (0x09 << USB_OTG_GUSBCFG_TRDT_Pos);
+  }
+  else
+  {
+    // Turnaround timeout depends on the MCU clock
+    extern uint32_t SystemCoreClock;
+    uint32_t turnaround;
+
+    if ( SystemCoreClock >= 32000000U )
+      turnaround = 0x6U;
+    else if ( SystemCoreClock >= 27500000U )
+      turnaround = 0x7U;
+    else if ( SystemCoreClock >= 24000000U )
+      turnaround = 0x8U;
+    else if ( SystemCoreClock >= 21800000U )
+      turnaround = 0x9U;
+    else if ( SystemCoreClock >= 20000000U )
+      turnaround = 0xAU;
+    else if ( SystemCoreClock >= 18500000U )
+      turnaround = 0xBU;
+    else if ( SystemCoreClock >= 17200000U )
+      turnaround = 0xCU;
+    else if ( SystemCoreClock >= 16000000U )
+      turnaround = 0xDU;
+    else if ( SystemCoreClock >= 15000000U )
+      turnaround = 0xEU;
+    else
+      turnaround = 0xFU;
+
+    // Fullspeed depends on MCU clocks, but we will use 0x06 for 32+ Mhz
+    usb_otg->GUSBCFG |= (turnaround << USB_OTG_GUSBCFG_TRDT_Pos);
   }
 }
 
+static tusb_speed_t get_speed(uint8_t rhport)
+{
+  (void) rhport;
+  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE(rhport);
+  uint32_t const enum_spd = (dev->DSTS & USB_OTG_DSTS_ENUMSPD_Msk) >> USB_OTG_DSTS_ENUMSPD_Pos;
+  return (enum_spd == DCD_HIGH_SPEED) ? TUSB_SPEED_HIGH : TUSB_SPEED_FULL;
+}
+
+static void set_speed(uint8_t rhport, tusb_speed_t speed)
+{
+  uint32_t bitvalue;
+
+  if ( rhport == 1 )
+  {
+    bitvalue = ((TUSB_SPEED_HIGH == speed) ? DCD_HIGH_SPEED : DCD_FULL_SPEED_USE_HS);
+  }
+  else
+  {
+    bitvalue = DCD_FULL_SPEED;
+  }
+
+  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE(rhport);
+
+  // Clear and set speed bits
+  dev->DCFG &= ~(3 << USB_OTG_DCFG_DSPD_Pos);
+  dev->DCFG |= (bitvalue << USB_OTG_DCFG_DSPD_Pos);
+}
+
+
+#if defined(USB_HS_PHYC)
+static bool USB_HS_PHYCInit(void)
+{
+  USB_HS_PHYC_GlobalTypeDef *usb_hs_phyc = (USB_HS_PHYC_GlobalTypeDef*) USB_HS_PHYC_CONTROLLER_BASE;
+
+  // Enable LDO
+  usb_hs_phyc->USB_HS_PHYC_LDO |= USB_HS_PHYC_LDO_ENABLE;
+
+  // Wait until LDO ready
+  while ( 0 == (usb_hs_phyc->USB_HS_PHYC_LDO & USB_HS_PHYC_LDO_STATUS) ) {}
+
+  uint32_t phyc_pll = 0;
+
+  // TODO Try to get HSE_VALUE from registers instead of depending CFLAGS
+  switch ( HSE_VALUE )
+  {
+    case 12000000: phyc_pll = USB_HS_PHYC_PLL1_PLLSEL_12MHZ   ; break;
+    case 12500000: phyc_pll = USB_HS_PHYC_PLL1_PLLSEL_12_5MHZ ; break;
+    case 16000000: phyc_pll = USB_HS_PHYC_PLL1_PLLSEL_16MHZ   ; break;
+    case 24000000: phyc_pll = USB_HS_PHYC_PLL1_PLLSEL_24MHZ   ; break;
+    case 25000000: phyc_pll = USB_HS_PHYC_PLL1_PLLSEL_25MHZ   ; break;
+    case 32000000: phyc_pll = USB_HS_PHYC_PLL1_PLLSEL_Msk     ; break; // Value not defined in header
+    default:
+      TU_ASSERT(0);
+  }
+  usb_hs_phyc->USB_HS_PHYC_PLL = phyc_pll;
+
+  // Control the tuning interface of the High Speed PHY
+  // Use magic value (USB_HS_PHYC_TUNE_VALUE) from ST driver
+  usb_hs_phyc->USB_HS_PHYC_TUNE |= 0x00000F13U;
+
+  // Enable PLL internal PHY
+  usb_hs_phyc->USB_HS_PHYC_PLL |= USB_HS_PHYC_PLL_PLLEN;
+
+  // Original ST code has 2 ms delay for PLL stabilization.
+  // Primitive test shows that more than 10 USB un/replug cycle showed no error with enumeration
+
+  return true;
+}
+#endif
+
+static void edpt_schedule_packets(uint8_t rhport, uint8_t const epnum, uint8_t const dir, uint16_t const num_packets, uint16_t total_bytes)
+{
+  (void) rhport;
+
+  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE(rhport);
+  USB_OTG_OUTEndpointTypeDef * out_ep = OUT_EP_BASE(rhport);
+  USB_OTG_INEndpointTypeDef * in_ep = IN_EP_BASE(rhport);
+
+  // EP0 is limited to one packet each xfer
+  // We use multiple transaction of xfer->max_size length to get a whole transfer done
+  if(epnum == 0) {
+    xfer_ctl_t * const xfer = XFER_CTL_BASE(epnum, dir);
+    total_bytes = tu_min16(ep0_pending[dir], xfer->max_size);
+    ep0_pending[dir] -= total_bytes;
+  }
+
+  // IN and OUT endpoint xfers are interrupt-driven, we just schedule them here.
+  if(dir == TUSB_DIR_IN) {
+    // A full IN transfer (multiple packets, possibly) triggers XFRC.
+    in_ep[epnum].DIEPTSIZ = (num_packets << USB_OTG_DIEPTSIZ_PKTCNT_Pos) |
+        ((total_bytes << USB_OTG_DIEPTSIZ_XFRSIZ_Pos) & USB_OTG_DIEPTSIZ_XFRSIZ_Msk);
+
+    in_ep[epnum].DIEPCTL |= USB_OTG_DIEPCTL_EPENA | USB_OTG_DIEPCTL_CNAK;
+    // Enable fifo empty interrupt only if there are something to put in the fifo.
+    if(total_bytes != 0) {
+      dev->DIEPEMPMSK |= (1 << epnum);
+    }
+  } else {
+    // A full OUT transfer (multiple packets, possibly) triggers XFRC.
+    out_ep[epnum].DOEPTSIZ &= ~(USB_OTG_DOEPTSIZ_PKTCNT_Msk | USB_OTG_DOEPTSIZ_XFRSIZ);
+    out_ep[epnum].DOEPTSIZ |= (num_packets << USB_OTG_DOEPTSIZ_PKTCNT_Pos) |
+        ((total_bytes << USB_OTG_DOEPTSIZ_XFRSIZ_Pos) & USB_OTG_DOEPTSIZ_XFRSIZ_Msk);
+
+    out_ep[epnum].DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
+  }
+}
 
 /*------------------------------------------------------------------*/
 /* Controller API
  *------------------------------------------------------------------*/
 void dcd_init (uint8_t rhport)
 {
-  (void) rhport;
-
   // Programming model begins in the last section of the chapter on the USB
   // peripheral in each Reference Manual.
-  USB_OTG_FS->GAHBCFG |= USB_OTG_GAHBCFG_GINT;
 
-  // No HNP/SRP (no OTG support), program timeout later, turnaround
-  // programmed for 32+ MHz.
-  // TODO: PHYSEL is read-only on some cores (STM32F407). Worth gating?
-  USB_OTG_FS->GUSBCFG |= (0x06 << USB_OTG_GUSBCFG_TRDT_Pos) | USB_OTG_GUSBCFG_PHYSEL;
+  USB_OTG_GlobalTypeDef * usb_otg = GLOBAL_BASE(rhport);
+
+  // No HNP/SRP (no OTG support), program timeout later.
+  if ( rhport == 1 )
+  {
+    // On selected MCUs HS port1 can be used with external PHY via ULPI interface
+
+    // deactivate internal PHY
+    usb_otg->GCCFG &= ~USB_OTG_GCCFG_PWRDWN;
+
+    // Init The UTMI Interface
+    usb_otg->GUSBCFG &= ~(USB_OTG_GUSBCFG_TSDPS | USB_OTG_GUSBCFG_ULPIFSLS | USB_OTG_GUSBCFG_PHYSEL);
+
+    // Select default internal VBUS Indicator and Drive for ULPI
+    usb_otg->GUSBCFG &= ~(USB_OTG_GUSBCFG_ULPIEVBUSD | USB_OTG_GUSBCFG_ULPIEVBUSI);
+
+#if defined(USB_HS_PHYC)
+    // Highspeed with embedded UTMI PHYC
+
+    // Select UTMI Interface
+    usb_otg->GUSBCFG &= ~USB_OTG_GUSBCFG_ULPI_UTMI_SEL;
+    usb_otg->GCCFG |= USB_OTG_GCCFG_PHYHSEN;
+
+    // Enables control of a High Speed USB PHY
+    USB_HS_PHYCInit();
+#endif
+  } else
+  {
+    // Enable internal PHY
+    usb_otg->GUSBCFG |= USB_OTG_GUSBCFG_PHYSEL;
+  }
+
+  // Reset core after selecting PHY
+  // Wait AHB IDLE, reset then wait until it is cleared
+  while ((usb_otg->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0U) {}
+  usb_otg->GRSTCTL |= USB_OTG_GRSTCTL_CSRST;
+  while ((usb_otg->GRSTCTL & USB_OTG_GRSTCTL_CSRST) == USB_OTG_GRSTCTL_CSRST) {}
+
+  // Restart PHY clock
+  *((volatile uint32_t *)(RHPORT_REGS_BASE + USB_OTG_PCGCCTL_BASE)) = 0;
 
   // Clear all interrupts
-  USB_OTG_FS->GINTSTS |= USB_OTG_FS->GINTSTS;
+  usb_otg->GINTSTS |= usb_otg->GINTSTS;
 
   // Required as part of core initialization.
   // TODO: How should mode mismatch be handled? It will cause
   // the core to stop working/require reset.
-  USB_OTG_FS->GINTMSK |= USB_OTG_GINTMSK_OTGINT | USB_OTG_GINTMSK_MMISM;
+  usb_otg->GINTMSK |= USB_OTG_GINTMSK_OTGINT | USB_OTG_GINTMSK_MMISM;
 
-  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE;
+  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE(rhport);
 
   // If USB host misbehaves during status portion of control xfer
-  // (non zero-length packet), send STALL back and discard. Full speed.
-  dev->DCFG |=  USB_OTG_DCFG_NZLSOHSK | (3 << USB_OTG_DCFG_DSPD_Pos);
+  // (non zero-length packet), send STALL back and discard.
+  dev->DCFG |=  USB_OTG_DCFG_NZLSOHSK;
+
+  set_speed(rhport, TUD_OPT_HIGH_SPEED ? TUSB_SPEED_HIGH : TUSB_SPEED_FULL);
+
+  // Enable internal USB transceiver.
+  if ( rhport == 0 ) usb_otg->GCCFG |= USB_OTG_GCCFG_PWRDWN;
 
-  USB_OTG_FS->GINTMSK |= USB_OTG_GINTMSK_USBRST   | USB_OTG_GINTMSK_ENUMDNEM |
-                         USB_OTG_GINTMSK_USBSUSPM | USB_OTG_GINTMSK_WUIM     |
-                         USB_OTG_GINTMSK_RXFLVLM  | (USE_SOF ? USB_OTG_GINTMSK_SOFM : 0);
+  usb_otg->GINTMSK |= USB_OTG_GINTMSK_USBRST   | USB_OTG_GINTMSK_ENUMDNEM |
+                      USB_OTG_GINTMSK_USBSUSPM | USB_OTG_GINTMSK_WUIM     |
+                      USB_OTG_GINTMSK_RXFLVLM  | (USE_SOF ? USB_OTG_GINTMSK_SOFM : 0);
 
-  // Enable USB transceiver.
-  USB_OTG_FS->GCCFG |= USB_OTG_GCCFG_PWRDWN;
+  // Enable global interrupt
+  usb_otg->GAHBCFG |= USB_OTG_GAHBCFG_GINT;
 }
 
 void dcd_int_enable (uint8_t rhport)
 {
   (void) rhport;
-  NVIC_EnableIRQ(OTG_FS_IRQn);
+  NVIC_EnableIRQ(RHPORT_IRQn);
 }
 
 void dcd_int_disable (uint8_t rhport)
 {
   (void) rhport;
-  NVIC_DisableIRQ(OTG_FS_IRQn);
+  NVIC_DisableIRQ(RHPORT_IRQn);
 }
 
 void dcd_set_address (uint8_t rhport, uint8_t dev_addr)
 {
-  (void) rhport;
-
-  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE;
+  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE(rhport);
   dev->DCFG |= (dev_addr << USB_OTG_DCFG_DAD_Pos) & USB_OTG_DCFG_DAD_Msk;
 
   // Response with status after changing device address
@@ -255,7 +484,7 @@ void dcd_remote_wakeup(uint8_t rhport)
 void dcd_connect(uint8_t rhport)
 {
   (void) rhport;
-  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE;
+  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE(rhport);
 
   dev->DCTL &= ~USB_OTG_DCTL_SDIS;
 }
@@ -263,7 +492,7 @@ void dcd_connect(uint8_t rhport)
 void dcd_disconnect(uint8_t rhport)
 {
   (void) rhport;
-  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE;
+  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE(rhport);
 
   dev->DCTL |= USB_OTG_DCTL_SDIS;
 }
@@ -275,17 +504,19 @@ void dcd_disconnect(uint8_t rhport)
 
 bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
 {
-  (void) rhport;
-  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE;
-  USB_OTG_OUTEndpointTypeDef * out_ep = OUT_EP_BASE;
-  USB_OTG_INEndpointTypeDef * in_ep = IN_EP_BASE;
+  USB_OTG_GlobalTypeDef * usb_otg = GLOBAL_BASE(rhport);
+  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE(rhport);
+  USB_OTG_OUTEndpointTypeDef * out_ep = OUT_EP_BASE(rhport);
+  USB_OTG_INEndpointTypeDef * in_ep = IN_EP_BASE(rhport);
 
   uint8_t const epnum = tu_edpt_number(desc_edpt->bEndpointAddress);
   uint8_t const dir   = tu_edpt_dir(desc_edpt->bEndpointAddress);
 
-  TU_ASSERT(desc_edpt->wMaxPacketSize.size <= 64);
   TU_ASSERT(epnum < EP_MAX);
 
+  // TODO ISO endpoint can be up to 1024 bytes
+  TU_ASSERT(desc_edpt->wMaxPacketSize.size <= (get_speed(rhport) == TUSB_SPEED_HIGH ? 512 : 64));
+
   xfer_ctl_t * xfer = XFER_CTL_BASE(epnum, dir);
   xfer->max_size = desc_edpt->wMaxPacketSize.size;
 
@@ -317,10 +548,36 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
     // | ( Shared )  |
     // --------------- 0
     //
-    // Since OUT FIFO = GRXFSIZ, FIFO 0 = 16, for simplicity, we equally allocated for the rest of endpoints
-    // - Size  : (FIFO_SIZE/4 - GRXFSIZ - 16) / (EP_MAX-1)
-    // - Offset: GRXFSIZ + 16 + Size*(epnum-1)
+    // In FIFO is allocated by following rules:
     // - IN EP 1 gets FIFO 1, IN EP "n" gets FIFO "n".
+    // - Offset: allocated so far
+    // - Size
+    //    - Interrupt is EPSize
+    //    - Bulk/ISO is max(EPSize, remaining-fifo / non-opened-EPIN)
+
+    uint16_t const fifo_remaining = EP_FIFO_SIZE/4 - _allocated_fifo_words;
+    uint16_t fifo_size = desc_edpt->wMaxPacketSize.size / 4;
+
+    if ( desc_edpt->bmAttributes.xfer != TUSB_XFER_INTERRUPT )
+    {
+      uint8_t opened = 0;
+      for(uint8_t i = 0; i < EP_MAX; i++)
+      {
+        if ( (i != epnum) && (xfer_status[i][TUSB_DIR_IN].max_size > 0) ) opened++;
+      }
+
+      // EP Size or equally divided of remaining whichever is larger
+      fifo_size = tu_max16(fifo_size, fifo_remaining / (EP_MAX - opened));
+    }
+
+    // FIFO overflows, we probably need a better allocating scheme
+    TU_ASSERT(fifo_size <= fifo_remaining);
+
+    // DIEPTXF starts at FIFO #1.
+    // Both TXFD and TXSA are in unit of 32-bit words.
+    usb_otg->DIEPTXF[epnum - 1] = (fifo_size << USB_OTG_DIEPTXF_INEPTXFD_Pos) | _allocated_fifo_words;
+
+    _allocated_fifo_words += fifo_size;
 
     in_ep[epnum].DIEPCTL |= (1 << USB_OTG_DIEPCTL_USBAEP_Pos) |
                             (epnum << USB_OTG_DIEPCTL_TXFNUM_Pos) |
@@ -329,15 +586,6 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
                             (desc_edpt->wMaxPacketSize.size << USB_OTG_DIEPCTL_MPSIZ_Pos);
 
     dev->DAINTMSK |= (1 << (USB_OTG_DAINTMSK_IEPM_Pos + epnum));
-
-    // Both TXFD and TXSA are in unit of 32-bit words.
-    // IN FIFO 0 was configured during enumeration, hence the "+ 16".
-    uint16_t const allocated_size = (USB_OTG_FS->GRXFSIZ & 0x0000ffff) + 16;
-    uint16_t const fifo_size = (EP_FIFO_SIZE/4 - allocated_size) / (EP_MAX-1);
-    uint32_t const fifo_offset = allocated_size + fifo_size*(epnum-1);
-
-    // DIEPTXF starts at FIFO #1.
-    USB_OTG_FS->DIEPTXF[epnum - 1] = (fifo_size << USB_OTG_DIEPTXF_INEPTXFD_Pos) | fifo_offset;
   }
 
   return true;
@@ -345,46 +593,31 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
 
 bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
 {
-  (void) rhport;
-  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE;
-  USB_OTG_OUTEndpointTypeDef * out_ep = OUT_EP_BASE;
-  USB_OTG_INEndpointTypeDef * in_ep = IN_EP_BASE;
-
   uint8_t const epnum = tu_edpt_number(ep_addr);
   uint8_t const dir   = tu_edpt_dir(ep_addr);
 
   xfer_ctl_t * xfer = XFER_CTL_BASE(epnum, dir);
-  xfer->buffer       = buffer;
-  xfer->total_len    = total_bytes;
-  xfer->queued_len   = 0;
-  xfer->short_packet = false;
+  xfer->buffer      = buffer;
+  xfer->total_len   = total_bytes;
+
+  // EP0 can only handle one packet
+  if(epnum == 0) {
+    ep0_pending[dir] = total_bytes;
+    // Schedule the first transaction for EP0 transfer
+    edpt_schedule_packets(rhport, epnum, dir, 1, ep0_pending[dir]);
+    return true;
+  }
 
   uint16_t num_packets = (total_bytes / xfer->max_size);
-  uint8_t short_packet_size = total_bytes % xfer->max_size;
+  uint8_t const short_packet_size = total_bytes % xfer->max_size;
 
   // Zero-size packet is special case.
   if(short_packet_size > 0 || (total_bytes == 0)) {
     num_packets++;
   }
 
-  // IN and OUT endpoint xfers are interrupt-driven, we just schedule them here.
-  if(dir == TUSB_DIR_IN) {
-    // A full IN transfer (multiple packets, possibly) triggers XFRC.
-    in_ep[epnum].DIEPTSIZ = (num_packets << USB_OTG_DIEPTSIZ_PKTCNT_Pos) |
-                            ((total_bytes & USB_OTG_DIEPTSIZ_XFRSIZ_Msk) << USB_OTG_DIEPTSIZ_XFRSIZ_Pos);
-
-    in_ep[epnum].DIEPCTL |= USB_OTG_DIEPCTL_EPENA | USB_OTG_DIEPCTL_CNAK;
-    // Enable fifo empty interrupt only if there are something to put in the fifo.
-    if(total_bytes != 0) {
-      dev->DIEPEMPMSK |= (1 << epnum);
-    }
-  } else {
-    // Each complete packet for OUT xfers triggers XFRC.
-    out_ep[epnum].DOEPTSIZ |= (1 << USB_OTG_DOEPTSIZ_PKTCNT_Pos) |
-                              ((xfer->max_size & USB_OTG_DOEPTSIZ_XFRSIZ_Msk) << USB_OTG_DOEPTSIZ_XFRSIZ_Pos);
-
-    out_ep[epnum].DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
-  }
+  // Schedule packets to be sent within interrupt
+  edpt_schedule_packets(rhport, epnum, dir, num_packets, total_bytes);
 
   return true;
 }
@@ -394,9 +627,11 @@ bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t
 void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
 {
   (void) rhport;
-  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE;
-  USB_OTG_OUTEndpointTypeDef * out_ep = OUT_EP_BASE;
-  USB_OTG_INEndpointTypeDef * in_ep = IN_EP_BASE;
+
+  USB_OTG_GlobalTypeDef * usb_otg = GLOBAL_BASE(rhport);
+  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE(rhport);
+  USB_OTG_OUTEndpointTypeDef * out_ep = OUT_EP_BASE(rhport);
+  USB_OTG_INEndpointTypeDef * in_ep = IN_EP_BASE(rhport);
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
   uint8_t const dir   = tu_edpt_dir(ep_addr);
@@ -417,9 +652,9 @@ void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
     }
 
     // Flush the FIFO, and wait until we have confirmed it cleared.
-    USB_OTG_FS->GRSTCTL |= ((epnum - 1) << USB_OTG_GRSTCTL_TXFNUM_Pos);
-    USB_OTG_FS->GRSTCTL |= USB_OTG_GRSTCTL_TXFFLSH;
-    while((USB_OTG_FS->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH_Msk) != 0);
+    usb_otg->GRSTCTL |= ((epnum - 1) << USB_OTG_GRSTCTL_TXFNUM_Pos);
+    usb_otg->GRSTCTL |= USB_OTG_GRSTCTL_TXFFLSH;
+    while((usb_otg->GRSTCTL & USB_OTG_GRSTCTL_TXFFLSH_Msk) != 0);
   } else {
     // Only disable currently enabled non-control endpoint
     if ( (epnum == 0) || !(out_ep[epnum].DOEPCTL & USB_OTG_DOEPCTL_EPENA) ){
@@ -430,7 +665,7 @@ void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
       // anyway, and it can't be cleared by user code. If this while loop never
       // finishes, we have bigger problems than just the stack.
       dev->DCTL |= USB_OTG_DCTL_SGONAK;
-      while((USB_OTG_FS->GINTSTS & USB_OTG_GINTSTS_BOUTNAKEFF_Msk) == 0);
+      while((usb_otg->GINTSTS & USB_OTG_GINTSTS_BOUTNAKEFF_Msk) == 0);
 
       // Ditto here- disable the endpoint.
       out_ep[epnum].DOEPCTL |= (USB_OTG_DOEPCTL_STALL | USB_OTG_DOEPCTL_EPDIS);
@@ -446,8 +681,9 @@ void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
 void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
 {
   (void) rhport;
-  USB_OTG_OUTEndpointTypeDef * out_ep = OUT_EP_BASE;
-  USB_OTG_INEndpointTypeDef * in_ep = IN_EP_BASE;
+
+  USB_OTG_OUTEndpointTypeDef * out_ep = OUT_EP_BASE(rhport);
+  USB_OTG_INEndpointTypeDef * in_ep = IN_EP_BASE(rhport);
 
   uint8_t const epnum = tu_edpt_number(ep_addr);
   uint8_t const dir   = tu_edpt_dir(ep_addr);
@@ -455,20 +691,16 @@ void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
   if(dir == TUSB_DIR_IN) {
     in_ep[epnum].DIEPCTL &= ~USB_OTG_DIEPCTL_STALL;
 
-    uint8_t eptype = (in_ep[epnum].DIEPCTL & USB_OTG_DIEPCTL_EPTYP_Msk) >> \
-      USB_OTG_DIEPCTL_EPTYP_Pos;
-    // Required by USB spec to reset DATA toggle bit to DATA0 on interrupt
-    // and bulk endpoints.
+    uint8_t eptype = (in_ep[epnum].DIEPCTL & USB_OTG_DIEPCTL_EPTYP_Msk) >> USB_OTG_DIEPCTL_EPTYP_Pos;
+    // Required by USB spec to reset DATA toggle bit to DATA0 on interrupt and bulk endpoints.
     if(eptype == 2 || eptype == 3) {
       in_ep[epnum].DIEPCTL |= USB_OTG_DIEPCTL_SD0PID_SEVNFRM;
     }
   } else {
     out_ep[epnum].DOEPCTL &= ~USB_OTG_DOEPCTL_STALL;
 
-    uint8_t eptype = (out_ep[epnum].DOEPCTL & USB_OTG_DOEPCTL_EPTYP_Msk) >> \
-      USB_OTG_DOEPCTL_EPTYP_Pos;
-    // Required by USB spec to reset DATA toggle bit to DATA0 on interrupt
-    // and bulk endpoints.
+    uint8_t eptype = (out_ep[epnum].DOEPCTL & USB_OTG_DOEPCTL_EPTYP_Msk) >> USB_OTG_DOEPCTL_EPTYP_Pos;
+    // Required by USB spec to reset DATA toggle bit to DATA0 on interrupt and bulk endpoints.
     if(eptype == 2 || eptype == 3) {
       out_ep[epnum].DOEPCTL |= USB_OTG_DOEPCTL_SD0PID_SEVNFRM;
     }
@@ -477,70 +709,44 @@ void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
 
 /*------------------------------------------------------------------*/
 
-// TODO: Split into "receive on endpoint 0" and "receive generic"; endpoint 0's
-// DOEPTSIZ register is smaller than the others, and so is insufficient for
-// determining how much of an OUT transfer is actually remaining.
-static void receive_packet(xfer_ctl_t * xfer, /* USB_OTG_OUTEndpointTypeDef * out_ep, */ uint16_t xfer_size) {
-  usb_fifo_t rx_fifo = FIFO_BASE(0);
-
-  // See above TODO
-  // uint16_t remaining = (out_ep->DOEPTSIZ & USB_OTG_DOEPTSIZ_XFRSIZ_Msk) >> USB_OTG_DOEPTSIZ_XFRSIZ_Pos;
-  // xfer->queued_len = xfer->total_len - remaining;
-
-  uint16_t remaining = xfer->total_len - xfer->queued_len;
-  uint16_t to_recv_size;
-
-  if(remaining <= xfer->max_size) {
-    // Avoid buffer overflow.
-    to_recv_size = (xfer_size > remaining) ? remaining : xfer_size;
-  } else {
-    // Room for full packet, choose recv_size based on what the microcontroller
-    // claims.
-    to_recv_size = (xfer_size > xfer->max_size) ? xfer->max_size : xfer_size;
-  }
-
-  uint8_t to_recv_rem = to_recv_size % 4;
-  uint16_t to_recv_size_aligned = to_recv_size - to_recv_rem;
+// Read a single data packet from receive FIFO
+static void read_fifo_packet(uint8_t rhport, uint8_t * dst, uint16_t len)
+{
+  (void) rhport;
 
-  // Do not assume xfer buffer is aligned.
-  uint8_t * base = (xfer->buffer + xfer->queued_len);
+  usb_fifo_t rx_fifo = FIFO_BASE(rhport, 0);
 
-  // This for loop always runs at least once- skip if less than 4 bytes
-  // to collect.
-  if(to_recv_size >= 4) {
-    for(uint16_t i = 0; i < to_recv_size_aligned; i += 4) {
-      uint32_t tmp = (* rx_fifo);
-      base[i] = tmp & 0x000000FF;
-      base[i + 1] = (tmp & 0x0000FF00) >> 8;
-      base[i + 2] = (tmp & 0x00FF0000) >> 16;
-      base[i + 3] = (tmp & 0xFF000000) >> 24;
-    }
+  // Reading full available 32 bit words from fifo
+  uint16_t full_words = len >> 2;
+  for(uint16_t i = 0; i < full_words; i++) {
+    uint32_t tmp = *rx_fifo;
+    dst[0] = tmp & 0x000000FF;
+    dst[1] = (tmp & 0x0000FF00) >> 8;
+    dst[2] = (tmp & 0x00FF0000) >> 16;
+    dst[3] = (tmp & 0xFF000000) >> 24;
+    dst += 4;
   }
 
-  // Do not read invalid bytes from RX FIFO.
-  if(to_recv_rem != 0) {
-    uint32_t tmp = (* rx_fifo);
-    uint8_t * last_32b_bound = base + to_recv_size_aligned;
-
-    last_32b_bound[0] = tmp & 0x000000FF;
-    if(to_recv_rem > 1) {
-      last_32b_bound[1] = (tmp & 0x0000FF00) >> 8;
+  // Read the remaining 1-3 bytes from fifo
+  uint8_t bytes_rem = len & 0x03;
+  if(bytes_rem != 0) {
+    uint32_t tmp = *rx_fifo;
+    dst[0] = tmp & 0x000000FF;
+    if(bytes_rem > 1) {
+      dst[1] = (tmp & 0x0000FF00) >> 8;
     }
-    if(to_recv_rem > 2) {
-      last_32b_bound[2] = (tmp & 0x00FF0000) >> 16;
+    if(bytes_rem > 2) {
+      dst[2] = (tmp & 0x00FF0000) >> 16;
     }
   }
-
-  xfer->queued_len += xfer_size;
-
-  // Per USB spec, a short OUT packet (including length 0) is always
-  // indicative of the end of a transfer (at least for ctl, bulk, int).
-  xfer->short_packet = (xfer_size < xfer->max_size);
 }
 
 // Write a single data packet to EPIN FIFO
-static void write_fifo_packet(uint8_t fifo_num, uint8_t * src, uint16_t len){
-  usb_fifo_t tx_fifo = FIFO_BASE(fifo_num);
+static void write_fifo_packet(uint8_t rhport, uint8_t fifo_num, uint8_t * src, uint16_t len)
+{
+  (void) rhport;
+
+  usb_fifo_t tx_fifo = FIFO_BASE(rhport, fifo_num);
 
   // Pushing full available 32 bit words to fifo
   uint16_t full_words = len >> 2;
@@ -564,48 +770,62 @@ static void write_fifo_packet(uint8_t fifo_num, uint8_t * src, uint16_t len){
   }
 }
 
-static void read_rx_fifo(USB_OTG_OUTEndpointTypeDef * out_ep) {
-  usb_fifo_t rx_fifo = FIFO_BASE(0);
+static void handle_rxflvl_ints(uint8_t rhport, USB_OTG_OUTEndpointTypeDef * out_ep) {
+  USB_OTG_GlobalTypeDef * usb_otg = GLOBAL_BASE(rhport);
+  usb_fifo_t rx_fifo = FIFO_BASE(rhport, 0);
 
-  // Pop control word off FIFO (completed xfers will have 2 control words,
-  // we only pop one ctl word each interrupt).
-  uint32_t ctl_word = USB_OTG_FS->GRXSTSP;
+  // Pop control word off FIFO
+  uint32_t ctl_word = usb_otg->GRXSTSP;
   uint8_t pktsts = (ctl_word & USB_OTG_GRXSTSP_PKTSTS_Msk) >> USB_OTG_GRXSTSP_PKTSTS_Pos;
   uint8_t epnum = (ctl_word &  USB_OTG_GRXSTSP_EPNUM_Msk) >>  USB_OTG_GRXSTSP_EPNUM_Pos;
   uint16_t bcnt = (ctl_word & USB_OTG_GRXSTSP_BCNT_Msk) >> USB_OTG_GRXSTSP_BCNT_Pos;
 
   switch(pktsts) {
     case 0x01: // Global OUT NAK (Interrupt)
-      break;
+    break;
+
     case 0x02: // Out packet recvd
-      {
-        xfer_ctl_t * xfer = XFER_CTL_BASE(epnum, TUSB_DIR_OUT);
-        receive_packet(xfer, bcnt);
+    {
+      xfer_ctl_t * xfer = XFER_CTL_BASE(epnum, TUSB_DIR_OUT);
+
+      // Read packet off RxFIFO
+      read_fifo_packet(rhport, xfer->buffer, bcnt);
+
+      // Increment pointer to xfer data
+      xfer->buffer += bcnt;
+
+      // Truncate transfer length in case of short packet
+      if(bcnt < xfer->max_size) {
+        xfer->total_len -= (out_ep[epnum].DOEPTSIZ & USB_OTG_DOEPTSIZ_XFRSIZ_Msk) >> USB_OTG_DOEPTSIZ_XFRSIZ_Pos;
+        if(epnum == 0) {
+          xfer->total_len -= ep0_pending[TUSB_DIR_OUT];
+          ep0_pending[TUSB_DIR_OUT] = 0;
+        }
       }
-      break;
+    }
+    break;
+
     case 0x03: // Out packet done (Interrupt)
       break;
+
     case 0x04: // Setup packet done (Interrupt)
-      _setup_offs = 2 - ((out_ep[epnum].DOEPTSIZ & USB_OTG_DOEPTSIZ_STUPCNT_Msk) >> USB_OTG_DOEPTSIZ_STUPCNT_Pos);
       out_ep[epnum].DOEPTSIZ |= (3 << USB_OTG_DOEPTSIZ_STUPCNT_Pos);
-      break;
+    break;
+
     case 0x06: // Setup packet recvd
-      {
-        uint8_t setup_left = ((out_ep[epnum].DOEPTSIZ & USB_OTG_DOEPTSIZ_STUPCNT_Msk) >> USB_OTG_DOEPTSIZ_STUPCNT_Pos);
+      // We can receive up to three setup packets in succession, but
+      // only the last one is valid.
+      _setup_packet[0] = (* rx_fifo);
+      _setup_packet[1] = (* rx_fifo);
+    break;
 
-        // We can receive up to three setup packets in succession, but
-        // only the last one is valid.
-        _setup_packet[4 - 2*setup_left] = (* rx_fifo);
-        _setup_packet[5 - 2*setup_left] = (* rx_fifo);
-      }
-      break;
     default: // Invalid
       TU_BREAKPOINT();
-      break;
+    break;
   }
 }
 
-static void handle_epout_ints(USB_OTG_DeviceTypeDef * dev, USB_OTG_OUTEndpointTypeDef * out_ep) {
+static void handle_epout_ints(uint8_t rhport, USB_OTG_DeviceTypeDef * dev, USB_OTG_OUTEndpointTypeDef * out_ep) {
   // DAINT for a given EP clears when DOEPINTx is cleared.
   // OEPINT will be cleared when DAINT's out bits are cleared.
   for(uint8_t n = 0; n < EP_MAX; n++) {
@@ -615,35 +835,26 @@ static void handle_epout_ints(USB_OTG_DeviceTypeDef * dev, USB_OTG_OUTEndpointTy
       // SETUP packet Setup Phase done.
       if(out_ep[n].DOEPINT & USB_OTG_DOEPINT_STUP) {
         out_ep[n].DOEPINT =  USB_OTG_DOEPINT_STUP;
-        dcd_event_setup_received(0, (uint8_t*) &_setup_packet[2*_setup_offs], true);
-        _setup_offs = 0;
+        dcd_event_setup_received(rhport, (uint8_t*) &_setup_packet[0], true);
       }
 
-      // OUT XFER complete (single packet).
+      // OUT XFER complete
       if(out_ep[n].DOEPINT & USB_OTG_DOEPINT_XFRC) {
         out_ep[n].DOEPINT = USB_OTG_DOEPINT_XFRC;
 
-        // TODO: Because of endpoint 0's constrained size, we handle XFRC
-        // on a packet-basis. The core can internally handle multiple OUT
-        // packets; it would be more efficient to only trigger XFRC on a
-        // completed transfer for non-0 endpoints.
-
-        // Transfer complete if short packet or total len is transferred
-        if(xfer->short_packet || (xfer->queued_len == xfer->total_len)) {
-          xfer->short_packet = false;
-          dcd_event_xfer_complete(0, n, xfer->queued_len, XFER_RESULT_SUCCESS, true);
-        } else {
+        // EP0 can only handle one packet
+        if((n == 0) && ep0_pending[TUSB_DIR_OUT]) {
           // Schedule another packet to be received.
-          out_ep[n].DOEPTSIZ |= (1 << USB_OTG_DOEPTSIZ_PKTCNT_Pos) | \
-              ((xfer->max_size & USB_OTG_DOEPTSIZ_XFRSIZ_Msk) << USB_OTG_DOEPTSIZ_XFRSIZ_Pos);
-          out_ep[n].DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK;
+          edpt_schedule_packets(rhport, n, TUSB_DIR_OUT, 1, ep0_pending[TUSB_DIR_OUT]);
+        } else {
+          dcd_event_xfer_complete(rhport, n, xfer->total_len, XFER_RESULT_SUCCESS, true);
         }
       }
     }
   }
 }
 
-static void handle_epin_ints(USB_OTG_DeviceTypeDef * dev, USB_OTG_INEndpointTypeDef * in_ep) {
+static void handle_epin_ints(uint8_t rhport, USB_OTG_DeviceTypeDef * dev, USB_OTG_INEndpointTypeDef * in_ep) {
   // DAINT for a given EP clears when DIEPINTx is cleared.
   // IEPINT will be cleared when DAINT's out bits are cleared.
   for ( uint8_t n = 0; n < EP_MAX; n++ )
@@ -652,12 +863,18 @@ static void handle_epin_ints(USB_OTG_DeviceTypeDef * dev, USB_OTG_INEndpointType
 
     if ( dev->DAINT & (1 << (USB_OTG_DAINT_IEPINT_Pos + n)) )
     {
-
       // IN XFER complete (entire xfer).
       if ( in_ep[n].DIEPINT & USB_OTG_DIEPINT_XFRC )
       {
         in_ep[n].DIEPINT = USB_OTG_DIEPINT_XFRC;
-        dcd_event_xfer_complete(0, n | TUSB_DIR_IN_MASK, xfer->total_len, XFER_RESULT_SUCCESS, true);
+
+        // EP0 can only handle one packet
+        if((n == 0) && ep0_pending[TUSB_DIR_IN]) {
+          // Schedule another packet to be transmitted.
+          edpt_schedule_packets(rhport, n, TUSB_DIR_IN, 1, ep0_pending[TUSB_DIR_IN]);
+        } else {
+          dcd_event_xfer_complete(rhport, n | TUSB_DIR_IN_MASK, xfer->total_len, XFER_RESULT_SUCCESS, true);
+        }
       }
 
       // XFER FIFO empty
@@ -682,11 +899,11 @@ static void handle_epin_ints(USB_OTG_DeviceTypeDef * dev, USB_OTG_INEndpointType
             break;
           }
 
-          // TODO: queued_len can be removed later
-          xfer->queued_len = xfer->total_len - remaining_bytes;
-
           // Push packet to Tx-FIFO
-          write_fifo_packet(n, (xfer->buffer + xfer->queued_len), packet_size);
+          write_fifo_packet(rhport, n, xfer->buffer, packet_size);
+
+          // Increment pointer to xfer data
+          xfer->buffer += packet_size;
         }
 
         // Turn off TXFE if all bytes are written.
@@ -699,82 +916,90 @@ static void handle_epin_ints(USB_OTG_DeviceTypeDef * dev, USB_OTG_INEndpointType
   }
 }
 
-void dcd_int_handler(uint8_t rhport) {
-
-  (void) rhport;
-
-  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE;
-  USB_OTG_OUTEndpointTypeDef * out_ep = OUT_EP_BASE;
-  USB_OTG_INEndpointTypeDef * in_ep = IN_EP_BASE;
+void dcd_int_handler(uint8_t rhport)
+{
+  USB_OTG_GlobalTypeDef * usb_otg = GLOBAL_BASE(rhport);
+  USB_OTG_DeviceTypeDef * dev = DEVICE_BASE(rhport);
+  USB_OTG_OUTEndpointTypeDef * out_ep = OUT_EP_BASE(rhport);
+  USB_OTG_INEndpointTypeDef * in_ep = IN_EP_BASE(rhport);
 
-  uint32_t int_status = USB_OTG_FS->GINTSTS;
+  uint32_t int_status = usb_otg->GINTSTS;
 
   if(int_status & USB_OTG_GINTSTS_USBRST) {
     // USBRST is start of reset.
-    USB_OTG_FS->GINTSTS = USB_OTG_GINTSTS_USBRST;
-    bus_reset();
+    usb_otg->GINTSTS = USB_OTG_GINTSTS_USBRST;
+    bus_reset(rhport);
   }
 
   if(int_status & USB_OTG_GINTSTS_ENUMDNE) {
-    // ENUMDNE detects speed of the link. For full-speed, we
-    // always expect the same value. This interrupt is considered
-    // the end of reset.
-    USB_OTG_FS->GINTSTS = USB_OTG_GINTSTS_ENUMDNE;
-    end_of_reset();
-    dcd_event_bus_signal(0, DCD_EVENT_BUS_RESET, true);
+    // ENUMDNE is the end of reset where speed of the link is detected
+
+    usb_otg->GINTSTS = USB_OTG_GINTSTS_ENUMDNE;
+
+    tusb_speed_t const speed = get_speed(rhport);
+
+    set_turnaround(usb_otg, speed);
+    dcd_event_bus_reset(rhport, speed, true);
   }
 
   if(int_status & USB_OTG_GINTSTS_USBSUSP)
   {
-    USB_OTG_FS->GINTSTS = USB_OTG_GINTSTS_USBSUSP;
-    dcd_event_bus_signal(0, DCD_EVENT_SUSPEND, true);
+    usb_otg->GINTSTS = USB_OTG_GINTSTS_USBSUSP;
+    dcd_event_bus_signal(rhport, DCD_EVENT_SUSPEND, true);
   }
 
   if(int_status & USB_OTG_GINTSTS_WKUINT)
   {
-    USB_OTG_FS->GINTSTS = USB_OTG_GINTSTS_WKUINT;
-    dcd_event_bus_signal(0, DCD_EVENT_RESUME, true);
+    usb_otg->GINTSTS = USB_OTG_GINTSTS_WKUINT;
+    dcd_event_bus_signal(rhport, DCD_EVENT_RESUME, true);
   }
 
   if(int_status & USB_OTG_GINTSTS_OTGINT)
   {
     // OTG INT bit is read-only
-    uint32_t const otg_int = USB_OTG_FS->GOTGINT;
+    uint32_t const otg_int = usb_otg->GOTGINT;
 
     if (otg_int & USB_OTG_GOTGINT_SEDET)
     {
-      dcd_event_bus_signal(0, DCD_EVENT_UNPLUGGED, true);
+      dcd_event_bus_signal(rhport, DCD_EVENT_UNPLUGGED, true);
     }
 
-    USB_OTG_FS->GOTGINT = otg_int;
+    usb_otg->GOTGINT = otg_int;
   }
 
 #if USE_SOF
   if(int_status & USB_OTG_GINTSTS_SOF) {
-    USB_OTG_FS->GINTSTS = USB_OTG_GINTSTS_SOF;
-    dcd_event_bus_signal(0, DCD_EVENT_SOF, true);
+    usb_otg->GINTSTS = USB_OTG_GINTSTS_SOF;
+    dcd_event_bus_signal(rhport, DCD_EVENT_SOF, true);
   }
 #endif
 
+  // RxFIFO non-empty interrupt handling.
   if(int_status & USB_OTG_GINTSTS_RXFLVL) {
     // RXFLVL bit is read-only
 
     // Mask out RXFLVL while reading data from FIFO
-    USB_OTG_FS->GINTMSK &= ~USB_OTG_GINTMSK_RXFLVLM;
-    read_rx_fifo(out_ep);
-    USB_OTG_FS->GINTMSK |= USB_OTG_GINTMSK_RXFLVLM;
+    usb_otg->GINTMSK &= ~USB_OTG_GINTMSK_RXFLVLM;
+
+    // Loop until all available packets were handled
+    do {
+      handle_rxflvl_ints(rhport, out_ep);
+      int_status = usb_otg->GINTSTS;
+    } while(int_status & USB_OTG_GINTSTS_RXFLVL);
+
+    usb_otg->GINTMSK |= USB_OTG_GINTMSK_RXFLVLM;
   }
 
   // OUT endpoint interrupt handling.
   if(int_status & USB_OTG_GINTSTS_OEPINT) {
     // OEPINT is read-only
-    handle_epout_ints(dev, out_ep);
+    handle_epout_ints(rhport, dev, out_ep);
   }
 
   // IN endpoint interrupt handling.
   if(int_status & USB_OTG_GINTSTS_IEPINT) {
     // IEPINT bit read-only
-    handle_epin_ints(dev, in_ep);
+    handle_epin_ints(rhport, dev, in_ep);
   }
 }
 
diff --git a/src/tusb.h b/src/tusb.h
index 411b6e08b..8b0dd103c 100644
--- a/src/tusb.h
+++ b/src/tusb.h
@@ -113,6 +113,8 @@
  *  @{ */
 
 // Initialize device/host stack
+// Note: when using with RTOS, this should be called after scheduler/kernel is started.
+// Otherwise it could cause kernel issue since USB IRQ handler does use RTOS queue API.
 bool tusb_init(void);
 
 // Check if stack is initialized
diff --git a/src/tusb_option.h b/src/tusb_option.h
index cc91b1863..2f7a60e75 100644
--- a/src/tusb_option.h
+++ b/src/tusb_option.h
@@ -58,6 +58,7 @@
 #define OPT_MCU_SAMD21            200 ///< MicroChip SAMD21
 #define OPT_MCU_SAMD51            201 ///< MicroChip SAMD51
 #define OPT_MCU_SAMG              202 ///< MicroChip SAMDG series
+#define OPT_MCU_SAME5X            203 ///< MicroChip SAM E5x
 
 // STM32
 #define OPT_MCU_STM32F0           300 ///< ST STM32F0
@@ -92,6 +93,9 @@
 // Espressif
 #define OPT_MCU_ESP32S2           900 ///< Espressif ESP32-S2
 
+// Dialog
+#define OPT_MCU_DA1469X          1000 ///< Dialog Semiconductor DA1469x
+
 /** @} */
 
 /** \defgroup group_supported_os Supported RTOS
@@ -114,31 +118,35 @@
 /** \addtogroup group_configuration
  *  @{ */
 
+
 //--------------------------------------------------------------------
-// CONTROLLER
-// Only 1 roothub port can be configured to be device and/or host.
-// tinyusb does not support dual devices or dual host configuration
+// RootHub Mode Configuration
+// CFG_TUSB_RHPORTx_MODE contains operation mode and speed for that port
 //--------------------------------------------------------------------
-/** \defgroup group_mode Controller Mode Selection
- * \brief CFG_TUSB_CONTROLLER_N_MODE must be defined with these
- *  @{ */
+
+// Lower 4-bit is operational mode
 #define OPT_MODE_NONE         0x00 ///< Disabled
 #define OPT_MODE_DEVICE       0x01 ///< Device Mode
 #define OPT_MODE_HOST         0x02 ///< Host Mode
-#define OPT_MODE_HIGH_SPEED   0x10 ///< High speed
-/** @} */
+
+// Higher 4-bit is max operational speed (corresponding to tusb_speed_t)
+#define OPT_MODE_FULL_SPEED   0x00 ///< Max Full Speed
+#define OPT_MODE_LOW_SPEED    0x10 ///< Max Low Speed
+#define OPT_MODE_HIGH_SPEED   0x20 ///< Max High Speed
+
 
 #ifndef CFG_TUSB_RHPORT0_MODE
   #define CFG_TUSB_RHPORT0_MODE OPT_MODE_NONE
 #endif
 
+
 #ifndef CFG_TUSB_RHPORT1_MODE
   #define CFG_TUSB_RHPORT1_MODE OPT_MODE_NONE
 #endif
 
-#if ((CFG_TUSB_RHPORT0_MODE & OPT_MODE_HOST) && (CFG_TUSB_RHPORT1_MODE & OPT_MODE_HOST)) || \
+#if ((CFG_TUSB_RHPORT0_MODE & OPT_MODE_HOST  ) && (CFG_TUSB_RHPORT1_MODE & OPT_MODE_HOST  )) || \
     ((CFG_TUSB_RHPORT0_MODE & OPT_MODE_DEVICE) && (CFG_TUSB_RHPORT1_MODE & OPT_MODE_DEVICE))
-  #error "tinyusb does not support same modes on more than 1 roothub port"
+  #error "TinyUSB currently does not support same modes on more than 1 roothub port"
 #endif
 
 // Which roothub port is configured as host
@@ -156,7 +164,6 @@
 
 #define TUSB_OPT_DEVICE_ENABLED ( TUD_OPT_RHPORT >= 0 )
 
-
 //--------------------------------------------------------------------+
 // COMMON OPTIONS
 //--------------------------------------------------------------------+
@@ -168,15 +175,15 @@
 
 // place data in accessible RAM for usb controller
 #ifndef CFG_TUSB_MEM_SECTION
-#define CFG_TUSB_MEM_SECTION
+  #define CFG_TUSB_MEM_SECTION
 #endif
 
 #ifndef CFG_TUSB_MEM_ALIGN
-#define CFG_TUSB_MEM_ALIGN        TU_ATTR_ALIGNED(4)
+  #define CFG_TUSB_MEM_ALIGN      TU_ATTR_ALIGNED(4)
 #endif
 
 #ifndef CFG_TUSB_OS
-#define CFG_TUSB_OS               OPT_OS_NONE
+  #define CFG_TUSB_OS             OPT_OS_NONE
 #endif
 
 //--------------------------------------------------------------------
@@ -184,7 +191,7 @@
 //--------------------------------------------------------------------
 
 #ifndef CFG_TUD_ENDPOINT0_SIZE
-  #define CFG_TUD_ENDPOINT0_SIZE   64
+  #define CFG_TUD_ENDPOINT0_SIZE  64
 #endif
 
 #ifndef CFG_TUD_CDC