diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1b0fca085..bbf8583cb 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -34,7 +34,4 @@ add_custom_command(
 )
 add_custom_target(mem-variant DEPENDS "mem_variant")
 
-##################
-### ESP Matter ###
-##################
-idf_build_set_property(CXX_COMPILE_OPTIONS "-std=gnu++17;-DCHIP_HAVE_CONFIG_H" APPEND)
+idf_build_set_property(COMPILE_DEFINITIONS "-DESP32_ARDUINO_LIB_BUILDER" APPEND)
diff --git a/components/arduino_tinyusb/CMakeLists.txt b/components/arduino_tinyusb/CMakeLists.txt
index 6a2e35150..fd6f5983d 100755
--- a/components/arduino_tinyusb/CMakeLists.txt
+++ b/components/arduino_tinyusb/CMakeLists.txt
@@ -27,7 +27,8 @@ if(CONFIG_TINYUSB_ENABLED)
       # espressif:
       "${COMPONENT_DIR}/src/dcd_dwc2.c"
       # tusb:
-      #"{COMPONENT_DIR}/tinyusb/src/portable/synopsys/dwc2/dcd_dwc2.c"
+      #"${COMPONENT_DIR}/tinyusb/src/portable/synopsys/dwc2/dcd_dwc2.c"
+      "${COMPONENT_DIR}/tinyusb/src/portable/synopsys/dwc2/dwc2_common.c"
       "${COMPONENT_DIR}/tinyusb/src/class/cdc/cdc_device.c"
       "${COMPONENT_DIR}/tinyusb/src/class/hid/hid_device.c"
       "${COMPONENT_DIR}/tinyusb/src/class/midi/midi_device.c"
diff --git a/components/arduino_tinyusb/patches/dcd_dwc2.patch b/components/arduino_tinyusb/patches/dcd_dwc2.patch
index a57df7ef6..d3529f434 100644
--- a/components/arduino_tinyusb/patches/dcd_dwc2.patch
+++ b/components/arduino_tinyusb/patches/dcd_dwc2.patch
@@ -1,6 +1,6 @@
 --- a/components/arduino_tinyusb/src/dcd_dwc2.c	2024-10-02 12:17:40.000000000 +0300
 +++ b/components/arduino_tinyusb/src/dcd_dwc2.c	2024-10-02 12:19:48.000000000 +0300
-@@ -316,6 +316,16 @@
+@@ -243,6 +243,17 @@
  //--------------------------------------------------------------------
  // Endpoint
  //--------------------------------------------------------------------
@@ -14,14 +14,16 @@
 +  return 0;
 +}
 +#endif
- 
- static void edpt_activate(uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc) {
++
+ static void edpt_activate(uint8_t rhport, const tusb_desc_endpoint_t* p_endpoint_desc) {
    dwc2_regs_t* dwc2 = DWC2_REG(rhport);
-@@ -336,7 +346,18 @@
-     dwc2->epout[epnum].doepctl = dxepctl;
-     dwc2->daintmsk |= TU_BIT(DAINTMSK_OEPM_Pos + epnum);
-   } else {
--    dwc2->epin[epnum].diepctl = dxepctl | (epnum << DIEPCTL_TXFNUM_Pos);
+   const uint8_t epnum = tu_edpt_number(p_endpoint_desc->bEndpointAddress);
+@@ -266,7 +277,18 @@
+     depctl.bm.set_data0_iso_even = 1;
+   }
+   if (dir == TUSB_DIR_IN) {
+-    depctl.bm.tx_fifo_num = epnum;
++    //depctl.bm.tx_fifo_num = epnum;
 +    uint8_t fifo_num = epnum;
 +#if defined(TUP_USBIP_DWC2_ESP32)
 +    // Special Case for EP5, which is used by CDC but not actually called by the driver
@@ -31,14 +33,13 @@
 +    } else {
 +      fifo_num = get_free_fifo();
 +    }
-+    //TU_ASSERT(fifo_num != 0);
 +#endif
-+    dwc2->epin[epnum].diepctl = dxepctl | (fifo_num << DIEPCTL_TXFNUM_Pos);
-     dwc2->daintmsk |= TU_BIT(DAINTMSK_IEPM_Pos + epnum);
++    depctl.bm.tx_fifo_num = fifo_num;
    }
- }
-@@ -850,6 +871,10 @@
-     xfer_status[n][TUSB_DIR_IN].max_size = 0;
+ 
+   dwc2_dep_t* dep = &dwc2->ep[dir == TUSB_DIR_IN ? 0 : 1][epnum];
+@@ -557,6 +579,10 @@
+     }
    }
  
 +#if defined(TUP_USBIP_DWC2_ESP32)
@@ -48,19 +49,19 @@
    dfifo_flush_tx(dwc2, 0x10); // all tx fifo
    dfifo_flush_rx(dwc2);
  
-@@ -1204,6 +1229,9 @@
-   if (int_status & GINTSTS_USBRST) {
+@@ -997,6 +1023,9 @@
+   if (gintsts & GINTSTS_USBRST) {
      // USBRST is start of reset.
      dwc2->gintsts = GINTSTS_USBRST;
 +#if defined(TUP_USBIP_DWC2_ESP32)
 +    _allocated_fifos = 1;
 +#endif
-     bus_reset(rhport);
+     handle_bus_reset(rhport);
    }
  
-@@ -1235,7 +1263,11 @@
+@@ -1008,7 +1037,11 @@
  
-   if (int_status & GINTSTS_USBSUSP) {
+   if (gintsts & GINTSTS_USBSUSP) {
      dwc2->gintsts = GINTSTS_USBSUSP;
 -    dcd_event_bus_signal(rhport, DCD_EVENT_SUSPEND, true);
 +    //dcd_event_bus_signal(rhport, DCD_EVENT_SUSPEND, true);
@@ -70,8 +71,8 @@
 +#endif
    }
  
-   if (int_status & GINTSTS_WKUINT) {
-@@ -1252,6 +1284,9 @@
+   if (gintsts & GINTSTS_WKUINT) {
+@@ -1025,6 +1058,9 @@
  
      if (otg_int & GOTGINT_SEDET) {
        dcd_event_bus_signal(rhport, DCD_EVENT_UNPLUGGED, true);
diff --git a/components/arduino_tinyusb/src/dcd_dwc2.c b/components/arduino_tinyusb/src/dcd_dwc2.c
index 8bcc98166..aae711ed7 100644
--- a/components/arduino_tinyusb/src/dcd_dwc2.c
+++ b/components/arduino_tinyusb/src/dcd_dwc2.c
@@ -31,56 +31,25 @@
 
 #if CFG_TUD_ENABLED && defined(TUP_USBIP_DWC2)
 
+#if !(CFG_TUD_DWC2_SLAVE_ENABLE || CFG_TUD_DWC2_DMA_ENABLE)
+#error DWC2 require either CFG_TUD_DWC2_SLAVE_ENABLE or CFG_TUD_DWC2_DMA_ENABLE to be enabled
+#endif
+
 // Debug level for DWC2
 #define DWC2_DEBUG    2
 
 #include "device/dcd.h"
-#include "dwc2_type.h"
-
-// Following symbols must be defined by port header
-// - _dwc2_controller[]: array of controllers
-// - DWC2_EP_MAX: largest EP counts of all controllers
-// - dwc2_phy_init/dwc2_phy_update: phy init called before and after core reset
-// - dwc2_dcd_int_enable/dwc2_dcd_int_disable
-// - dwc2_remote_wakeup_delay
-
-#if defined(TUP_USBIP_DWC2_STM32)
-  #include "dwc2_stm32.h"
-#elif defined(TUP_USBIP_DWC2_ESP32)
-  #include "dwc2_esp32.h"
-#elif TU_CHECK_MCU(OPT_MCU_GD32VF103)
-  #include "dwc2_gd32.h"
-#elif TU_CHECK_MCU(OPT_MCU_BCM2711, OPT_MCU_BCM2835, OPT_MCU_BCM2837)
-  #include "dwc2_bcm.h"
-#elif TU_CHECK_MCU(OPT_MCU_EFM32GG)
-  #include "dwc2_efm32.h"
-#elif TU_CHECK_MCU(OPT_MCU_XMC4000)
-  #include "dwc2_xmc.h"
+#include "dwc2_common.h"
+
+#if TU_CHECK_MCU(OPT_MCU_GD32VF103)
+  #define DWC2_EP_COUNT(_dwc2)   DWC2_EP_MAX
 #else
-  #error "Unsupported MCUs"
+  #define DWC2_EP_COUNT(_dwc2)  ((_dwc2)->ghwcfg2_bm.num_dev_ep + 1)
 #endif
 
-enum {
-  DWC2_CONTROLLER_COUNT = TU_ARRAY_SIZE(_dwc2_controller)
-};
-
-// DWC2 registers
-//#define DWC2_REG(_port)       ((dwc2_regs_t*) _dwc2_controller[_port].reg_base)
-
-TU_ATTR_ALWAYS_INLINE static inline dwc2_regs_t* DWC2_REG(uint8_t rhport) {
-  if (rhport >= DWC2_CONTROLLER_COUNT) {
-    // user mis-configured, ignore and use first controller
-    rhport = 0;
-  }
-  return (dwc2_regs_t*) _dwc2_controller[rhport].reg_base;
-}
-
 //--------------------------------------------------------------------+
 // MACRO TYPEDEF CONSTANT ENUM
 //--------------------------------------------------------------------+
-
-static CFG_TUD_MEM_SECTION TU_ATTR_ALIGNED(4) uint32_t _setup_packet[2];
-
 typedef struct {
   uint8_t* buffer;
   tu_fifo_t* ff;
@@ -92,34 +61,48 @@ typedef struct {
 static xfer_ctl_t xfer_status[DWC2_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
-static uint16_t _dfifo_top;      // top free location in FIFO RAM
+typedef struct {
+  // EP0 transfers are limited to 1 packet - larger sizes has to be split
+  uint16_t ep0_pending[2];  // Index determines direction as tusb_dir_t type
+  uint16_t dfifo_top;      // top free location in DFIFO in words
 
-// Number of IN endpoints active
-static uint8_t _allocated_ep_in_count;
+  // Number of IN endpoints active
+  uint8_t allocated_epin_count;
 
-// SOF enabling flag - required for SOF to not get disabled in ISR when SOF was enabled by
-static bool _sof_en;
+  // SOF enabling flag - required for SOF to not get disabled in ISR when SOF was enabled by
+  bool sof_en;
+} dcd_data_t;
+
+static dcd_data_t _dcd_data;
+
+CFG_TUD_MEM_SECTION static struct {
+  TUD_EPBUF_DEF(setup_packet, 8);
+} _dcd_usbbuf;
 
 //--------------------------------------------------------------------
 // DMA
 //--------------------------------------------------------------------
+#if CFG_TUD_MEM_DCACHE_ENABLE
+bool dcd_dcache_clean(const void* addr, uint32_t data_size) {
+  TU_VERIFY(addr && data_size);
+  return dwc2_dcache_clean(addr, data_size);
+}
 
-TU_ATTR_ALWAYS_INLINE static inline bool dma_enabled(const dwc2_regs_t* dwc2) {
-  #if !CFG_TUD_DWC2_DMA
-  (void) dwc2;
-  return false;
-  #else
-  // Internal DMA only
-  return (dwc2->ghwcfg2_bm.arch == GHWCFG2_ARCH_INTERNAL_DMA);
-  #endif
+bool dcd_dcache_invalidate(const void* addr, uint32_t data_size) {
+  TU_VERIFY(addr && data_size);
+  return dwc2_dcache_invalidate(addr, data_size);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline uint16_t dma_cal_epfifo_base(uint8_t rhport) {
-  // Scatter/Gather DMA mode is not yet supported. Buffer DMA only need 1 words per endpoint direction
-  const dwc2_controller_t* dwc2_controller = &_dwc2_controller[rhport];
-  return dwc2_controller->ep_fifo_size/4 - 2*dwc2_controller->ep_count;
+bool dcd_dcache_clean_invalidate(const void* addr, uint32_t data_size) {
+  TU_VERIFY(addr && data_size);
+  return dwc2_dcache_clean_invalidate(addr, data_size);
+}
+#endif
+
+TU_ATTR_ALWAYS_INLINE static inline bool dma_device_enabled(const dwc2_regs_t* dwc2) {
+  (void) dwc2;
+  // Internal DMA only
+  return CFG_TUD_DWC2_DMA_ENABLE && dwc2->ghwcfg2_bm.arch == GHWCFG2_ARCH_INTERNAL_DMA;
 }
 
 static void dma_setup_prepare(uint8_t rhport) {
@@ -133,7 +116,7 @@ static void dma_setup_prepare(uint8_t rhport) {
 
   // Receive only 1 packet
   dwc2->epout[0].doeptsiz = (1 << DOEPTSIZ_STUPCNT_Pos) | (1 << DOEPTSIZ_PKTCNT_Pos) | (8 << DOEPTSIZ_XFRSIZ_Pos);
-  dwc2->epout[0].doepdma = (uintptr_t)_setup_packet;
+  dwc2->epout[0].doepdma = (uintptr_t) _dcd_usbbuf.setup_packet;
   dwc2->epout[0].doepctl |= DOEPCTL_EPENA | DOEPCTL_USBAEP;
 }
 
@@ -141,18 +124,8 @@ static void dma_setup_prepare(uint8_t rhport) {
 // Data FIFO
 //--------------------------------------------------------------------+
 
-TU_ATTR_ALWAYS_INLINE static inline void dfifo_flush_tx(dwc2_regs_t* dwc2, uint8_t epnum) {
-  // flush TX fifo and wait for it cleared
-  dwc2->grstctl = GRSTCTL_TXFFLSH | (epnum << GRSTCTL_TXFNUM_Pos);
-  while (dwc2->grstctl & GRSTCTL_TXFFLSH_Msk) {}
-}
-TU_ATTR_ALWAYS_INLINE static inline void dfifo_flush_rx(dwc2_regs_t* dwc2) {
-  // flush RX fifo and wait for it cleared
-  dwc2->grstctl = GRSTCTL_RXFFLSH;
-  while (dwc2->grstctl & GRSTCTL_RXFFLSH_Msk) {}
-}
 
-/* USB Data FIFO Layout
+/* Device Data FIFO scheme
 
   The FIFO is split up into
   - EPInfo: for storing DMA metadata, only required when use DMA. Maximum size is called
@@ -167,11 +140,9 @@ TU_ATTR_ALWAYS_INLINE static inline void dfifo_flush_rx(dwc2_regs_t* dwc2) {
   possible since the free space is located between the RX and TX FIFOs.
 
    ---------------- ep_fifo_size
-  |   EPInfo    |
-  |   for DMA   |
+  |  DxEPIDMAn  |
   |-------------|-- gdfifocfg.EPINFOBASE (max is ghwcfg3.dfifo_depth)
-  | IN FIFO 0   |
-  |  control    |
+  | IN FIFO 0   |       control EP
   |-------------|
   | IN FIFO 1   |
   |-------------|
@@ -190,128 +161,81 @@ TU_ATTR_ALWAYS_INLINE static inline void dfifo_flush_rx(dwc2_regs_t* dwc2) {
   - All EP OUT shared a unique OUT FIFO which uses (for Slave or Buffer DMA, Scatt/Gather DMA use different formula):
     - 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"
+    - 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 + 2 x (Largest-EPsize/4 + 1) + 2 x EPOUTnum
 */
 
-TU_ATTR_ALWAYS_INLINE static inline uint16_t calc_grxfsiz(uint16_t largest_ep_size, uint8_t ep_count) {
+TU_ATTR_ALWAYS_INLINE static inline uint16_t calc_device_grxfsiz(uint16_t largest_ep_size, uint8_t ep_count) {
   return 13 + 1 + 2 * ((largest_ep_size / 4) + 1) + 2 * ep_count;
 }
 
 static bool dfifo_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t packet_size) {
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);
   const dwc2_controller_t* dwc2_controller = &_dwc2_controller[rhport];
-  uint8_t const ep_count = dwc2_controller->ep_count;
-  uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir = tu_edpt_dir(ep_addr);
+  const uint8_t ep_count = dwc2_controller->ep_count;
+  const uint8_t epnum = tu_edpt_number(ep_addr);
+  const uint8_t dir = tu_edpt_dir(ep_addr);
 
   TU_ASSERT(epnum < ep_count);
 
   uint16_t fifo_size = tu_div_ceil(packet_size, 4);
   if (dir == TUSB_DIR_OUT) {
     // Calculate required size of RX FIFO
-    uint16_t const new_sz = calc_grxfsiz(4 * fifo_size, ep_count);
+    const uint16_t new_sz = calc_device_grxfsiz(4 * fifo_size, ep_count);
 
     // If size_rx needs to be extended check if there is enough free space
     if (dwc2->grxfsiz < new_sz) {
-      TU_ASSERT(new_sz <= _dfifo_top);
+      TU_ASSERT(new_sz <= _dcd_data.dfifo_top);
       dwc2->grxfsiz = new_sz; // Enlarge RX FIFO
     }
   } else {
     // Check IN endpoints concurrently active limit
-    if(_dwc2_controller->ep_in_count) {
-      TU_ASSERT(_allocated_ep_in_count < _dwc2_controller->ep_in_count);
-      _allocated_ep_in_count++;
+    if(dwc2_controller->ep_in_count) {
+      TU_ASSERT(_dcd_data.allocated_epin_count < dwc2_controller->ep_in_count);
+      _dcd_data.allocated_epin_count++;
     }
 
     // If The TXFELVL is configured as half empty, the fifo must be twice the max_size.
-    if ((dwc2->gahbcfg & GAHBCFG_TXFELVL) == 0) {
+    if ((dwc2->gahbcfg & GAHBCFG_TX_FIFO_EPMTY_LVL) == 0) {
       fifo_size *= 2;
     }
 
     // Check if free space is available
-    TU_ASSERT(_dfifo_top >= fifo_size + dwc2->grxfsiz);
-    _dfifo_top -= fifo_size;
-    TU_LOG(DWC2_DEBUG, "    TX FIFO %u: allocated %u words at offset %u\r\n", epnum, fifo_size, _dfifo_top);
+    TU_ASSERT(_dcd_data.dfifo_top >= fifo_size + dwc2->grxfsiz);
+    _dcd_data.dfifo_top -= fifo_size;
+    // TU_LOG(DWC2_DEBUG, "    TX FIFO %u: allocated %u words at offset %u\r\n", epnum, fifo_size, dfifo_top);
 
     // Both TXFD and TXSA are in unit of 32-bit words.
     if (epnum == 0) {
-      dwc2->dieptxf0 = (fifo_size << DIEPTXF0_TX0FD_Pos) | _dfifo_top;
+      dwc2->dieptxf0 = (fifo_size << DIEPTXF0_TX0FD_Pos) | _dcd_data.dfifo_top;
     } else {
       // DIEPTXF starts at FIFO #1.
-      dwc2->dieptxf[epnum - 1] = (fifo_size << DIEPTXF_INEPTXFD_Pos) | _dfifo_top;
+      dwc2->dieptxf[epnum - 1] = (fifo_size << DIEPTXF_INEPTXFD_Pos) | _dcd_data.dfifo_top;
     }
   }
 
   return true;
 }
 
-static void dfifo_init(uint8_t rhport) {
+static void dfifo_device_init(uint8_t rhport) {
   const dwc2_controller_t* dwc2_controller = &_dwc2_controller[rhport];
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);
-  dwc2->grxfsiz = calc_grxfsiz(CFG_TUD_ENDPOINT0_SIZE, dwc2_controller->ep_count);
+  dwc2->grxfsiz = calc_device_grxfsiz(CFG_TUD_ENDPOINT0_SIZE, dwc2_controller->ep_count);
 
-  if(dma_enabled(dwc2)) {
-    // DMA use last DFIFO to store metadata
-    _dfifo_top = dma_cal_epfifo_base(rhport);
-  }else {
-    _dfifo_top = dwc2_controller->ep_fifo_size / 4;
+  // Scatter/Gather DMA mode is not yet supported. Buffer DMA only need 1 words per endpoint direction
+  const bool is_dma = dma_device_enabled(dwc2);
+  _dcd_data.dfifo_top = dwc2_controller->ep_fifo_size/4;
+  if (is_dma) {
+    _dcd_data.dfifo_top -= 2 * dwc2_controller->ep_count;
   }
+  dwc2->gdfifocfg = (_dcd_data.dfifo_top << GDFIFOCFG_EPINFOBASE_SHIFT) | _dcd_data.dfifo_top;
 
   // Allocate FIFO for EP0 IN
   dfifo_alloc(rhport, 0x80, CFG_TUD_ENDPOINT0_SIZE);
 }
 
-// Read a single data packet from receive FIFO
-static void dfifo_read_packet(uint8_t rhport, uint8_t* dst, uint16_t len) {
-  (void) rhport;
-
-  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
-  volatile const uint32_t* rx_fifo = dwc2->fifo[0];
-
-  // Reading full available 32 bit words from fifo
-  uint16_t full_words = len >> 2;
-  while (full_words--) {
-    tu_unaligned_write32(dst, *rx_fifo);
-    dst += 4;
-  }
-
-  // Read the remaining 1-3 bytes from fifo
-  uint8_t const bytes_rem = len & 0x03;
-  if (bytes_rem != 0) {
-    uint32_t const tmp = *rx_fifo;
-    dst[0] = tu_u32_byte0(tmp);
-    if (bytes_rem > 1) dst[1] = tu_u32_byte1(tmp);
-    if (bytes_rem > 2) dst[2] = tu_u32_byte2(tmp);
-  }
-}
-
-// Write a single data packet to EPIN FIFO
-static void dfifo_write_packet(uint8_t rhport, uint8_t fifo_num, uint8_t const* src, uint16_t len) {
-  (void) rhport;
-
-  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
-  volatile uint32_t* tx_fifo = dwc2->fifo[fifo_num];
-
-  // Pushing full available 32 bit words to fifo
-  uint16_t full_words = len >> 2;
-  while (full_words--) {
-    *tx_fifo = tu_unaligned_read32(src);
-    src += 4;
-  }
-
-  // Write the remaining 1-3 bytes into fifo
-  uint8_t const bytes_rem = len & 0x03;
-  if (bytes_rem) {
-    uint32_t tmp_word = src[0];
-    if (bytes_rem > 1) tmp_word |= (src[1] << 8);
-    if (bytes_rem > 2) tmp_word |= (src[2] << 16);
-
-    *tx_fifo = tmp_word;
-  }
-}
 
 //--------------------------------------------------------------------
 // Endpoint
@@ -327,25 +251,30 @@ static uint8_t get_free_fifo(void) {
 }
 #endif
 
-static void edpt_activate(uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc) {
+static void edpt_activate(uint8_t rhport, const tusb_desc_endpoint_t* p_endpoint_desc) {
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);
-  uint8_t const epnum = tu_edpt_number(p_endpoint_desc->bEndpointAddress);
-  uint8_t const dir = tu_edpt_dir(p_endpoint_desc->bEndpointAddress);
+  const uint8_t epnum = tu_edpt_number(p_endpoint_desc->bEndpointAddress);
+  const uint8_t dir = tu_edpt_dir(p_endpoint_desc->bEndpointAddress);
 
   xfer_ctl_t* xfer = XFER_CTL_BASE(epnum, dir);
   xfer->max_size = tu_edpt_packet_size(p_endpoint_desc);
   xfer->interval = p_endpoint_desc->bInterval;
 
-  // USBAEP, EPTYP, SD0PID_SEVNFRM, MPSIZ are the same for IN and OUT endpoints.
-  uint32_t const dxepctl = (1 << DOEPCTL_USBAEP_Pos) |
-                           (p_endpoint_desc->bmAttributes.xfer << DOEPCTL_EPTYP_Pos) |
-                           (p_endpoint_desc->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS ? DOEPCTL_SD0PID_SEVNFRM : 0) |
-                           (xfer->max_size << DOEPCTL_MPSIZ_Pos);
-
-  if (dir == TUSB_DIR_OUT) {
-    dwc2->epout[epnum].doepctl = dxepctl;
-    dwc2->daintmsk |= TU_BIT(DAINTMSK_OEPM_Pos + epnum);
-  } else {
+  // Endpoint control
+  union {
+    uint32_t value;
+    dwc2_depctl_t bm;
+  } depctl;
+  depctl.value = 0;
+
+  depctl.bm.mps = xfer->max_size;
+  depctl.bm.active = 1;
+  depctl.bm.type = p_endpoint_desc->bmAttributes.xfer;
+  if (p_endpoint_desc->bmAttributes.xfer != TUSB_XFER_ISOCHRONOUS) {
+    depctl.bm.set_data0_iso_even = 1;
+  }
+  if (dir == TUSB_DIR_IN) {
+    //depctl.bm.tx_fifo_num = epnum;
     uint8_t fifo_num = epnum;
 #if defined(TUP_USBIP_DWC2_ESP32)
     // Special Case for EP5, which is used by CDC but not actually called by the driver
@@ -355,46 +284,45 @@ static void edpt_activate(uint8_t rhport, tusb_desc_endpoint_t const * p_endpoin
     } else {
       fifo_num = get_free_fifo();
     }
-    //TU_ASSERT(fifo_num != 0);
 #endif
-    dwc2->epin[epnum].diepctl = dxepctl | (fifo_num << DIEPCTL_TXFNUM_Pos);
-    dwc2->daintmsk |= TU_BIT(DAINTMSK_IEPM_Pos + epnum);
+    depctl.bm.tx_fifo_num = fifo_num;
   }
+
+  dwc2_dep_t* dep = &dwc2->ep[dir == TUSB_DIR_IN ? 0 : 1][epnum];
+  dep->ctl = depctl.value;
+  dwc2->daintmsk |= TU_BIT(epnum + DAINT_SHIFT(dir));
 }
 
 static void edpt_disable(uint8_t rhport, uint8_t ep_addr, bool stall) {
   (void) rhport;
 
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);
-  uint8_t const epnum = tu_edpt_number(ep_addr);
-  uint8_t const dir = tu_edpt_dir(ep_addr);
+  const uint8_t epnum = tu_edpt_number(ep_addr);
+  const uint8_t dir = tu_edpt_dir(ep_addr);
+  dwc2_dep_t* dep = &dwc2->ep[dir == TUSB_DIR_IN ? 0 : 1][epnum];
 
   if (dir == TUSB_DIR_IN) {
-    dwc2_epin_t* epin = dwc2->epin;
-
     // Only disable currently enabled non-control endpoint
-    if ((epnum == 0) || !(epin[epnum].diepctl & DIEPCTL_EPENA)) {
-      epin[epnum].diepctl |= DIEPCTL_SNAK | (stall ? DIEPCTL_STALL : 0);
+    if ((epnum == 0) || !(dep->diepctl & DIEPCTL_EPENA)) {
+      dep->diepctl |= DIEPCTL_SNAK | (stall ? DIEPCTL_STALL : 0);
     } else {
       // Stop transmitting packets and NAK IN xfers.
-      epin[epnum].diepctl |= DIEPCTL_SNAK;
-      while ((epin[epnum].diepint & DIEPINT_INEPNE) == 0) {}
+      dep->diepctl |= DIEPCTL_SNAK;
+      while ((dep->diepint & DIEPINT_INEPNE) == 0) {}
 
       // Disable the endpoint.
-      epin[epnum].diepctl |= DIEPCTL_EPDIS | (stall ? DIEPCTL_STALL : 0);
-      while ((epin[epnum].diepint & DIEPINT_EPDISD_Msk) == 0) {}
+      dep->diepctl |= DIEPCTL_EPDIS | (stall ? DIEPCTL_STALL : 0);
+      while ((dep->diepint & DIEPINT_EPDISD_Msk) == 0) {}
 
-      epin[epnum].diepint = DIEPINT_EPDISD;
+      dep->diepint = DIEPINT_EPDISD;
     }
 
     // Flush the FIFO, and wait until we have confirmed it cleared.
     dfifo_flush_tx(dwc2, epnum);
   } else {
-    dwc2_epout_t* epout = dwc2->epout;
-
     // Only disable currently enabled non-control endpoint
-    if ((epnum == 0) || !(epout[epnum].doepctl & DOEPCTL_EPENA)) {
-      epout[epnum].doepctl |= stall ? DOEPCTL_STALL : 0;
+    if ((epnum == 0) || !(dep->doepctl & DOEPCTL_EPENA)) {
+      dep->doepctl |= stall ? DOEPCTL_STALL : 0;
     } else {
       // Asserting GONAK is required to STALL an OUT endpoint.
       // Simpler to use polling here, we don't use the "B"OUTNAKEFF interrupt
@@ -403,11 +331,11 @@ static void edpt_disable(uint8_t rhport, uint8_t ep_addr, bool stall) {
       dwc2->dctl |= DCTL_SGONAK;
       while ((dwc2->gintsts & GINTSTS_BOUTNAKEFF_Msk) == 0) {}
 
-      // Ditto here- disable the endpoint.
-      epout[epnum].doepctl |= DOEPCTL_EPDIS | (stall ? DOEPCTL_STALL : 0);
-      while ((epout[epnum].doepint & DOEPINT_EPDISD_Msk) == 0) {}
+      // Ditto here disable the endpoint.
+      dep->doepctl |= DOEPCTL_EPDIS | (stall ? DOEPCTL_STALL : 0);
+      while ((dep->doepint & DOEPINT_EPDISD_Msk) == 0) {}
 
-      epout[epnum].doepint = DOEPINT_EPDISD;
+      dep->doepint = DOEPINT_EPDISD;
 
       // Allow other OUT endpoints to keep receiving.
       dwc2->dctl |= DCTL_CGONAK;
@@ -415,343 +343,124 @@ static void edpt_disable(uint8_t rhport, uint8_t ep_addr, bool stall) {
   }
 }
 
-// Start of Bus Reset
-static void bus_reset(uint8_t rhport) {
-  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
-  uint8_t const ep_count = _dwc2_controller[rhport].ep_count;
-
-  tu_memclr(xfer_status, sizeof(xfer_status));
-
-  _sof_en = false;
-
-  _allocated_ep_in_count = 1;
-
-  // clear device address
-  dwc2->dcfg &= ~DCFG_DAD_Msk;
-
-  // 1. NAK for all OUT endpoints
-  for (uint8_t n = 0; n < ep_count; n++) {
-    dwc2->epout[n].doepctl |= DOEPCTL_SNAK;
-  }
-
-  // 2. Disable all IN endpoints
-  for (uint8_t n = 0; n < ep_count; n++) {
-    if (dwc2->epin[n].diepctl & DIEPCTL_EPENA) {
-      dwc2->epin[n].diepctl |= DIEPCTL_SNAK | DIEPCTL_EPDIS;
-    }
-  }
-
-  dfifo_flush_tx(dwc2, 0x10); // all tx fifo
-  dfifo_flush_rx(dwc2);
-
-  // 3. Set up interrupt mask
-  dwc2->daintmsk = TU_BIT(DAINTMSK_OEPM_Pos) | TU_BIT(DAINTMSK_IEPM_Pos);
-  dwc2->doepmsk = DOEPMSK_STUPM | DOEPMSK_XFRCM;
-  dwc2->diepmsk = DIEPMSK_TOM | DIEPMSK_XFRCM;
-
-  dfifo_init(rhport);
-
-  // Fixed control EP0 size to 64 bytes
-  dwc2->epin[0].diepctl &= ~(0x03 << DIEPCTL_MPSIZ_Pos);
-  dwc2->epout[0].doepctl &= ~(0x03 << DOEPCTL_MPSIZ_Pos);
-
-  xfer_status[0][TUSB_DIR_OUT].max_size = 64;
-  xfer_status[0][TUSB_DIR_IN].max_size = 64;
-
-  if(dma_enabled(dwc2)) {
-    dma_setup_prepare(rhport);
-  } else {
-    dwc2->epout[0].doeptsiz |= (3 << DOEPTSIZ_STUPCNT_Pos);
-  }
-
-  dwc2->gintmsk |= GINTMSK_OEPINT | GINTMSK_IEPINT;
-}
-
-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;
-
+static void edpt_schedule_packets(uint8_t rhport, const uint8_t epnum, const uint8_t dir) {
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);
   xfer_ctl_t* const xfer = XFER_CTL_BASE(epnum, dir);
+  dwc2_dep_t* dep = &dwc2->ep[dir == TUSB_DIR_IN ? 0 : 1][epnum];
 
-  // EP0 is limited to one packet each xfer
-  // We use multiple transaction of xfer->max_size length to get a whole transfer done
+  uint16_t num_packets;
+  uint16_t total_bytes;
+
+  // EP0 is limited to one packet per xfer
   if (epnum == 0) {
-    total_bytes = tu_min16(ep0_pending[dir], xfer->max_size);
-    ep0_pending[dir] -= total_bytes;
+    total_bytes = tu_min16(_dcd_data.ep0_pending[dir], xfer->max_size);
+    _dcd_data.ep0_pending[dir] -= total_bytes;
+    num_packets = 1;
+  } else {
+    total_bytes = xfer->total_len;
+    num_packets = tu_div_ceil(total_bytes, xfer->max_size);
+    if (num_packets == 0) {
+      num_packets = 1; // zero length packet still count as 1
+    }
   }
 
-  // IN and OUT endpoint xfers are interrupt-driven, we just schedule them here.
-  if (dir == TUSB_DIR_IN) {
-    dwc2_epin_t* epin = dwc2->epin;
-
-    // A full IN transfer (multiple packets, possibly) triggers XFRC.
-    epin[epnum].dieptsiz = (num_packets << DIEPTSIZ_PKTCNT_Pos) |
-                           ((total_bytes << DIEPTSIZ_XFRSIZ_Pos) & DIEPTSIZ_XFRSIZ_Msk);
-
-    if(dma_enabled(dwc2)) {
-      epin[epnum].diepdma = (uintptr_t)xfer->buffer;
-
-      // For ISO endpoint set correct odd/even bit for next frame.
-      if ((epin[epnum].diepctl & DIEPCTL_EPTYP) == DIEPCTL_EPTYP_0 && (XFER_CTL_BASE(epnum, dir))->interval == 1) {
-        // Take odd/even bit from frame counter.
-        uint32_t const odd_frame_now = (dwc2->dsts & (1u << DSTS_FNSOF_Pos));
-        epin[epnum].diepctl |= (odd_frame_now ? DIEPCTL_SD0PID_SEVNFRM_Msk : DIEPCTL_SODDFRM_Msk);
-      }
-
-      epin[epnum].diepctl |= DIEPCTL_EPENA | DIEPCTL_CNAK;
+  // transfer size: A full OUT transfer (multiple packets, possibly) triggers XFRC.
+  union {
+    uint32_t value;
+    dwc2_ep_tsize_t bm;
+  } deptsiz;
+  deptsiz.value = 0;
+  deptsiz.bm.xfer_size =  total_bytes;
+  deptsiz.bm.packet_count = num_packets;
+
+  dep->tsiz = deptsiz.value;
+
+  // control
+  union {
+    dwc2_depctl_t bm;
+    uint32_t value;
+  } depctl;
+  depctl.value = dep->ctl;
+
+  depctl.bm.clear_nak = 1;
+  depctl.bm.enable = 1;
+  if (depctl.bm.type == DEPCTL_EPTYPE_ISOCHRONOUS && xfer->interval == 1) {
+    const uint32_t odd_now = (dwc2->dsts_bm.frame_number & 1u);
+    if (odd_now) {
+      depctl.bm.set_data0_iso_even = 1;
     } else {
+      depctl.bm.set_data1_iso_odd = 1;
+    }
+  }
 
-      epin[epnum].diepctl |= DIEPCTL_EPENA | DIEPCTL_CNAK;
-
-      // For ISO endpoint set correct odd/even bit for next frame.
-      if ((epin[epnum].diepctl & DIEPCTL_EPTYP) == DIEPCTL_EPTYP_0 && (XFER_CTL_BASE(epnum, dir))->interval == 1) {
-        // Take odd/even bit from frame counter.
-        uint32_t const odd_frame_now = (dwc2->dsts & (1u << DSTS_FNSOF_Pos));
-        epin[epnum].diepctl |= (odd_frame_now ? DIEPCTL_SD0PID_SEVNFRM_Msk : DIEPCTL_SODDFRM_Msk);
-      }
-      // Enable fifo empty interrupt only if there are something to put in the fifo.
-      if (total_bytes != 0) {
-        dwc2->diepempmsk |= (1 << epnum);
-      }
+  const bool is_dma = dma_device_enabled(dwc2);
+  if(is_dma) {
+    if (dir == TUSB_DIR_IN && total_bytes != 0) {
+      dcd_dcache_clean(xfer->buffer, total_bytes);
     }
+    dep->diepdma = (uintptr_t) xfer->buffer;
+    dep->diepctl = depctl.value; // enable endpoint
   } else {
-    dwc2_epout_t* epout = dwc2->epout;
-
-    // A full OUT transfer (multiple packets, possibly) triggers XFRC.
-    epout[epnum].doeptsiz &= ~(DOEPTSIZ_PKTCNT_Msk | DOEPTSIZ_XFRSIZ);
-    epout[epnum].doeptsiz |= (num_packets << DOEPTSIZ_PKTCNT_Pos) |
-                             ((total_bytes << DOEPTSIZ_XFRSIZ_Pos) & DOEPTSIZ_XFRSIZ_Msk);
-
-    if ((epout[epnum].doepctl & DOEPCTL_EPTYP) == DOEPCTL_EPTYP_0 &&
-        XFER_CTL_BASE(epnum, dir)->interval == 1) {
-      // Take odd/even bit from frame counter.
-      uint32_t const odd_frame_now = (dwc2->dsts & (1u << DSTS_FNSOF_Pos));
-      epout[epnum].doepctl |= (odd_frame_now ? DOEPCTL_SD0PID_SEVNFRM_Msk : DOEPCTL_SODDFRM_Msk);
-    }
+    dep->diepctl = depctl.value; // enable endpoint
 
-    if(dma_enabled(dwc2)) {
-      epout[epnum].doepdma = (uintptr_t)xfer->buffer;
+    // Enable tx fifo empty interrupt only if there is data. Note must after depctl enable
+    if (dir == TUSB_DIR_IN && total_bytes != 0) {
+      dwc2->diepempmsk |= (1 << epnum);
     }
-
-    epout[epnum].doepctl |= DOEPCTL_EPENA | DOEPCTL_CNAK;
   }
 }
 
-/*------------------------------------------------------------------*/
-/* Controller API
- *------------------------------------------------------------------*/
-
-static void reset_core(dwc2_regs_t* dwc2) {
-  // reset core
-  dwc2->grstctl |= GRSTCTL_CSRST;
-
-  // wait for reset bit is cleared
-  // TODO version 4.20a should wait for RESET DONE mask
-  while (dwc2->grstctl & GRSTCTL_CSRST) {}
-
-  // wait for AHB master IDLE
-  while (!(dwc2->grstctl & GRSTCTL_AHBIDL)) {}
-
-  // wait for device mode ?
-}
-
-static bool phy_hs_supported(dwc2_regs_t* dwc2) {
-  (void) dwc2;
-
-#if !TUD_OPT_HIGH_SPEED
-  return false;
-#else
-  return dwc2->ghwcfg2_bm.hs_phy_type != GHWCFG2_HSPHY_NOT_SUPPORTED;
-#endif
-}
-
-static void phy_fs_init(dwc2_regs_t* dwc2) {
-  TU_LOG(DWC2_DEBUG, "Fullspeed PHY init\r\n");
-
-  // Select FS PHY
-  dwc2->gusbcfg |= GUSBCFG_PHYSEL;
-
-  // MCU specific PHY init before reset
-  dwc2_phy_init(dwc2, GHWCFG2_HSPHY_NOT_SUPPORTED);
-
-  // Reset core after selecting PHY
-  reset_core(dwc2);
-
-  // USB turnaround time is critical for certification where long cables and 5-Hubs are used.
-  // So if you need the AHB to run at less than 30 MHz, and if USB turnaround time is not critical,
-  // these bits can be programmed to a larger value. Default is 5
-  dwc2->gusbcfg = (dwc2->gusbcfg & ~GUSBCFG_TRDT_Msk) | (5u << GUSBCFG_TRDT_Pos);
-
-  // MCU specific PHY update post reset
-  dwc2_phy_update(dwc2, GHWCFG2_HSPHY_NOT_SUPPORTED);
-
-  // set max speed
-  dwc2->dcfg = (dwc2->dcfg & ~DCFG_DSPD_Msk) | (DCFG_DSPD_FS << DCFG_DSPD_Pos);
-}
-
-static void phy_hs_init(dwc2_regs_t* dwc2) {
-  uint32_t gusbcfg = dwc2->gusbcfg;
-
-  // De-select FS PHY
-  gusbcfg &= ~GUSBCFG_PHYSEL;
-
-  if (dwc2->ghwcfg2_bm.hs_phy_type == GHWCFG2_HSPHY_ULPI) {
-    TU_LOG(DWC2_DEBUG, "Highspeed ULPI PHY init\r\n");
-
-    // Select ULPI
-    gusbcfg |= GUSBCFG_ULPI_UTMI_SEL;
-
-    // ULPI 8-bit interface, single data rate
-    gusbcfg &= ~(GUSBCFG_PHYIF16 | GUSBCFG_DDRSEL);
+//--------------------------------------------------------------------
+// Controller API
+//--------------------------------------------------------------------
+bool dcd_init(uint8_t rhport, const tusb_rhport_init_t* rh_init) {
+  (void) rh_init;
+  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
 
-    // default internal VBUS Indicator and Drive
-    gusbcfg &= ~(GUSBCFG_ULPIEVBUSD | GUSBCFG_ULPIEVBUSI);
+  tu_memclr(&_dcd_data, sizeof(_dcd_data));
 
-    // Disable FS/LS ULPI
-    gusbcfg &= ~(GUSBCFG_ULPIFSLS | GUSBCFG_ULPICSM);
-  } else {
-    TU_LOG(DWC2_DEBUG, "Highspeed UTMI+ PHY init\r\n");
+  // Core Initialization
+  const bool is_highspeed = dwc2_core_is_highspeed(dwc2, TUSB_ROLE_DEVICE);
+  const bool is_dma = dma_device_enabled(dwc2);
+  TU_ASSERT(dwc2_core_init(rhport, is_highspeed, is_dma));
 
-    // Select UTMI+ with 8-bit interface
-    gusbcfg &= ~(GUSBCFG_ULPI_UTMI_SEL | GUSBCFG_PHYIF16);
+  //------------- 7.1 Device Initialization -------------//
+  // Set device max speed
+  uint32_t dcfg = dwc2->dcfg & ~DCFG_DSPD_Msk;
+  if (is_highspeed) {
+    dcfg |= DCFG_DSPD_HS << DCFG_DSPD_Pos;
 
-    // Set 16-bit interface if supported
-    if (dwc2->ghwcfg4_bm.phy_data_width) {
-      gusbcfg |= GUSBCFG_PHYIF16;
+    // XCVRDLY: transceiver delay between xcvr_sel and txvalid during device chirp is required
+    // when using with some PHYs such as USB334x (USB3341, USB3343, USB3346, USB3347)
+    if (dwc2->ghwcfg2_bm.hs_phy_type == GHWCFG2_HSPHY_ULPI) {
+      dcfg |= DCFG_XCVRDLY;
     }
+  } else {
+    dcfg |= DCFG_DSPD_FS << DCFG_DSPD_Pos;
   }
 
-  // Apply config
-  dwc2->gusbcfg = gusbcfg;
-
-  // mcu specific phy init
-  dwc2_phy_init(dwc2, dwc2->ghwcfg2_bm.hs_phy_type);
-
-  // Reset core after selecting PHY
-  reset_core(dwc2);
-
-  // Set turn-around, must after core reset otherwise it will be clear
-  // - 9 if using 8-bit PHY interface
-  // - 5 if using 16-bit PHY interface
-  gusbcfg &= ~GUSBCFG_TRDT_Msk;
-  gusbcfg |= (dwc2->ghwcfg4_bm.phy_data_width ? 5u : 9u) << GUSBCFG_TRDT_Pos;
-  dwc2->gusbcfg = gusbcfg;
-
-  // MCU specific PHY update post reset
-  dwc2_phy_update(dwc2, dwc2->ghwcfg2_bm.hs_phy_type);
-
-  // Set max speed
-  uint32_t dcfg = dwc2->dcfg;
-  dcfg &= ~DCFG_DSPD_Msk;
-  dcfg |= DCFG_DSPD_HS << DCFG_DSPD_Pos;
-
-  // XCVRDLY: transceiver delay between xcvr_sel and txvalid during device chirp is required
-  // when using with some PHYs such as USB334x (USB3341, USB3343, USB3346, USB3347)
-  if (dwc2->ghwcfg2_bm.hs_phy_type == GHWCFG2_HSPHY_ULPI) {
-    dcfg |= DCFG_XCVRDLY;
-  }
-
+  dcfg |= DCFG_NZLSOHSK; // send STALL back and discard if host send non-zlp during control status
   dwc2->dcfg = dcfg;
-}
-
-static bool check_dwc2(dwc2_regs_t* dwc2) {
-#if CFG_TUSB_DEBUG >= DWC2_DEBUG
-  // print guid, gsnpsid, ghwcfg1, ghwcfg2, ghwcfg3, ghwcfg4
-  // Run 'dwc2_info.py render-md' and check dwc2_info.md for bit-field value and comparison with other ports
-  volatile uint32_t const* p = (volatile uint32_t const*) &dwc2->guid;
-  TU_LOG1("guid, gsnpsid, ghwcfg1, ghwcfg2, ghwcfg3, ghwcfg4\r\n");
-  for (size_t i = 0; i < 5; i++) {
-    TU_LOG1("0x%08" PRIX32 ", ", p[i]);
-  }
-  TU_LOG1("0x%08" PRIX32 "\r\n", p[5]);
-#endif
 
-  // For some reason: GD32VF103 snpsid and all hwcfg register are always zero (skip it)
-  (void) dwc2;
-#if !TU_CHECK_MCU(OPT_MCU_GD32VF103)
-  uint32_t const gsnpsid = dwc2->gsnpsid & GSNPSID_ID_MASK;
-  TU_ASSERT(gsnpsid == DWC2_OTG_ID || gsnpsid == DWC2_FS_IOT_ID || gsnpsid == DWC2_HS_IOT_ID);
-#endif
-
-  return true;
-}
-
-void dcd_init(uint8_t rhport) {
-  // Programming model begins in the last section of the chapter on the USB
-  // peripheral in each Reference Manual.
-  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
-
-  // Check Synopsys ID register, failed if controller clock/power is not enabled
-  TU_ASSERT(check_dwc2(dwc2), );
   dcd_disconnect(rhport);
 
-  if (phy_hs_supported(dwc2)) {
-    phy_hs_init(dwc2); // Highspeed
-  } else {
-    phy_fs_init(dwc2); // core does not support highspeed or hs phy is not present
-  }
-
-  // Restart PHY clock
-  dwc2->pcgctl &= ~(PCGCTL_STOPPCLK | PCGCTL_GATEHCLK | PCGCTL_PWRCLMP | PCGCTL_RSTPDWNMODULE);
-
-  /* Set HS/FS Timeout Calibration to 7 (max available value).
-   * The number of PHY clocks that the application programs in
-   * this field is added to the high/full speed interpacket timeout
-   * duration in the core to account for any additional delays
-   * introduced by the PHY. This can be required, because the delay
-   * introduced by the PHY in generating the linestate condition
-   * can vary from one PHY to another.
-   */
-  dwc2->gusbcfg |= (7ul << GUSBCFG_TOCAL_Pos);
-
   // Force device mode
   dwc2->gusbcfg = (dwc2->gusbcfg & ~GUSBCFG_FHMOD) | GUSBCFG_FDMOD;
 
   // Clear A override, force B Valid
   dwc2->gotgctl = (dwc2->gotgctl & ~GOTGCTL_AVALOEN) | GOTGCTL_BVALOEN | GOTGCTL_BVALOVAL;
 
-  // If USB host misbehaves during status portion of control xfer
-  // (non zero-length packet), send STALL back and discard.
-  dwc2->dcfg |= DCFG_NZLSOHSK;
-
-  dfifo_flush_tx(dwc2, 0x10); // all tx fifo
-  dfifo_flush_rx(dwc2);
+  // Enable required interrupts
+  dwc2->gintmsk |= GINTMSK_OTGINT | GINTMSK_USBSUSPM | GINTMSK_USBRST | GINTMSK_ENUMDNEM | GINTMSK_WUIM;
 
-  // Clear all interrupts
-  uint32_t int_mask = dwc2->gintsts;
-  dwc2->gintsts |= int_mask;
-  int_mask = dwc2->gotgint;
-  dwc2->gotgint |= int_mask;
-
-  // Required as part of core initialization.
-  dwc2->gintmsk = GINTMSK_OTGINT | GINTMSK_USBSUSPM | GINTMSK_USBRST | GINTMSK_ENUMDNEM | GINTMSK_WUIM;
-
-  // Configure TX FIFO empty level for interrupt. Default is complete empty
-  dwc2->gahbcfg |= GAHBCFG_TXFELVL;
-
-  if (dma_enabled(dwc2)) {
-    const uint16_t epinfo_base = dma_cal_epfifo_base(rhport);
-    dwc2->gdfifocfg = (epinfo_base << GDFIFOCFG_EPINFOBASE_SHIFT) | epinfo_base;
-
-    // DMA seems to be only settable after a core reset
-    dwc2->gahbcfg |= GAHBCFG_DMAEN | GAHBCFG_HBSTLEN_2;
-  }else {
-    dwc2->gintmsk |= GINTMSK_RXFLVLM;
-  }
-
-  // Enable global interrupt
-  dwc2->gahbcfg |= GAHBCFG_GINT;
-
-  // make sure we are in device mode
-//  TU_ASSERT(!(dwc2->gintsts & GINTSTS_CMOD), );
-
-//  TU_LOG_HEX(DWC2_DEBUG, dwc2->gotgctl);
-//  TU_LOG_HEX(DWC2_DEBUG, dwc2->gusbcfg);
-//  TU_LOG_HEX(DWC2_DEBUG, dwc2->dcfg);
-//  TU_LOG_HEX(DWC2_DEBUG, dwc2->gahbcfg);
+  // TX FIFO empty level for interrupt is complete empty
+  uint32_t gahbcfg = dwc2->gahbcfg;
+  gahbcfg |= GAHBCFG_TX_FIFO_EPMTY_LVL;
+  gahbcfg |= GAHBCFG_GINT; // Enable global interrupt
+  dwc2->gahbcfg = gahbcfg;
 
   dcd_connect(rhport);
+  return true;
 }
 
 void dcd_int_enable(uint8_t rhport) {
@@ -827,7 +536,7 @@ void dcd_sof_enable(uint8_t rhport, bool en) {
   (void) rhport;
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);
 
-  _sof_en = en;
+  _dcd_data.sof_en = en;
 
   if (en) {
     dwc2->gintsts = GINTSTS_SOF;
@@ -852,23 +561,19 @@ void dcd_edpt_close_all(uint8_t rhport) {
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);
   uint8_t const ep_count = _dwc2_controller[rhport].ep_count;
 
-  _allocated_ep_in_count = 1;
+  _dcd_data.allocated_epin_count = 0;
 
   // Disable non-control interrupt
   dwc2->daintmsk = (1 << DAINTMSK_OEPM_Pos) | (1 << DAINTMSK_IEPM_Pos);
 
   for (uint8_t n = 1; n < ep_count; n++) {
-    // disable OUT endpoint
-    if (dwc2->epout[n].doepctl & DOEPCTL_EPENA) {
-      dwc2->epout[n].doepctl |= DOEPCTL_SNAK | DOEPCTL_EPDIS;
-    }
-    xfer_status[n][TUSB_DIR_OUT].max_size = 0;
-
-    // disable IN endpoint
-    if (dwc2->epin[n].diepctl & DIEPCTL_EPENA) {
-      dwc2->epin[n].diepctl |= DIEPCTL_SNAK | DIEPCTL_EPDIS;
+    for (uint8_t d = 0; d < 2; d++) {
+      dwc2_dep_t* dep = &dwc2->ep[d][n];
+      if (dep->ctl & EPCTL_EPENA) {
+        dep->ctl |= EPCTL_SNAK | EPCTL_EPDIS;
+      }
+      xfer_status[n][1-d].max_size = 0;
     }
-    xfer_status[n][TUSB_DIR_IN].max_size = 0;
   }
 
 #if defined(TUP_USBIP_DWC2_ESP32)
@@ -878,7 +583,7 @@ void dcd_edpt_close_all(uint8_t rhport) {
   dfifo_flush_tx(dwc2, 0x10); // all tx fifo
   dfifo_flush_rx(dwc2);
 
-  dfifo_init(rhport); // re-init dfifo
+  dfifo_device_init(rhport); // re-init dfifo
 }
 
 bool dcd_edpt_iso_alloc(uint8_t rhport, uint8_t ep_addr, uint16_t largest_packet_size) {
@@ -904,21 +609,12 @@ bool dcd_edpt_xfer(uint8_t rhport, uint8_t ep_addr, uint8_t* buffer, uint16_t to
 
   // 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]);
-  } else {
-    uint16_t num_packets = (total_bytes / xfer->max_size);
-    uint16_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++;
-
-    // Schedule packets to be sent within interrupt
-    edpt_schedule_packets(rhport, epnum, dir, num_packets, total_bytes);
+    _dcd_data.ep0_pending[dir] = total_bytes;
   }
 
+  // Schedule packets to be sent within interrupt
+  edpt_schedule_packets(rhport, epnum, dir);
+
   return true;
 }
 
@@ -938,330 +634,401 @@ bool dcd_edpt_xfer_fifo(uint8_t rhport, uint8_t ep_addr, tu_fifo_t* ff, uint16_t
   xfer->ff = ff;
   xfer->total_len = total_bytes;
 
-  uint16_t num_packets = (total_bytes / xfer->max_size);
-  uint16_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++;
-
   // Schedule packets to be sent within interrupt
-  edpt_schedule_packets(rhport, epnum, dir, num_packets, total_bytes);
+  // TODO xfer fifo may only available for slave mode
+  edpt_schedule_packets(rhport, epnum, dir);
 
   return true;
 }
 
-void dcd_edpt_close(uint8_t rhport, uint8_t ep_addr) {
-  edpt_disable(rhport, ep_addr, false);
-}
-
 void dcd_edpt_stall(uint8_t rhport, uint8_t ep_addr) {
+  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
   edpt_disable(rhport, ep_addr, true);
-  if((tu_edpt_number(ep_addr) == 0) && dma_enabled(DWC2_REG(rhport))) {
+  if((tu_edpt_number(ep_addr) == 0) && dma_device_enabled(dwc2)) {
     dma_setup_prepare(rhport);
   }
 }
 
 void dcd_edpt_clear_stall(uint8_t rhport, uint8_t ep_addr) {
-  (void) rhport;
-
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);
-
   uint8_t const epnum = tu_edpt_number(ep_addr);
   uint8_t const dir = tu_edpt_dir(ep_addr);
+  dwc2_dep_t* dep = &dwc2->ep[dir == TUSB_DIR_IN ? 0 : 1][epnum];
 
   // Clear stall and reset data toggle
-  if (dir == TUSB_DIR_IN) {
-    dwc2->epin[epnum].diepctl &= ~DIEPCTL_STALL;
-    dwc2->epin[epnum].diepctl |= DIEPCTL_SD0PID_SEVNFRM;
-  } else {
-    dwc2->epout[epnum].doepctl &= ~DOEPCTL_STALL;
-    dwc2->epout[epnum].doepctl |= DOEPCTL_SD0PID_SEVNFRM;
-  }
+  dep->ctl &= ~EPCTL_STALL;;
+  dep->ctl |= EPCTL_SD0PID_SEVNFRM;
 }
 
 //--------------------------------------------------------------------
 // Interrupt Handler
 //--------------------------------------------------------------------
 
+// 7.4.1 Initialization on USB Reset
+static void handle_bus_reset(uint8_t rhport) {
+  dwc2_regs_t *dwc2 = DWC2_REG(rhport);
+  const uint8_t ep_count =  DWC2_EP_COUNT(dwc2);
+
+  tu_memclr(xfer_status, sizeof(xfer_status));
+
+  _dcd_data.sof_en = false;
+  _dcd_data.allocated_epin_count = 0;
+
+  // 1. NAK for all OUT endpoints
+  for (uint8_t n = 0; n < ep_count; n++) {
+    dwc2->epout[n].doepctl |= DOEPCTL_SNAK;
+  }
+
+  // Disable all IN endpoints
+  for (uint8_t n = 0; n < ep_count; n++) {
+    if (dwc2->epin[n].diepctl & DIEPCTL_EPENA) {
+      dwc2->epin[n].diepctl |= DIEPCTL_SNAK | DIEPCTL_EPDIS;
+    }
+  }
+
+  // 2. Set up interrupt mask for EP0
+  dwc2->daintmsk = TU_BIT(DAINTMSK_OEPM_Pos) | TU_BIT(DAINTMSK_IEPM_Pos);
+  dwc2->doepmsk = DOEPMSK_STUPM | DOEPMSK_XFRCM;
+  dwc2->diepmsk = DIEPMSK_TOM | DIEPMSK_XFRCM;
+
+  // 4. Set up DFIFO
+  dfifo_flush_tx(dwc2, 0x10); // all tx fifo
+  dfifo_flush_rx(dwc2);
+  dfifo_device_init(rhport);
+
+  // 5. Reset device address
+  dwc2->dcfg_bm.address = 0;
+
+  // Fixed both control EP0 size to 64 bytes
+  dwc2->epin[0].ctl &= ~(0x03 << DIEPCTL_MPSIZ_Pos);
+  dwc2->epout[0].ctl &= ~(0x03 << DOEPCTL_MPSIZ_Pos);
+
+  xfer_status[0][TUSB_DIR_OUT].max_size = 64;
+  xfer_status[0][TUSB_DIR_IN].max_size = 64;
+
+  if(dma_device_enabled(dwc2)) {
+    dma_setup_prepare(rhport);
+  } else {
+    dwc2->epout[0].doeptsiz |= (3 << DOEPTSIZ_STUPCNT_Pos);
+  }
+
+  dwc2->gintmsk |= GINTMSK_OEPINT | GINTMSK_IEPINT;
+}
+
+static void handle_enum_done(uint8_t rhport) {
+  dwc2_regs_t *dwc2 = DWC2_REG(rhport);
+  tusb_speed_t speed;
+  switch (dwc2->dsts_bm.enum_speed) {
+    case DCFG_SPEED_HIGH:
+      speed = TUSB_SPEED_HIGH;
+    break;
+
+    case DCFG_SPEED_LOW:
+      speed = TUSB_SPEED_LOW;
+    break;
+
+    case DCFG_SPEED_FULL_30_60MHZ:
+    case DCFG_SPEED_FULL_48MHZ:
+    default:
+      speed = TUSB_SPEED_FULL;
+    break;
+  }
+
+  // TODO must update GUSBCFG_TRDT according to link speed
+  dcd_event_bus_reset(rhport, speed, true);
+}
+
+#if 0
+TU_ATTR_ALWAYS_INLINE static inline void print_doepint(uint32_t doepint) {
+  const char* str[] = {
+    "XFRC", "DIS", "AHBERR", "SETUP_DONE",
+    "ORXED", "STATUS_RX", "SETUP_B2B", "RSV7",
+    "OPERR", "BNA", "RSV10", "ISODROP",
+    "BBLERR", "NAK", "NYET", "SETUP_RX"
+  };
+
+  for(uint32_t i=0; i<TU_ARRAY_SIZE(str); i++) {
+    if (doepint & TU_BIT(i)) {
+      TU_LOG1("%s ", str[i]);
+    }
+  }
+  TU_LOG1("\r\n");
+}
+#endif
+
+#if CFG_TUD_DWC2_SLAVE_ENABLE
+// Process shared receive FIFO, this interrupt is only used in Slave mode
 static void handle_rxflvl_irq(uint8_t rhport) {
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);
-  volatile uint32_t const* rx_fifo = dwc2->fifo[0];
+  const volatile uint32_t* rx_fifo = dwc2->fifo[0];
 
   // Pop control word off FIFO
-  uint32_t const ctl_word = dwc2->grxstsp;
-  uint8_t const pktsts = (ctl_word & GRXSTSP_PKTSTS_Msk) >> GRXSTSP_PKTSTS_Pos;
-  uint8_t const epnum = (ctl_word & GRXSTSP_EPNUM_Msk) >> GRXSTSP_EPNUM_Pos;
-  uint16_t const bcnt = (ctl_word & GRXSTSP_BCNT_Msk) >> GRXSTSP_BCNT_Pos;
-
-  dwc2_epout_t* epout = &dwc2->epout[epnum];
-
-//#if CFG_TUSB_DEBUG >= DWC2_DEBUG
-//  const char * pktsts_str[] =
-//  {
-//    "ASSERT", "Global NAK (ISR)", "Out Data Received", "Out Transfer Complete (ISR)",
-//    "Setup Complete (ISR)", "ASSERT", "Setup Data Received"
-//  };
-//  TU_LOG_LOCATION();
-//  TU_LOG(DWC2_DEBUG, "  EP %02X, Byte Count %u, %s\r\n", epnum, bcnt, pktsts_str[pktsts]);
-//  TU_LOG(DWC2_DEBUG, "  daint = %08lX, doepint = %04X\r\n", (unsigned long) dwc2->daint, (unsigned int) epout->doepint);
-//#endif
-
-  switch (pktsts) {
-    // Global OUT NAK: do nothing
-    case GRXSTS_PKTSTS_GLOBALOUTNAK:
+  const dwc2_grxstsp_t grxstsp_bm = dwc2->grxstsp_bm;
+  const uint8_t epnum = grxstsp_bm.ep_ch_num;
+
+  dwc2_dep_t* epout = &dwc2->epout[epnum];
+
+  switch (grxstsp_bm.packet_status) {
+    case GRXSTS_PKTSTS_GLOBAL_OUT_NAK:
+      // Global OUT NAK: do nothing
       break;
 
-    case GRXSTS_PKTSTS_SETUPRX:
+    case GRXSTS_PKTSTS_SETUP_RX: {
       // Setup packet received
-
-      // 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);
+      uint32_t* setup = (uint32_t*)(uintptr_t) _dcd_usbbuf.setup_packet;
+      // We can receive up to three setup packets in succession, but only the last one is valid.
+      setup[0] = (*rx_fifo);
+      setup[1] = (*rx_fifo);
       break;
+    }
 
-    case GRXSTS_PKTSTS_SETUPDONE:
-      // Setup packet done (Interrupt)
+    case GRXSTS_PKTSTS_SETUP_DONE:
+      // Setup packet done:
+      // After popping this out, dwc2 asserts a DOEPINT_SETUP interrupt which is handled by handle_epout_irq()
       epout->doeptsiz |= (3 << DOEPTSIZ_STUPCNT_Pos);
       break;
 
-    case GRXSTS_PKTSTS_OUTRX: {
+    case GRXSTS_PKTSTS_RX_DATA: {
       // Out packet received
+      const uint16_t byte_count = grxstsp_bm.byte_count;
       xfer_ctl_t* xfer = XFER_CTL_BASE(epnum, TUSB_DIR_OUT);
 
-      // Read packet off RxFIFO
-      if (xfer->ff) {
-        // Ring buffer
-        tu_fifo_write_n_const_addr_full_words(xfer->ff, (const void*) (uintptr_t) rx_fifo, bcnt);
-      } else {
-        // Linear buffer
-        dfifo_read_packet(rhport, xfer->buffer, bcnt);
-
-        // Increment pointer to xfer data
-        xfer->buffer += bcnt;
-      }
+      if (byte_count) {
+        // Read packet off RxFIFO
+        if (xfer->ff) {
+          tu_fifo_write_n_const_addr_full_words(xfer->ff, (const void*) (uintptr_t) rx_fifo, byte_count);
+        } else {
+          dfifo_read_packet(dwc2, xfer->buffer, byte_count);
+          xfer->buffer += byte_count;
+        }
 
-      // Truncate transfer length in case of short packet
-      if (bcnt < xfer->max_size) {
-        xfer->total_len -= (epout->doeptsiz & DOEPTSIZ_XFRSIZ_Msk) >> DOEPTSIZ_XFRSIZ_Pos;
-        if (epnum == 0) {
-          xfer->total_len -= ep0_pending[TUSB_DIR_OUT];
-          ep0_pending[TUSB_DIR_OUT] = 0;
+        // short packet, minus remaining bytes (xfer_size)
+        if (byte_count < xfer->max_size) {
+          xfer->total_len -= epout->tsiz_bm.xfer_size;
+          if (epnum == 0) {
+            xfer->total_len -= _dcd_data.ep0_pending[TUSB_DIR_OUT];
+            _dcd_data.ep0_pending[TUSB_DIR_OUT] = 0;
+          }
         }
       }
-    }
       break;
+    }
 
-      // Out packet done (Interrupt)
-    case GRXSTS_PKTSTS_OUTDONE:
-      // Occurred on STM32L47 with dwc2 version 3.10a but not found on other version like 2.80a or 3.30a
-      // May (or not) be 3.10a specific feature/bug or depending on MCU configuration
-      // XFRC complete is additionally generated when
-      // - setup packet is received
-      // - complete the data stage of control write is complete
-      // It will be handled in handle_epout_irq()
+    case GRXSTS_PKTSTS_RX_COMPLETE:
+      // Out packet done
+      // After this entry is popped from the receive FIFO, dwc2 asserts a Transfer Completed interrupt on
+      // the specified OUT endpoint which will be handled by handle_epout_irq()
       break;
 
-    default:    // Invalid
-      TU_BREAKPOINT();
-      break;
+    default: break;
   }
 }
 
-static void handle_epout_irq(uint8_t rhport) {
-  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
-  uint8_t const ep_count = _dwc2_controller[rhport].ep_count;
-
-  // 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_count; n++) {
-    if (dwc2->daint & TU_BIT(DAINT_OEPINT_Pos + n)) {
-      dwc2_epout_t* epout = &dwc2->epout[n];
-
-      uint32_t const doepint = epout->doepint;
-
-      TU_ASSERT((epout->doepint & DOEPINT_AHBERR) == 0, );
-
-      // OUT XFER complete
-      if (epout->doepint & DOEPINT_XFRC) {
-        epout->doepint = DOEPINT_XFRC;
-
-        xfer_ctl_t* xfer = XFER_CTL_BASE(n, TUSB_DIR_OUT);
-
-        if(dma_enabled(dwc2)) {
-          if (doepint & DOEPINT_STUP) {
-            // STPKTRX is only available for version from 3_00a
-            if ((doepint & DOEPINT_STPKTRX) && (dwc2->gsnpsid > DWC2_CORE_REV_3_00a)) {
-              epout->doepint = DOEPINT_STPKTRX;
-            }
-          } else if (doepint & DOEPINT_OTEPSPR) {
-            epout->doepint = DOEPINT_OTEPSPR;
-          } else {
-            if ((doepint & DOEPINT_STPKTRX) && (dwc2->gsnpsid > DWC2_CORE_REV_3_00a)) {
-              epout->doepint = DOEPINT_STPKTRX;
-            } else {
-              // EP0 can only handle one packet
-              if ((n == 0) && ep0_pending[TUSB_DIR_OUT]) {
-                // Schedule another packet to be received.
-                edpt_schedule_packets(rhport, n, TUSB_DIR_OUT, 1, ep0_pending[TUSB_DIR_OUT]);
-              } else {
-                // Fix packet length
-                uint16_t remain = (epout->doeptsiz & DOEPTSIZ_XFRSIZ_Msk) >> DOEPTSIZ_XFRSIZ_Pos;
-                xfer->total_len -= remain;
-                // this is ZLP, so prepare EP0 for next setup
-                if(n == 0 && xfer->total_len == 0) {
-                  dma_setup_prepare(rhport);
-                }
-
-                dcd_event_xfer_complete(rhport, n, xfer->total_len, XFER_RESULT_SUCCESS, true);
-              }
-            }
-          }
-        } else {
-          if ((doepint & DOEPINT_STPKTRX) && (dwc2->gsnpsid == DWC2_CORE_REV_3_10a)) {
-            epout->doepint = DOEPINT_STPKTRX;
-          } else {
-            if ((doepint & DOEPINT_OTEPSPR) && (dwc2->gsnpsid == DWC2_CORE_REV_3_10a)) {
-              epout->doepint = DOEPINT_OTEPSPR;
-            }
-
-            // EP0 can only handle one packet
-            if ((n == 0) && ep0_pending[TUSB_DIR_OUT]) {
-              // Schedule another packet to be received.
-              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_epout_slave(uint8_t rhport, uint8_t epnum, dwc2_doepint_t doepint_bm) {
+  if (doepint_bm.setup_phase_done) {
+    dcd_event_setup_received(rhport, _dcd_usbbuf.setup_packet, true);
+    return;
+  }
 
-      // SETUP packet Setup Phase done.
-      if (doepint & DOEPINT_STUP) {
-        epout->doepint = DOEPINT_STUP;
-        if ((doepint & DOEPINT_STPKTRX) && (dwc2->gsnpsid > DWC2_CORE_REV_3_00a)) {
-          epout->doepint = DOEPINT_STPKTRX;
-        }
-        if(dma_enabled(dwc2) && (dwc2->gsnpsid > DWC2_CORE_REV_3_00a)) {
-          dma_setup_prepare(rhport);
-        }
+  // Normal OUT transfer complete
+  if (doepint_bm.xfer_complete) {
+    // only handle data skip if it is setup or status related
+    // Note: even though (xfer_complete + status_phase_rx) is for buffered DMA only, for STM32L47x (dwc2 v3.00a) they
+    // can is set when GRXSTS_PKTSTS_SETUP_RX is popped therefore they can bet set before/together with setup_phase_done
+    if (!doepint_bm.status_phase_rx && !doepint_bm.setup_packet_rx) {
+      xfer_ctl_t* xfer = XFER_CTL_BASE(epnum, TUSB_DIR_OUT);
 
-        dcd_event_setup_received(rhport, (uint8_t*) _setup_packet, true);
+      if ((epnum == 0) && _dcd_data.ep0_pending[TUSB_DIR_OUT]) {
+        // EP0 can only handle one packet, Schedule another packet to be received.
+        edpt_schedule_packets(rhport, epnum, TUSB_DIR_OUT);
+      } else {
+        dcd_event_xfer_complete(rhport, epnum, xfer->total_len, XFER_RESULT_SUCCESS, true);
       }
     }
   }
 }
 
-static void handle_epin_irq(uint8_t rhport) {
+static void handle_epin_slave(uint8_t rhport, uint8_t epnum, dwc2_diepint_t diepint_bm) {
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);
-  uint8_t const ep_count = _dwc2_controller[rhport].ep_count;
-  dwc2_epin_t* epin = dwc2->epin;
+  dwc2_dep_t* epin = &dwc2->epin[epnum];
+  xfer_ctl_t* xfer = XFER_CTL_BASE(epnum, TUSB_DIR_IN);
 
-  // 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_count; n++) {
-    if (dwc2->daint & TU_BIT(DAINT_IEPINT_Pos + n)) {
-      // IN XFER complete (entire xfer).
-      xfer_ctl_t* xfer = XFER_CTL_BASE(n, TUSB_DIR_IN);
+  if (diepint_bm.xfer_complete) {
+    if ((epnum == 0) && _dcd_data.ep0_pending[TUSB_DIR_IN]) {
+      // EP0 can only handle one packet. Schedule another packet to be transmitted.
+      edpt_schedule_packets(rhport, epnum, TUSB_DIR_IN);
+    } else {
+      dcd_event_xfer_complete(rhport, epnum | TUSB_DIR_IN_MASK, xfer->total_len, XFER_RESULT_SUCCESS, true);
+    }
+  }
 
-      if (epin[n].diepint & DIEPINT_XFRC) {
-        epin[n].diepint = DIEPINT_XFRC;
+  // TX FIFO empty bit is read-only. It will only be cleared by hardware when written bytes is more than
+  // - 64 bytes or
+  // - Half/Empty of TX FIFO size (configured by GAHBCFG.TXFELVL)
+  if (diepint_bm.txfifo_empty && (dwc2->diepempmsk & (1 << epnum))) {
+    const uint16_t remain_packets = epin->tsiz_bm.packet_count;
 
-        // 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 {
-          if((n == 0) && dma_enabled(dwc2)) {
-            dma_setup_prepare(rhport);
-          }
-          dcd_event_xfer_complete(rhport, n | TUSB_DIR_IN_MASK, xfer->total_len, XFER_RESULT_SUCCESS, true);
-        }
+    // Process every single packet (only whole packets can be written to fifo)
+    for (uint16_t i = 0; i < remain_packets; i++) {
+      const uint16_t remain_bytes = (uint16_t) epin->tsiz_bm.xfer_size;
+      const uint16_t xact_bytes = tu_min16(remain_bytes, xfer->max_size);
+
+      // Check if dtxfsts has enough space available
+      if (xact_bytes > ((epin->dtxfsts & DTXFSTS_INEPTFSAV_Msk) << 2)) {
+        break;
       }
 
-      // XFER FIFO empty
-      if ((epin[n].diepint & DIEPINT_TXFE) && (dwc2->diepempmsk & (1 << n))) {
-        // diepint's TXFE bit is read-only, software cannot clear it.
-        // It will only be cleared by hardware when written bytes is more than
-        // - 64 bytes or
-        // - Half of TX FIFO size (configured by DIEPTXF)
+      // Push packet to Tx-FIFO
+      if (xfer->ff) {
+        volatile uint32_t* tx_fifo = dwc2->fifo[epnum];
+        tu_fifo_read_n_const_addr_full_words(xfer->ff, (void*)(uintptr_t)tx_fifo, xact_bytes);
+      } else {
+        dfifo_write_packet(dwc2, epnum, xfer->buffer, xact_bytes);
+        xfer->buffer += xact_bytes;
+      }
+    }
 
-        uint16_t remaining_packets = (epin[n].dieptsiz & DIEPTSIZ_PKTCNT_Msk) >> DIEPTSIZ_PKTCNT_Pos;
+    // Turn off TXFE if all bytes are written.
+    if (epin->tsiz_bm.xfer_size == 0) {
+      dwc2->diepempmsk &= ~(1 << epnum);
+    }
+  }
+}
+#endif
 
-        // Process every single packet (only whole packets can be written to fifo)
-        for (uint16_t i = 0; i < remaining_packets; i++) {
-          uint16_t const remaining_bytes = (epin[n].dieptsiz & DIEPTSIZ_XFRSIZ_Msk) >> DIEPTSIZ_XFRSIZ_Pos;
+#if CFG_TUD_DWC2_DMA_ENABLE
+static void handle_epout_dma(uint8_t rhport, uint8_t epnum, dwc2_doepint_t doepint_bm) {
+  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
 
-          // Packet can not be larger than ep max size
-          uint16_t const packet_size = tu_min16(remaining_bytes, xfer->max_size);
+  if (doepint_bm.setup_phase_done) {
+    dma_setup_prepare(rhport);
+    dcd_dcache_invalidate(_dcd_usbbuf.setup_packet, 8);
+    dcd_event_setup_received(rhport, _dcd_usbbuf.setup_packet, true);
+    return;
+  }
 
-          // It's only possible to write full packets into FIFO. Therefore DTXFSTS register of current
-          // EP has to be checked if the buffer can take another WHOLE packet
-          if (packet_size > ((epin[n].dtxfsts & DTXFSTS_INEPTFSAV_Msk) << 2)) break;
+  // OUT XFER complete
+  if (doepint_bm.xfer_complete) {
+    // only handle data skip if it is setup or status related
+    // Normal OUT transfer complete
+    if (!doepint_bm.status_phase_rx && !doepint_bm.setup_packet_rx) {
+      if ((epnum == 0) && _dcd_data.ep0_pending[TUSB_DIR_OUT]) {
+        // EP0 can only handle one packet Schedule another packet to be received.
+        edpt_schedule_packets(rhport, epnum, TUSB_DIR_OUT);
+      } else {
+        dwc2_dep_t* epout = &dwc2->epout[epnum];
+        xfer_ctl_t* xfer = XFER_CTL_BASE(epnum, TUSB_DIR_OUT);
 
-          // Push packet to Tx-FIFO
-          if (xfer->ff) {
-            volatile uint32_t* tx_fifo = dwc2->fifo[n];
-            tu_fifo_read_n_const_addr_full_words(xfer->ff, (void*) (uintptr_t) tx_fifo, packet_size);
-          } else {
-            dfifo_write_packet(rhport, n, xfer->buffer, packet_size);
+        // determine actual received bytes
+        const uint16_t remain = epout->tsiz_bm.xfer_size;
+        xfer->total_len -= remain;
 
-            // Increment pointer to xfer data
-            xfer->buffer += packet_size;
-          }
+        // this is ZLP, so prepare EP0 for next setup
+        // TODO use status phase rx
+        if(epnum == 0 && xfer->total_len == 0) {
+          dma_setup_prepare(rhport);
         }
 
-        // Turn off TXFE if all bytes are written.
-        if (((epin[n].dieptsiz & DIEPTSIZ_XFRSIZ_Msk) >> DIEPTSIZ_XFRSIZ_Pos) == 0) {
-          dwc2->diepempmsk &= ~(1 << n);
+        dcd_dcache_invalidate(xfer->buffer, xfer->total_len);
+        dcd_event_xfer_complete(rhport, epnum, xfer->total_len, XFER_RESULT_SUCCESS, true);
+      }
+    }
+  }
+}
+
+static void handle_epin_dma(uint8_t rhport, uint8_t epnum, dwc2_diepint_t diepint_bm) {
+  xfer_ctl_t* xfer = XFER_CTL_BASE(epnum, TUSB_DIR_IN);
+
+  if (diepint_bm.xfer_complete) {
+    if ((epnum == 0) && _dcd_data.ep0_pending[TUSB_DIR_IN]) {
+      // EP0 can only handle one packet. Schedule another packet to be transmitted.
+      edpt_schedule_packets(rhport, epnum, TUSB_DIR_IN);
+    } else {
+      if(epnum == 0) {
+        dma_setup_prepare(rhport);
+      }
+      dcd_event_xfer_complete(rhport, epnum | TUSB_DIR_IN_MASK, xfer->total_len, XFER_RESULT_SUCCESS, true);
+    }
+  }
+}
+#endif
+
+static void handle_ep_irq(uint8_t rhport, uint8_t dir) {
+  dwc2_regs_t* dwc2 = DWC2_REG(rhport);
+  const bool is_dma = dma_device_enabled(dwc2);
+  const uint8_t ep_count = DWC2_EP_COUNT(dwc2);
+  const uint8_t daint_offset = (dir == TUSB_DIR_IN) ? DAINT_IEPINT_Pos : DAINT_OEPINT_Pos;
+  dwc2_dep_t* ep_base = &dwc2->ep[dir == TUSB_DIR_IN ? 0 : 1][0];
+
+  // DAINT for a given EP clears when DEPINTx is cleared.
+  // EPINT will be cleared when DAINT bits are cleared.
+  for (uint8_t epnum = 0; epnum < ep_count; epnum++) {
+    if (dwc2->daint & TU_BIT(daint_offset + epnum)) {
+      dwc2_dep_t* epout = &ep_base[epnum];
+      union {
+        uint32_t value;
+        dwc2_diepint_t diepint_bm;
+        dwc2_doepint_t doepint_bm;
+      } intr;
+      intr.value = epout->intr;
+
+      epout->intr = intr.value; // Clear interrupt
+
+      if (is_dma) {
+        #if CFG_TUD_DWC2_DMA_ENABLE
+        if (dir == TUSB_DIR_IN) {
+          handle_epin_dma(rhport, epnum, intr.diepint_bm);
+        } else {
+          handle_epout_dma(rhport, epnum, intr.doepint_bm);
+        }
+        #endif
+      } else {
+        #if CFG_TUD_DWC2_SLAVE_ENABLE
+        if (dir == TUSB_DIR_IN) {
+          handle_epin_slave(rhport, epnum, intr.diepint_bm);
+        } else {
+          handle_epout_slave(rhport, epnum, intr.doepint_bm);
         }
+        #endif
       }
     }
   }
 }
 
+/* Interrupt Hierarchy
+                 DIEPINT  DIEPINT
+                    \       /
+                     \     /
+                      DAINT
+                     /     \
+                    /       \
+     GINTSTS:    OEPInt    IEPInt | USBReset | EnumDone | USBSusp | WkUpInt | OTGInt | SOF | RXFLVL
+
+  Note: when OTG_MULTI_PROC_INTRPT = 1, Device Each endpoint interrupt deachint/deachmsk/diepeachmsk/doepeachmsk
+  are combined to generate dedicated interrupt line for each endpoint.
+ */
 void dcd_int_handler(uint8_t rhport) {
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);
 
-  uint32_t const int_mask = dwc2->gintmsk;
-  uint32_t const int_status = dwc2->gintsts & int_mask;
+  const uint32_t gintmask = dwc2->gintmsk;
+  const uint32_t gintsts = dwc2->gintsts & gintmask;
 
-  if (int_status & GINTSTS_USBRST) {
+  if (gintsts & GINTSTS_USBRST) {
     // USBRST is start of reset.
     dwc2->gintsts = GINTSTS_USBRST;
 #if defined(TUP_USBIP_DWC2_ESP32)
     _allocated_fifos = 1;
 #endif
-    bus_reset(rhport);
+    handle_bus_reset(rhport);
   }
 
-  if (int_status & GINTSTS_ENUMDNE) {
+  if (gintsts & GINTSTS_ENUMDNE) {
     // ENUMDNE is the end of reset where speed of the link is detected
     dwc2->gintsts = GINTSTS_ENUMDNE;
-
-    tusb_speed_t speed;
-    switch ((dwc2->dsts & DSTS_ENUMSPD_Msk) >> DSTS_ENUMSPD_Pos) {
-      case DSTS_ENUMSPD_HS:
-        speed = TUSB_SPEED_HIGH;
-        break;
-
-      case DSTS_ENUMSPD_LS:
-        speed = TUSB_SPEED_LOW;
-        break;
-
-      case DSTS_ENUMSPD_FS_HSPHY:
-      case DSTS_ENUMSPD_FS:
-      default:
-        speed = TUSB_SPEED_FULL;
-        break;
-    }
-
-    // TODO must update GUSBCFG_TRDT according to link speed
-
-    dcd_event_bus_reset(rhport, speed, true);
+    handle_enum_done(rhport);
   }
 
-  if (int_status & GINTSTS_USBSUSP) {
+  if (gintsts & GINTSTS_USBSUSP) {
     dwc2->gintsts = GINTSTS_USBSUSP;
     //dcd_event_bus_signal(rhport, DCD_EVENT_SUSPEND, true);
     dcd_event_bus_signal(rhport, DCD_EVENT_UNPLUGGED, true);
@@ -1270,7 +1037,7 @@ void dcd_int_handler(uint8_t rhport) {
 #endif
   }
 
-  if (int_status & GINTSTS_WKUINT) {
+  if (gintsts & GINTSTS_WKUINT) {
     dwc2->gintsts = GINTSTS_WKUINT;
     dcd_event_bus_signal(rhport, DCD_EVENT_RESUME, true);
   }
@@ -1278,9 +1045,9 @@ void dcd_int_handler(uint8_t rhport) {
   // TODO check GINTSTS_DISCINT for disconnect detection
   // if(int_status & GINTSTS_DISCINT)
 
-  if (int_status & GINTSTS_OTGINT) {
+  if (gintsts & GINTSTS_OTGINT) {
     // OTG INT bit is read-only
-    uint32_t const otg_int = dwc2->gotgint;
+    const uint32_t otg_int = dwc2->gotgint;
 
     if (otg_int & GOTGINT_SEDET) {
       dcd_event_bus_signal(rhport, DCD_EVENT_UNPLUGGED, true);
@@ -1292,50 +1059,43 @@ void dcd_int_handler(uint8_t rhport) {
     dwc2->gotgint = otg_int;
   }
 
-  if(int_status & GINTSTS_SOF) {
+  if(gintsts & GINTSTS_SOF) {
     dwc2->gintsts = GINTSTS_SOF;
     const uint32_t frame = (dwc2->dsts & DSTS_FNSOF) >> DSTS_FNSOF_Pos;
 
     // Disable SOF interrupt if SOF was not explicitly enabled since SOF was used for remote wakeup detection
-    if (!_sof_en) {
+    if (!_dcd_data.sof_en) {
       dwc2->gintmsk &= ~GINTMSK_SOFM;
     }
 
     dcd_event_sof(rhport, frame, true);
   }
 
+#if CFG_TUD_DWC2_SLAVE_ENABLE
   // RxFIFO non-empty interrupt handling.
-  if (int_status & GINTSTS_RXFLVL) {
+  if (gintsts & GINTSTS_RXFLVL) {
     // RXFLVL bit is read-only
+    dwc2->gintmsk &= ~GINTMSK_RXFLVLM; // disable RXFLVL interrupt while reading
 
-    // Mask out RXFLVL while reading data from FIFO
-    dwc2->gintmsk &= ~GINTMSK_RXFLVLM;
-
-    // Loop until all available packets were handled
     do {
-      handle_rxflvl_irq(rhport);
+      handle_rxflvl_irq(rhport); // read all packets
     } while(dwc2->gintsts & GINTSTS_RXFLVL);
 
     dwc2->gintmsk |= GINTMSK_RXFLVLM;
   }
+#endif
 
   // OUT endpoint interrupt handling.
-  if (int_status & GINTSTS_OEPINT) {
+  if (gintsts & GINTSTS_OEPINT) {
     // OEPINT is read-only, clear using DOEPINTn
-    handle_epout_irq(rhport);
+    handle_ep_irq(rhport, TUSB_DIR_OUT);
   }
 
   // IN endpoint interrupt handling.
-  if (int_status & GINTSTS_IEPINT) {
+  if (gintsts & GINTSTS_IEPINT) {
     // IEPINT bit read-only, clear using DIEPINTn
-    handle_epin_irq(rhport);
+    handle_ep_irq(rhport, TUSB_DIR_IN);
   }
-
-  //  // Check for Incomplete isochronous IN transfer
-  //  if(int_status & GINTSTS_IISOIXFR) {
-  //    printf("      IISOIXFR!\r\n");
-  ////    TU_LOG(DWC2_DEBUG, "      IISOIXFR!\r\n");
-  //  }
 }
 
 #if CFG_TUD_TEST_MODE
diff --git a/configs/defconfig.common b/configs/defconfig.common
index 5bb28cad5..3512a071e 100644
--- a/configs/defconfig.common
+++ b/configs/defconfig.common
@@ -75,14 +75,8 @@ CONFIG_LWIP_HOOK_ND6_GET_GW_DEFAULT=y
 CONFIG_LWIP_HOOK_IP6_SELECT_SRC_ADDR_DEFAULT=y
 CONFIG_LWIP_HOOK_NETCONN_EXT_RESOLVE_DEFAULT=y
 CONFIG_LWIP_HOOK_IP6_INPUT_CUSTOM=y
-CONFIG_LWIP_MULTICAST_PING=y
-CONFIG_LWIP_BROADCAST_PING=y
-CONFIG_LWIP_IPV6_NUM_ADDRESSES=8
 CONFIG_MBEDTLS_PSK_MODES=y
 CONFIG_MBEDTLS_KEY_EXCHANGE_PSK=y
-CONFIG_MBEDTLS_KEY_EXCHANGE_ECJPAKE=y
-CONFIG_MBEDTLS_ECJPAKE_C=y
-CONFIG_MBEDTLS_HKDF_C=y
 CONFIG_MBEDTLS_CAMELLIA_C=y
 CONFIG_MBEDTLS_GCM_SUPPORT_NON_AES_CIPHER=y
 # CONFIG_MBEDTLS_ASYMMETRIC_CONTENT_LEN is not set
@@ -117,13 +111,3 @@ CONFIG_ESP_COREDUMP_STACK_SIZE=0
 CONFIG_MBEDTLS_DYNAMIC_BUFFER=y
 CONFIG_MBEDTLS_DYNAMIC_FREE_PEER_CERT=y
 CONFIG_MBEDTLS_DYNAMIC_FREE_CONFIG_DATA=y
-#
-# Matter Settings
-#
-# Disable Matter BLE
-CONFIG_ENABLE_CHIPOBLE=n
-CONFIG_USE_BLE_ONLY_FOR_COMMISSIONING=n
-# ESP Insights
-CONFIG_ENABLE_ESP_INSIGHTS_TRACE=n
-# Use compact attribute storage mode
-CONFIG_ESP_MATTER_NVS_USE_COMPACT_ATTR_STORAGE=y
diff --git a/configs/defconfig.esp32c6 b/configs/defconfig.esp32c6
index c39022997..0651a8f9b 100644
--- a/configs/defconfig.esp32c6
+++ b/configs/defconfig.esp32c6
@@ -8,6 +8,23 @@ CONFIG_FREERTOS_IDLE_TASK_STACKSIZE=2304
 #CONFIG_ULP_COPROC_LP_CORE=y
 #CONFIG_ULP_COPROC_RESERVE_MEM=4096
 
+#
+# ESP32 Arduino OpenThread Configuration
+#
+# lwIP
+#
+CONFIG_LWIP_IPV6_NUM_ADDRESSES=8
+CONFIG_LWIP_MULTICAST_PING=y
+CONFIG_LWIP_BROADCAST_PING=y
+# end of lwip
+
+#
+# mbedTLS
+#
+CONFIG_MBEDTLS_KEY_EXCHANGE_ECJPAKE=y
+CONFIG_MBEDTLS_ECJPAKE_C=y
+# end of mbedTLS
+
 #
 # OpenThread
 #
diff --git a/configs/defconfig.esp32h2 b/configs/defconfig.esp32h2
index fe4a47eb6..639cd4d8d 100644
--- a/configs/defconfig.esp32h2
+++ b/configs/defconfig.esp32h2
@@ -4,6 +4,23 @@ CONFIG_RTC_CLK_CAL_CYCLES=576
 # CONFIG_ESP_TASK_WDT_CHECK_IDLE_TASK_CPU0 is not set
 CONFIG_FREERTOS_IDLE_TASK_STACKSIZE=2304
 
+#
+# ESP32 Arduino OpenThread Configuration
+#
+# lwIP
+#
+CONFIG_LWIP_IPV6_NUM_ADDRESSES=8
+CONFIG_LWIP_MULTICAST_PING=y
+CONFIG_LWIP_BROADCAST_PING=y
+# end of lwip
+
+#
+# mbedTLS
+#
+CONFIG_MBEDTLS_KEY_EXCHANGE_ECJPAKE=y
+CONFIG_MBEDTLS_ECJPAKE_C=y
+# end of mbedTLS
+
 #
 # OpenThread
 #
diff --git a/main/idf_component.yml b/main/idf_component.yml
index 714b3f0d6..d98a1778b 100644
--- a/main/idf_component.yml
+++ b/main/idf_component.yml
@@ -12,21 +12,16 @@ dependencies:
     require: public
     rules:
       - if: "target not in [esp32c2]"
-  # espressif/esp-dl:
-  #   version: "af7808ba09448ce82c704455975d4cf1e4305fd7"
-  #   git: https://github.com/espressif/esp-dl.git
-  #   require: public
-  #   rules:
-  #     - if: "target in [esp32s3, esp32s2, esp32]"
+  espressif/esp-dl:
+    version: "af7808ba09448ce82c704455975d4cf1e4305fd7"
+    git: https://github.com/espressif/esp-dl.git
+    require: public
+    rules:
+      - if: "target in [esp32s3, esp32s2, esp32]"
   espressif/esp-sr:
     version: ">=1.4.2"
     rules:
       - if: "target in [esp32s3]"
-  espressif/esp_matter:
-    version: "^1.3.0"
-    require: public
-    rules:
-      - if: "target not in [esp32c2, esp32h2]"
 
   # esp-sr: "^1.3.1"
   # esp32-camera: "^2.0.4"
diff --git a/tools/config.sh b/tools/config.sh
index 37805cb48..6884c4667 100755
--- a/tools/config.sh
+++ b/tools/config.sh
@@ -10,7 +10,7 @@ if [ -z $IDF_BRANCH ]; then
 fi
 
 if [ -z $AR_PR_TARGET_BRANCH ]; then
-    AR_PR_TARGET_BRANCH="master"
+    AR_PR_TARGET_BRANCH="release/v3.0.x"
 fi
 
 if [ -z $IDF_TARGET ]; then
diff --git a/tools/copy-libs.sh b/tools/copy-libs.sh
index bbc89e089..db3d85a6c 100755
--- a/tools/copy-libs.sh
+++ b/tools/copy-libs.sh
@@ -39,7 +39,9 @@ fi
 if [ -e "$AR_SDK/platformio-build.py" ]; then
 	rm -rf "$AR_SDK/platformio-build.py"
 fi
+
 mkdir -p "$AR_SDK"
+mkdir -p "$AR_SDK/lib"
 
 function get_actual_path(){
 	p="$PWD"; cd "$1"; r="$PWD"; cd "$p"; echo "$r";
@@ -56,6 +58,8 @@ AS_FLAGS=""
 INCLUDES=""
 DEFINES=""
 
+EXCLUDE_LIBS=";"
+
 LD_FLAGS=""
 LD_LIBS=""
 LD_LIB_FILES=""
@@ -78,6 +82,17 @@ else
 	TOOLCHAIN="riscv32-esp-elf"
 fi
 
+# copy zigbee + zboss lib
+if [ -d "managed_components/espressif__esp-zigbee-lib/lib/$IDF_TARGET/" ]; then
+	cp -r "managed_components/espressif__esp-zigbee-lib/lib/$IDF_TARGET"/* "$AR_SDK/lib/"
+	EXCLUDE_LIBS+="esp_zb_api_ed;"
+fi
+
+if [ -d "managed_components/espressif__esp-zboss-lib/lib/$IDF_TARGET/" ]; then
+	cp -r "managed_components/espressif__esp-zboss-lib/lib/$IDF_TARGET"/* "$AR_SDK/lib/"
+	EXCLUDE_LIBS+="zboss_stack.ed;zboss_port.debug;"
+fi
+
 #collect includes, defines and c-flags
 str=`cat build/compile_commands.json | grep arduino-lib-builder-gcc.c | grep command | cut -d':' -f2 | cut -d',' -f1`
 str="${str:2:${#str}-1}" #remove leading space and quotes
@@ -200,12 +215,14 @@ for item; do
 				add_next=1
 				LD_FLAGS+="$item "
 			elif [ "${item:0:2}" = "-l" ]; then # -l[lib_name]
-				LD_LIBS+="$item "
-				exclude_libs=";m;c;gcc;stdc++;"
 				short_name="${item:2}"
-				if [[ $exclude_libs != *";$short_name;"* && $LD_LIBS_SEARCH != *"lib$short_name.a"* ]]; then
-					LD_LIBS_SEARCH+="lib$short_name.a "
-					#echo "lib add: $item"
+				if [[ $EXCLUDE_LIBS != *";$short_name;"* ]]; then
+					LD_LIBS+="$item "
+					exclude_libs=";m;c;gcc;stdc++;"
+					if [[ $exclude_libs != *";$short_name;"* && $LD_LIBS_SEARCH != *"lib$short_name.a"* ]]; then
+						LD_LIBS_SEARCH+="lib$short_name.a "
+						#echo "1. lib add: $item"
+					fi
 				fi
 			elif [ "$item" = "-o" ]; then
 				add_next=0
@@ -244,30 +261,38 @@ for item; do
 						if [[ $LD_LIB_FILES != *"$item"* ]]; then
 							# do we already have lib with the same name?
 							if [[ $LD_LIBS != *"-l$lname"* ]]; then
-								# echo "collecting lib '$lname' and file: $item"
-								LD_LIB_FILES+="$item "
-								LD_LIBS+="-l$lname "
+								if [[ $EXCLUDE_LIBS != *";$lname;"* ]]; then
+									#echo "2. collecting lib '$lname' and file: $item"
+									LD_LIB_FILES+="$item "
+									LD_LIBS+="-l$lname "
+								fi
 							else 
 								# echo "!!! need to rename: '$lname'"
 								for i in {2..9}; do
 									n_item="${item:0:${#item}-2}_$i.a"
 									n_name=$lname"_$i"
 									if [ -f "$n_item" ]; then
-										# echo "renamed add: -l$n_name"
-										LD_LIBS+="-l$n_name "
+										if [[ $EXCLUDE_LIBS != *";$lname;"* ]]; then
+											#echo "3. renamed add: -l$n_name"
+											LD_LIBS+="-l$n_name "
+										fi
 										break
 									elif [[ $LD_LIB_FILES != *"$n_item"* && $LD_LIBS != *"-l$n_name"* ]]; then
-										echo "Renaming '$lname' to '$n_name': $item"
-										cp -f "$item" "$n_item"
-										LD_LIB_FILES+="$n_item "
-										LD_LIBS+="-l$n_name "
+										if [[ $EXCLUDE_LIBS != *";$lname;"* ]]; then
+											#echo "4. Renaming '$lname' to '$n_name': $item"
+											cp -f "$item" "$n_item"
+											LD_LIB_FILES+="$n_item "
+											LD_LIBS+="-l$n_name "
+										fi
 										break
 									fi
 								done
 							fi
 						else
-							# echo "just add: -l$lname"
-							LD_LIBS+="-l$lname "
+							if [[ $EXCLUDE_LIBS != *";$lname;"* ]]; then
+								#echo "5. just add: -l$lname"
+								LD_LIBS+="-l$lname "
+							fi
 						fi
 					else
 						echo "*** Skipping $(basename $item): size too small $lsize"
@@ -427,8 +452,6 @@ echo "        join(FRAMEWORK_DIR, \"cores\", board_config.get(\"build.core\"))"
 echo "    ]," >> "$AR_PLATFORMIO_PY"
 echo "" >> "$AR_PLATFORMIO_PY"
 
-mkdir -p "$AR_SDK/lib"
-
 AR_LIBS="$LD_LIBS"
 PIO_LIBS=""
 set -- $LD_LIBS
@@ -502,15 +525,6 @@ sed 's/CHIP_ADDRESS_RESOLVE_IMPL_INCLUDE_HEADER/<lib\/address_resolve\/AddressRe
 mv $CHIP_RESOLVE_DIR/AddressResolve_temp.h $CHIP_RESOLVE_DIR/AddressResolve.h
 # End of Matter Library adjustments
 
-# copy zigbee + zboss lib
-if [ -d "managed_components/espressif__esp-zigbee-lib/lib/$IDF_TARGET/" ]; then
-	cp -r "managed_components/espressif__esp-zigbee-lib/lib/$IDF_TARGET"/* "$AR_SDK/lib/"
-fi
-
-if [ -d "managed_components/espressif__esp-zboss-lib/lib/$IDF_TARGET/" ]; then
-	cp -r "managed_components/espressif__esp-zboss-lib/lib/$IDF_TARGET"/* "$AR_SDK/lib/"
-fi
-
 # sdkconfig
 cp -f "sdkconfig" "$AR_SDK/sdkconfig"
 
@@ -541,7 +555,7 @@ for item; do
 	done
 done
 
-for lib in "openthread" "espressif__esp-tflite-micro" "bt" "espressif__esp_matter"; do
+for lib in "openthread" "espressif__esp-tflite-micro" "bt"; do
 	if [ -f "$AR_SDK/lib/lib$lib.a" ]; then
 		echo "Stripping $AR_SDK/lib/lib$lib.a"
 		"$TOOLCHAIN-strip" -g "$AR_SDK/lib/lib$lib.a"
diff --git a/tools/docker/README.md b/tools/docker/README.md
index 109665c4a..00e88a693 100644
--- a/tools/docker/README.md
+++ b/tools/docker/README.md
@@ -24,7 +24,7 @@ Multiple tags of this image are maintained:
 ## Basic Usage
 
 ```bash
-docker run --rm -it -e "TERM=xterm-256color" -v <path to arduino-esp32>:/arduino-esp32 espressif/esp32-arduino-lib-builder:latest
+docker run --rm -it -e "TERM=xterm-256color" -v <path to arduino-esp32>:/arduino-esp32 espressif/esp32-arduino-lib-builder:release-v5.1
 ```
 
 The above command explained:
@@ -35,18 +35,16 @@ The above command explained:
   - `-t`: Allocates a pseudo-TTY.
   - `-e "TERM=xterm-256color"`: Optional. Sets the terminal type to `xterm-256color` to display colors correctly.
   - `-v <path to arduino-esp32>:/arduino-esp32`: Optional. Mounts the Arduino Core for ESP32 repository at `/arduino-esp32` inside the container. Replace `<path to arduino-esp32>` with the path to the repository on the host machine. If not provided, the container will not copy the compiled libraries to the host machine.
-  - `espressif/esp32-arduino-lib-builder:latest`: The Docker image to use.
+  - `espressif/esp32-arduino-lib-builder:release-v5.1`: The Docker image to use.
 
 After running the above command, you will be inside the container and can build the libraries using the user interface.
 
 By default the docker container will run the user interface script. If you want to run a specific command, you can pass it as an argument to the docker run command. For example, to run a terminal inside the container, you can run:
 
 ```bash
-docker run -it espressif/esp32-arduino-lib-builder:latest /bin/bash
+docker run -it espressif/esp32-arduino-lib-builder:release-v5.1 /bin/bash
 ```
 
 ## Documentation
 
-<!-- Pending: Add link to the full documentation. -->
-
-For more information about this image and the detailed usage instructions, please refer to the Arduino Core for ESP32 documentation.
+For more information about this image and the detailed usage instructions, please refer to the [Arduino Core for ESP32 documentation](https://docs.espressif.com/projects/arduino-esp32/en/latest/lib_builder.html#docker-image).
diff --git a/tools/docker/run.ps1 b/tools/docker/run.ps1
index 4c49ac505..bcdf1c63e 100644
--- a/tools/docker/run.ps1
+++ b/tools/docker/run.ps1
@@ -1,5 +1,8 @@
 # This is an example of how to run the docker container.
 # This script is not part of the container, it is meant to be run on the host machine.
+# Note that this file will build the release/v5.1 branch. For other branches, change the tag accordingly.
+# You can check the available tags at https://hub.docker.com/r/espressif/esp32-arduino-lib-builder/tags
+# As this script is unsigned, you may need to run `Set-ExecutionPolicy -Scope Process -ExecutionPolicy Bypass` before running it.
 # Usage: .\run.ps1 <path_to_arduino_esp32>
 
 # Exit on error
@@ -58,4 +61,4 @@ if ($env:LIBBUILDER_GIT_SAFE_DIR) {
 }
 
 Write-Output "Running: docker run $($DOCKER_ARGS -join ' ') espressif/esp32-arduino-lib-builder"
-docker run @($DOCKER_ARGS) espressif/esp32-arduino-lib-builder
+docker run @($DOCKER_ARGS) espressif/esp32-arduino-lib-builder:release-v5.1
diff --git a/tools/docker/run.sh b/tools/docker/run.sh
index 59e967363..2a149bd6c 100755
--- a/tools/docker/run.sh
+++ b/tools/docker/run.sh
@@ -2,6 +2,8 @@
 
 # This is an example of how to run the docker container.
 # This script is not part of the container, it is meant to be run on the host machine.
+# Note that this file will build the release/v5.1 branch. For other branches, change the tag accordingly.
+# You can check the available tags at https://hub.docker.com/r/espressif/esp32-arduino-lib-builder/tags
 # Usage: ./run.sh <path_to_arduino_esp32>
 
 if ! [ -x "$(command -v docker)" ]; then
@@ -31,4 +33,4 @@ if [ -n "$LIBBUILDER_GIT_SAFE_DIR" ]; then
 fi
 
 echo "Running: docker run ${DOCKER_ARGS[@]} espressif/esp32-arduino-lib-builder"
-docker run ${DOCKER_ARGS[@]} espressif/esp32-arduino-lib-builder
+docker run ${DOCKER_ARGS[@]} espressif/esp32-arduino-lib-builder:release-v5.1
diff --git a/tools/patch-tinyusb.sh b/tools/patch-tinyusb.sh
new file mode 100755
index 000000000..eeaa4d43b
--- /dev/null
+++ b/tools/patch-tinyusb.sh
@@ -0,0 +1,4 @@
+#!/bin/bash
+mv components/arduino_tinyusb/src/dcd_dwc2.c components/arduino_tinyusb/src/dcd_dwc2.c.prev
+cp components/arduino_tinyusb/tinyusb/src/portable/synopsys/dwc2/dcd_dwc2.c components/arduino_tinyusb/src/dcd_dwc2.c
+patch -p1 -N -i components/arduino_tinyusb/patches/dcd_dwc2.patch