Merge branch 'master' into fork/atoktoto/midihost

# Conflicts:
#	hw/bsp/rp2040/family.cmake
#	src/class/midi/midi.h
#	src/class/midi/midi_device.c
#	src/device/usbd_control.c
#	src/host/hcd.h
#	src/host/usbh.c
#	src/host/usbh.h

47 files changed, 8458 insertions, 4664 deletions

diff --git a/src/class/audio/audio.h b/src/class/audio/audio.h
index 6f9c1a6b5..2f97c0f23 100644
--- a/src/class/audio/audio.h
+++ b/src/class/audio/audio.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -489,7 +489,7 @@ typedef enum
   AUDIO_DATA_FORMAT_TYPE_I_IEEE_FLOAT     = (uint32_t) (1 << 2),
   AUDIO_DATA_FORMAT_TYPE_I_ALAW           = (uint32_t) (1 << 3),
   AUDIO_DATA_FORMAT_TYPE_I_MULAW          = (uint32_t) (1 << 4),
-  AUDIO_DATA_FORMAT_TYPE_I_RAW_DATA       = 0x80000000,
+  AUDIO_DATA_FORMAT_TYPE_I_RAW_DATA       = 0x80000000u,
 } audio_data_format_type_I_t;
 
 /// All remaining definitions are taken from the descriptor descriptions in the UAC2 main specification
@@ -640,7 +640,7 @@ typedef enum
   AUDIO_CHANNEL_CONFIG_BOTTOM_CENTER              = 0x01000000,
   AUDIO_CHANNEL_CONFIG_BACK_LEFT_OF_CENTER        = 0x02000000,
   AUDIO_CHANNEL_CONFIG_BACK_RIGHT_OF_CENTER       = 0x04000000,
-  AUDIO_CHANNEL_CONFIG_RAW_DATA                   = 0x80000000,
+  AUDIO_CHANNEL_CONFIG_RAW_DATA                   = 0x80000000u,
 } audio_channel_config_t;
 
 /// AUDIO Channel Cluster Descriptor (4.1)
@@ -721,11 +721,13 @@ typedef struct TU_ATTR_PACKED
   uint8_t bLength            ; ///< Size of this descriptor, in bytes: 17.
   uint8_t bDescriptorType    ; ///< Descriptor Type. Value: TUSB_DESC_CS_INTERFACE.
   uint8_t bDescriptorSubType ; ///< Descriptor SubType. Value: AUDIO_CS_AC_INTERFACE_INPUT_TERMINAL.
+  uint8_t bTerminalID        ; ///< Constant uniquely identifying the Terminal within the audio function. This value is used in all requests to address this terminal.
   uint16_t wTerminalType     ; ///< Constant characterizing the type of Terminal. See: audio_terminal_type_t for USB streaming and audio_terminal_input_type_t for other input types.
   uint8_t bAssocTerminal     ; ///< ID of the Output Terminal to which this Input Terminal is associated.
   uint8_t bCSourceID         ; ///< ID of the Clock Entity to which this Input Terminal is connected.
   uint8_t bNrChannels        ; ///< Number of logical output channels in the Terminal’s output audio channel cluster.
   uint32_t bmChannelConfig   ; ///< Describes the spatial location of the logical channels. See:audio_channel_config_t.
+  uint8_t iChannelNames      ; ///< Index of a string descriptor, describing the name of the first logical channel.
   uint16_t bmControls        ; ///< See: audio_terminal_input_control_pos_t.
   uint8_t iTerminal          ; ///< Index of a string descriptor, describing the Input Terminal.
 } audio_desc_input_terminal_t;
@@ -822,10 +824,10 @@ typedef struct TU_ATTR_PACKED
             uint8_t type      :  2; ///< Request type tusb_request_type_t.
             uint8_t direction :  1; ///< Direction type. tusb_dir_t
         } bmRequestType_bit;
-        
+
         uint8_t bmRequestType;
     };
-    
+
     uint8_t bRequest;  ///< Request type audio_cs_req_t
     uint8_t bChannelNumber;
     uint8_t bControlSelector;
@@ -922,6 +924,31 @@ typedef struct TU_ATTR_PACKED {
     } subrange[numSubRanges];                   \
 }
 
+// 6.1 Interrupt Data Message Format
+typedef struct TU_ATTR_PACKED
+{
+  uint8_t bInfo;
+  uint8_t bAttribute;
+  union
+  {
+    uint16_t wValue;
+    struct
+    {
+      uint8_t wValue_cn_or_mcn;
+      uint8_t wValue_cs;
+    };
+  };
+  union
+  {
+    uint16_t wIndex;
+    struct
+    {
+      uint8_t wIndex_ep_or_int;
+      uint8_t wIndex_entity_id;
+    };
+  };
+} audio_interrupt_data_t;
+
 /** @} */
 
 #ifdef __cplusplus
diff --git a/src/class/audio/audio_device.c b/src/class/audio/audio_device.c
index 698fba566..8a130b7e2 100644
--- a/src/class/audio/audio_device.c
+++ b/src/class/audio/audio_device.c
@@ -1,7 +1,8 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2020 Reinhard Panhuber, Jerzy Kasenberg
+ * Copyright (c) 2023 HiFiPhile
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -65,414 +66,589 @@
 //--------------------------------------------------------------------+
 
 // Use ring buffer if it's available, some MCUs need extra RAM requirements
+// For DWC2 enable ring buffer will disable DMA (if available)
 #ifndef TUD_AUDIO_PREFER_RING_BUFFER
-#if CFG_TUSB_MCU == OPT_MCU_LPC43XX || CFG_TUSB_MCU == OPT_MCU_LPC18XX || CFG_TUSB_MCU == OPT_MCU_MIMXRT
-#define TUD_AUDIO_PREFER_RING_BUFFER 0
-#else
-#define TUD_AUDIO_PREFER_RING_BUFFER 1
-#endif
+  #if CFG_TUSB_MCU == OPT_MCU_LPC43XX || CFG_TUSB_MCU == OPT_MCU_LPC18XX || CFG_TUSB_MCU == OPT_MCU_MIMXRT1XXX || \
+      defined(TUP_USBIP_DWC2)
+    #define TUD_AUDIO_PREFER_RING_BUFFER 0
+  #else
+    #define TUD_AUDIO_PREFER_RING_BUFFER 1
+  #endif
 #endif
 
 // Linear buffer in case target MCU is not capable of handling a ring buffer FIFO e.g. no hardware buffer
 // is available or driver is would need to be changed dramatically
 
-// Only STM32 synopsys and dcd_transdimension use non-linear buffer for now
-// Synopsys detection copied from dcd_synopsys.c (refactor later on)
-#if defined (STM32F105x8) || defined (STM32F105xB) || defined (STM32F105xC) || \
-    defined (STM32F107xB) || defined (STM32F107xC)
-#define STM32F1_SYNOPSYS
-#endif
-
-#if defined (STM32L475xx) || defined (STM32L476xx) ||                          \
-    defined (STM32L485xx) || defined (STM32L486xx) || defined (STM32L496xx) || \
-    defined (STM32L4R5xx) || defined (STM32L4R7xx) || defined (STM32L4R9xx) || \
-    defined (STM32L4S5xx) || defined (STM32L4S7xx) || defined (STM32L4S9xx)
-#define STM32L4_SYNOPSYS
-#endif
-
-#if (CFG_TUSB_MCU == OPT_MCU_STM32F1 && defined(STM32F1_SYNOPSYS)) || \
-    CFG_TUSB_MCU == OPT_MCU_STM32F2                               || \
-    CFG_TUSB_MCU == OPT_MCU_STM32F4                               || \
-    CFG_TUSB_MCU == OPT_MCU_STM32F7                               || \
-    CFG_TUSB_MCU == OPT_MCU_STM32H7                               || \
-    (CFG_TUSB_MCU == OPT_MCU_STM32L4 && defined(STM32L4_SYNOPSYS)) || \
-    CFG_TUSB_MCU == OPT_MCU_RX63X                                 || \
-    CFG_TUSB_MCU == OPT_MCU_RX65X                                 || \
-    CFG_TUSB_MCU == OPT_MCU_RX72N                                 || \
-    CFG_TUSB_MCU == OPT_MCU_GD32VF103                             || \
-    CFG_TUSB_MCU == OPT_MCU_LPC18XX                               || \
-    CFG_TUSB_MCU == OPT_MCU_LPC43XX                               || \
-    CFG_TUSB_MCU == OPT_MCU_MIMXRT                                || \
+// Only STM32 and dcd_transdimension use non-linear buffer for now
+// dwc2 except esp32sx (since it may use dcd_esp32sx)
+// Ring buffer is incompatible with dcache, since neither address nor size is aligned to cache line
+#if (defined(TUP_USBIP_DWC2) && !TU_CHECK_MCU(OPT_MCU_ESP32S2, OPT_MCU_ESP32S3)) || \
+    defined(TUP_USBIP_FSDEV) ||                                                     \
+    CFG_TUSB_MCU == OPT_MCU_RX63X ||                                                \
+    CFG_TUSB_MCU == OPT_MCU_RX65X ||                                                \
+    CFG_TUSB_MCU == OPT_MCU_RX72N ||                                                \
+    CFG_TUSB_MCU == OPT_MCU_LPC18XX ||                                              \
+    CFG_TUSB_MCU == OPT_MCU_LPC43XX ||                                              \
+    CFG_TUSB_MCU == OPT_MCU_MIMXRT1XXX ||                                           \
     CFG_TUSB_MCU == OPT_MCU_MSP432E4
-#if TUD_AUDIO_PREFER_RING_BUFFER
-#define  USE_LINEAR_BUFFER     0
+  #if TUD_AUDIO_PREFER_RING_BUFFER && !CFG_TUD_MEM_DCACHE_ENABLE
+    #define USE_LINEAR_BUFFER 0
+  #else
+    #define USE_LINEAR_BUFFER 1
+  #endif
 #else
-#define  USE_LINEAR_BUFFER     1
-#endif
-#else
-#define  USE_LINEAR_BUFFER     1
+  #define USE_LINEAR_BUFFER 1
 #endif
 
 // Declaration of buffers
 
 // Check for maximum supported numbers
 #if CFG_TUD_AUDIO > 3
-#error Maximum number of audio functions restricted to three!
+  #error Maximum number of audio functions restricted to three!
 #endif
 
-// EP IN software buffers and mutexes
-#if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING
-#if CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t audio_ep_in_sw_buf_1[CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ];
-#if CFG_FIFO_MUTEX
-osal_mutex_def_t ep_in_ff_mutex_wr_1;                                                             // No need for read mutex as only USB driver reads from FIFO
-#endif
-#endif // CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ > 0
-#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t audio_ep_in_sw_buf_2[CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ];
-#if CFG_FIFO_MUTEX
-osal_mutex_def_t ep_in_ff_mutex_wr_2;                                                             // No need for read mutex as only USB driver reads from FIFO
+// Put swap buffer in USB section only if necessary
+#if USE_LINEAR_BUFFER || CFG_TUD_AUDIO_ENABLE_ENCODING
+  #define IN_SW_BUF_MEM_ATTR TU_ATTR_ALIGNED(4)
+#else
+  #define IN_SW_BUF_MEM_ATTR CFG_TUD_MEM_SECTION CFG_TUD_MEM_ALIGN
 #endif
-#endif // CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ > 0
-#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t audio_ep_in_sw_buf_3[CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ];
-#if CFG_FIFO_MUTEX
-osal_mutex_def_t ep_in_ff_mutex_wr_3;                                                             // No need for read mutex as only USB driver reads from FIFO
+#if USE_LINEAR_BUFFER || CFG_TUD_AUDIO_ENABLE_DECODING
+  #define OUT_SW_BUF_MEM_ATTR TU_ATTR_ALIGNED(4)
+#else
+  #define OUT_SW_BUF_MEM_ATTR CFG_TUD_MEM_SECTION CFG_TUD_MEM_ALIGN
 #endif
-#endif // CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ > 0
-#endif // CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING
+
+// EP IN software buffers and mutexes
+#if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING
+tu_static IN_SW_BUF_MEM_ATTR struct {
+  #if CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ > 0
+  TUD_EPBUF_DEF(buf_1, CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ);
+  #endif
+  #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ > 0
+  TUD_EPBUF_DEF(buf_2, CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ);
+  #endif
+  #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ > 0
+  TUD_EPBUF_DEF(buf_3, CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ);
+  #endif
+} ep_in_sw_buf;
+
+  #if CFG_FIFO_MUTEX
+    #if CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ > 0
+    tu_static osal_mutex_def_t ep_in_ff_mutex_wr_1;
+    #endif
+    #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ > 0
+    tu_static osal_mutex_def_t ep_in_ff_mutex_wr_2;
+    #endif
+    #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ > 0
+    tu_static osal_mutex_def_t ep_in_ff_mutex_wr_3;
+    #endif
+  #endif
+#endif// CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING
 
 // Linear buffer TX in case:
 // - target MCU is not capable of handling a ring buffer FIFO e.g. no hardware buffer is available or driver is would need to be changed dramatically OR
 // - the software encoding is used - in this case the linear buffers serve as a target memory where logical channels are encoded into
 #if CFG_TUD_AUDIO_ENABLE_EP_IN && (USE_LINEAR_BUFFER || CFG_TUD_AUDIO_ENABLE_ENCODING)
-#if CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t lin_buf_in_1[CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX];
-#endif
-#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_IN_SZ_MAX > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t lin_buf_in_2[CFG_TUD_AUDIO_FUNC_2_EP_IN_SZ_MAX];
-#endif
-#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_IN_SZ_MAX > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t lin_buf_in_3[CFG_TUD_AUDIO_FUNC_3_EP_IN_SZ_MAX];
-#endif
-#endif // CFG_TUD_AUDIO_ENABLE_EP_IN && (USE_LINEAR_BUFFER || CFG_TUD_AUDIO_ENABLE_DECODING)
+tu_static CFG_TUD_MEM_SECTION struct {
+  #if CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX > 0
+  TUD_EPBUF_DEF(buf_1, CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX);
+  #endif
+  #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_IN_SZ_MAX > 0
+  TUD_EPBUF_DEF(buf_2, CFG_TUD_AUDIO_FUNC_2_EP_IN_SZ_MAX);
+  #endif
+  #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_IN_SZ_MAX > 0
+  TUD_EPBUF_DEF(buf_3, CFG_TUD_AUDIO_FUNC_3_EP_IN_SZ_MAX);
+  #endif
+} lin_buf_in;
+#endif// CFG_TUD_AUDIO_ENABLE_EP_IN && (USE_LINEAR_BUFFER || CFG_TUD_AUDIO_ENABLE_DECODING)
 
 // EP OUT software buffers and mutexes
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING
-#if CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t audio_ep_out_sw_buf_1[CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ];
-#if CFG_FIFO_MUTEX
-osal_mutex_def_t ep_out_ff_mutex_rd_1;                                                            // No need for write mutex as only USB driver writes into FIFO
-#endif
-#endif // CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ > 0
-#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t audio_ep_out_sw_buf_2[CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ];
-#if CFG_FIFO_MUTEX
-osal_mutex_def_t ep_out_ff_mutex_rd_2;                                                            // No need for write mutex as only USB driver writes into FIFO
-#endif
-#endif // CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ > 0
-#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t audio_ep_out_sw_buf_3[CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ];
-#if CFG_FIFO_MUTEX
-osal_mutex_def_t ep_out_ff_mutex_rd_3;                                                            // No need for write mutex as only USB driver writes into FIFO
-#endif
-#endif // CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ > 0
-#endif // CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING
+tu_static OUT_SW_BUF_MEM_ATTR struct {
+  #if CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ > 0
+  TUD_EPBUF_DEF(buf_1, CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ);
+  #endif
+  #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ > 0
+  TUD_EPBUF_DEF(buf_2, CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ);
+  #endif
+  #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ > 0
+  TUD_EPBUF_DEF(buf_3, CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ);
+  #endif
+} ep_out_sw_buf;
+
+  #if CFG_FIFO_MUTEX
+    #if CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ > 0
+    tu_static osal_mutex_def_t ep_out_ff_mutex_rd_1;
+    #endif
+    #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ > 0
+    tu_static osal_mutex_def_t ep_out_ff_mutex_rd_2;
+    #endif
+    #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ > 0
+    tu_static osal_mutex_def_t ep_out_ff_mutex_rd_3;
+    #endif
+  #endif
+#endif// CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING
 
 // Linear buffer RX in case:
 // - target MCU is not capable of handling a ring buffer FIFO e.g. no hardware buffer is available or driver is would need to be changed dramatically OR
 // - the software encoding is used - in this case the linear buffers serve as a target memory where logical channels are encoded into
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT && (USE_LINEAR_BUFFER || CFG_TUD_AUDIO_ENABLE_DECODING)
-#if CFG_TUD_AUDIO_FUNC_1_EP_OUT_SZ_MAX > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t lin_buf_out_1[CFG_TUD_AUDIO_FUNC_1_EP_OUT_SZ_MAX];
-#endif
-#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SZ_MAX > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t lin_buf_out_2[CFG_TUD_AUDIO_FUNC_2_EP_OUT_SZ_MAX];
-#endif
-#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SZ_MAX > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t lin_buf_out_3[CFG_TUD_AUDIO_FUNC_3_EP_OUT_SZ_MAX];
-#endif
-#endif // CFG_TUD_AUDIO_ENABLE_EP_OUT && (USE_LINEAR_BUFFER || CFG_TUD_AUDIO_ENABLE_DECODING)
+tu_static CFG_TUD_MEM_SECTION struct {
+  #if CFG_TUD_AUDIO_FUNC_1_EP_OUT_SZ_MAX > 0
+  TUD_EPBUF_DEF(buf_1, CFG_TUD_AUDIO_FUNC_1_EP_OUT_SZ_MAX);
+  #endif
+  #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SZ_MAX > 0
+  TUD_EPBUF_DEF(buf_2, CFG_TUD_AUDIO_FUNC_2_EP_OUT_SZ_MAX);
+  #endif
+  #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SZ_MAX > 0
+  TUD_EPBUF_DEF(buf_3, CFG_TUD_AUDIO_FUNC_3_EP_OUT_SZ_MAX);
+  #endif
+} lin_buf_out;
+#endif// CFG_TUD_AUDIO_ENABLE_EP_OUT && (USE_LINEAR_BUFFER || CFG_TUD_AUDIO_ENABLE_DECODING)
 
 // Control buffers
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t ctrl_buf_1[CFG_TUD_AUDIO_FUNC_1_CTRL_BUF_SZ];
+tu_static uint8_t ctrl_buf_1[CFG_TUD_AUDIO_FUNC_1_CTRL_BUF_SZ];
+
 #if CFG_TUD_AUDIO > 1
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t ctrl_buf_2[CFG_TUD_AUDIO_FUNC_2_CTRL_BUF_SZ];
+tu_static uint8_t ctrl_buf_2[CFG_TUD_AUDIO_FUNC_2_CTRL_BUF_SZ];
 #endif
+
 #if CFG_TUD_AUDIO > 2
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t ctrl_buf_3[CFG_TUD_AUDIO_FUNC_3_CTRL_BUF_SZ];
+tu_static uint8_t ctrl_buf_3[CFG_TUD_AUDIO_FUNC_3_CTRL_BUF_SZ];
 #endif
 
 // Active alternate setting of interfaces
-uint8_t alt_setting_1[CFG_TUD_AUDIO_FUNC_1_N_AS_INT];
+tu_static uint8_t alt_setting_1[CFG_TUD_AUDIO_FUNC_1_N_AS_INT];
+
 #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_N_AS_INT > 0
-uint8_t alt_setting_2[CFG_TUD_AUDIO_FUNC_2_N_AS_INT];
+tu_static uint8_t alt_setting_2[CFG_TUD_AUDIO_FUNC_2_N_AS_INT];
 #endif
+
 #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_N_AS_INT > 0
-uint8_t alt_setting_3[CFG_TUD_AUDIO_FUNC_3_N_AS_INT];
+tu_static uint8_t alt_setting_3[CFG_TUD_AUDIO_FUNC_3_N_AS_INT];
 #endif
 
 // Software encoding/decoding support FIFOs
 #if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING
-#if CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t tx_supp_ff_buf_1[CFG_TUD_AUDIO_FUNC_1_N_TX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ];
-tu_fifo_t tx_supp_ff_1[CFG_TUD_AUDIO_FUNC_1_N_TX_SUPP_SW_FIFO];
-#if CFG_FIFO_MUTEX
-osal_mutex_def_t tx_supp_ff_mutex_wr_1[CFG_TUD_AUDIO_FUNC_1_N_TX_SUPP_SW_FIFO];                   // No need for read mutex as only USB driver reads from FIFO
-#endif
-#endif
-#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t tx_supp_ff_buf_2[CFG_TUD_AUDIO_FUNC_2_N_TX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ];
-tu_fifo_t tx_supp_ff_2[CFG_TUD_AUDIO_FUNC_2_N_TX_SUPP_SW_FIFO];
-#if CFG_FIFO_MUTEX
-osal_mutex_def_t tx_supp_ff_mutex_wr_2[CFG_TUD_AUDIO_FUNC_2_N_TX_SUPP_SW_FIFO];                   // No need for read mutex as only USB driver reads from FIFO
-#endif
-#endif
-#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t tx_supp_ff_buf_3[CFG_TUD_AUDIO_FUNC_3_N_TX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ];
-tu_fifo_t tx_supp_ff_3[CFG_TUD_AUDIO_FUNC_3_N_TX_SUPP_SW_FIFO];
-#if CFG_FIFO_MUTEX
-osal_mutex_def_t tx_supp_ff_mutex_wr_3[CFG_TUD_AUDIO_FUNC_3_N_TX_SUPP_SW_FIFO];                   // No need for read mutex as only USB driver reads from FIFO
-#endif
-#endif
+  #if CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ > 0
+    tu_static TU_ATTR_ALIGNED(4) uint8_t tx_supp_ff_buf_1[CFG_TUD_AUDIO_FUNC_1_N_TX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ];
+    tu_static tu_fifo_t tx_supp_ff_1[CFG_TUD_AUDIO_FUNC_1_N_TX_SUPP_SW_FIFO];
+    #if CFG_FIFO_MUTEX
+    tu_static osal_mutex_def_t tx_supp_ff_mutex_wr_1[CFG_TUD_AUDIO_FUNC_1_N_TX_SUPP_SW_FIFO];// No need for read mutex as only USB driver reads from FIFO
+    #endif
+  #endif
+
+  #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ > 0
+    tu_static TU_ATTR_ALIGNED(4) uint8_t tx_supp_ff_buf_2[CFG_TUD_AUDIO_FUNC_2_N_TX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ];
+    tu_static tu_fifo_t tx_supp_ff_2[CFG_TUD_AUDIO_FUNC_2_N_TX_SUPP_SW_FIFO];
+    #if CFG_FIFO_MUTEX
+    tu_static osal_mutex_def_t tx_supp_ff_mutex_wr_2[CFG_TUD_AUDIO_FUNC_2_N_TX_SUPP_SW_FIFO];// No need for read mutex as only USB driver reads from FIFO
+    #endif
+  #endif
+
+  #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ > 0
+    tu_static TU_ATTR_ALIGNED(4) uint8_t tx_supp_ff_buf_3[CFG_TUD_AUDIO_FUNC_3_N_TX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ];
+    tu_static tu_fifo_t tx_supp_ff_3[CFG_TUD_AUDIO_FUNC_3_N_TX_SUPP_SW_FIFO];
+    #if CFG_FIFO_MUTEX
+    tu_static osal_mutex_def_t tx_supp_ff_mutex_wr_3[CFG_TUD_AUDIO_FUNC_3_N_TX_SUPP_SW_FIFO];// No need for read mutex as only USB driver reads from FIFO
+    #endif
+  #endif
 #endif
 
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING
-#if CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t rx_supp_ff_buf_1[CFG_TUD_AUDIO_FUNC_1_N_RX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ];
-tu_fifo_t rx_supp_ff_1[CFG_TUD_AUDIO_FUNC_1_N_RX_SUPP_SW_FIFO];
-#if CFG_FIFO_MUTEX
-osal_mutex_def_t rx_supp_ff_mutex_rd_1[CFG_TUD_AUDIO_FUNC_1_N_RX_SUPP_SW_FIFO];                   // No need for write mutex as only USB driver writes into FIFO
-#endif
-#endif
-#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t rx_supp_ff_buf_2[CFG_TUD_AUDIO_FUNC_2_N_RX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ];
-tu_fifo_t rx_supp_ff_2[CFG_TUD_AUDIO_FUNC_2_N_RX_SUPP_SW_FIFO];
-#if CFG_FIFO_MUTEX
-osal_mutex_def_t rx_supp_ff_mutex_rd_2[CFG_TUD_AUDIO_FUNC_2_N_RX_SUPP_SW_FIFO];                   // No need for write mutex as only USB driver writes into FIFO
-#endif
-#endif
-#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ > 0
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t rx_supp_ff_buf_3[CFG_TUD_AUDIO_FUNC_3_N_RX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ];
-tu_fifo_t rx_supp_ff_3[CFG_TUD_AUDIO_FUNC_3_N_RX_SUPP_SW_FIFO];
-#if CFG_FIFO_MUTEX
-osal_mutex_def_t rx_supp_ff_mutex_rd_3[CFG_TUD_AUDIO_FUNC_3_N_RX_SUPP_SW_FIFO];                   // No need for write mutex as only USB driver writes into FIFO
+  #if CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ > 0
+    tu_static TU_ATTR_ALIGNED(4) uint8_t rx_supp_ff_buf_1[CFG_TUD_AUDIO_FUNC_1_N_RX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ];
+    tu_static tu_fifo_t rx_supp_ff_1[CFG_TUD_AUDIO_FUNC_1_N_RX_SUPP_SW_FIFO];
+    #if CFG_FIFO_MUTEX
+    tu_static osal_mutex_def_t rx_supp_ff_mutex_rd_1[CFG_TUD_AUDIO_FUNC_1_N_RX_SUPP_SW_FIFO];// No need for write mutex as only USB driver writes into FIFO
+    #endif
+  #endif
+
+  #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ > 0
+    tu_static TU_ATTR_ALIGNED(4) uint8_t rx_supp_ff_buf_2[CFG_TUD_AUDIO_FUNC_2_N_RX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ];
+    tu_static tu_fifo_t rx_supp_ff_2[CFG_TUD_AUDIO_FUNC_2_N_RX_SUPP_SW_FIFO];
+    #if CFG_FIFO_MUTEX
+    tu_static osal_mutex_def_t rx_supp_ff_mutex_rd_2[CFG_TUD_AUDIO_FUNC_2_N_RX_SUPP_SW_FIFO];// No need for write mutex as only USB driver writes into FIFO
+    #endif
+  #endif
+
+  #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ > 0
+    tu_static TU_ATTR_ALIGNED(4) uint8_t rx_supp_ff_buf_3[CFG_TUD_AUDIO_FUNC_3_N_RX_SUPP_SW_FIFO][CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ];
+    tu_static tu_fifo_t rx_supp_ff_3[CFG_TUD_AUDIO_FUNC_3_N_RX_SUPP_SW_FIFO];
+    #if CFG_FIFO_MUTEX
+    tu_static osal_mutex_def_t rx_supp_ff_mutex_rd_3[CFG_TUD_AUDIO_FUNC_3_N_RX_SUPP_SW_FIFO];// No need for write mutex as only USB driver writes into FIFO
+    #endif
+  #endif
 #endif
+
+// Aligned buffer for feedback EP
+#if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+tu_static CFG_TUD_MEM_SECTION struct {
+  #if CFG_TUD_AUDIO_FUNC_1_EP_OUT_SZ_MAX > 0
+  TUD_EPBUF_TYPE_DEF(uint32_t, buf_1);
+  #endif
+  #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SZ_MAX > 0
+  TUD_EPBUF_TYPE_DEF(uint32_t, buf_2);
+  #endif
+  #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SZ_MAX > 0
+  TUD_EPBUF_TYPE_DEF(uint32_t, buf_3);
+  #endif
+} fb_ep_buf;
 #endif
+
+// Aligned buffer for interrupt EP
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+tu_static CFG_TUD_MEM_SECTION struct {
+  TUD_EPBUF_DEF(buf, CFG_TUD_AUDIO_INTERRUPT_EP_SZ);
+} int_ep_buf[CFG_TUD_AUDIO];
 #endif
 
 typedef struct
 {
   uint8_t rhport;
-  uint8_t const * p_desc;       // Pointer pointing to Standard AC Interface Descriptor(4.7.1) - Audio Control descriptor defining audio function
+  uint8_t const *p_desc;// Pointer pointing to Standard AC Interface Descriptor(4.7.1) - Audio Control descriptor defining audio function
 
 #if CFG_TUD_AUDIO_ENABLE_EP_IN
-  uint8_t ep_in;                // TX audio data EP.
-  uint16_t ep_in_sz;            // Current size of TX EP
-  uint8_t ep_in_as_intf_num;    // Corresponding Standard AS Interface Descriptor (4.9.1) belonging to output terminal to which this EP belongs - 0 is invalid (this fits to UAC2 specification since AS interfaces can not have interface number equal to zero)
+  uint8_t ep_in;            // TX audio data EP.
+  uint16_t ep_in_sz;        // Current size of TX EP
+  uint8_t ep_in_as_intf_num;// Corresponding Standard AS Interface Descriptor (4.9.1) belonging to output terminal to which this EP belongs - 0 is invalid (this fits to UAC2 specification since AS interfaces can not have interface number equal to zero)
 #endif
 
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT
-  uint8_t ep_out;               // Incoming (into uC) audio data EP.
-  uint16_t ep_out_sz;           // Current size of RX EP
-  uint8_t ep_out_as_intf_num;   // Corresponding Standard AS Interface Descriptor (4.9.1) belonging to input terminal to which this EP belongs - 0 is invalid (this fits to UAC2 specification since AS interfaces can not have interface number equal to zero)
+  uint8_t ep_out;            // Incoming (into uC) audio data EP.
+  uint16_t ep_out_sz;        // Current size of RX EP
+  uint8_t ep_out_as_intf_num;// Corresponding Standard AS Interface Descriptor (4.9.1) belonging to input terminal to which this EP belongs - 0 is invalid (this fits to UAC2 specification since AS interfaces can not have interface number equal to zero)
 
-#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
-  uint8_t ep_fb;                // Feedback EP.
-#endif
+  #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+  uint8_t ep_fb;// Feedback EP.
+  #endif
 
 #endif
 
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-  uint8_t ep_int_ctr;           // Audio control interrupt EP.
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+  uint8_t ep_int;// Audio control interrupt EP.
 #endif
 
-  /*------------- From this point, data is not cleared by bus reset -------------*/
-
-  uint16_t desc_length;         // Length of audio function descriptor
-
-  // Buffer for control requests
-  uint8_t * ctrl_buf;
-  uint8_t ctrl_buf_sz;
-
-  // Current active alternate settings
-  uint8_t * alt_setting;   // We need to save the current alternate setting this way, because it is possible that there are AS interfaces which do not have an EP!
+  bool mounted;// Device opened
 
-  // EP Transfer buffers and FIFOs
-#if CFG_TUD_AUDIO_ENABLE_EP_OUT
-#if !CFG_TUD_AUDIO_ENABLE_DECODING
-  tu_fifo_t ep_out_ff;
-#endif
+  uint16_t desc_length;// Length of audio function descriptor
 
 #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
   struct {
-    uint32_t value;       // Feedback value for asynchronous mode (in 16.16 format).
-    uint32_t min_value;   // min value according to UAC2 FMT-2.0 section 2.3.1.1.
-    uint32_t max_value;   // max value according to UAC2 FMT-2.0 section 2.3.1.1.
+    uint32_t value;    // Feedback value for asynchronous mode (in 16.16 format).
+    uint32_t min_value;// min value according to UAC2 FMT-2.0 section 2.3.1.1.
+    uint32_t max_value;// max value according to UAC2 FMT-2.0 section 2.3.1.1.
 
-    uint8_t frame_shift;  // bInterval-1 in unit of frame (FS), micro-frame (HS)
+    uint8_t frame_shift;// bInterval-1 in unit of frame (FS), micro-frame (HS)
     uint8_t compute_method;
-
+    bool format_correction;
     union {
-      uint8_t power_of_2; // pre-computed power of 2 shift
-      float float_const;  // pre-computed float constant
+      uint8_t power_of_2;// pre-computed power of 2 shift
+      float float_const; // pre-computed float constant
 
       struct {
         uint32_t sample_freq;
         uint32_t mclk_freq;
-      }fixed;
+      } fixed;
 
-#if 0 // implement later
       struct {
-        uint32_t nominal_value;
-        uint32_t threshold_bytes;
-      }fifo_count;
-#endif
-    }compute;
+        uint32_t nom_value;    // In 16.16 format
+        uint32_t fifo_lvl_avg; // In 16.16 format
+        uint16_t fifo_lvl_thr; // fifo level threshold
+        uint16_t rate_const[2];// pre-computed feedback/fifo_depth rate
+      } fifo_count;
+    } compute;
 
   } feedback;
-#endif // CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
-
-#endif // CFG_TUD_AUDIO_ENABLE_EP_OUT
-
-#if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING
-  tu_fifo_t ep_in_ff;
-#endif
+#endif// CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
 
-  // Audio control interrupt buffer - no FIFO - 6 Bytes according to UAC 2 specification (p. 74)
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-  CFG_TUSB_MEM_ALIGN uint8_t ep_int_ctr_buf[CFG_TUD_AUDIO_INT_CTR_EP_IN_SW_BUFFER_SIZE];
-#endif
-
-  // Decoding parameters - parameters are set when alternate AS interface is set by host
-  // Coding is currently only supported for EP. Software coding corresponding to AS interfaces without EPs are not supported currently.
+// Decoding parameters - parameters are set when alternate AS interface is set by host
+// Coding is currently only supported for EP. Software coding corresponding to AS interfaces without EPs are not supported currently.
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING
   audio_format_type_t format_type_rx;
   uint8_t n_channels_rx;
 
-#if CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING
+  #if CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING
   audio_data_format_type_I_t format_type_I_rx;
-  uint8_t n_bytes_per_sampe_rx;
-  uint8_t n_channels_per_ff_rx;
+  uint8_t n_bytes_per_sample_rx;
   uint8_t n_ff_used_rx;
+  #endif
 #endif
+
+#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
+  uint32_t sample_rate_tx;
+  uint16_t packet_sz_tx[3];
+  uint8_t bclock_id_tx;
+  uint8_t interval_tx;
 #endif
 
-  // Encoding parameters - parameters are set when alternate AS interface is set by host
-#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING
+// Encoding parameters - parameters are set when alternate AS interface is set by host
+#if CFG_TUD_AUDIO_ENABLE_EP_IN && (CFG_TUD_AUDIO_ENABLE_ENCODING || CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL)
   audio_format_type_t format_type_tx;
   uint8_t n_channels_tx;
+  uint8_t n_bytes_per_sample_tx;
 
-#if CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING
+  #if CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING
   audio_data_format_type_I_t format_type_I_tx;
-  uint8_t n_bytes_per_sampe_tx;
-  uint8_t n_channels_per_ff_tx;
   uint8_t n_ff_used_tx;
+  #endif
 #endif
+
+  /*------------- From this point, data is not cleared by bus reset -------------*/
+
+  // Buffer for control requests
+  uint8_t *ctrl_buf;
+  uint8_t ctrl_buf_sz;
+
+  // Current active alternate settings
+  uint8_t *alt_setting;// We need to save the current alternate setting this way, because it is possible that there are AS interfaces which do not have an EP!
+
+// EP Transfer buffers and FIFOs
+#if CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING
+  tu_fifo_t ep_out_ff;
 #endif
 
-  // Support FIFOs for software encoding and decoding
+#if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING
+  tu_fifo_t ep_in_ff;
+#endif
+
+// Support FIFOs for software encoding and decoding
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING
-  tu_fifo_t * rx_supp_ff;
+  tu_fifo_t *rx_supp_ff;
   uint8_t n_rx_supp_ff;
   uint16_t rx_supp_ff_sz_max;
+  #if CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING
+  uint8_t n_channels_per_ff_rx;
+  #endif
 #endif
 
 #if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING
-  tu_fifo_t * tx_supp_ff;
+  tu_fifo_t *tx_supp_ff;
   uint8_t n_tx_supp_ff;
   uint16_t tx_supp_ff_sz_max;
+  #if CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING
+  uint8_t n_channels_per_ff_tx;
+  #endif
 #endif
 
-  // Linear buffer in case target MCU is not capable of handling a ring buffer FIFO e.g. no hardware buffer is available or driver is would need to be changed dramatically OR the support FIFOs are used
+// Linear buffer in case target MCU is not capable of handling a ring buffer FIFO e.g. no hardware buffer is available or driver is would need to be changed dramatically OR the support FIFOs are used
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT && (USE_LINEAR_BUFFER || CFG_TUD_AUDIO_ENABLE_DECODING)
-  uint8_t * lin_buf_out;
-#define USE_LINEAR_BUFFER_RX   1
+  uint8_t *lin_buf_out;
+  #define USE_LINEAR_BUFFER_RX 1
 #endif
 
 #if CFG_TUD_AUDIO_ENABLE_EP_IN && (USE_LINEAR_BUFFER || CFG_TUD_AUDIO_ENABLE_ENCODING)
-  uint8_t * lin_buf_in;
-#define USE_LINEAR_BUFFER_TX   1
+  uint8_t *lin_buf_in;
+  #define USE_LINEAR_BUFFER_TX 1
 #endif
 
+#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+  uint32_t *fb_buf;
+#endif
 } audiod_function_t;
 
 #ifndef USE_LINEAR_BUFFER_TX
-#define USE_LINEAR_BUFFER_TX   0
+  #define USE_LINEAR_BUFFER_TX 0
 #endif
 
 #ifndef USE_LINEAR_BUFFER_RX
-#define USE_LINEAR_BUFFER_RX   0
+  #define USE_LINEAR_BUFFER_RX 0
 #endif
 
-#define ITF_MEM_RESET_SIZE   offsetof(audiod_function_t, ctrl_buf)
+#define ITF_MEM_RESET_SIZE offsetof(audiod_function_t, ctrl_buf)
+
+//--------------------------------------------------------------------+
+// WEAK FUNCTION STUBS
+//--------------------------------------------------------------------+
+
+#if CFG_TUD_AUDIO_ENABLE_EP_IN
+TU_ATTR_WEAK bool tud_audio_tx_done_pre_load_cb(uint8_t rhport, uint8_t func_id, uint8_t ep_in, uint8_t cur_alt_setting) {
+  (void) rhport;
+  (void) func_id;
+  (void) ep_in;
+  (void) cur_alt_setting;
+  return true;
+}
+
+TU_ATTR_WEAK bool tud_audio_tx_done_post_load_cb(uint8_t rhport, uint16_t n_bytes_copied, uint8_t func_id, uint8_t ep_in, uint8_t cur_alt_setting) {
+  (void) rhport;
+  (void) n_bytes_copied;
+  (void) func_id;
+  (void) ep_in;
+  (void) cur_alt_setting;
+  return true;
+}
+#endif
+
+#if CFG_TUD_AUDIO_ENABLE_EP_OUT
+TU_ATTR_WEAK bool tud_audio_rx_done_pre_read_cb(uint8_t rhport, uint16_t n_bytes_received, uint8_t func_id, uint8_t ep_out, uint8_t cur_alt_setting) {
+  (void) rhport;
+  (void) n_bytes_received;
+  (void) func_id;
+  (void) ep_out;
+  (void) cur_alt_setting;
+  return true;
+}
+
+TU_ATTR_WEAK bool tud_audio_rx_done_post_read_cb(uint8_t rhport, uint16_t n_bytes_received, uint8_t func_id, uint8_t ep_out, uint8_t cur_alt_setting) {
+  (void) rhport;
+  (void) n_bytes_received;
+  (void) func_id;
+  (void) ep_out;
+  (void) cur_alt_setting;
+  return true;
+}
+#endif
+
+#if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+TU_ATTR_WEAK void tud_audio_fb_done_cb(uint8_t func_id) {
+  (void) func_id;
+}
+
+TU_ATTR_WEAK void tud_audio_feedback_params_cb(uint8_t func_id, uint8_t alt_itf, audio_feedback_params_t *feedback_param) {
+  (void) func_id;
+  (void) alt_itf;
+  feedback_param->method = AUDIO_FEEDBACK_METHOD_DISABLED;
+}
+
+TU_ATTR_WEAK bool tud_audio_feedback_format_correction_cb(uint8_t func_id) {
+  (void) func_id;
+  return CFG_TUD_AUDIO_ENABLE_FEEDBACK_FORMAT_CORRECTION;
+}
+
+TU_ATTR_WEAK TU_ATTR_FAST_FUNC void tud_audio_feedback_interval_isr(uint8_t func_id, uint32_t frame_number, uint8_t interval_shift) {
+  (void) func_id;
+  (void) frame_number;
+  (void) interval_shift;
+}
+#endif
+
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+TU_ATTR_WEAK void tud_audio_int_done_cb(uint8_t rhport) {
+  (void) rhport;
+}
+#endif
+
+// Invoked when audio set interface request received
+TU_ATTR_WEAK bool tud_audio_set_itf_cb(uint8_t rhport, tusb_control_request_t const *p_request) {
+  (void) rhport;
+  (void) p_request;
+  return true;
+}
+
+// Invoked when audio set interface request received which closes an EP
+TU_ATTR_WEAK bool tud_audio_set_itf_close_EP_cb(uint8_t rhport, tusb_control_request_t const *p_request) {
+  (void) rhport;
+  (void) p_request;
+  return true;
+}
+
+// Invoked when audio class specific set request received for an EP
+TU_ATTR_WEAK bool tud_audio_set_req_ep_cb(uint8_t rhport, tusb_control_request_t const *p_request, uint8_t *pBuff) {
+  (void) rhport;
+  (void) p_request;
+  (void) pBuff;
+  TU_LOG2("  No EP set request callback available!\r\n");
+  return false;// In case no callback function is present or request can not be conducted we stall it
+}
+
+// Invoked when audio class specific set request received for an interface
+TU_ATTR_WEAK bool tud_audio_set_req_itf_cb(uint8_t rhport, tusb_control_request_t const *p_request, uint8_t *pBuff) {
+  (void) rhport;
+  (void) p_request;
+  (void) pBuff;
+  TU_LOG2("  No interface set request callback available!\r\n");
+  return false;// In case no callback function is present or request can not be conducted we stall it
+}
+
+// Invoked when audio class specific set request received for an entity
+TU_ATTR_WEAK bool tud_audio_set_req_entity_cb(uint8_t rhport, tusb_control_request_t const *p_request, uint8_t *pBuff) {
+  (void) rhport;
+  (void) p_request;
+  (void) pBuff;
+  TU_LOG2("  No entity set request callback available!\r\n");
+  return false;// In case no callback function is present or request can not be conducted we stall it
+}
+
+// Invoked when audio class specific get request received for an EP
+TU_ATTR_WEAK bool tud_audio_get_req_ep_cb(uint8_t rhport, tusb_control_request_t const *p_request) {
+  (void) rhport;
+  (void) p_request;
+  TU_LOG2("  No EP get request callback available!\r\n");
+  return false;// Stall
+}
+
+// Invoked when audio class specific get request received for an interface
+TU_ATTR_WEAK bool tud_audio_get_req_itf_cb(uint8_t rhport, tusb_control_request_t const *p_request) {
+  (void) rhport;
+  (void) p_request;
+  TU_LOG2("  No interface get request callback available!\r\n");
+  return false;// Stall
+}
+
+// Invoked when audio class specific get request received for an entity
+TU_ATTR_WEAK bool tud_audio_get_req_entity_cb(uint8_t rhport, tusb_control_request_t const *p_request) {
+  (void) rhport;
+  (void) p_request;
+  TU_LOG2("  No entity get request callback available!\r\n");
+  return false;// Stall
+}
 
 //--------------------------------------------------------------------+
 // INTERNAL OBJECT & FUNCTION DECLARATION
 //--------------------------------------------------------------------+
-CFG_TUSB_MEM_SECTION audiod_function_t _audiod_fct[CFG_TUD_AUDIO];
+tu_static CFG_TUD_MEM_SECTION audiod_function_t _audiod_fct[CFG_TUD_AUDIO];
 
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT
-static bool audiod_rx_done_cb(uint8_t rhport, audiod_function_t* audio, uint16_t n_bytes_received);
+static bool audiod_rx_done_cb(uint8_t rhport, audiod_function_t *audio, uint16_t n_bytes_received);
 #endif
 
 #if CFG_TUD_AUDIO_ENABLE_DECODING && CFG_TUD_AUDIO_ENABLE_EP_OUT
-static bool audiod_decode_type_I_pcm(uint8_t rhport, audiod_function_t* audio, uint16_t n_bytes_received);
+static bool audiod_decode_type_I_pcm(uint8_t rhport, audiod_function_t *audio, uint16_t n_bytes_received);
 #endif
 
 #if CFG_TUD_AUDIO_ENABLE_EP_IN
-static bool audiod_tx_done_cb(uint8_t rhport, audiod_function_t* audio);
+static bool audiod_tx_done_cb(uint8_t rhport, audiod_function_t *audio);
 #endif
 
 #if CFG_TUD_AUDIO_ENABLE_ENCODING && CFG_TUD_AUDIO_ENABLE_EP_IN
-static uint16_t audiod_encode_type_I_pcm(uint8_t rhport, audiod_function_t* audio);
+static uint16_t audiod_encode_type_I_pcm(uint8_t rhport, audiod_function_t *audio);
 #endif
 
-static bool audiod_get_interface(uint8_t rhport, tusb_control_request_t const * p_request);
-static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const * p_request);
+static bool audiod_get_interface(uint8_t rhport, tusb_control_request_t const *p_request);
+static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *p_request);
 
 static bool audiod_get_AS_interface_index_global(uint8_t itf, uint8_t *func_id, uint8_t *idxItf, uint8_t const **pp_desc_int);
-static bool audiod_get_AS_interface_index(uint8_t itf, audiod_function_t * audio, uint8_t *idxItf, uint8_t const **pp_desc_int);
+static bool audiod_get_AS_interface_index(uint8_t itf, audiod_function_t *audio, uint8_t *idxItf, uint8_t const **pp_desc_int);
 static bool audiod_verify_entity_exists(uint8_t itf, uint8_t entityID, uint8_t *func_id);
 static bool audiod_verify_itf_exists(uint8_t itf, uint8_t *func_id);
 static bool audiod_verify_ep_exists(uint8_t ep, uint8_t *func_id);
-static uint8_t audiod_get_audio_fct_idx(audiod_function_t * audio);
+static uint8_t audiod_get_audio_fct_idx(audiod_function_t *audio);
 
-#if CFG_TUD_AUDIO_ENABLE_ENCODING || CFG_TUD_AUDIO_ENABLE_DECODING
-static void audiod_parse_for_AS_params(audiod_function_t* audio, uint8_t const * p_desc, uint8_t const * p_desc_end, uint8_t const as_itf);
+#if (CFG_TUD_AUDIO_ENABLE_EP_IN && (CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL || CFG_TUD_AUDIO_ENABLE_ENCODING)) || (CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING)
+static void audiod_parse_for_AS_params(audiod_function_t *audio, uint8_t const *p_desc, uint8_t const *p_desc_end, uint8_t const as_itf);
 
-static inline uint8_t tu_desc_subtype(void const* desc)
-{
-  return ((uint8_t const*) desc)[2];
+static inline uint8_t tu_desc_subtype(void const *desc) {
+  return ((uint8_t const *) desc)[2];
 }
 #endif
 
-#if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
-static bool set_fb_params_freq(audiod_function_t* audio, uint32_t sample_freq, uint32_t mclk_freq);
+#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
+static bool audiod_calc_tx_packet_sz(audiod_function_t *audio);
+static uint16_t audiod_tx_packet_size(const uint16_t *norminal_size, uint16_t data_count, uint16_t fifo_depth, uint16_t max_size);
 #endif
 
-bool tud_audio_n_mounted(uint8_t func_id)
-{
-  TU_VERIFY(func_id < CFG_TUD_AUDIO);
-  audiod_function_t* audio = &_audiod_fct[func_id];
-
-#if CFG_TUD_AUDIO_ENABLE_EP_OUT
-  if (audio->ep_out == 0) return false;
-#endif
-
-#if CFG_TUD_AUDIO_ENABLE_EP_IN
-  if (audio->ep_in == 0) return false;
-#endif
-
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-  if (audio->ep_int_ctr == 0) return false;
+#if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+static bool audiod_set_fb_params_freq(audiod_function_t *audio, uint32_t sample_freq, uint32_t mclk_freq);
+static void audiod_fb_fifo_count_update(audiod_function_t *audio, uint16_t lvl_new);
 #endif
 
-#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
-  if (audio->ep_fb == 0) return false;
-#endif
+bool tud_audio_n_mounted(uint8_t func_id) {
+  TU_VERIFY(func_id < CFG_TUD_AUDIO);
+  audiod_function_t *audio = &_audiod_fct[func_id];
 
-  return true;
+  return audio->mounted;
 }
 
 //--------------------------------------------------------------------+
@@ -481,27 +657,23 @@ bool tud_audio_n_mounted(uint8_t func_id)
 
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING
 
-uint16_t tud_audio_n_available(uint8_t func_id)
-{
+uint16_t tud_audio_n_available(uint8_t func_id) {
   TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL);
   return tu_fifo_count(&_audiod_fct[func_id].ep_out_ff);
 }
 
-uint16_t tud_audio_n_read(uint8_t func_id, void* buffer, uint16_t bufsize)
-{
+uint16_t tud_audio_n_read(uint8_t func_id, void *buffer, uint16_t bufsize) {
   TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL);
   return tu_fifo_read_n(&_audiod_fct[func_id].ep_out_ff, buffer, bufsize);
 }
 
-bool tud_audio_n_clear_ep_out_ff(uint8_t func_id)
-{
+bool tud_audio_n_clear_ep_out_ff(uint8_t func_id) {
   TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL);
   return tu_fifo_clear(&_audiod_fct[func_id].ep_out_ff);
 }
 
-tu_fifo_t* tud_audio_n_get_ep_out_ff(uint8_t func_id)
-{
-  if(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL) return &_audiod_fct[func_id].ep_out_ff;
+tu_fifo_t *tud_audio_n_get_ep_out_ff(uint8_t func_id) {
+  if (func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL) return &_audiod_fct[func_id].ep_out_ff;
   return NULL;
 }
 
@@ -509,27 +681,23 @@ tu_fifo_t* tud_audio_n_get_ep_out_ff(uint8_t func_id)
 
 #if CFG_TUD_AUDIO_ENABLE_DECODING && CFG_TUD_AUDIO_ENABLE_EP_OUT
 // Delete all content in the support RX FIFOs
-bool tud_audio_n_clear_rx_support_ff(uint8_t func_id, uint8_t ff_idx)
-{
+bool tud_audio_n_clear_rx_support_ff(uint8_t func_id, uint8_t ff_idx) {
   TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL && ff_idx < _audiod_fct[func_id].n_rx_supp_ff);
   return tu_fifo_clear(&_audiod_fct[func_id].rx_supp_ff[ff_idx]);
 }
 
-uint16_t tud_audio_n_available_support_ff(uint8_t func_id, uint8_t ff_idx)
-{
+uint16_t tud_audio_n_available_support_ff(uint8_t func_id, uint8_t ff_idx) {
   TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL && ff_idx < _audiod_fct[func_id].n_rx_supp_ff);
   return tu_fifo_count(&_audiod_fct[func_id].rx_supp_ff[ff_idx]);
 }
 
-uint16_t tud_audio_n_read_support_ff(uint8_t func_id, uint8_t ff_idx, void* buffer, uint16_t bufsize)
-{
+uint16_t tud_audio_n_read_support_ff(uint8_t func_id, uint8_t ff_idx, void *buffer, uint16_t bufsize) {
   TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL && ff_idx < _audiod_fct[func_id].n_rx_supp_ff);
   return tu_fifo_read_n(&_audiod_fct[func_id].rx_supp_ff[ff_idx], buffer, bufsize);
 }
 
-tu_fifo_t* tud_audio_n_get_rx_support_ff(uint8_t func_id, uint8_t ff_idx)
-{
-  if(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL && ff_idx < _audiod_fct[func_id].n_rx_supp_ff) return &_audiod_fct[func_id].rx_supp_ff[ff_idx];
+tu_fifo_t *tud_audio_n_get_rx_support_ff(uint8_t func_id, uint8_t ff_idx) {
+  if (func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL && ff_idx < _audiod_fct[func_id].n_rx_supp_ff) return &_audiod_fct[func_id].rx_supp_ff[ff_idx];
   return NULL;
 }
 #endif
@@ -539,28 +707,20 @@ tu_fifo_t* tud_audio_n_get_rx_support_ff(uint8_t func_id, uint8_t ff_idx)
 
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT
 
-static bool audiod_rx_done_cb(uint8_t rhport, audiod_function_t* audio, uint16_t n_bytes_received)
-{
+static bool audiod_rx_done_cb(uint8_t rhport, audiod_function_t *audio, uint16_t n_bytes_received) {
   uint8_t idxItf = 0;
   uint8_t const *dummy2;
   uint8_t idx_audio_fct = 0;
 
-  if (tud_audio_rx_done_pre_read_cb || tud_audio_rx_done_post_read_cb)
-  {
-    idx_audio_fct = audiod_get_audio_fct_idx(audio);
-    TU_VERIFY(audiod_get_AS_interface_index(audio->ep_out_as_intf_num, audio, &idxItf, &dummy2));
-  }
+  idx_audio_fct = audiod_get_audio_fct_idx(audio);
+  TU_VERIFY(audiod_get_AS_interface_index(audio->ep_out_as_intf_num, audio, &idxItf, &dummy2));
 
   // Call a weak callback here - a possibility for user to get informed an audio packet was received and data gets now loaded into EP FIFO (or decoded into support RX software FIFO)
-  if (tud_audio_rx_done_pre_read_cb)
-  {
-    TU_VERIFY(tud_audio_rx_done_pre_read_cb(rhport, n_bytes_received, idx_audio_fct, audio->ep_out, audio->alt_setting[idxItf]));
-  }
+  TU_VERIFY(tud_audio_rx_done_pre_read_cb(rhport, n_bytes_received, idx_audio_fct, audio->ep_out, audio->alt_setting[idxItf]));
 
-#if CFG_TUD_AUDIO_ENABLE_DECODING && CFG_TUD_AUDIO_ENABLE_EP_OUT
+  #if CFG_TUD_AUDIO_ENABLE_DECODING
 
-  switch (audio->format_type_rx)
-  {
+  switch (audio->format_type_rx) {
     case AUDIO_FORMAT_TYPE_UNDEFINED:
       // INDIVIDUAL DECODING PROCEDURE REQUIRED HERE!
       TU_LOG2("  Desired CFG_TUD_AUDIO_FORMAT encoding not implemented!\r\n");
@@ -569,8 +729,7 @@ static bool audiod_rx_done_cb(uint8_t rhport, audiod_function_t* audio, uint16_t
 
     case AUDIO_FORMAT_TYPE_I:
 
-      switch (audio->format_type_I_rx)
-      {
+      switch (audio->format_type_I_rx) {
         case AUDIO_DATA_FORMAT_TYPE_I_PCM:
           TU_VERIFY(audiod_decode_type_I_pcm(rhport, audio, n_bytes_received));
           break;
@@ -583,41 +742,44 @@ static bool audiod_rx_done_cb(uint8_t rhport, audiod_function_t* audio, uint16_t
       }
       break;
 
-        default:
-          // Desired CFG_TUD_AUDIO_FORMAT_TYPE_RX not implemented!
-          TU_LOG2("  Desired CFG_TUD_AUDIO_FORMAT_TYPE_RX not implemented!\r\n");
-          TU_BREAKPOINT();
-          break;
+    default:
+      // Desired CFG_TUD_AUDIO_FORMAT_TYPE_RX not implemented!
+      TU_LOG2("  Desired CFG_TUD_AUDIO_FORMAT_TYPE_RX not implemented!\r\n");
+      TU_BREAKPOINT();
+      break;
   }
 
   // Prepare for next transmission
   TU_VERIFY(usbd_edpt_xfer(rhport, audio->ep_out, audio->lin_buf_out, audio->ep_out_sz), false);
 
-#else
+  #else
 
-#if USE_LINEAR_BUFFER_RX
+    #if USE_LINEAR_BUFFER_RX
   // Data currently is in linear buffer, copy into EP OUT FIFO
   TU_VERIFY(tu_fifo_write_n(&audio->ep_out_ff, audio->lin_buf_out, n_bytes_received));
 
   // Schedule for next receive
   TU_VERIFY(usbd_edpt_xfer(rhport, audio->ep_out, audio->lin_buf_out, audio->ep_out_sz), false);
-#else
+    #else
   // Data is already placed in EP FIFO, schedule for next receive
   TU_VERIFY(usbd_edpt_xfer_fifo(rhport, audio->ep_out, &audio->ep_out_ff, audio->ep_out_sz), false);
-#endif
+    #endif
 
-#endif
+    #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+  if (audio->feedback.compute_method == AUDIO_FEEDBACK_METHOD_FIFO_COUNT) {
+    audiod_fb_fifo_count_update(audio, tu_fifo_count(&audio->ep_out_ff));
+  }
+    #endif
+
+  #endif
 
   // Call a weak callback here - a possibility for user to get informed decoding was completed
-  if (tud_audio_rx_done_post_read_cb)
-  {
-    TU_VERIFY(tud_audio_rx_done_post_read_cb(rhport, n_bytes_received, idx_audio_fct, audio->ep_out, audio->alt_setting[idxItf]));
-  }
+  TU_VERIFY(tud_audio_rx_done_post_read_cb(rhport, n_bytes_received, idx_audio_fct, audio->ep_out, audio->alt_setting[idxItf]));
 
   return true;
 }
 
-#endif //CFG_TUD_AUDIO_ENABLE_EP_OUT
+#endif//CFG_TUD_AUDIO_ENABLE_EP_OUT
 
 // The following functions are used in case CFG_TUD_AUDIO_ENABLE_DECODING != 0
 #if CFG_TUD_AUDIO_ENABLE_DECODING && CFG_TUD_AUDIO_ENABLE_EP_OUT
@@ -625,107 +787,74 @@ static bool audiod_rx_done_cb(uint8_t rhport, audiod_function_t* audio, uint16_t
 // Decoding according to 2.3.1.5 Audio Streams
 
 // Helper function
-static inline uint8_t * audiod_interleaved_copy_bytes_fast_decode(uint16_t const nBytesToCopy, void * dst, uint8_t * dst_end, uint8_t * src, uint8_t const n_ff_used)
-{
-
-  // This function is an optimized version of
-  //  while((uint8_t *)dst < dst_end)
-  //  {
-  //    memcpy(dst, src, nBytesToCopy);
-  //    dst = (uint8_t *)dst + nBytesToCopy;
-  //    src += nBytesToCopy * n_ff_used;
-  //  }
-
-  // Optimize for fast half word copies
-  typedef struct{
-    uint16_t val;
-  } __attribute((__packed__)) unaligned_uint16_t;
-
-  // Optimize for fast word copies
-  typedef struct{
-    uint32_t val;
-  } __attribute((__packed__)) unaligned_uint32_t;
+static inline void *audiod_interleaved_copy_bytes_fast_decode(uint16_t const nBytesPerSample, void *dst, const void *dst_end, void *src, uint8_t const n_ff_used) {
+  // Due to one FIFO contains 2 channels, data always aligned to (nBytesPerSample * 2)
+  uint16_t *dst16 = dst;
+  uint16_t *src16 = src;
+  const uint16_t *dst_end16 = dst_end;
+  uint32_t *dst32 = dst;
+  uint32_t *src32 = src;
+  const uint32_t *dst_end32 = dst_end;
 
-  switch (nBytesToCopy)
+  if (nBytesPerSample == 1) {
+    while (dst16 < dst_end16) {
+      *dst16++ = *src16++;
+      src16 += n_ff_used - 1;
+    }
+    return src16;
+  } else if (nBytesPerSample == 2) {
+    while (dst32 < dst_end32) {
+      *dst32++ = *src32++;
+      src32 += n_ff_used - 1;
+    }
+    return src32;
+  } else if (nBytesPerSample == 3) {
+    while (dst16 < dst_end16) {
+      *dst16++ = *src16++;
+      *dst16++ = *src16++;
+      *dst16++ = *src16++;
+      src16 += 3 * (n_ff_used - 1);
+    }
+    return src16;
+  } else// nBytesPerSample == 4
   {
-    case 1:
-      while((uint8_t *)dst < dst_end)
-      {
-        *(uint8_t *)dst++ = *src;
-        src += n_ff_used;
-      }
-      break;
-
-    case 2:
-      while((uint8_t *)dst < dst_end)
-      {
-        *(unaligned_uint16_t*)dst = *(unaligned_uint16_t*)src;
-        dst += 2;
-        src += 2 * n_ff_used;
-      }
-      break;
-
-    case 3:
-      while((uint8_t *)dst < dst_end)
-      {
-        //        memcpy(dst, src, 3);
-        //        dst = (uint8_t *)dst + 3;
-        //        src += 3 * n_ff_used;
-
-        // TODO: Is there a faster way to copy 3 bytes?
-        *(uint8_t *)dst++ = *src++;
-        *(uint8_t *)dst++ = *src++;
-        *(uint8_t *)dst++ = *src++;
-
-        src += 3 * (n_ff_used - 1);
-      }
-      break;
-
-    case 4:
-      while((uint8_t *)dst < dst_end)
-      {
-        *(unaligned_uint32_t*)dst = *(unaligned_uint32_t*)src;
-        dst += 4;
-        src += 4 * n_ff_used;
-      }
-      break;
+    while (dst32 < dst_end32) {
+      *dst32++ = *src32++;
+      *dst32++ = *src32++;
+      src32 += 2 * (n_ff_used - 1);
+    }
+    return src32;
   }
-
-  return src;
 }
 
-static bool audiod_decode_type_I_pcm(uint8_t rhport, audiod_function_t* audio, uint16_t n_bytes_received)
-{
+static bool audiod_decode_type_I_pcm(uint8_t rhport, audiod_function_t *audio, uint16_t n_bytes_received) {
   (void) rhport;
 
   // Determine amount of samples
-  uint8_t const n_ff_used               = audio->n_ff_used_rx;
-  uint16_t const nBytesPerFFToRead      = n_bytes_received / n_ff_used;
+  uint8_t const n_ff_used = audio->n_ff_used_rx;
+  uint16_t const nBytesPerFFToRead = n_bytes_received / n_ff_used;
   uint8_t cnt_ff;
 
   // Decode
-  uint8_t * src;
-  uint8_t * dst_end;
+  uint8_t *src;
+  uint8_t *dst_end;
 
   tu_fifo_buffer_info_t info;
 
-  for (cnt_ff = 0; cnt_ff < n_ff_used; cnt_ff++)
-  {
+  for (cnt_ff = 0; cnt_ff < n_ff_used; cnt_ff++) {
     tu_fifo_get_write_info(&audio->rx_supp_ff[cnt_ff], &info);
 
-    if (info.len_lin != 0)
-    {
+    if (info.len_lin != 0) {
       info.len_lin = tu_min16(nBytesPerFFToRead, info.len_lin);
-      src = &audio->lin_buf_out[cnt_ff*audio->n_channels_per_ff_rx * audio->n_bytes_per_sampe_rx];
+      src = &audio->lin_buf_out[cnt_ff * audio->n_channels_per_ff_rx * audio->n_bytes_per_sample_rx];
       dst_end = info.ptr_lin + info.len_lin;
-      src = audiod_interleaved_copy_bytes_fast_decode(audio->n_bytes_per_sampe_rx, info.ptr_lin, dst_end, src, n_ff_used);
+      src = audiod_interleaved_copy_bytes_fast_decode(audio->n_bytes_per_sample_rx, info.ptr_lin, dst_end, src, n_ff_used);
 
       // Handle wrapped part of FIFO
       info.len_wrap = tu_min16(nBytesPerFFToRead - info.len_lin, info.len_wrap);
-      if (info.len_wrap != 0)
-      {
+      if (info.len_wrap != 0) {
         dst_end = info.ptr_wrap + info.len_wrap;
-        audiod_interleaved_copy_bytes_fast_decode(audio->n_bytes_per_sampe_rx, info.ptr_wrap, dst_end, src, n_ff_used);
+        audiod_interleaved_copy_bytes_fast_decode(audio->n_bytes_per_sample_rx, info.ptr_wrap, dst_end, src, n_ff_used);
       }
       tu_fifo_advance_write_pointer(&audio->rx_supp_ff[cnt_ff], info.len_lin + info.len_wrap);
     }
@@ -734,9 +863,15 @@ static bool audiod_decode_type_I_pcm(uint8_t rhport, audiod_function_t* audio, u
   // Number of bytes should be a multiple of CFG_TUD_AUDIO_N_BYTES_PER_SAMPLE_RX * CFG_TUD_AUDIO_N_CHANNELS_RX but checking makes no sense - no way to correct it
   // TU_VERIFY(cnt != n_bytes);
 
+  #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+  if (audio->feedback.compute_method == AUDIO_FEEDBACK_METHOD_FIFO_COUNT) {
+    audiod_fb_fifo_count_update(audio, tu_fifo_count(&audio->rx_supp_ff[0]));
+  }
+  #endif
+
   return true;
 }
-#endif //CFG_TUD_AUDIO_ENABLE_DECODING
+#endif//CFG_TUD_AUDIO_ENABLE_DECODING
 
 //--------------------------------------------------------------------+
 // WRITE API
@@ -755,21 +890,19 @@ static bool audiod_decode_type_I_pcm(uint8_t rhport, audiod_function_t* audio, u
  * \param[in]       len: # of array elements to copy
  * \return          Number of bytes actually written
  */
-uint16_t tud_audio_n_write(uint8_t func_id, const void * data, uint16_t len)
-{
+uint16_t tud_audio_n_write(uint8_t func_id, const void *data, uint16_t len) {
   TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL);
   return tu_fifo_write_n(&_audiod_fct[func_id].ep_in_ff, data, len);
 }
 
-bool tud_audio_n_clear_ep_in_ff(uint8_t func_id)                          // Delete all content in the EP IN FIFO
+bool tud_audio_n_clear_ep_in_ff(uint8_t func_id)// Delete all content in the EP IN FIFO
 {
   TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL);
   return tu_fifo_clear(&_audiod_fct[func_id].ep_in_ff);
 }
 
-tu_fifo_t* tud_audio_n_get_ep_in_ff(uint8_t func_id)
-{
-  if(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL) return &_audiod_fct[func_id].ep_in_ff;
+tu_fifo_t *tud_audio_n_get_ep_in_ff(uint8_t func_id) {
+  if (func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL) return &_audiod_fct[func_id].ep_in_ff;
   return NULL;
 }
 
@@ -777,73 +910,68 @@ tu_fifo_t* tud_audio_n_get_ep_in_ff(uint8_t func_id)
 
 #if CFG_TUD_AUDIO_ENABLE_ENCODING && CFG_TUD_AUDIO_ENABLE_EP_IN
 
-uint16_t tud_audio_n_flush_tx_support_ff(uint8_t func_id)                 // Force all content in the support TX FIFOs to be written into linear buffer and schedule a transmit
+uint16_t tud_audio_n_flush_tx_support_ff(uint8_t func_id)// Force all content in the support TX FIFOs to be written into linear buffer and schedule a transmit
 {
   TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL);
-  audiod_function_t* audio = &_audiod_fct[func_id];
+  audiod_function_t *audio = &_audiod_fct[func_id];
 
   uint16_t n_bytes_copied = tu_fifo_count(&audio->tx_supp_ff[0]);
 
   TU_VERIFY(audiod_tx_done_cb(audio->rhport, audio));
 
   n_bytes_copied -= tu_fifo_count(&audio->tx_supp_ff[0]);
-  n_bytes_copied = n_bytes_copied*audio->tx_supp_ff[0].item_size;
+  n_bytes_copied = n_bytes_copied * audio->tx_supp_ff[0].item_size;
 
   return n_bytes_copied;
 }
 
-bool tud_audio_n_clear_tx_support_ff(uint8_t func_id, uint8_t ff_idx)
-{
+bool tud_audio_n_clear_tx_support_ff(uint8_t func_id, uint8_t ff_idx) {
   TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL && ff_idx < _audiod_fct[func_id].n_tx_supp_ff);
   return tu_fifo_clear(&_audiod_fct[func_id].tx_supp_ff[ff_idx]);
 }
 
-uint16_t tud_audio_n_write_support_ff(uint8_t func_id, uint8_t ff_idx, const void * data, uint16_t len)
-{
+uint16_t tud_audio_n_write_support_ff(uint8_t func_id, uint8_t ff_idx, const void *data, uint16_t len) {
   TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL && ff_idx < _audiod_fct[func_id].n_tx_supp_ff);
   return tu_fifo_write_n(&_audiod_fct[func_id].tx_supp_ff[ff_idx], data, len);
 }
 
-tu_fifo_t* tud_audio_n_get_tx_support_ff(uint8_t func_id, uint8_t ff_idx)
-{
-  if(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL && ff_idx < _audiod_fct[func_id].n_tx_supp_ff) return &_audiod_fct[func_id].tx_supp_ff[ff_idx];
+tu_fifo_t *tud_audio_n_get_tx_support_ff(uint8_t func_id, uint8_t ff_idx) {
+  if (func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL && ff_idx < _audiod_fct[func_id].n_tx_supp_ff) return &_audiod_fct[func_id].tx_supp_ff[ff_idx];
   return NULL;
 }
 
 #endif
 
 
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-
-// If no interrupt transmit is pending bytes get written into buffer and a transmit is scheduled - once transmit completed tud_audio_int_ctr_done_cb() is called in inform user
-uint16_t tud_audio_int_ctr_n_write(uint8_t func_id, uint8_t const* buffer, uint16_t len)
-{
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+// If no interrupt transmit is pending bytes get written into buffer and a transmit is scheduled - once transmit completed tud_audio_int_done_cb() is called in inform user
+bool tud_audio_int_n_write(uint8_t func_id, const audio_interrupt_data_t *data) {
   TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL);
 
+  TU_VERIFY(_audiod_fct[func_id].ep_int != 0);
+
   // We write directly into the EP's buffer - abort if previous transfer not complete
-  TU_VERIFY(!usbd_edpt_busy(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_int_ctr));
+  TU_VERIFY(usbd_edpt_claim(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_int));
 
   // Check length
-  TU_VERIFY(len <= CFG_TUD_AUDIO_INT_CTR_EP_IN_SW_BUFFER_SIZE);
-
-  memcpy(_audiod_fct[func_id].ep_int_ctr_buf, buffer, len);
-
-  // Schedule transmit
-  TU_VERIFY(usbd_edpt_xfer(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_int_ctr, _audiod_fct[func_id].ep_int_ctr_buf, len));
+  if (tu_memcpy_s(int_ep_buf[func_id].buf, sizeof(int_ep_buf[func_id].buf), data, sizeof(audio_interrupt_data_t)) == 0) {
+    // Schedule transmit
+    TU_ASSERT(usbd_edpt_xfer(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_int, int_ep_buf[func_id].buf, sizeof(int_ep_buf[func_id].buf)), 0);
+  } else {
+    // Release endpoint since we don't make any transfer
+    usbd_edpt_release(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_int);
+  }
 
   return true;
 }
-
 #endif
 
-
 // This function is called once a transmit of an audio packet was successfully completed. Here, we encode samples and place it in IN EP's buffer for next transmission.
 // If you prefer your own (more efficient) implementation suiting your purpose set CFG_TUD_AUDIO_ENABLE_ENCODING = 0 and use tud_audio_n_write.
 
 // n_bytes_copied - Informs caller how many bytes were loaded. In case n_bytes_copied = 0, a ZLP is scheduled to inform host no data is available for current frame.
 #if CFG_TUD_AUDIO_ENABLE_EP_IN
-static bool audiod_tx_done_cb(uint8_t rhport, audiod_function_t * audio)
-{
+static bool audiod_tx_done_cb(uint8_t rhport, audiod_function_t *audio) {
   uint8_t idxItf;
   uint8_t const *dummy2;
 
@@ -855,15 +983,14 @@ static bool audiod_tx_done_cb(uint8_t rhport, audiod_function_t * audio)
 
   // Call a weak callback here - a possibility for user to get informed former TX was completed and data gets now loaded into EP in buffer (in case FIFOs are used) or
   // if no FIFOs are used the user may use this call back to load its data into the EP IN buffer by use of tud_audio_n_write_ep_in_buffer().
-  if (tud_audio_tx_done_pre_load_cb) TU_VERIFY(tud_audio_tx_done_pre_load_cb(rhport, idx_audio_fct, audio->ep_in, audio->alt_setting[idxItf]));
+  TU_VERIFY(tud_audio_tx_done_pre_load_cb(rhport, idx_audio_fct, audio->ep_in, audio->alt_setting[idxItf]));
 
   // Send everything in ISO EP FIFO
   uint16_t n_bytes_tx;
 
   // If support FIFOs are used, encode and schedule transmit
-#if CFG_TUD_AUDIO_ENABLE_ENCODING && CFG_TUD_AUDIO_ENABLE_EP_IN
-  switch (audio->format_type_tx)
-  {
+  #if CFG_TUD_AUDIO_ENABLE_ENCODING && CFG_TUD_AUDIO_ENABLE_EP_IN
+  switch (audio->format_type_tx) {
     case AUDIO_FORMAT_TYPE_UNDEFINED:
       // INDIVIDUAL ENCODING PROCEDURE REQUIRED HERE!
       TU_LOG2("  Desired CFG_TUD_AUDIO_FORMAT encoding not implemented!\r\n");
@@ -873,8 +1000,7 @@ static bool audiod_tx_done_cb(uint8_t rhport, audiod_function_t * audio)
 
     case AUDIO_FORMAT_TYPE_I:
 
-      switch (audio->format_type_I_tx)
-      {
+      switch (audio->format_type_I_tx) {
         case AUDIO_DATA_FORMAT_TYPE_I_PCM:
 
           n_bytes_tx = audiod_encode_type_I_pcm(rhport, audio);
@@ -889,38 +1015,41 @@ static bool audiod_tx_done_cb(uint8_t rhport, audiod_function_t * audio)
       }
       break;
 
-        default:
-          // Desired CFG_TUD_AUDIO_FORMAT_TYPE_TX not implemented!
-          TU_LOG2("  Desired CFG_TUD_AUDIO_FORMAT_TYPE_TX not implemented!\r\n");
-          TU_BREAKPOINT();
-          n_bytes_tx = 0;
-          break;
+    default:
+      // Desired CFG_TUD_AUDIO_FORMAT_TYPE_TX not implemented!
+      TU_LOG2("  Desired CFG_TUD_AUDIO_FORMAT_TYPE_TX not implemented!\r\n");
+      TU_BREAKPOINT();
+      n_bytes_tx = 0;
+      break;
   }
 
   TU_VERIFY(usbd_edpt_xfer(rhport, audio->ep_in, audio->lin_buf_in, n_bytes_tx));
 
-#else
-  // No support FIFOs, if no linear buffer required schedule transmit, else put data into linear buffer and schedule
-
-  n_bytes_tx = tu_min16(tu_fifo_count(&audio->ep_in_ff), audio->ep_in_sz);      // Limit up to max packet size, more can not be done for ISO
-
-#if USE_LINEAR_BUFFER_TX
+  #else
+    // No support FIFOs, if no linear buffer required schedule transmit, else put data into linear buffer and schedule
+    #if CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
+  // packet_sz_tx is based on total packet size, here we want size for each support buffer.
+  n_bytes_tx = audiod_tx_packet_size(audio->packet_sz_tx, tu_fifo_count(&audio->ep_in_ff), audio->ep_in_ff.depth, audio->ep_in_sz);
+    #else
+  n_bytes_tx = tu_min16(tu_fifo_count(&audio->ep_in_ff), audio->ep_in_sz);// Limit up to max packet size, more can not be done for ISO
+    #endif
+    #if USE_LINEAR_BUFFER_TX
   tu_fifo_read_n(&audio->ep_in_ff, audio->lin_buf_in, n_bytes_tx);
   TU_VERIFY(usbd_edpt_xfer(rhport, audio->ep_in, audio->lin_buf_in, n_bytes_tx));
-#else
+    #else
   // Send everything in ISO EP FIFO
   TU_VERIFY(usbd_edpt_xfer_fifo(rhport, audio->ep_in, &audio->ep_in_ff, n_bytes_tx));
-#endif
+    #endif
 
-#endif
+  #endif
 
   // Call a weak callback here - a possibility for user to get informed former TX was completed and how many bytes were loaded for the next frame
-  if (tud_audio_tx_done_post_load_cb) TU_VERIFY(tud_audio_tx_done_post_load_cb(rhport, n_bytes_tx, idx_audio_fct, audio->ep_in, audio->alt_setting[idxItf]));
+  TU_VERIFY(tud_audio_tx_done_post_load_cb(rhport, n_bytes_tx, idx_audio_fct, audio->ep_in, audio->alt_setting[idxItf]));
 
   return true;
 }
 
-#endif //CFG_TUD_AUDIO_ENABLE_EP_IN
+#endif//CFG_TUD_AUDIO_ENABLE_EP_IN
 
 #if CFG_TUD_AUDIO_ENABLE_ENCODING && CFG_TUD_AUDIO_ENABLE_EP_IN
 // Take samples from the support buffer and encode them into the IN EP software FIFO
@@ -941,68 +1070,47 @@ range [-1, +1)
  * */
 
 // Helper function
-static inline uint8_t * audiod_interleaved_copy_bytes_fast_encode(uint16_t const nBytesToCopy, uint8_t * src, uint8_t * src_end, uint8_t * dst, uint8_t const n_ff_used)
-{
-  // Optimize for fast half word copies
-  typedef struct{
-    uint16_t val;
-  } __attribute((__packed__)) unaligned_uint16_t;
-
-  // Optimize for fast word copies
-  typedef struct{
-    uint32_t val;
-  } __attribute((__packed__)) unaligned_uint32_t;
+static inline void *audiod_interleaved_copy_bytes_fast_encode(uint16_t const nBytesPerSample, void *src, const void *src_end, void *dst, uint8_t const n_ff_used) {
+  // Due to one FIFO contains 2 channels, data always aligned to (nBytesPerSample * 2)
+  uint16_t *dst16 = dst;
+  uint16_t *src16 = src;
+  const uint16_t *src_end16 = src_end;
+  uint32_t *dst32 = dst;
+  uint32_t *src32 = src;
+  const uint32_t *src_end32 = src_end;
 
-  switch (nBytesToCopy)
+  if (nBytesPerSample == 1) {
+    while (src16 < src_end16) {
+      *dst16++ = *src16++;
+      dst16 += n_ff_used - 1;
+    }
+    return dst16;
+  } else if (nBytesPerSample == 2) {
+    while (src32 < src_end32) {
+      *dst32++ = *src32++;
+      dst32 += n_ff_used - 1;
+    }
+    return dst32;
+  } else if (nBytesPerSample == 3) {
+    while (src16 < src_end16) {
+      *dst16++ = *src16++;
+      *dst16++ = *src16++;
+      *dst16++ = *src16++;
+      dst16 += 3 * (n_ff_used - 1);
+    }
+    return dst16;
+  } else// nBytesPerSample == 4
   {
-    case 1:
-      while(src < src_end)
-      {
-        *dst = *src++;
-        dst += n_ff_used;
-      }
-      break;
-
-    case 2:
-      while(src < src_end)
-      {
-        *(unaligned_uint16_t*)dst = *(unaligned_uint16_t*)src;
-        src += 2;
-        dst += 2 * n_ff_used;
-      }
-      break;
-
-    case 3:
-      while(src < src_end)
-      {
-        //        memcpy(dst, src, 3);
-        //        src = (uint8_t *)src + 3;
-        //        dst += 3 * n_ff_used;
-
-        // TODO: Is there a faster way to copy 3 bytes?
-        *dst++ = *src++;
-        *dst++ = *src++;
-        *dst++ = *src++;
-
-        dst += 3 * (n_ff_used - 1);
-      }
-      break;
-
-    case 4:
-      while(src < src_end)
-      {
-        *(unaligned_uint32_t*)dst = *(unaligned_uint32_t*)src;
-        src += 4;
-        dst += 4 * n_ff_used;
-      }
-      break;
+    while (src32 < src_end32) {
+      *dst32++ = *src32++;
+      *dst32++ = *src32++;
+      dst32 += 2 * (n_ff_used - 1);
+    }
+    return dst32;
   }
-
-  return dst;
 }
 
-static uint16_t audiod_encode_type_I_pcm(uint8_t rhport, audiod_function_t* audio)
-{
+static uint16_t audiod_encode_type_I_pcm(uint8_t rhport, audiod_function_t *audio) {
   // This function relies on the fact that the length of the support FIFOs was configured to be a multiple of the active sample size in bytes s.t. no sample is split within a wrap
   // This is ensured within set_interface, where the FIFOs are reconfigured according to this size
 
@@ -1010,56 +1118,58 @@ static uint16_t audiod_encode_type_I_pcm(uint8_t rhport, audiod_function_t* audi
   TU_VERIFY(!usbd_edpt_busy(rhport, audio->ep_in));
 
   // Determine amount of samples
-  uint8_t const n_ff_used               = audio->n_ff_used_tx;
-  uint16_t const nBytesToCopy           = audio->n_channels_per_ff_tx * audio->n_bytes_per_sampe_tx;
-  uint16_t const capPerFF               = audio->ep_in_sz / n_ff_used;                                        // Sample capacity per FIFO in bytes
-  uint16_t nBytesPerFFToSend            = tu_fifo_count(&audio->tx_supp_ff[0]);
+  uint8_t const n_ff_used = audio->n_ff_used_tx;
+  uint16_t nBytesPerFFToSend = tu_fifo_count(&audio->tx_supp_ff[0]);
   uint8_t cnt_ff;
 
-  for (cnt_ff = 1; cnt_ff < n_ff_used; cnt_ff++)
-  {
+  for (cnt_ff = 1; cnt_ff < n_ff_used; cnt_ff++) {
     uint16_t const count = tu_fifo_count(&audio->tx_supp_ff[cnt_ff]);
-    if (count < nBytesPerFFToSend)
-    {
+    if (count < nBytesPerFFToSend) {
       nBytesPerFFToSend = count;
     }
   }
 
-  // Check if there is enough
-  if (nBytesPerFFToSend == 0)    return 0;
-
+  #if CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
+  const uint16_t norm_packet_sz_tx[3] = {audio->packet_sz_tx[0] / n_ff_used,
+                                         audio->packet_sz_tx[1] / n_ff_used,
+                                         audio->packet_sz_tx[2] / n_ff_used};
+  // packet_sz_tx is based on total packet size, here we want size for each support buffer.
+  nBytesPerFFToSend = audiod_tx_packet_size(norm_packet_sz_tx, nBytesPerFFToSend, audio->tx_supp_ff[0].depth, audio->ep_in_sz / n_ff_used);
+  // Check if there is enough data
+  if (nBytesPerFFToSend == 0) return 0;
+  #else
+  // Check if there is enough data
+  if (nBytesPerFFToSend == 0) return 0;
   // Limit to maximum sample number - THIS IS A POSSIBLE ERROR SOURCE IF TOO MANY SAMPLE WOULD NEED TO BE SENT BUT CAN NOT!
-  nBytesPerFFToSend = tu_min16(nBytesPerFFToSend, capPerFF);
-
+  nBytesPerFFToSend = tu_min16(nBytesPerFFToSend, audio->ep_in_sz / n_ff_used);
   // Round to full number of samples (flooring)
-  nBytesPerFFToSend = (nBytesPerFFToSend / nBytesToCopy) * nBytesToCopy;
+  uint16_t const nSlotSize = audio->n_channels_per_ff_tx * audio->n_bytes_per_sample_tx;
+  nBytesPerFFToSend = (nBytesPerFFToSend / nSlotSize) * nSlotSize;
+  #endif
 
   // Encode
-  uint8_t * dst;
-  uint8_t * src_end;
+  uint8_t *dst;
+  uint8_t *src_end;
 
   tu_fifo_buffer_info_t info;
 
-  for (cnt_ff = 0; cnt_ff < n_ff_used; cnt_ff++)
-  {
-    dst = &audio->lin_buf_in[cnt_ff*audio->n_channels_per_ff_tx*audio->n_bytes_per_sampe_tx];
+  for (cnt_ff = 0; cnt_ff < n_ff_used; cnt_ff++) {
+    dst = &audio->lin_buf_in[cnt_ff * audio->n_channels_per_ff_tx * audio->n_bytes_per_sample_tx];
 
     tu_fifo_get_read_info(&audio->tx_supp_ff[cnt_ff], &info);
 
-    if (info.len_lin != 0)
-    {
-      info.len_lin = tu_min16(nBytesPerFFToSend, info.len_lin);       // Limit up to desired length
-      src_end = (uint8_t *)info.ptr_lin + info.len_lin;
-      dst = audiod_interleaved_copy_bytes_fast_encode(audio->n_bytes_per_sampe_tx, info.ptr_lin, src_end, dst, n_ff_used);
+    if (info.len_lin != 0) {
+      info.len_lin = tu_min16(nBytesPerFFToSend, info.len_lin);// Limit up to desired length
+      src_end = (uint8_t *) info.ptr_lin + info.len_lin;
+      dst = audiod_interleaved_copy_bytes_fast_encode(audio->n_bytes_per_sample_tx, info.ptr_lin, src_end, dst, n_ff_used);
 
       // Limit up to desired length
       info.len_wrap = tu_min16(nBytesPerFFToSend - info.len_lin, info.len_wrap);
 
       // Handle wrapped part of FIFO
-      if (info.len_wrap != 0)
-      {
-        src_end = (uint8_t *)info.ptr_wrap + info.len_wrap;
-        audiod_interleaved_copy_bytes_fast_encode(audio->n_bytes_per_sampe_tx, info.ptr_wrap, src_end, dst, n_ff_used);
+      if (info.len_wrap != 0) {
+        src_end = (uint8_t *) info.ptr_wrap + info.len_wrap;
+        audiod_interleaved_copy_bytes_fast_encode(audio->n_bytes_per_sample_tx, info.ptr_wrap, src_end, dst, n_ff_used);
       }
 
       tu_fifo_advance_read_pointer(&audio->tx_supp_ff[cnt_ff], info.len_lin + info.len_wrap);
@@ -1068,31 +1178,54 @@ static uint16_t audiod_encode_type_I_pcm(uint8_t rhport, audiod_function_t* audi
 
   return nBytesPerFFToSend * n_ff_used;
 }
-#endif //CFG_TUD_AUDIO_ENABLE_ENCODING
+#endif//CFG_TUD_AUDIO_ENABLE_ENCODING
 
 // This function is called once a transmit of a feedback packet was successfully completed. Here, we get the next feedback value to be sent
 
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
-static inline bool audiod_fb_send(uint8_t rhport, audiod_function_t *audio)
-{
-  return usbd_edpt_xfer(rhport, audio->ep_fb, (uint8_t *) &audio->feedback.value, 4);
+static inline bool audiod_fb_send(audiod_function_t *audio) {
+  bool apply_correction = (TUSB_SPEED_FULL == tud_speed_get()) && audio->feedback.format_correction;
+  // Format the feedback value
+  if (apply_correction) {
+    uint8_t *fb = (uint8_t *) audio->fb_buf;
+
+    // For FS format is 10.14
+    *(fb++) = (audio->feedback.value >> 2) & 0xFF;
+    *(fb++) = (audio->feedback.value >> 10) & 0xFF;
+    *(fb++) = (audio->feedback.value >> 18) & 0xFF;
+    *fb = 0;
+  } else {
+    *audio->fb_buf = audio->feedback.value;
+  }
+
+  // About feedback format on FS
+  //
+  // 3 variables: Format | packetSize | sendSize | Working OS:
+  //              16.16    4            4          Linux, Windows
+  //              16.16    4            3          Linux
+  //              16.16    3            4          Linux
+  //              16.16    3            3          Linux
+  //              10.14    4            4          Linux
+  //              10.14    4            3          Linux
+  //              10.14    3            4          Linux, OSX
+  //              10.14    3            3          Linux, OSX
+  //
+  // We send 3 bytes since sending packet larger than wMaxPacketSize is pretty ugly
+  return usbd_edpt_xfer(audio->rhport, audio->ep_fb, (uint8_t *) audio->fb_buf, apply_correction ? 3 : 4);
 }
 #endif
 
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void audiod_init(void)
-{
+void audiod_init(void) {
   tu_memclr(_audiod_fct, sizeof(_audiod_fct));
 
-  for(uint8_t i=0; i<CFG_TUD_AUDIO; i++)
-  {
-    audiod_function_t* audio = &_audiod_fct[i];
+  for (uint8_t i = 0; i < CFG_TUD_AUDIO; i++) {
+    audiod_function_t *audio = &_audiod_fct[i];
 
     // Initialize control buffers
-    switch (i)
-    {
+    switch (i) {
       case 0:
         audio->ctrl_buf = ctrl_buf_1;
         audio->ctrl_buf_sz = CFG_TUD_AUDIO_FUNC_1_CTRL_BUF_SZ;
@@ -1112,8 +1245,7 @@ void audiod_init(void)
     }
 
     // Initialize active alternate interface buffers
-    switch (i)
-    {
+    switch (i) {
 #if CFG_TUD_AUDIO_FUNC_1_N_AS_INT > 0
       case 0:
         audio->alt_setting = alt_setting_1;
@@ -1131,277 +1263,285 @@ void audiod_init(void)
 #endif
     }
 
-    // Initialize IN EP FIFO if required
+      // Initialize IN EP FIFO if required
 #if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING
 
-    switch (i)
-    {
-#if CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ > 0
+    switch (i) {
+  #if CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ > 0
       case 0:
-        tu_fifo_config(&audio->ep_in_ff, audio_ep_in_sw_buf_1, CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ, 1, true);
-#if CFG_FIFO_MUTEX
+        tu_fifo_config(&audio->ep_in_ff, ep_in_sw_buf.buf_1, CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ, 1, true);
+    #if CFG_FIFO_MUTEX
         tu_fifo_config_mutex(&audio->ep_in_ff, osal_mutex_create(&ep_in_ff_mutex_wr_1), NULL);
-#endif
+    #endif
         break;
-#endif
-#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ > 0
+  #endif
+  #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ > 0
       case 1:
-        tu_fifo_config(&audio->ep_in_ff, audio_ep_in_sw_buf_2, CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ, 1, true);
-#if CFG_FIFO_MUTEX
+        tu_fifo_config(&audio->ep_in_ff, ep_in_sw_buf.buf_2, CFG_TUD_AUDIO_FUNC_2_EP_IN_SW_BUF_SZ, 1, true);
+    #if CFG_FIFO_MUTEX
         tu_fifo_config_mutex(&audio->ep_in_ff, osal_mutex_create(&ep_in_ff_mutex_wr_2), NULL);
-#endif
+    #endif
         break;
-#endif
-#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ > 0
+  #endif
+  #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ > 0
       case 2:
-        tu_fifo_config(&audio->ep_in_ff, audio_ep_in_sw_buf_3, CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ, 1, true);
-#if CFG_FIFO_MUTEX
+        tu_fifo_config(&audio->ep_in_ff, ep_in_sw_buf.buf_3, CFG_TUD_AUDIO_FUNC_3_EP_IN_SW_BUF_SZ, 1, true);
+    #if CFG_FIFO_MUTEX
         tu_fifo_config_mutex(&audio->ep_in_ff, osal_mutex_create(&ep_in_ff_mutex_wr_3), NULL);
-#endif
+    #endif
         break;
-#endif
+  #endif
     }
-#endif // CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING
+#endif// CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING
 
-    // Initialize linear buffers
+      // Initialize linear buffers
 #if USE_LINEAR_BUFFER_TX
-    switch (i)
-    {
-#if CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX > 0
+    switch (i) {
+  #if CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX > 0
       case 0:
-        audio->lin_buf_in = lin_buf_in_1;
+        audio->lin_buf_in = lin_buf_in.buf_1;
         break;
-#endif
-#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_IN_SZ_MAX > 0
+  #endif
+  #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_IN_SZ_MAX > 0
       case 1:
-        audio->lin_buf_in = lin_buf_in_2;
+        audio->lin_buf_in = lin_buf_in.buf_2;
         break;
-#endif
-#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_IN_SZ_MAX > 0
+  #endif
+  #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_IN_SZ_MAX > 0
       case 2:
-        audio->lin_buf_in = lin_buf_in_3;
+        audio->lin_buf_in = lin_buf_in.buf_3;
         break;
-#endif
+  #endif
     }
-#endif // USE_LINEAR_BUFFER_TX
+#endif// USE_LINEAR_BUFFER_TX
 
-    // Initialize OUT EP FIFO if required
+      // Initialize OUT EP FIFO if required
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING
 
-    switch (i)
-    {
-#if CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ > 0
+    switch (i) {
+  #if CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ > 0
       case 0:
-        tu_fifo_config(&audio->ep_out_ff, audio_ep_out_sw_buf_1, CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ, 1, true);
-#if CFG_FIFO_MUTEX
+        tu_fifo_config(&audio->ep_out_ff, ep_out_sw_buf.buf_1, CFG_TUD_AUDIO_FUNC_1_EP_OUT_SW_BUF_SZ, 1, true);
+    #if CFG_FIFO_MUTEX
         tu_fifo_config_mutex(&audio->ep_out_ff, NULL, osal_mutex_create(&ep_out_ff_mutex_rd_1));
-#endif
+    #endif
         break;
-#endif
-#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ > 0
+  #endif
+  #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ > 0
       case 1:
-        tu_fifo_config(&audio->ep_out_ff, audio_ep_out_sw_buf_2, CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ, 1, true);
-#if CFG_FIFO_MUTEX
+        tu_fifo_config(&audio->ep_out_ff, ep_out_sw_buf.buf_2, CFG_TUD_AUDIO_FUNC_2_EP_OUT_SW_BUF_SZ, 1, true);
+    #if CFG_FIFO_MUTEX
         tu_fifo_config_mutex(&audio->ep_out_ff, NULL, osal_mutex_create(&ep_out_ff_mutex_rd_2));
-#endif
+    #endif
         break;
-#endif
-#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ > 0
+  #endif
+  #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ > 0
       case 2:
-        tu_fifo_config(&audio->ep_out_ff, audio_ep_out_sw_buf_3, CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ, 1, true);
-#if CFG_FIFO_MUTEX
+        tu_fifo_config(&audio->ep_out_ff, ep_out_sw_buf.buf_3, CFG_TUD_AUDIO_FUNC_3_EP_OUT_SW_BUF_SZ, 1, true);
+    #if CFG_FIFO_MUTEX
         tu_fifo_config_mutex(&audio->ep_out_ff, NULL, osal_mutex_create(&ep_out_ff_mutex_rd_3));
-#endif
+    #endif
         break;
-#endif
+  #endif
     }
-#endif // CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING
+#endif// CFG_TUD_AUDIO_ENABLE_EP_OUT && !CFG_TUD_AUDIO_ENABLE_DECODING
 
-    // Initialize linear buffers
+      // Initialize linear buffers
 #if USE_LINEAR_BUFFER_RX
-    switch (i)
-    {
-#if CFG_TUD_AUDIO_FUNC_1_EP_OUT_SZ_MAX > 0
+    switch (i) {
+  #if CFG_TUD_AUDIO_FUNC_1_EP_OUT_SZ_MAX > 0
       case 0:
-        audio->lin_buf_out = lin_buf_out_1;
+        audio->lin_buf_out = lin_buf_out.buf_1;
         break;
-#endif
-#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SZ_MAX > 0
+  #endif
+  #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SZ_MAX > 0
       case 1:
-        audio->lin_buf_out = lin_buf_out_2;
+        audio->lin_buf_out = lin_buf_out.buf_2;
         break;
-#endif
-#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SZ_MAX > 0
+  #endif
+  #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SZ_MAX > 0
       case 2:
-        audio->lin_buf_out = lin_buf_out_3;
+        audio->lin_buf_out = lin_buf_out.buf_3;
         break;
-#endif
+  #endif
     }
-#endif // USE_LINEAR_BUFFER_TX
+#endif// USE_LINEAR_BUFFER_RX
 
-    // Initialize TX support FIFOs if required
+#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+    switch (i) {
+  #if CFG_TUD_AUDIO_FUNC_1_EP_OUT_SZ_MAX > 0
+      case 0:
+        audio->fb_buf = &fb_ep_buf.buf_1;
+        break;
+  #endif
+  #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_EP_OUT_SZ_MAX > 0
+      case 1:
+        audio->fb_buf = &fb_ep_buf.buf_2;
+        break;
+  #endif
+  #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_EP_OUT_SZ_MAX > 0
+      case 2:
+        audio->fb_buf = &fb_ep_buf.buf_3;
+        break;
+  #endif
+    }
+#endif// CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+
+      // Initialize TX support FIFOs if required
 #if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING
 
-    switch (i)
-    {
-#if CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ > 0
+    switch (i) {
+  #if CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ > 0
       case 0:
         audio->tx_supp_ff = tx_supp_ff_1;
         audio->n_tx_supp_ff = CFG_TUD_AUDIO_FUNC_1_N_TX_SUPP_SW_FIFO;
         audio->tx_supp_ff_sz_max = CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ;
-        for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_1_N_TX_SUPP_SW_FIFO; cnt++)
-        {
+        for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_1_N_TX_SUPP_SW_FIFO; cnt++) {
           tu_fifo_config(&tx_supp_ff_1[cnt], tx_supp_ff_buf_1[cnt], CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ, 1, true);
-#if CFG_FIFO_MUTEX
+    #if CFG_FIFO_MUTEX
           tu_fifo_config_mutex(&tx_supp_ff_1[cnt], osal_mutex_create(&tx_supp_ff_mutex_wr_1[cnt]), NULL);
-#endif
+    #endif
         }
 
         break;
-#endif // CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ > 0
+  #endif// CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ > 0
 
-#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ > 0
+  #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ > 0
       case 1:
         audio->tx_supp_ff = tx_supp_ff_2;
         audio->n_tx_supp_ff = CFG_TUD_AUDIO_FUNC_2_N_TX_SUPP_SW_FIFO;
         audio->tx_supp_ff_sz_max = CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ;
-        for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_2_N_TX_SUPP_SW_FIFO; cnt++)
-        {
+        for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_2_N_TX_SUPP_SW_FIFO; cnt++) {
           tu_fifo_config(&tx_supp_ff_2[cnt], tx_supp_ff_buf_2[cnt], CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ, 1, true);
-#if CFG_FIFO_MUTEX
+    #if CFG_FIFO_MUTEX
           tu_fifo_config_mutex(&tx_supp_ff_2[cnt], osal_mutex_create(&tx_supp_ff_mutex_wr_2[cnt]), NULL);
-#endif
+    #endif
         }
 
         break;
-#endif // CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ > 0
+  #endif// CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ > 0
 
-#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ > 0
+  #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ > 0
       case 2:
         audio->tx_supp_ff = tx_supp_ff_3;
         audio->n_tx_supp_ff = CFG_TUD_AUDIO_FUNC_3_N_TX_SUPP_SW_FIFO;
         audio->tx_supp_ff_sz_max = CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ;
-        for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_3_N_TX_SUPP_SW_FIFO; cnt++)
-        {
+        for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_3_N_TX_SUPP_SW_FIFO; cnt++) {
           tu_fifo_config(&tx_supp_ff_3[cnt], tx_supp_ff_buf_3[cnt], CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ, 1, true);
-#if CFG_FIFO_MUTEX
+    #if CFG_FIFO_MUTEX
           tu_fifo_config_mutex(&tx_supp_ff_3[cnt], osal_mutex_create(&tx_supp_ff_mutex_wr_3[cnt]), NULL);
-#endif
+    #endif
         }
 
         break;
-#endif // CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ > 0
+  #endif// CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ > 0
     }
-#endif // CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING
+#endif// CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING
 
-    // Set encoding parameters for Type_I formats
-#if CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING
-    switch (i)
-    {
-#if CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ > 0
+      // Set encoding parameters for Type_I formats
+#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING
+    switch (i) {
+  #if CFG_TUD_AUDIO_FUNC_1_TX_SUPP_SW_FIFO_SZ > 0
       case 0:
         audio->n_channels_per_ff_tx = CFG_TUD_AUDIO_FUNC_1_CHANNEL_PER_FIFO_TX;
         break;
-#endif
-#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ > 0
+  #endif
+  #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_TX_SUPP_SW_FIFO_SZ > 0
       case 1:
         audio->n_channels_per_ff_tx = CFG_TUD_AUDIO_FUNC_2_CHANNEL_PER_FIFO_TX;
         break;
-#endif
-#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ > 0
+  #endif
+  #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_TX_SUPP_SW_FIFO_SZ > 0
       case 2:
         audio->n_channels_per_ff_tx = CFG_TUD_AUDIO_FUNC_3_CHANNEL_PER_FIFO_TX;
         break;
-#endif
+  #endif
     }
-#endif // CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING
+#endif// CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING
 
-    // Initialize RX support FIFOs if required
+      // Initialize RX support FIFOs if required
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING
 
-    switch (i)
-    {
-#if CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ > 0
+    switch (i) {
+  #if CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ > 0
       case 0:
         audio->rx_supp_ff = rx_supp_ff_1;
         audio->n_rx_supp_ff = CFG_TUD_AUDIO_FUNC_1_N_RX_SUPP_SW_FIFO;
         audio->rx_supp_ff_sz_max = CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ;
-        for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_1_N_RX_SUPP_SW_FIFO; cnt++)
-        {
+        for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_1_N_RX_SUPP_SW_FIFO; cnt++) {
           tu_fifo_config(&rx_supp_ff_1[cnt], rx_supp_ff_buf_1[cnt], CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ, 1, true);
-#if CFG_FIFO_MUTEX
+    #if CFG_FIFO_MUTEX
           tu_fifo_config_mutex(&rx_supp_ff_1[cnt], osal_mutex_create(&rx_supp_ff_mutex_rd_1[cnt]), NULL);
-#endif
+    #endif
         }
 
         break;
-#endif // CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ > 0
+  #endif// CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ > 0
 
-#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ > 0
+  #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ > 0
       case 1:
         audio->rx_supp_ff = rx_supp_ff_2;
         audio->n_rx_supp_ff = CFG_TUD_AUDIO_FUNC_2_N_RX_SUPP_SW_FIFO;
         audio->rx_supp_ff_sz_max = CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ;
-        for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_2_N_RX_SUPP_SW_FIFO; cnt++)
-        {
+        for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_2_N_RX_SUPP_SW_FIFO; cnt++) {
           tu_fifo_config(&rx_supp_ff_2[cnt], rx_supp_ff_buf_2[cnt], CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ, 1, true);
-#if CFG_FIFO_MUTEX
+    #if CFG_FIFO_MUTEX
           tu_fifo_config_mutex(&rx_supp_ff_2[cnt], osal_mutex_create(&rx_supp_ff_mutex_rd_2[cnt]), NULL);
-#endif
+    #endif
         }
 
         break;
-#endif // CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ > 0
+  #endif// CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ > 0
 
-#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ > 0
+  #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ > 0
       case 2:
         audio->rx_supp_ff = rx_supp_ff_3;
         audio->n_rx_supp_ff = CFG_TUD_AUDIO_FUNC_3_N_RX_SUPP_SW_FIFO;
         audio->rx_supp_ff_sz_max = CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ;
-        for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_3_N_RX_SUPP_SW_FIFO; cnt++)
-        {
+        for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO_FUNC_3_N_RX_SUPP_SW_FIFO; cnt++) {
           tu_fifo_config(&rx_supp_ff_3[cnt], rx_supp_ff_buf_3[cnt], CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ, 1, true);
-#if CFG_FIFO_MUTEX
+    #if CFG_FIFO_MUTEX
           tu_fifo_config_mutex(&rx_supp_ff_3[cnt], osal_mutex_create(&rx_supp_ff_mutex_rd_3[cnt]), NULL);
-#endif
+    #endif
         }
 
         break;
-#endif // CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ > 0
+  #endif// CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ > 0
     }
-#endif // CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING
+#endif// CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING
 
-    // Set encoding parameters for Type_I formats
+      // Set encoding parameters for Type_I formats
 #if CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING
-    switch (i)
-    {
-#if CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ > 0
+    switch (i) {
+  #if CFG_TUD_AUDIO_FUNC_1_RX_SUPP_SW_FIFO_SZ > 0
       case 0:
         audio->n_channels_per_ff_rx = CFG_TUD_AUDIO_FUNC_1_CHANNEL_PER_FIFO_RX;
         break;
-#endif
-#if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ > 0
+  #endif
+  #if CFG_TUD_AUDIO > 1 && CFG_TUD_AUDIO_FUNC_2_RX_SUPP_SW_FIFO_SZ > 0
       case 1:
         audio->n_channels_per_ff_rx = CFG_TUD_AUDIO_FUNC_2_CHANNEL_PER_FIFO_RX;
         break;
-#endif
-#if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ > 0
+  #endif
+  #if CFG_TUD_AUDIO > 2 && CFG_TUD_AUDIO_FUNC_3_RX_SUPP_SW_FIFO_SZ > 0
       case 2:
         audio->n_channels_per_ff_rx = CFG_TUD_AUDIO_FUNC_3_CHANNEL_PER_FIFO_RX;
         break;
-#endif
+  #endif
     }
-#endif // CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING
+#endif// CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING
   }
 }
 
-void audiod_reset(uint8_t rhport)
-{
+bool audiod_deinit(void) {
+  return false;// TODO not implemented yet
+}
+
+void audiod_reset(uint8_t rhport) {
   (void) rhport;
 
-  for(uint8_t i=0; i<CFG_TUD_AUDIO; i++)
-  {
-    audiod_function_t* audio = &_audiod_fct[i];
+  for (uint8_t i = 0; i < CFG_TUD_AUDIO; i++) {
+    audiod_function_t *audio = &_audiod_fct[i];
     tu_memclr(audio, ITF_MEM_RESET_SIZE);
 
 #if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_ENCODING
@@ -1413,35 +1553,32 @@ void audiod_reset(uint8_t rhport)
 #endif
 
 #if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_ENCODING
-    for (uint8_t cnt = 0; cnt < audio->n_tx_supp_ff; cnt++)
-    {
+    for (uint8_t cnt = 0; cnt < audio->n_tx_supp_ff; cnt++) {
       tu_fifo_clear(&audio->tx_supp_ff[cnt]);
     }
 #endif
 
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING
-    for (uint8_t cnt = 0; cnt < audio->n_rx_supp_ff; cnt++)
-    {
+    for (uint8_t cnt = 0; cnt < audio->n_rx_supp_ff; cnt++) {
       tu_fifo_clear(&audio->rx_supp_ff[cnt]);
     }
 #endif
   }
 }
 
-uint16_t audiod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len)
-{
+uint16_t audiod_open(uint8_t rhport, tusb_desc_interface_t const *itf_desc, uint16_t max_len) {
   (void) max_len;
 
-  TU_VERIFY ( TUSB_CLASS_AUDIO  == itf_desc->bInterfaceClass &&
-              AUDIO_SUBCLASS_CONTROL    == itf_desc->bInterfaceSubClass);
+  TU_VERIFY(TUSB_CLASS_AUDIO == itf_desc->bInterfaceClass &&
+            AUDIO_SUBCLASS_CONTROL == itf_desc->bInterfaceSubClass);
 
   // Verify version is correct - this check can be omitted
   TU_VERIFY(itf_desc->bInterfaceProtocol == AUDIO_INT_PROTOCOL_CODE_V2);
 
-  // Verify interrupt control EP is enabled if demanded by descriptor - this should be best some static check however - this check can be omitted
-  if (itf_desc->bNumEndpoints == 1) // 0 or 1 EPs are allowed
-  {
-    TU_VERIFY(CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN > 0);
+  // Verify interrupt control EP is enabled if demanded by descriptor
+  TU_ASSERT(itf_desc->bNumEndpoints <= 1);// 0 or 1 EPs are allowed
+  if (itf_desc->bNumEndpoints == 1) {
+    TU_ASSERT(CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP);
   }
 
   // Alternate setting MUST be zero - this check can be omitted
@@ -1449,16 +1586,13 @@ uint16_t audiod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uin
 
   // Find available audio driver interface
   uint8_t i;
-  for (i = 0; i < CFG_TUD_AUDIO; i++)
-  {
-    if (!_audiod_fct[i].p_desc)
-    {
-      _audiod_fct[i].p_desc = (uint8_t const *)itf_desc;    // Save pointer to AC descriptor which is by specification always the first one
+  for (i = 0; i < CFG_TUD_AUDIO; i++) {
+    if (!_audiod_fct[i].p_desc) {
+      _audiod_fct[i].p_desc = (uint8_t const *) itf_desc;// Save pointer to AC descriptor which is by specification always the first one
       _audiod_fct[i].rhport = rhport;
 
       // Setup descriptor lengths
-      switch (i)
-      {
+      switch (i) {
         case 0:
           _audiod_fct[i].desc_length = CFG_TUD_AUDIO_FUNC_1_DESC_LEN;
           break;
@@ -1474,21 +1608,136 @@ uint16_t audiod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uin
 #endif
       }
 
+#ifdef TUP_DCD_EDPT_ISO_ALLOC
+      {
+  #if CFG_TUD_AUDIO_ENABLE_EP_IN
+        uint8_t ep_in = 0;
+        uint16_t ep_in_size = 0;
+  #endif
+
+  #if CFG_TUD_AUDIO_ENABLE_EP_OUT
+        uint8_t ep_out = 0;
+        uint16_t ep_out_size = 0;
+  #endif
+
+  #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+        uint8_t ep_fb = 0;
+  #endif
+        uint8_t const *p_desc = _audiod_fct[i].p_desc;
+        uint8_t const *p_desc_end = p_desc + _audiod_fct[i].desc_length - TUD_AUDIO_DESC_IAD_LEN;
+        // Condition modified from p_desc < p_desc_end to prevent gcc>=12 strict-overflow warning
+        while (p_desc_end - p_desc > 0) {
+          if (tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT) {
+            tusb_desc_endpoint_t const *desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+            if (desc_ep->bmAttributes.xfer == TUSB_XFER_ISOCHRONOUS) {
+  #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+              // Explicit feedback EP
+              if (desc_ep->bmAttributes.usage == 1) {
+                ep_fb = desc_ep->bEndpointAddress;
+              }
+  #endif
+              // Data EP
+              if (desc_ep->bmAttributes.usage == 0) {
+                if (tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN) {
+  #if CFG_TUD_AUDIO_ENABLE_EP_IN
+                  ep_in = desc_ep->bEndpointAddress;
+                  ep_in_size = TU_MAX(tu_edpt_packet_size(desc_ep), ep_in_size);
+  #endif
+                } else {
+  #if CFG_TUD_AUDIO_ENABLE_EP_OUT
+                  ep_out = desc_ep->bEndpointAddress;
+                  ep_out_size = TU_MAX(tu_edpt_packet_size(desc_ep), ep_out_size);
+  #endif
+                }
+              }
+            }
+          }
+
+          p_desc = tu_desc_next(p_desc);
+        }
+
+  #if CFG_TUD_AUDIO_ENABLE_EP_IN
+        if (ep_in) {
+          usbd_edpt_iso_alloc(rhport, ep_in, ep_in_size);
+        }
+  #endif
+
+  #if CFG_TUD_AUDIO_ENABLE_EP_OUT
+        if (ep_out) {
+          usbd_edpt_iso_alloc(rhport, ep_out, ep_out_size);
+        }
+  #endif
+
+  #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+        if (ep_fb) {
+          usbd_edpt_iso_alloc(rhport, ep_fb, 4);
+        }
+  #endif
+      }
+#endif// TUP_DCD_EDPT_ISO_ALLOC
+
+#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
+      {
+        uint8_t const *p_desc = _audiod_fct[i].p_desc;
+        uint8_t const *p_desc_end = p_desc + _audiod_fct[i].desc_length - TUD_AUDIO_DESC_IAD_LEN;
+        // Condition modified from p_desc < p_desc_end to prevent gcc>=12 strict-overflow warning
+        while (p_desc_end - p_desc > 0) {
+          if (tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT) {
+            tusb_desc_endpoint_t const *desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+            if (desc_ep->bmAttributes.xfer == TUSB_XFER_ISOCHRONOUS) {
+              if (desc_ep->bmAttributes.usage == 0) {
+                if (tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN) {
+                  _audiod_fct[i].interval_tx = desc_ep->bInterval;
+                }
+              }
+            }
+          } else if (tu_desc_type(p_desc) == TUSB_DESC_CS_INTERFACE && tu_desc_subtype(p_desc) == AUDIO_CS_AC_INTERFACE_OUTPUT_TERMINAL) {
+            if (tu_unaligned_read16(p_desc + 4) == AUDIO_TERM_TYPE_USB_STREAMING) {
+              _audiod_fct[i].bclock_id_tx = p_desc[8];
+            }
+          }
+          p_desc = tu_desc_next(p_desc);
+        }
+      }
+#endif// CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
+
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+      {
+        uint8_t const *p_desc = _audiod_fct[i].p_desc;
+        uint8_t const *p_desc_end = p_desc + _audiod_fct[i].desc_length - TUD_AUDIO_DESC_IAD_LEN;
+        // Condition modified from p_desc < p_desc_end to prevent gcc>=12 strict-overflow warning
+        while (p_desc_end - p_desc > 0) {
+          // For each endpoint
+          if (tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT) {
+            tusb_desc_endpoint_t const *desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+            uint8_t const ep_addr = desc_ep->bEndpointAddress;
+            // If endpoint is input-direction and interrupt-type
+            if (tu_edpt_dir(ep_addr) == TUSB_DIR_IN && desc_ep->bmAttributes.xfer == TUSB_XFER_INTERRUPT) {
+              // Store endpoint number and open endpoint
+              _audiod_fct[i].ep_int = ep_addr;
+              TU_ASSERT(usbd_edpt_open(_audiod_fct[i].rhport, desc_ep));
+            }
+          }
+          p_desc = tu_desc_next(p_desc);
+        }
+      }
+#endif
+
+      _audiod_fct[i].mounted = true;
       break;
     }
   }
 
   // Verify we found a free one
-  TU_ASSERT( i < CFG_TUD_AUDIO );
+  TU_ASSERT(i < CFG_TUD_AUDIO);
 
   // This is all we need so far - the EPs are setup by a later set_interface request (as per UAC2 specification)
-  uint16_t drv_len = _audiod_fct[i].desc_length - TUD_AUDIO_DESC_IAD_LEN;    // - TUD_AUDIO_DESC_IAD_LEN since tinyUSB already handles the IAD descriptor
+  uint16_t drv_len = _audiod_fct[i].desc_length - TUD_AUDIO_DESC_IAD_LEN;// - TUD_AUDIO_DESC_IAD_LEN since tinyUSB already handles the IAD descriptor
 
   return drv_len;
 }
 
-static bool audiod_get_interface(uint8_t rhport, tusb_control_request_t const * p_request)
-{
+static bool audiod_get_interface(uint8_t rhport, tusb_control_request_t const *p_request) {
   uint8_t const itf = tu_u16_low(p_request->wIndex);
 
   // Find index of audio streaming interface
@@ -1503,8 +1752,7 @@ static bool audiod_get_interface(uint8_t rhport, tusb_control_request_t const *
   return true;
 }
 
-static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const * p_request)
-{
+static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *p_request) {
   (void) rhport;
 
   // Here we need to do the following:
@@ -1528,62 +1776,69 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
   uint8_t const *p_desc;
   TU_VERIFY(audiod_get_AS_interface_index_global(itf, &func_id, &idxItf, &p_desc));
 
-  audiod_function_t* audio = &_audiod_fct[func_id];
+  audiod_function_t *audio = &_audiod_fct[func_id];
 
-  // Look if there is an EP to be closed - for this driver, there are only 3 possible EPs which may be closed (only AS related EPs can be closed, AC EP (if present) is always open)
+// Look if there is an EP to be closed - for this driver, there are only 3 possible EPs which may be closed (only AS related EPs can be closed, AC EP (if present) is always open)
 #if CFG_TUD_AUDIO_ENABLE_EP_IN
-  if (audio->ep_in_as_intf_num == itf)
-  {
+  if (audio->ep_in_as_intf_num == itf) {
     audio->ep_in_as_intf_num = 0;
+  #ifndef TUP_DCD_EDPT_ISO_ALLOC
     usbd_edpt_close(rhport, audio->ep_in);
+  #endif
 
     // Clear FIFOs, since data is no longer valid
-#if !CFG_TUD_AUDIO_ENABLE_ENCODING
+  #if !CFG_TUD_AUDIO_ENABLE_ENCODING
     tu_fifo_clear(&audio->ep_in_ff);
-#else
-    for (uint8_t cnt = 0; cnt < audio->n_tx_supp_ff; cnt++)
-    {
+  #else
+    for (uint8_t cnt = 0; cnt < audio->n_tx_supp_ff; cnt++) {
       tu_fifo_clear(&audio->tx_supp_ff[cnt]);
     }
-#endif
+  #endif
 
     // Invoke callback - can be used to stop data sampling
-    if (tud_audio_set_itf_close_EP_cb) TU_VERIFY(tud_audio_set_itf_close_EP_cb(rhport, p_request));
+    TU_VERIFY(tud_audio_set_itf_close_EP_cb(rhport, p_request));
 
-    audio->ep_in = 0;                           // Necessary?
+    audio->ep_in = 0;// Necessary?
 
+  #if CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
+    audio->packet_sz_tx[0] = 0;
+    audio->packet_sz_tx[1] = 0;
+    audio->packet_sz_tx[2] = 0;
+  #endif
   }
-#endif
+#endif// CFG_TUD_AUDIO_ENABLE_EP_IN
 
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT
-  if (audio->ep_out_as_intf_num == itf)
-  {
+  if (audio->ep_out_as_intf_num == itf) {
     audio->ep_out_as_intf_num = 0;
+  #ifndef TUP_DCD_EDPT_ISO_ALLOC
     usbd_edpt_close(rhport, audio->ep_out);
+  #endif
 
     // Clear FIFOs, since data is no longer valid
-#if !CFG_TUD_AUDIO_ENABLE_DECODING
+  #if !CFG_TUD_AUDIO_ENABLE_DECODING
     tu_fifo_clear(&audio->ep_out_ff);
-#else
-    for (uint8_t cnt = 0; cnt < audio->n_rx_supp_ff; cnt++)
-    {
+  #else
+    for (uint8_t cnt = 0; cnt < audio->n_rx_supp_ff; cnt++) {
       tu_fifo_clear(&audio->rx_supp_ff[cnt]);
     }
-#endif
+  #endif
 
     // Invoke callback - can be used to stop data sampling
-    if (tud_audio_set_itf_close_EP_cb) TU_VERIFY(tud_audio_set_itf_close_EP_cb(rhport, p_request));
+    TU_VERIFY(tud_audio_set_itf_close_EP_cb(rhport, p_request));
 
-    audio->ep_out = 0;                          // Necessary?
+    audio->ep_out = 0;// Necessary?
 
     // Close corresponding feedback EP
-#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+  #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+    #ifndef TUP_DCD_EDPT_ISO_ALLOC
     usbd_edpt_close(rhport, audio->ep_fb);
+    #endif
     audio->ep_fb = 0;
     tu_memclr(&audio->feedback, sizeof(audio->feedback));
-#endif
+  #endif
   }
-#endif
+#endif// CFG_TUD_AUDIO_ENABLE_EP_OUT
 
   // Save current alternative interface setting
   audio->alt_setting[idxItf] = alt;
@@ -1593,30 +1848,31 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
   uint8_t const *p_desc_end = audio->p_desc + audio->desc_length - TUD_AUDIO_DESC_IAD_LEN;
 
   // p_desc starts at required interface with alternate setting zero
-  while (p_desc < p_desc_end)
-  {
+  // Condition modified from p_desc < p_desc_end to prevent gcc>=12 strict-overflow warning
+  while (p_desc_end - p_desc > 0) {
     // Find correct interface
-    if (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE && ((tusb_desc_interface_t const * )p_desc)->bInterfaceNumber == itf && ((tusb_desc_interface_t const * )p_desc)->bAlternateSetting == alt)
-    {
-#if CFG_TUD_AUDIO_ENABLE_ENCODING || CFG_TUD_AUDIO_ENABLE_DECODING
-      uint8_t const * p_desc_parse_for_params = p_desc;
+    if (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE && ((tusb_desc_interface_t const *) p_desc)->bInterfaceNumber == itf && ((tusb_desc_interface_t const *) p_desc)->bAlternateSetting == alt) {
+#if (CFG_TUD_AUDIO_ENABLE_EP_IN && (CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL || CFG_TUD_AUDIO_ENABLE_ENCODING)) || (CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING)
+      uint8_t const *p_desc_parse_for_params = p_desc;
 #endif
       // From this point forward follow the EP descriptors associated to the current alternate setting interface - Open EPs if necessary
-      uint8_t foundEPs = 0, nEps = ((tusb_desc_interface_t const * )p_desc)->bNumEndpoints;
-      while (foundEPs < nEps && p_desc < p_desc_end)
-      {
-        if (tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT)
-        {
-          tusb_desc_endpoint_t const* desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+      uint8_t foundEPs = 0, nEps = ((tusb_desc_interface_t const *) p_desc)->bNumEndpoints;
+      // Condition modified from p_desc < p_desc_end to prevent gcc>=12 strict-overflow warning
+      while (foundEPs < nEps && (p_desc_end - p_desc > 0)) {
+        if (tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT) {
+          tusb_desc_endpoint_t const *desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+#ifdef TUP_DCD_EDPT_ISO_ALLOC
+          TU_ASSERT(usbd_edpt_iso_activate(rhport, desc_ep));
+#else
           TU_ASSERT(usbd_edpt_open(rhport, desc_ep));
-
+#endif
           uint8_t const ep_addr = desc_ep->bEndpointAddress;
 
           //TODO: We need to set EP non busy since this is not taken care of right now in ep_close() - THIS IS A WORKAROUND!
           usbd_edpt_clear_stall(rhport, ep_addr);
 
 #if CFG_TUD_AUDIO_ENABLE_EP_IN
-          if (tu_edpt_dir(ep_addr) == TUSB_DIR_IN && desc_ep->bmAttributes.usage == 0x00)   // Check if usage is data EP
+          if (tu_edpt_dir(ep_addr) == TUSB_DIR_IN && desc_ep->bmAttributes.usage == 0x00)// Check if usage is data EP
           {
             // Save address
             audio->ep_in = ep_addr;
@@ -1624,71 +1880,65 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
             audio->ep_in_sz = tu_edpt_packet_size(desc_ep);
 
             // If software encoding is enabled, parse for the corresponding parameters - doing this here means only AS interfaces with EPs get scanned for parameters
-#if CFG_TUD_AUDIO_ENABLE_ENCODING
+  #if CFG_TUD_AUDIO_ENABLE_ENCODING || CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
             audiod_parse_for_AS_params(audio, p_desc_parse_for_params, p_desc_end, itf);
 
-            // Reconfigure size of support FIFOs - this is necessary to avoid samples to get split in case of a wrap
-#if CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING
-            const uint16_t active_fifo_depth = (uint16_t) ((audio->tx_supp_ff_sz_max / audio->n_bytes_per_sampe_tx) * audio->n_bytes_per_sampe_tx);
-            for (uint8_t cnt = 0; cnt < audio->n_tx_supp_ff; cnt++)
-            {
+              // Reconfigure size of support FIFOs - this is necessary to avoid samples to get split in case of a wrap
+    #if CFG_TUD_AUDIO_ENABLE_ENCODING && CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING
+            const uint16_t active_fifo_depth = (uint16_t) ((audio->tx_supp_ff_sz_max / (audio->n_channels_per_ff_tx * audio->n_bytes_per_sample_tx)) * (audio->n_channels_per_ff_tx * audio->n_bytes_per_sample_tx));
+            for (uint8_t cnt = 0; cnt < audio->n_tx_supp_ff; cnt++) {
               tu_fifo_config(&audio->tx_supp_ff[cnt], audio->tx_supp_ff[cnt].buffer, active_fifo_depth, 1, true);
             }
             audio->n_ff_used_tx = audio->n_channels_tx / audio->n_channels_per_ff_tx;
-            TU_ASSERT( audio->n_ff_used_tx <= audio->n_tx_supp_ff );
-#endif
-
-#endif
+            TU_ASSERT(audio->n_ff_used_tx <= audio->n_tx_supp_ff);
+    #endif
+  #endif
 
             // Schedule first transmit if alternate interface is not zero i.e. streaming is disabled - in case no sample data is available a ZLP is loaded
             // It is necessary to trigger this here since the refill is done with an RX FIFO empty interrupt which can only trigger if something was in there
             TU_VERIFY(audiod_tx_done_cb(rhport, &_audiod_fct[func_id]));
           }
-#endif // CFG_TUD_AUDIO_ENABLE_EP_IN
+#endif// CFG_TUD_AUDIO_ENABLE_EP_IN
 
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT
 
-          if (tu_edpt_dir(ep_addr) == TUSB_DIR_OUT)     // Checking usage not necessary
+          if (tu_edpt_dir(ep_addr) == TUSB_DIR_OUT)// Checking usage not necessary
           {
             // Save address
             audio->ep_out = ep_addr;
             audio->ep_out_as_intf_num = itf;
             audio->ep_out_sz = tu_edpt_packet_size(desc_ep);
 
-#if CFG_TUD_AUDIO_ENABLE_DECODING
+  #if CFG_TUD_AUDIO_ENABLE_DECODING
             audiod_parse_for_AS_params(audio, p_desc_parse_for_params, p_desc_end, itf);
 
-            // Reconfigure size of support FIFOs - this is necessary to avoid samples to get split in case of a wrap
-#if CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING
-            const uint16_t active_fifo_depth = (audio->rx_supp_ff_sz_max / audio->n_bytes_per_sampe_rx) * audio->n_bytes_per_sampe_rx;
-            for (uint8_t cnt = 0; cnt < audio->n_rx_supp_ff; cnt++)
-            {
+              // Reconfigure size of support FIFOs - this is necessary to avoid samples to get split in case of a wrap
+    #if CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING
+            const uint16_t active_fifo_depth = (audio->rx_supp_ff_sz_max / audio->n_bytes_per_sample_rx) * audio->n_bytes_per_sample_rx;
+            for (uint8_t cnt = 0; cnt < audio->n_rx_supp_ff; cnt++) {
               tu_fifo_config(&audio->rx_supp_ff[cnt], audio->rx_supp_ff[cnt].buffer, active_fifo_depth, 1, true);
             }
             audio->n_ff_used_rx = audio->n_channels_rx / audio->n_channels_per_ff_rx;
-            TU_ASSERT( audio->n_ff_used_rx <= audio->n_rx_supp_ff );
-#endif
-#endif
+            TU_ASSERT(audio->n_ff_used_rx <= audio->n_rx_supp_ff);
+    #endif
+  #endif
 
             // Prepare for incoming data
-#if USE_LINEAR_BUFFER_RX
+  #if USE_LINEAR_BUFFER_RX
             TU_VERIFY(usbd_edpt_xfer(rhport, audio->ep_out, audio->lin_buf_out, audio->ep_out_sz), false);
-#else
+  #else
             TU_VERIFY(usbd_edpt_xfer_fifo(rhport, audio->ep_out, &audio->ep_out_ff, audio->ep_out_sz), false);
-#endif
+  #endif
           }
 
-#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
-          if (tu_edpt_dir(ep_addr) == TUSB_DIR_IN && desc_ep->bmAttributes.usage == 1)   // Check if usage is explicit data feedback
+  #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+          if (tu_edpt_dir(ep_addr) == TUSB_DIR_IN && desc_ep->bmAttributes.usage == 1)// Check if usage is explicit data feedback
           {
             audio->ep_fb = ep_addr;
-            audio->feedback.frame_shift = desc_ep->bInterval -1;
-
-            // Enable SOF interrupt if callback is implemented
-            if (tud_audio_feedback_interval_isr) usbd_sof_enable(rhport, true);
+            audio->feedback.frame_shift = desc_ep->bInterval - 1;
           }
-#endif
-#endif // CFG_TUD_AUDIO_ENABLE_EP_OUT
+  #endif
+#endif// CFG_TUD_AUDIO_ENABLE_EP_OUT
 
           foundEPs += 1;
         }
@@ -1698,47 +1948,59 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
       TU_VERIFY(foundEPs == nEps);
 
       // Invoke one callback for a final set interface
-      if (tud_audio_set_itf_cb) TU_VERIFY(tud_audio_set_itf_cb(rhport, p_request));
+      TU_VERIFY(tud_audio_set_itf_cb(rhport, p_request));
 
 #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
-      // Prepare feedback computation if callback is available
-      if (tud_audio_feedback_params_cb)
-      {
+      // Prepare feedback computation if endpoint is available
+      if (audio->ep_fb != 0) {
         audio_feedback_params_t fb_param;
 
         tud_audio_feedback_params_cb(func_id, alt, &fb_param);
         audio->feedback.compute_method = fb_param.method;
 
+        if (TUSB_SPEED_FULL == tud_speed_get())
+          audio->feedback.format_correction = tud_audio_feedback_format_correction_cb(func_id);
+
         // Minimal/Maximum value in 16.16 format for full speed (1ms per frame) or high speed (125 us per frame)
-        uint32_t const frame_div  = (TUSB_SPEED_FULL == tud_speed_get()) ? 1000 : 8000;
-        audio->feedback.min_value = (fb_param.sample_freq/frame_div - 1) << 16;
-        audio->feedback.max_value = (fb_param.sample_freq/frame_div + 1) << 16;
+        uint32_t const frame_div = (TUSB_SPEED_FULL == tud_speed_get()) ? 1000 : 8000;
+        audio->feedback.min_value = ((fb_param.sample_freq - 1) / frame_div) << 16;
+        audio->feedback.max_value = (fb_param.sample_freq / frame_div + 1) << 16;
 
-        switch(fb_param.method)
-        {
+        switch (fb_param.method) {
           case AUDIO_FEEDBACK_METHOD_FREQUENCY_FIXED:
           case AUDIO_FEEDBACK_METHOD_FREQUENCY_FLOAT:
           case AUDIO_FEEDBACK_METHOD_FREQUENCY_POWER_OF_2:
-            set_fb_params_freq(audio, fb_param.sample_freq, fb_param.frequency.mclk_freq);
-          break;
+            audiod_set_fb_params_freq(audio, fb_param.sample_freq, fb_param.frequency.mclk_freq);
+            break;
 
-          #if 0 // implement later
-          case AUDIO_FEEDBACK_METHOD_FIFO_COUNT:
-          {
-            uint64_t fb64 = ((uint64_t) fb_param.sample_freq) << 16;
-            audio->feedback.compute.fifo_count.nominal_value = (uint32_t) (fb64 / frame_div);
-            audio->feedback.compute.fifo_count.threshold_bytes = fb_param.fifo_count.threshold_bytes;
-
-            tud_audio_fb_set(audio->feedback.compute.fifo_count.nominal_value);
-          }
-          break;
-          #endif
+          case AUDIO_FEEDBACK_METHOD_FIFO_COUNT: {
+            // Initialize the threshold level to half filled
+            uint16_t fifo_lvl_thr;
+  #if CFG_TUD_AUDIO_ENABLE_DECODING
+            fifo_lvl_thr = tu_fifo_depth(&audio->rx_supp_ff[0]) / 2;
+  #else
+            fifo_lvl_thr = tu_fifo_depth(&audio->ep_out_ff) / 2;
+  #endif
+            audio->feedback.compute.fifo_count.fifo_lvl_thr = fifo_lvl_thr;
+            audio->feedback.compute.fifo_count.fifo_lvl_avg = ((uint32_t) fifo_lvl_thr) << 16;
+            // Avoid 64bit division
+            uint32_t nominal = ((fb_param.sample_freq / 100) << 16) / (frame_div / 100);
+            audio->feedback.compute.fifo_count.nom_value = nominal;
+            audio->feedback.compute.fifo_count.rate_const[0] = (uint16_t) ((audio->feedback.max_value - nominal) / fifo_lvl_thr);
+            audio->feedback.compute.fifo_count.rate_const[1] = (uint16_t) ((nominal - audio->feedback.min_value) / fifo_lvl_thr);
+            // On HS feedback is more sensitive since packet size can vary every MSOF, could cause instability
+            if (tud_speed_get() == TUSB_SPEED_HIGH) {
+              audio->feedback.compute.fifo_count.rate_const[0] /= 8;
+              audio->feedback.compute.fifo_count.rate_const[1] /= 8;
+            }
+          } break;
 
           // nothing to do
-          default: break;
+          default:
+            break;
         }
       }
-#endif
+#endif// CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
 
       // We are done - abort loop
       break;
@@ -1750,16 +2012,21 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
 
 #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
   // Disable SOF interrupt if no driver has any enabled feedback EP
-  bool disable = true;
-  for(uint8_t i=0; i < CFG_TUD_AUDIO; i++)
-  {
-    if (_audiod_fct[i].ep_fb != 0)
-    {
-      disable = false;
+  bool enable_sof = false;
+  for (uint8_t i = 0; i < CFG_TUD_AUDIO; i++) {
+    if (_audiod_fct[i].ep_fb != 0 &&
+        (_audiod_fct[i].feedback.compute_method == AUDIO_FEEDBACK_METHOD_FREQUENCY_FIXED ||
+         _audiod_fct[i].feedback.compute_method == AUDIO_FEEDBACK_METHOD_FREQUENCY_FLOAT ||
+         _audiod_fct[i].feedback.compute_method == AUDIO_FEEDBACK_METHOD_FREQUENCY_POWER_OF_2)) {
+      enable_sof = true;
       break;
     }
   }
-  if (disable) usbd_sof_enable(rhport, false);
+  usbd_sof_enable(rhport, SOF_CONSUMER_AUDIO, enable_sof);
+#endif
+
+#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
+  audiod_calc_tx_packet_sz(audio);
 #endif
 
   tud_control_status(rhport, p_request);
@@ -1769,76 +2036,51 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
 
 // Invoked when class request DATA stage is finished.
 // return false to stall control EP (e.g Host send non-sense DATA)
-static bool audiod_control_complete(uint8_t rhport, tusb_control_request_t const * p_request)
-{
+static bool audiod_control_complete(uint8_t rhport, tusb_control_request_t const *p_request) {
   // Handle audio class specific set requests
-  if(p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS && p_request->bmRequestType_bit.direction == TUSB_DIR_OUT)
-  {
+  if (p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS && p_request->bmRequestType_bit.direction == TUSB_DIR_OUT) {
     uint8_t func_id;
 
-    switch (p_request->bmRequestType_bit.recipient)
-    {
-      case TUSB_REQ_RCPT_INTERFACE:
-      {
+    switch (p_request->bmRequestType_bit.recipient) {
+      case TUSB_REQ_RCPT_INTERFACE: {
         uint8_t itf = TU_U16_LOW(p_request->wIndex);
         uint8_t entityID = TU_U16_HIGH(p_request->wIndex);
 
-        if (entityID != 0)
-        {
-          if (tud_audio_set_req_entity_cb)
-          {
-            // Check if entity is present and get corresponding driver index
-            TU_VERIFY(audiod_verify_entity_exists(itf, entityID, &func_id));
+        if (entityID != 0) {
+          // Check if entity is present and get corresponding driver index
+          TU_VERIFY(audiod_verify_entity_exists(itf, entityID, &func_id));
 
-            // Invoke callback
-            return tud_audio_set_req_entity_cb(rhport, p_request, _audiod_fct[func_id].ctrl_buf);
-          }
-          else
-          {
-            TU_LOG2("  No entity set request callback available!\r\n");
-            return false;     // In case no callback function is present or request can not be conducted we stall it
+#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
+          uint8_t ctrlSel = TU_U16_HIGH(p_request->wValue);
+          if (_audiod_fct[func_id].bclock_id_tx == entityID && ctrlSel == AUDIO_CS_CTRL_SAM_FREQ && p_request->bRequest == AUDIO_CS_REQ_CUR) {
+            _audiod_fct[func_id].sample_rate_tx = tu_unaligned_read32(_audiod_fct[func_id].ctrl_buf);
           }
-        }
-        else
-        {
-          if (tud_audio_set_req_itf_cb)
-          {
-            // Find index of audio driver structure and verify interface really exists
-            TU_VERIFY(audiod_verify_itf_exists(itf, &func_id));
+#endif
 
-            // Invoke callback
-            return tud_audio_set_req_itf_cb(rhport, p_request, _audiod_fct[func_id].ctrl_buf);
-          }
-          else
-          {
-            TU_LOG2("  No interface set request callback available!\r\n");
-            return false;     // In case no callback function is present or request can not be conducted we stall it
-          }
+          // Invoke callback
+          return tud_audio_set_req_entity_cb(rhport, p_request, _audiod_fct[func_id].ctrl_buf);
+        } else {
+          // Find index of audio driver structure and verify interface really exists
+          TU_VERIFY(audiod_verify_itf_exists(itf, &func_id));
+
+          // Invoke callback
+          return tud_audio_set_req_itf_cb(rhport, p_request, _audiod_fct[func_id].ctrl_buf);
         }
-      }
-      break;
+      } break;
 
-      case TUSB_REQ_RCPT_ENDPOINT:
-      {
+      case TUSB_REQ_RCPT_ENDPOINT: {
         uint8_t ep = TU_U16_LOW(p_request->wIndex);
 
-        if (tud_audio_set_req_ep_cb)
-        {
-          // Check if entity is present and get corresponding driver index
-          TU_VERIFY(audiod_verify_ep_exists(ep, &func_id));
+        // Check if entity is present and get corresponding driver index
+        TU_VERIFY(audiod_verify_ep_exists(ep, &func_id));
 
-          // Invoke callback
-          return tud_audio_set_req_ep_cb(rhport, p_request, _audiod_fct[func_id].ctrl_buf);
-        }
-        else
-        {
-          TU_LOG2("  No EP set request callback available!\r\n");
-          return false;   // In case no callback function is present or request can not be conducted we stall it
-        }
-      }
-      break;
+        // Invoke callback
+        return tud_audio_set_req_ep_cb(rhport, p_request, _audiod_fct[func_id].ctrl_buf);
+      } break;
       // Unknown/Unsupported recipient
-      default: TU_BREAKPOINT(); return false;
+      default:
+        TU_BREAKPOINT();
+        return false;
     }
   }
   return true;
@@ -1846,106 +2088,75 @@ static bool audiod_control_complete(uint8_t rhport, tusb_control_request_t const
 
 // Handle class control request
 // return false to stall control endpoint (e.g unsupported request)
-static bool audiod_control_request(uint8_t rhport, tusb_control_request_t const * p_request)
-{
+static bool audiod_control_request(uint8_t rhport, tusb_control_request_t const *p_request) {
   (void) rhport;
 
   // Handle standard requests - standard set requests usually have no data stage so we also handle set requests here
-  if (p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD)
-  {
-    switch (p_request->bRequest)
-    {
+  if (p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD) {
+    switch (p_request->bRequest) {
       case TUSB_REQ_GET_INTERFACE:
         return audiod_get_interface(rhport, p_request);
 
       case TUSB_REQ_SET_INTERFACE:
         return audiod_set_interface(rhport, p_request);
 
-        // Unknown/Unsupported request
-      default: TU_BREAKPOINT(); return false;
+      case TUSB_REQ_CLEAR_FEATURE:
+        return true;
+
+      // Unknown/Unsupported request
+      default:
+        TU_BREAKPOINT();
+        return false;
     }
   }
 
   // Handle class requests
-  if (p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS)
-  {
+  if (p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS) {
     uint8_t itf = TU_U16_LOW(p_request->wIndex);
     uint8_t func_id;
 
     // Conduct checks which depend on the recipient
-    switch (p_request->bmRequestType_bit.recipient)
-    {
-      case TUSB_REQ_RCPT_INTERFACE:
-      {
+    switch (p_request->bmRequestType_bit.recipient) {
+      case TUSB_REQ_RCPT_INTERFACE: {
         uint8_t entityID = TU_U16_HIGH(p_request->wIndex);
 
         // Verify if entity is present
-        if (entityID != 0)
-        {
+        if (entityID != 0) {
           // Find index of audio driver structure and verify entity really exists
           TU_VERIFY(audiod_verify_entity_exists(itf, entityID, &func_id));
 
           // In case we got a get request invoke callback - callback needs to answer as defined in UAC2 specification page 89 - 5. Requests
-          if (p_request->bmRequestType_bit.direction == TUSB_DIR_IN)
-          {
-            if (tud_audio_get_req_entity_cb)
-            {
-              return tud_audio_get_req_entity_cb(rhport, p_request);
-            }
-            else
-            {
-              TU_LOG2("  No entity get request callback available!\r\n");
-              return false;   // Stall
-            }
+          if (p_request->bmRequestType_bit.direction == TUSB_DIR_IN) {
+            return tud_audio_get_req_entity_cb(rhport, p_request);
           }
-        }
-        else
-        {
+        } else {
           // Find index of audio driver structure and verify interface really exists
           TU_VERIFY(audiod_verify_itf_exists(itf, &func_id));
 
           // In case we got a get request invoke callback - callback needs to answer as defined in UAC2 specification page 89 - 5. Requests
-          if (p_request->bmRequestType_bit.direction == TUSB_DIR_IN)
-          {
-            if (tud_audio_get_req_itf_cb)
-            {
-              return tud_audio_get_req_itf_cb(rhport, p_request);
-            }
-            else
-            {
-              TU_LOG2("  No interface get request callback available!\r\n");
-              return false;   // Stall
-            }
+          if (p_request->bmRequestType_bit.direction == TUSB_DIR_IN) {
+            return tud_audio_get_req_itf_cb(rhport, p_request);
           }
         }
-      }
-      break;
+      } break;
 
-      case TUSB_REQ_RCPT_ENDPOINT:
-      {
+      case TUSB_REQ_RCPT_ENDPOINT: {
         uint8_t ep = TU_U16_LOW(p_request->wIndex);
 
         // Find index of audio driver structure and verify EP really exists
         TU_VERIFY(audiod_verify_ep_exists(ep, &func_id));
 
         // In case we got a get request invoke callback - callback needs to answer as defined in UAC2 specification page 89 - 5. Requests
-        if (p_request->bmRequestType_bit.direction == TUSB_DIR_IN)
-        {
-          if (tud_audio_get_req_ep_cb)
-          {
-            return tud_audio_get_req_ep_cb(rhport, p_request);
-          }
-          else
-          {
-            TU_LOG2("  No EP get request callback available!\r\n");
-            return false;     // Stall
-          }
+        if (p_request->bmRequestType_bit.direction == TUSB_DIR_IN) {
+          return tud_audio_get_req_ep_cb(rhport, p_request);
         }
-      }
-      break;
+      } break;
 
       // Unknown/Unsupported recipient
-      default: TU_LOG2("  Unsupported recipient: %d\r\n", p_request->bmRequestType_bit.recipient); TU_BREAKPOINT(); return false;
+      default:
+        TU_LOG2("  Unsupported recipient: %d\r\n", p_request->bmRequestType_bit.recipient);
+        TU_BREAKPOINT();
+        return false;
     }
 
     // If we end here, the received request is a set request - we schedule a receive for the data stage and return true here. We handle the rest later in audiod_control_complete() once the data stage was finished
@@ -1958,35 +2169,28 @@ static bool audiod_control_request(uint8_t rhport, tusb_control_request_t const
   return false;
 }
 
-bool audiod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
-{
-  if ( stage == CONTROL_STAGE_SETUP )
-  {
+bool audiod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const *request) {
+  if (stage == CONTROL_STAGE_SETUP) {
     return audiod_control_request(rhport, request);
-  }
-  else if ( stage == CONTROL_STAGE_DATA )
-  {
+  } else if (stage == CONTROL_STAGE_DATA) {
     return audiod_control_complete(rhport, request);
   }
 
   return true;
 }
 
-bool audiod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
-{
+bool audiod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) {
   (void) result;
   (void) xferred_bytes;
 
   // Search for interface belonging to given end point address and proceed as required
-  for (uint8_t func_id = 0; func_id < CFG_TUD_AUDIO; func_id++)
-  {
-    audiod_function_t* audio = &_audiod_fct[func_id];
+  for (uint8_t func_id = 0; func_id < CFG_TUD_AUDIO; func_id++) {
+    audiod_function_t *audio = &_audiod_fct[func_id];
 
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
 
     // Data transmission of control interrupt finished
-    if (audio->ep_int_ctr == ep_addr)
-    {
+    if (audio->ep_int == ep_addr) {
       // According to USB2 specification, maximum payload of interrupt EP is 8 bytes on low speed, 64 bytes on full speed, and 1024 bytes on high speed (but only if an alternate interface other than 0 is used - see specification p. 49)
       // In case there is nothing to send we have to return a NAK - this is taken care of by PHY ???
       // In case of an erroneous transmission a retransmission is conducted - this is taken care of by PHY ???
@@ -1994,7 +2198,8 @@ bool audiod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint3
       // I assume here, that things above are handled by PHY
       // All transmission is done - what remains to do is to inform job was completed
 
-      if (tud_audio_int_ctr_done_cb) TU_VERIFY(tud_audio_int_ctr_done_cb(rhport, (uint16_t) xferred_bytes));
+      tud_audio_int_done_cb(rhport);
+      return true;
     }
 
 #endif
@@ -2002,8 +2207,7 @@ bool audiod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint3
 #if CFG_TUD_AUDIO_ENABLE_EP_IN
 
     // Data transmission of audio packet finished
-    if (audio->ep_in == ep_addr && audio->alt_setting != 0)
-    {
+    if (audio->ep_in == ep_addr && audio->alt_setting != 0) {
       // USB 2.0, section 5.6.4, third paragraph, states "An isochronous endpoint must specify its required bus access period. However, an isochronous endpoint must be prepared to handle poll rates faster than the one specified."
       // That paragraph goes on to say "An isochronous IN endpoint must return a zero-length packet whenever data is requested at a faster interval than the specified interval and data is not available."
       // This can only be solved reliably if we load a ZLP after every IN transmission since we can not say if the host requests samples earlier than we declared! Once all samples are collected we overwrite the loaded ZLP.
@@ -2023,27 +2227,24 @@ bool audiod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint3
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT
 
     // New audio packet received
-    if (audio->ep_out == ep_addr)
-    {
+    if (audio->ep_out == ep_addr) {
       TU_VERIFY(audiod_rx_done_cb(rhport, audio, (uint16_t) xferred_bytes));
       return true;
     }
 
 
-#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+  #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
     // Transmission of feedback EP finished
-    if (audio->ep_fb == ep_addr)
-    {
-      if (tud_audio_fb_done_cb) tud_audio_fb_done_cb(func_id);
+    if (audio->ep_fb == ep_addr) {
+      tud_audio_fb_done_cb(func_id);
 
-      // Schedule a transmit with the new value if EP is not busy 
-      if (!usbd_edpt_busy(rhport, audio->ep_fb))
-      {
+      // Schedule a transmit with the new value if EP is not busy
+      if (usbd_edpt_claim(rhport, audio->ep_fb)) {
         // Schedule next transmission - value is changed bytud_audio_n_fb_set() in the meantime or the old value gets sent
-        return audiod_fb_send(rhport, audio);
+        return audiod_fb_send(audio);
       }
     }
-#endif
+  #endif
 #endif
   }
 
@@ -2052,8 +2253,7 @@ bool audiod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint3
 
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
 
-static bool set_fb_params_freq(audiod_function_t* audio, uint32_t sample_freq, uint32_t mclk_freq)
-{
+static bool audiod_set_fb_params_freq(audiod_function_t *audio, uint32_t sample_freq, uint32_t mclk_freq) {
   // Check if frame interval is within sane limits
   // The interval value n_frames was taken from the descriptors within audiod_set_interface()
 
@@ -2062,23 +2262,19 @@ static bool set_fb_params_freq(audiod_function_t* audio, uint32_t sample_freq, u
   uint32_t const k = (TUSB_SPEED_FULL == tud_speed_get()) ? 10 : 13;
   uint32_t const n_frame = (1UL << audio->feedback.frame_shift);
 
-  if ( (((1UL << k) * sample_freq / mclk_freq) + 1) > n_frame )
-  {
-    TU_LOG1("  UAC2 feedback interval too small\r\n"); TU_BREAKPOINT(); return false;
+  if ((((1UL << k) * sample_freq / mclk_freq) + 1) > n_frame) {
+    TU_LOG1("  UAC2 feedback interval too small\r\n");
+    TU_BREAKPOINT();
+    return false;
   }
 
   // Check if parameters really allow for a power of two division
-  if ((mclk_freq % sample_freq) == 0 && tu_is_power_of_two(mclk_freq / sample_freq))
-  {
-    audio->feedback.compute_method     = AUDIO_FEEDBACK_METHOD_FREQUENCY_POWER_OF_2;
-    audio->feedback.compute.power_of_2 = 16 - audio->feedback.frame_shift - tu_log2(mclk_freq / sample_freq);
-  }
-  else if ( audio->feedback.compute_method == AUDIO_FEEDBACK_METHOD_FREQUENCY_FLOAT)
-  {
-    audio->feedback.compute.float_const = (float)sample_freq / mclk_freq * (1UL << (16 - audio->feedback.frame_shift));
-  }
-  else
-  {
+  if ((mclk_freq % sample_freq) == 0 && tu_is_power_of_two(mclk_freq / sample_freq)) {
+    audio->feedback.compute_method = AUDIO_FEEDBACK_METHOD_FREQUENCY_POWER_OF_2;
+    audio->feedback.compute.power_of_2 = (uint8_t) (16 - (audio->feedback.frame_shift - 1) - tu_log2(mclk_freq / sample_freq));
+  } else if (audio->feedback.compute_method == AUDIO_FEEDBACK_METHOD_FREQUENCY_FLOAT) {
+    audio->feedback.compute.float_const = (float) sample_freq / (float) mclk_freq * (1UL << (16 - (audio->feedback.frame_shift - 1)));
+  } else {
     audio->feedback.compute.fixed.sample_freq = sample_freq;
     audio->feedback.compute.fixed.mclk_freq = mclk_freq;
   }
@@ -2086,46 +2282,83 @@ static bool set_fb_params_freq(audiod_function_t* audio, uint32_t sample_freq, u
   return true;
 }
 
-uint32_t tud_audio_feedback_update(uint8_t func_id, uint32_t cycles)
-{
-  audiod_function_t* audio = &_audiod_fct[func_id];
+static void audiod_fb_fifo_count_update(audiod_function_t *audio, uint16_t lvl_new) {
+  /* Low-pass (averaging) filter */
+  uint32_t lvl = audio->feedback.compute.fifo_count.fifo_lvl_avg;
+  lvl = (uint32_t) (((uint64_t) lvl * 63 + ((uint32_t) lvl_new << 16)) >> 6);
+  audio->feedback.compute.fifo_count.fifo_lvl_avg = lvl;
+
+  uint32_t const ff_lvl = lvl >> 16;
+  uint16_t const ff_thr = audio->feedback.compute.fifo_count.fifo_lvl_thr;
+  uint16_t const *rate = audio->feedback.compute.fifo_count.rate_const;
+
   uint32_t feedback;
 
-  switch (audio->feedback.compute_method)
-  {
+  if (ff_lvl < ff_thr) {
+    feedback = audio->feedback.compute.fifo_count.nom_value + (ff_thr - ff_lvl) * rate[0];
+  } else {
+    feedback = audio->feedback.compute.fifo_count.nom_value - (ff_lvl - ff_thr) * rate[1];
+  }
+
+  if (feedback > audio->feedback.max_value) feedback = audio->feedback.max_value;
+  if (feedback < audio->feedback.min_value) feedback = audio->feedback.min_value;
+  audio->feedback.value = feedback;
+
+  // Schedule a transmit with the new value if EP is not busy - this triggers repetitive scheduling of the feedback value
+  if (usbd_edpt_claim(audio->rhport, audio->ep_fb)) {
+    audiod_fb_send(audio);
+  }
+}
+
+uint32_t tud_audio_feedback_update(uint8_t func_id, uint32_t cycles) {
+  audiod_function_t *audio = &_audiod_fct[func_id];
+  uint32_t feedback;
+
+  switch (audio->feedback.compute_method) {
     case AUDIO_FEEDBACK_METHOD_FREQUENCY_POWER_OF_2:
       feedback = (cycles << audio->feedback.compute.power_of_2);
-    break;
+      break;
 
     case AUDIO_FEEDBACK_METHOD_FREQUENCY_FLOAT:
       feedback = (uint32_t) ((float) cycles * audio->feedback.compute.float_const);
-    break;
+      break;
 
-    case AUDIO_FEEDBACK_METHOD_FREQUENCY_FIXED:
-    {
-      uint64_t fb64 = (((uint64_t) cycles) * audio->feedback.compute.fixed.sample_freq) << (16 - audio->feedback.frame_shift);
+    case AUDIO_FEEDBACK_METHOD_FREQUENCY_FIXED: {
+      uint64_t fb64 = (((uint64_t) cycles) * audio->feedback.compute.fixed.sample_freq) << (16 - (audio->feedback.frame_shift - 1));
       feedback = (uint32_t) (fb64 / audio->feedback.compute.fixed.mclk_freq);
-    }
-    break;
+    } break;
 
-    default: return 0;
+    default:
+      return 0;
   }
 
   // For Windows: https://docs.microsoft.com/en-us/windows-hardware/drivers/audio/usb-2-0-audio-drivers
   // The size of isochronous packets created by the device must be within the limits specified in FMT-2.0 section 2.3.1.1.
   // This means that the deviation of actual packet size from nominal size must not exceed +/- one audio slot
   // (audio slot = channel count samples).
-  if ( feedback > audio->feedback.max_value ) feedback = audio->feedback.max_value;
-  if ( feedback < audio->feedback.min_value ) feedback = audio->feedback.min_value;
+  if (feedback > audio->feedback.max_value) feedback = audio->feedback.max_value;
+  if (feedback < audio->feedback.min_value) feedback = audio->feedback.min_value;
 
   tud_audio_n_fb_set(func_id, feedback);
 
   return feedback;
 }
+
+bool tud_audio_n_fb_set(uint8_t func_id, uint32_t feedback) {
+  TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL);
+
+  _audiod_fct[func_id].feedback.value = feedback;
+
+  // Schedule a transmit with the new value if EP is not busy - this triggers repetitive scheduling of the feedback value
+  if (usbd_edpt_claim(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_fb)) {
+    return audiod_fb_send(&_audiod_fct[func_id]);
+  }
+
+  return true;
+}
 #endif
 
-TU_ATTR_FAST_FUNC void audiod_sof_isr (uint8_t rhport, uint32_t frame_count)
-{
+TU_ATTR_FAST_FUNC void audiod_sof_isr(uint8_t rhport, uint32_t frame_count) {
   (void) rhport;
   (void) frame_count;
 
@@ -2137,26 +2370,22 @@ TU_ATTR_FAST_FUNC void audiod_sof_isr (uint8_t rhport, uint32_t frame_count)
   // feedback = n_cycles / n_frames * f_s / f_m in 16.16 format, where n_cycles are the number of main clock cycles within fb_n_frames
 
   // Iterate over audio functions and set feedback value
-  for(uint8_t i=0; i < CFG_TUD_AUDIO; i++)
-  {
-    audiod_function_t* audio = &_audiod_fct[i];
+  for (uint8_t i = 0; i < CFG_TUD_AUDIO; i++) {
+    audiod_function_t *audio = &_audiod_fct[i];
 
-    if (audio->ep_fb != 0)
-    {
+    if (audio->ep_fb != 0) {
       // HS shift need to be adjusted since SOF event is generated for frame only
       uint8_t const hs_adjust = (TUSB_SPEED_HIGH == tud_speed_get()) ? 3 : 0;
       uint32_t const interval = 1UL << (audio->feedback.frame_shift - hs_adjust);
-      if ( 0 == (frame_count & (interval-1)) )
-      {
-        if(tud_audio_feedback_interval_isr) tud_audio_feedback_interval_isr(i, frame_count, audio->feedback.frame_shift);
+      if (0 == (frame_count & (interval - 1))) {
+        tud_audio_feedback_interval_isr(i, frame_count, audio->feedback.frame_shift);
       }
     }
   }
-#endif // CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+#endif// CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
 }
 
-bool tud_audio_buffer_and_schedule_control_xfer(uint8_t rhport, tusb_control_request_t const * p_request, void* data, uint16_t len)
-{
+bool tud_audio_buffer_and_schedule_control_xfer(uint8_t rhport, tusb_control_request_t const *p_request, void *data, uint16_t len) {
   // Handles only sending of data not receiving
   if (p_request->bmRequestType_bit.direction == TUSB_DIR_OUT) return false;
 
@@ -2165,71 +2394,73 @@ bool tud_audio_buffer_and_schedule_control_xfer(uint8_t rhport, tusb_control_req
   uint8_t itf = TU_U16_LOW(p_request->wIndex);
 
   // Conduct checks which depend on the recipient
-  switch (p_request->bmRequestType_bit.recipient)
-  {
-    case TUSB_REQ_RCPT_INTERFACE:
-    {
+  switch (p_request->bmRequestType_bit.recipient) {
+    case TUSB_REQ_RCPT_INTERFACE: {
       uint8_t entityID = TU_U16_HIGH(p_request->wIndex);
 
       // Verify if entity is present
-      if (entityID != 0)
-      {
+      if (entityID != 0) {
         // Find index of audio driver structure and verify entity really exists
         TU_VERIFY(audiod_verify_entity_exists(itf, entityID, &func_id));
-      }
-      else
-      {
+      } else {
         // Find index of audio driver structure and verify interface really exists
         TU_VERIFY(audiod_verify_itf_exists(itf, &func_id));
       }
-    }
-    break;
+    } break;
 
-    case TUSB_REQ_RCPT_ENDPOINT:
-    {
+    case TUSB_REQ_RCPT_ENDPOINT: {
       uint8_t ep = TU_U16_LOW(p_request->wIndex);
 
       // Find index of audio driver structure and verify EP really exists
       TU_VERIFY(audiod_verify_ep_exists(ep, &func_id));
-    }
-    break;
+    } break;
 
     // Unknown/Unsupported recipient
-    default: TU_LOG2("  Unsupported recipient: %d\r\n", p_request->bmRequestType_bit.recipient); TU_BREAKPOINT(); return false;
+    default:
+      TU_LOG2("  Unsupported recipient: %d\r\n", p_request->bmRequestType_bit.recipient);
+      TU_BREAKPOINT();
+      return false;
   }
 
   // Crop length
   if (len > _audiod_fct[func_id].ctrl_buf_sz) len = _audiod_fct[func_id].ctrl_buf_sz;
 
   // Copy into buffer
-  memcpy((void *)_audiod_fct[func_id].ctrl_buf, data, (size_t)len);
+  TU_VERIFY(0 == tu_memcpy_s(_audiod_fct[func_id].ctrl_buf, _audiod_fct[func_id].ctrl_buf_sz, data, (size_t) len));
+
+#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
+  // Find data for sampling_frequency_control
+  if (p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS && p_request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_INTERFACE) {
+    uint8_t entityID = TU_U16_HIGH(p_request->wIndex);
+    uint8_t ctrlSel = TU_U16_HIGH(p_request->wValue);
+    if (_audiod_fct[func_id].bclock_id_tx == entityID && ctrlSel == AUDIO_CS_CTRL_SAM_FREQ && p_request->bRequest == AUDIO_CS_REQ_CUR) {
+      _audiod_fct[func_id].sample_rate_tx = tu_unaligned_read32(_audiod_fct[func_id].ctrl_buf);
+    }
+  }
+#endif
 
   // Schedule transmit
-  return tud_control_xfer(rhport, p_request, (void*)_audiod_fct[func_id].ctrl_buf, len);
+  return tud_control_xfer(rhport, p_request, (void *) _audiod_fct[func_id].ctrl_buf, len);
 }
 
 // This helper function finds for a given audio function and AS interface number the index of the attached driver structure, the index of the interface in the audio function
 // (e.g. the std. AS interface with interface number 15 is the first AS interface for the given audio function and thus gets index zero), and
 // finally a pointer to the std. AS interface, where the pointer always points to the first alternate setting i.e. alternate interface zero.
-static bool audiod_get_AS_interface_index(uint8_t itf, audiod_function_t * audio, uint8_t *idxItf, uint8_t const **pp_desc_int)
-{
-  if (audio->p_desc)
-  {
+static bool audiod_get_AS_interface_index(uint8_t itf, audiod_function_t *audio, uint8_t *idxItf, uint8_t const **pp_desc_int) {
+  if (audio->p_desc) {
     // Get pointer at end
     uint8_t const *p_desc_end = audio->p_desc + audio->desc_length - TUD_AUDIO_DESC_IAD_LEN;
 
     // Advance past AC descriptors
     uint8_t const *p_desc = tu_desc_next(audio->p_desc);
-    p_desc += ((audio_desc_cs_ac_interface_t const *)p_desc)->wTotalLength;
+    p_desc += ((audio_desc_cs_ac_interface_t const *) p_desc)->wTotalLength;
 
     uint8_t tmp = 0;
-    while (p_desc < p_desc_end)
-    {
+    // Condition modified from p_desc < p_desc_end to prevent gcc>=12 strict-overflow warning
+    while (p_desc_end - p_desc > 0) {
       // We assume the number of alternate settings is increasing thus we return the index of alternate setting zero!
-      if (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE && ((tusb_desc_interface_t const * )p_desc)->bAlternateSetting == 0)
-      {
-        if (((tusb_desc_interface_t const * )p_desc)->bInterfaceNumber == itf)
-        {
+      if (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE && ((tusb_desc_interface_t const *) p_desc)->bAlternateSetting == 0) {
+        if (((tusb_desc_interface_t const *) p_desc)->bInterfaceNumber == itf) {
           *idxItf = tmp;
           *pp_desc_int = p_desc;
           return true;
@@ -2246,14 +2477,11 @@ static bool audiod_get_AS_interface_index(uint8_t itf, audiod_function_t * audio
 // This helper function finds for a given AS interface number the index of the attached driver structure, the index of the interface in the audio function
 // (e.g. the std. AS interface with interface number 15 is the first AS interface for the given audio function and thus gets index zero), and
 // finally a pointer to the std. AS interface, where the pointer always points to the first alternate setting i.e. alternate interface zero.
-static bool audiod_get_AS_interface_index_global(uint8_t itf, uint8_t *func_id, uint8_t *idxItf, uint8_t const **pp_desc_int)
-{
+static bool audiod_get_AS_interface_index_global(uint8_t itf, uint8_t *func_id, uint8_t *idxItf, uint8_t const **pp_desc_int) {
   // Loop over audio driver interfaces
   uint8_t i;
-  for (i = 0; i < CFG_TUD_AUDIO; i++)
-  {
-    if (audiod_get_AS_interface_index(itf, &_audiod_fct[i], idxItf, pp_desc_int))
-    {
+  for (i = 0; i < CFG_TUD_AUDIO; i++) {
+    if (audiod_get_AS_interface_index(itf, &_audiod_fct[i], idxItf, pp_desc_int)) {
       *func_id = i;
       return true;
     }
@@ -2263,22 +2491,19 @@ static bool audiod_get_AS_interface_index_global(uint8_t itf, uint8_t *func_id,
 }
 
 // Verify an entity with the given ID exists and returns also the corresponding driver index
-static bool audiod_verify_entity_exists(uint8_t itf, uint8_t entityID, uint8_t *func_id)
-{
+static bool audiod_verify_entity_exists(uint8_t itf, uint8_t entityID, uint8_t *func_id) {
   uint8_t i;
-  for (i = 0; i < CFG_TUD_AUDIO; i++)
-  {
+  for (i = 0; i < CFG_TUD_AUDIO; i++) {
     // Look for the correct driver by checking if the unique standard AC interface number fits
-    if (_audiod_fct[i].p_desc && ((tusb_desc_interface_t const *)_audiod_fct[i].p_desc)->bInterfaceNumber == itf)
-    {
+    if (_audiod_fct[i].p_desc && ((tusb_desc_interface_t const *) _audiod_fct[i].p_desc)->bInterfaceNumber == itf) {
       // Get pointers after class specific AC descriptors and end of AC descriptors - entities are defined in between
-      uint8_t const *p_desc = tu_desc_next(_audiod_fct[i].p_desc);                                          // Points to CS AC descriptor
-      uint8_t const *p_desc_end = ((audio_desc_cs_ac_interface_t const *)p_desc)->wTotalLength + p_desc;
-      p_desc = tu_desc_next(p_desc);                                                                            // Get past CS AC descriptor
+      uint8_t const *p_desc = tu_desc_next(_audiod_fct[i].p_desc);// Points to CS AC descriptor
+      uint8_t const *p_desc_end = ((audio_desc_cs_ac_interface_t const *) p_desc)->wTotalLength + p_desc;
+      p_desc = tu_desc_next(p_desc);// Get past CS AC descriptor
 
-      while (p_desc < p_desc_end)
-      {
-        if (p_desc[3] == entityID)  // Entity IDs are always at offset 3
+      // Condition modified from p_desc < p_desc_end to prevent gcc>=12 strict-overflow warning
+      while (p_desc_end - p_desc > 0) {
+        if (p_desc[3] == entityID)// Entity IDs are always at offset 3
         {
           *func_id = i;
           return true;
@@ -2290,21 +2515,16 @@ static bool audiod_verify_entity_exists(uint8_t itf, uint8_t entityID, uint8_t *
   return false;
 }
 
-static bool audiod_verify_itf_exists(uint8_t itf, uint8_t *func_id)
-{
+static bool audiod_verify_itf_exists(uint8_t itf, uint8_t *func_id) {
   uint8_t i;
-  for (i = 0; i < CFG_TUD_AUDIO; i++)
-  {
-    if (_audiod_fct[i].p_desc)
-    {
+  for (i = 0; i < CFG_TUD_AUDIO; i++) {
+    if (_audiod_fct[i].p_desc) {
       // Get pointer at beginning and end
       uint8_t const *p_desc = _audiod_fct[i].p_desc;
       uint8_t const *p_desc_end = _audiod_fct[i].p_desc + _audiod_fct[i].desc_length - TUD_AUDIO_DESC_IAD_LEN;
-
-      while (p_desc < p_desc_end)
-      {
-        if (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE && ((tusb_desc_interface_t const *)_audiod_fct[i].p_desc)->bInterfaceNumber == itf)
-        {
+      // Condition modified from p_desc < p_desc_end to prevent gcc>=12 strict-overflow warning
+      while (p_desc_end - p_desc > 0) {
+        if (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE && ((tusb_desc_interface_t const *) _audiod_fct[i].p_desc)->bInterfaceNumber == itf) {
           *func_id = i;
           return true;
         }
@@ -2315,24 +2535,20 @@ static bool audiod_verify_itf_exists(uint8_t itf, uint8_t *func_id)
   return false;
 }
 
-static bool audiod_verify_ep_exists(uint8_t ep, uint8_t *func_id)
-{
+static bool audiod_verify_ep_exists(uint8_t ep, uint8_t *func_id) {
   uint8_t i;
-  for (i = 0; i < CFG_TUD_AUDIO; i++)
-  {
-    if (_audiod_fct[i].p_desc)
-    {
+  for (i = 0; i < CFG_TUD_AUDIO; i++) {
+    if (_audiod_fct[i].p_desc) {
       // Get pointer at end
       uint8_t const *p_desc_end = _audiod_fct[i].p_desc + _audiod_fct[i].desc_length;
 
       // Advance past AC descriptors - EP we look for are streaming EPs
       uint8_t const *p_desc = tu_desc_next(_audiod_fct[i].p_desc);
-      p_desc += ((audio_desc_cs_ac_interface_t const *)p_desc)->wTotalLength;
+      p_desc += ((audio_desc_cs_ac_interface_t const *) p_desc)->wTotalLength;
 
-      while (p_desc < p_desc_end)
-      {
-        if (tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT && ((tusb_desc_endpoint_t const * )p_desc)->bEndpointAddress == ep)
-        {
+      // Condition modified from p_desc < p_desc_end to prevent gcc>=12 strict-overflow warning
+      while (p_desc_end - p_desc > 0) {
+        if (tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT && ((tusb_desc_endpoint_t const *) p_desc)->bEndpointAddress == ep) {
           *func_id = i;
           return true;
         }
@@ -2343,85 +2559,77 @@ static bool audiod_verify_ep_exists(uint8_t ep, uint8_t *func_id)
   return false;
 }
 
-#if CFG_TUD_AUDIO_ENABLE_ENCODING || CFG_TUD_AUDIO_ENABLE_DECODING
+#if (CFG_TUD_AUDIO_ENABLE_EP_IN && (CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL || CFG_TUD_AUDIO_ENABLE_ENCODING)) || (CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING)
 // p_desc points to the AS interface of alternate setting zero
 // itf is the interface number of the corresponding interface - we check if the interface belongs to EP in or EP out to see if it is a TX or RX parameter
 // Currently, only AS interfaces with an EP (in or out) are supposed to be parsed for!
-static void audiod_parse_for_AS_params(audiod_function_t* audio, uint8_t const * p_desc, uint8_t const * p_desc_end, uint8_t const as_itf)
-{
-#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_EP_OUT
-  if (as_itf != audio->ep_in_as_intf_num && as_itf != audio->ep_out_as_intf_num) return;           // Abort, this interface has no EP, this driver does not support this currently
-#endif
-#if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_EP_OUT
+static void audiod_parse_for_AS_params(audiod_function_t *audio, uint8_t const *p_desc, uint8_t const *p_desc_end, uint8_t const as_itf) {
+  #if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_EP_OUT
+  if (as_itf != audio->ep_in_as_intf_num && as_itf != audio->ep_out_as_intf_num) return;// Abort, this interface has no EP, this driver does not support this currently
+  #endif
+  #if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_EP_OUT
   if (as_itf != audio->ep_in_as_intf_num) return;
-#endif
-#if !CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_EP_OUT
+  #endif
+  #if !CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_EP_OUT
   if (as_itf != audio->ep_out_as_intf_num) return;
-#endif
-
-  p_desc = tu_desc_next(p_desc);    // Exclude standard AS interface descriptor of current alternate interface descriptor
+  #endif
 
-  while (p_desc < p_desc_end)
-  {
+  p_desc = tu_desc_next(p_desc);// Exclude standard AS interface descriptor of current alternate interface descriptor
+  // Condition modified from p_desc < p_desc_end to prevent gcc>=12 strict-overflow warning
+  while (p_desc_end - p_desc > 0) {
     // Abort if follow up descriptor is a new standard interface descriptor - indicates the last AS descriptor was already finished
     if (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE) break;
 
     // Look for a Class-Specific AS Interface Descriptor(4.9.2) to verify format type and format and also to get number of physical channels
-    if (tu_desc_type(p_desc) == TUSB_DESC_CS_INTERFACE && tu_desc_subtype(p_desc) == AUDIO_CS_AS_INTERFACE_AS_GENERAL)
-    {
-#if CFG_TUD_AUDIO_ENABLE_EP_IN
-      if (as_itf == audio->ep_in_as_intf_num)
-      {
-        audio->n_channels_tx = ((audio_desc_cs_as_interface_t const * )p_desc)->bNrChannels;
-        audio->format_type_tx = (audio_format_type_t)(((audio_desc_cs_as_interface_t const * )p_desc)->bFormatType);
+    if (tu_desc_type(p_desc) == TUSB_DESC_CS_INTERFACE && tu_desc_subtype(p_desc) == AUDIO_CS_AS_INTERFACE_AS_GENERAL) {
+  #if CFG_TUD_AUDIO_ENABLE_EP_IN
+      if (as_itf == audio->ep_in_as_intf_num) {
+        audio->n_channels_tx = ((audio_desc_cs_as_interface_t const *) p_desc)->bNrChannels;
+        audio->format_type_tx = (audio_format_type_t) (((audio_desc_cs_as_interface_t const *) p_desc)->bFormatType);
 
-#if CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING
-        audio->format_type_I_tx = (audio_data_format_type_I_t)(((audio_desc_cs_as_interface_t const * )p_desc)->bmFormats);
-#endif
+    #if CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING
+        audio->format_type_I_tx = (audio_data_format_type_I_t) (((audio_desc_cs_as_interface_t const *) p_desc)->bmFormats);
+    #endif
       }
-#endif
+  #endif
 
-#if CFG_TUD_AUDIO_ENABLE_EP_OUT
-      if (as_itf == audio->ep_out_as_intf_num)
-      {
-        audio->n_channels_rx = ((audio_desc_cs_as_interface_t const * )p_desc)->bNrChannels;
-        audio->format_type_rx = ((audio_desc_cs_as_interface_t const * )p_desc)->bFormatType;
-#if CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING
-        audio->format_type_I_rx = ((audio_desc_cs_as_interface_t const * )p_desc)->bmFormats;
-#endif
+  #if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING
+      if (as_itf == audio->ep_out_as_intf_num) {
+        audio->n_channels_rx = ((audio_desc_cs_as_interface_t const *) p_desc)->bNrChannels;
+        audio->format_type_rx = ((audio_desc_cs_as_interface_t const *) p_desc)->bFormatType;
+    #if CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING
+        audio->format_type_I_rx = ((audio_desc_cs_as_interface_t const *) p_desc)->bmFormats;
+    #endif
       }
-#endif
+  #endif
     }
 
     // Look for a Type I Format Type Descriptor(2.3.1.6 - Audio Formats)
-#if CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING || CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING
-    if (tu_desc_type(p_desc) == TUSB_DESC_CS_INTERFACE && tu_desc_subtype(p_desc) == AUDIO_CS_AS_INTERFACE_FORMAT_TYPE && ((audio_desc_type_I_format_t const * )p_desc)->bFormatType == AUDIO_FORMAT_TYPE_I)
-    {
-#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_EP_OUT
-      if (as_itf != audio->ep_in_as_intf_num && as_itf != audio->ep_out_as_intf_num) break;           // Abort loop, this interface has no EP, this driver does not support this currently
-#endif
-#if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_EP_OUT
+  #if CFG_TUD_AUDIO_ENABLE_TYPE_I_ENCODING || CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL || CFG_TUD_AUDIO_ENABLE_TYPE_I_DECODING
+    if (tu_desc_type(p_desc) == TUSB_DESC_CS_INTERFACE && tu_desc_subtype(p_desc) == AUDIO_CS_AS_INTERFACE_FORMAT_TYPE && ((audio_desc_type_I_format_t const *) p_desc)->bFormatType == AUDIO_FORMAT_TYPE_I) {
+    #if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_EP_OUT
+      if (as_itf != audio->ep_in_as_intf_num && as_itf != audio->ep_out_as_intf_num) break;// Abort loop, this interface has no EP, this driver does not support this currently
+    #endif
+    #if CFG_TUD_AUDIO_ENABLE_EP_IN && !CFG_TUD_AUDIO_ENABLE_EP_OUT
       if (as_itf != audio->ep_in_as_intf_num) break;
-#endif
-#if !CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_EP_OUT
+    #endif
+    #if !CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_ENABLE_EP_OUT
       if (as_itf != audio->ep_out_as_intf_num) break;
-#endif
+    #endif
 
-#if CFG_TUD_AUDIO_ENABLE_EP_IN
-      if (as_itf == audio->ep_in_as_intf_num)
-      {
-        audio->n_bytes_per_sampe_tx = ((audio_desc_type_I_format_t const * )p_desc)->bSubslotSize;
+    #if CFG_TUD_AUDIO_ENABLE_EP_IN
+      if (as_itf == audio->ep_in_as_intf_num) {
+        audio->n_bytes_per_sample_tx = ((audio_desc_type_I_format_t const *) p_desc)->bSubslotSize;
       }
-#endif
+    #endif
 
-#if CFG_TUD_AUDIO_ENABLE_EP_OUT
-      if (as_itf == audio->ep_out_as_intf_num)
-      {
-        audio->n_bytes_per_sampe_rx = ((audio_desc_type_I_format_t const * )p_desc)->bSubslotSize;
+    #if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_DECODING
+      if (as_itf == audio->ep_out_as_intf_num) {
+        audio->n_bytes_per_sample_rx = ((audio_desc_type_I_format_t const *) p_desc)->bSubslotSize;
       }
-#endif
+    #endif
     }
-#endif
+  #endif
 
     // Other format types are not supported yet
 
@@ -2430,50 +2638,87 @@ static void audiod_parse_for_AS_params(audiod_function_t* audio, uint8_t const *
 }
 #endif
 
-#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+#if CFG_TUD_AUDIO_ENABLE_EP_IN && CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
 
-bool tud_audio_n_fb_set(uint8_t func_id, uint32_t feedback)
-{
-  TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL);
+static bool audiod_calc_tx_packet_sz(audiod_function_t *audio) {
+  TU_VERIFY(audio->format_type_tx == AUDIO_FORMAT_TYPE_I);
+  TU_VERIFY(audio->n_channels_tx);
+  TU_VERIFY(audio->n_bytes_per_sample_tx);
+  TU_VERIFY(audio->interval_tx);
+  TU_VERIFY(audio->sample_rate_tx);
 
-  // Format the feedback value
-#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_FORMAT_CORRECTION
-  if ( TUSB_SPEED_FULL == tud_speed_get() )
-  {
-    uint8_t * fb = (uint8_t *) &_audiod_fct[func_id].feedback.value;
+  const uint8_t interval = (tud_speed_get() == TUSB_SPEED_FULL) ? audio->interval_tx : 1 << (audio->interval_tx - 1);
 
-    // For FS format is 10.14
-    *(fb++) = (feedback >> 2) & 0xFF;
-    *(fb++) = (feedback >> 10) & 0xFF;
-    *(fb++) = (feedback >> 18) & 0xFF;
-    // 4th byte is needed to work correctly with MS Windows
-    *fb = 0;
-  }else
-#else
-  {
-    // Send value as-is, caller will choose the appropriate format
-    _audiod_fct[func_id].feedback.value = feedback;
-  }
-#endif
+  const uint16_t sample_normimal = (uint16_t) (audio->sample_rate_tx * interval / ((tud_speed_get() == TUSB_SPEED_FULL) ? 1000 : 8000));
+  const uint16_t sample_reminder = (uint16_t) (audio->sample_rate_tx * interval % ((tud_speed_get() == TUSB_SPEED_FULL) ? 1000 : 8000));
 
-  // Schedule a transmit with the new value if EP is not busy - this triggers repetitive scheduling of the feedback value
-  if (!usbd_edpt_busy(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_fb))
-  {
-    return audiod_fb_send(_audiod_fct[func_id].rhport, &_audiod_fct[func_id]);
+  const uint16_t packet_sz_tx_min = (uint16_t) ((sample_normimal - 1) * audio->n_channels_tx * audio->n_bytes_per_sample_tx);
+  const uint16_t packet_sz_tx_norm = (uint16_t) (sample_normimal * audio->n_channels_tx * audio->n_bytes_per_sample_tx);
+  const uint16_t packet_sz_tx_max = (uint16_t) ((sample_normimal + 1) * audio->n_channels_tx * audio->n_bytes_per_sample_tx);
+
+  // Endpoint size must larger than packet size
+  TU_ASSERT(packet_sz_tx_max <= audio->ep_in_sz);
+
+  // Frmt20.pdf 2.3.1.1 USB Packets
+  if (sample_reminder) {
+    // All virtual frame packets must either contain INT(nav) audio slots (small VFP) or INT(nav)+1 (large VFP) audio slots
+    audio->packet_sz_tx[0] = packet_sz_tx_norm;
+    audio->packet_sz_tx[1] = packet_sz_tx_norm;
+    audio->packet_sz_tx[2] = packet_sz_tx_max;
+  } else {
+    // In the case where nav = INT(nav), ni may vary between INT(nav)-1 (small VFP), INT(nav)
+    // (medium VFP) and INT(nav)+1 (large VFP).
+    audio->packet_sz_tx[0] = packet_sz_tx_min;
+    audio->packet_sz_tx[1] = packet_sz_tx_norm;
+    audio->packet_sz_tx[2] = packet_sz_tx_max;
   }
 
   return true;
 }
+
+static uint16_t audiod_tx_packet_size(const uint16_t *norminal_size, uint16_t data_count, uint16_t fifo_depth, uint16_t max_depth) {
+  // Flow control need a FIFO size of at least 4*Navg
+  if (norminal_size[1] && norminal_size[1] <= fifo_depth * 4) {
+    // Use blackout to prioritize normal size packet
+    static int ctrl_blackout = 0;
+    uint16_t packet_size;
+    uint16_t slot_size = norminal_size[2] - norminal_size[1];
+    if (data_count < norminal_size[0]) {
+      // If you get here frequently, then your I2S clock deviation is too big !
+      packet_size = 0;
+    } else if (data_count < fifo_depth / 2 - slot_size && !ctrl_blackout) {
+      packet_size = norminal_size[0];
+      ctrl_blackout = 10;
+    } else if (data_count > fifo_depth / 2 + slot_size && !ctrl_blackout) {
+      packet_size = norminal_size[2];
+      if (norminal_size[0] == norminal_size[1]) {
+        // nav > INT(nav), eg. 44.1k, 88.2k
+        ctrl_blackout = 0;
+      } else {
+        // nav = INT(nav), eg. 48k, 96k
+        ctrl_blackout = 10;
+      }
+    } else {
+      packet_size = norminal_size[1];
+      if (ctrl_blackout) {
+        ctrl_blackout--;
+      }
+    }
+    // Normally this cap is not necessary
+    return tu_min16(packet_size, max_depth);
+  } else {
+    return tu_min16(data_count, max_depth);
+  }
+}
+
 #endif
 
 // No security checks here - internal function only which should always succeed
-uint8_t audiod_get_audio_fct_idx(audiod_function_t * audio)
-{
-  for (uint8_t cnt=0; cnt < CFG_TUD_AUDIO; cnt++)
-  {
+static uint8_t audiod_get_audio_fct_idx(audiod_function_t *audio) {
+  for (uint8_t cnt = 0; cnt < CFG_TUD_AUDIO; cnt++) {
     if (&_audiod_fct[cnt] == audio) return cnt;
   }
   return 0;
 }
 
-#endif //CFG_TUD_ENABLED && CFG_TUD_AUDIO
+#endif // (CFG_TUD_ENABLED && CFG_TUD_AUDIO)
diff --git a/src/class/audio/audio_device.h b/src/class/audio/audio_device.h
index 0ef100fa4..0a7bff212 100644
--- a/src/class/audio/audio_device.h
+++ b/src/class/audio/audio_device.h
@@ -1,8 +1,9 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2020 Ha Thach (tinyusb.org)
  * Copyright (c) 2020 Reinhard Panhuber
+ * Copyright (c) 2023 HiFiPhile
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -181,24 +182,29 @@
 #endif
 #endif
 
+// (For TYPE-I format only) Flow control is necessary to allow IN ep send correct amount of data, unless it's a virtual device where data is perfectly synchronized to USB clock.
+#ifndef CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL
+#define CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL  1
+#endif
+
 // Enable/disable feedback EP (required for asynchronous RX applications)
 #ifndef CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
 #define CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP                    0                             // Feedback - 0 or 1
 #endif
 
 // Enable/disable conversion from 16.16 to 10.14 format on full-speed devices. See tud_audio_n_fb_set().
+// Can be override by tud_audio_feedback_format_correction_cb()
 #ifndef CFG_TUD_AUDIO_ENABLE_FEEDBACK_FORMAT_CORRECTION
 #define CFG_TUD_AUDIO_ENABLE_FEEDBACK_FORMAT_CORRECTION     0                             // 0 or 1
 #endif
 
-// Audio interrupt control EP size - disabled if 0
-#ifndef CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-#define CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN                     0                             // Audio interrupt control - if required - 6 Bytes according to UAC 2 specification (p. 74)
+// Enable/disable interrupt EP (required for notifying host of control changes)
+#ifndef CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+#define CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP                   0                             // Feedback - 0 or 1
 #endif
 
-#ifndef CFG_TUD_AUDIO_INT_CTR_EP_IN_SW_BUFFER_SIZE
-#define CFG_TUD_AUDIO_INT_CTR_EP_IN_SW_BUFFER_SIZE          6                             // Buffer size of audio control interrupt EP - 6 Bytes according to UAC 2 specification (p. 74)
-#endif
+// Audio control interrupt EP - 6 Bytes according to UAC 2 specification (p. 74)
+#define CFG_TUD_AUDIO_INTERRUPT_EP_SZ                       6
 
 // Use software encoding/decoding
 
@@ -241,7 +247,8 @@
 // Enable encoding/decodings - for these to work, support FIFOs need to be setup in appropriate numbers and size
 // The actual coding parameters of active AS alternate interface is parsed from the descriptors
 
-// The item size of the FIFO is always fixed to one i.e. bytes! Furthermore, the actively used FIFO depth is reconfigured such that the depth is a multiple of the current sample size in order to avoid samples to get split up in case of a wrap in the FIFO ring buffer (depth = (max_depth / sampe_sz) * sampe_sz)!
+// The item size of the FIFO is always fixed to one i.e. bytes! Furthermore, the actively used FIFO depth is reconfigured such that the depth is a multiple
+// of the current sample size in order to avoid samples to get split up in case of a wrap in the FIFO ring buffer (depth = (max_depth / sample_sz) * sample_sz)!
 // This is important to remind in case you use DMAs! If the sample sizes changes, the DMA MUST BE RECONFIGURED just like the FIFOs for a different depth!!!
 
 // For PCM encoding/decoding
@@ -388,10 +395,11 @@ uint16_t tud_audio_n_write_support_ff             (uint8_t func_id, uint8_t ff_i
 tu_fifo_t* tud_audio_n_get_tx_support_ff          (uint8_t func_id, uint8_t ff_idx);
 #endif
 
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-uint16_t    tud_audio_int_ctr_n_write             (uint8_t func_id, uint8_t const* buffer, uint16_t len);
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+bool    tud_audio_int_n_write                     (uint8_t func_id, const audio_interrupt_data_t * data);
 #endif
 
+
 //--------------------------------------------------------------------+
 // Application API (Interface0)
 //--------------------------------------------------------------------+
@@ -431,8 +439,8 @@ static inline tu_fifo_t* tud_audio_get_tx_support_ff        (uint8_t ff_idx);
 
 // INT CTR API
 
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-static inline uint16_t tud_audio_int_ctr_write              (uint8_t const* buffer, uint16_t len);
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+static inline bool tud_audio_int_write                      (const audio_interrupt_data_t * data);
 #endif
 
 // Buffer control EP data and schedule a transmit
@@ -444,63 +452,80 @@ static inline uint16_t tud_audio_int_ctr_write              (uint8_t const* buff
 bool tud_audio_buffer_and_schedule_control_xfer(uint8_t rhport, tusb_control_request_t const * p_request, void* data, uint16_t len);
 
 //--------------------------------------------------------------------+
-// Application Callback API (weak is optional)
+// Application Callback API
 //--------------------------------------------------------------------+
 
 #if CFG_TUD_AUDIO_ENABLE_EP_IN
-TU_ATTR_WEAK bool tud_audio_tx_done_pre_load_cb(uint8_t rhport, uint8_t func_id, uint8_t ep_in, uint8_t cur_alt_setting);
-TU_ATTR_WEAK bool tud_audio_tx_done_post_load_cb(uint8_t rhport, uint16_t n_bytes_copied, uint8_t func_id, uint8_t ep_in, uint8_t cur_alt_setting);
+bool tud_audio_tx_done_pre_load_cb(uint8_t rhport, uint8_t func_id, uint8_t ep_in, uint8_t cur_alt_setting);
+bool tud_audio_tx_done_post_load_cb(uint8_t rhport, uint16_t n_bytes_copied, uint8_t func_id, uint8_t ep_in, uint8_t cur_alt_setting);
 #endif
 
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT
-TU_ATTR_WEAK bool tud_audio_rx_done_pre_read_cb(uint8_t rhport, uint16_t n_bytes_received, uint8_t func_id, uint8_t ep_out, uint8_t cur_alt_setting);
-TU_ATTR_WEAK bool tud_audio_rx_done_post_read_cb(uint8_t rhport, uint16_t n_bytes_received, uint8_t func_id, uint8_t ep_out, uint8_t cur_alt_setting);
+bool tud_audio_rx_done_pre_read_cb(uint8_t rhport, uint16_t n_bytes_received, uint8_t func_id, uint8_t ep_out, uint8_t cur_alt_setting);
+bool tud_audio_rx_done_post_read_cb(uint8_t rhport, uint16_t n_bytes_received, uint8_t func_id, uint8_t ep_out, uint8_t cur_alt_setting);
 #endif
 
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
-TU_ATTR_WEAK void tud_audio_fb_done_cb(uint8_t func_id);
+void tud_audio_fb_done_cb(uint8_t func_id);
 
 
-// determined by the user itself and set by use of tud_audio_n_fb_set(). The feedback value may be determined e.g. from some fill status of some FIFO buffer. Advantage: No ISR interrupt is enabled, hence the CPU need not to handle an ISR every 1ms or 125us and thus less CPU load, disadvantage: typically a larger FIFO is needed to compensate for jitter (e.g. 8 frames), i.e. a larger delay is introduced.
-
-// Feedback value is calculated within the audio driver by use of SOF interrupt. The driver needs information about the master clock f_m from which the audio sample frequency f_s is derived, f_s itself, and the cycle count of f_m at time of the SOF interrupt (e.g. by use of a hardware counter) - see tud_audio_set_fb_params(). Advantage: Reduced jitter in the feedback value computation, hence, the receive FIFO can be smaller (e.g. 2 frames) and thus a smaller delay is possible, disadvantage: higher CPU load due to SOF ISR handling every frame i.e. 1ms or 125us. This option is a great starting point to try the SOF ISR option but depending on your hardware setup (performance of the CPU) it might not work. If so, figure out why and use the next option. (The most critical point is the reading of the cycle counter value of f_m. It is read from within the SOF ISR - see: audiod_sof() -, hence, the ISR must has a high priority such that no software dependent "random" delay i.e. jitter is introduced).
+// Note about feedback calculation
+//
+// Option 1 - AUDIO_FEEDBACK_METHOD_FIFO_COUNT
+// Feedback value is calculated within the audio driver by regulating the FIFO level to half fill.
+// Advantage: No ISR interrupt is enabled, hence the CPU need not to handle an ISR every 1ms or 125us and thus less CPU load, well tested
+// (Windows, Linux, OSX) with a reliable result so far.
+// Disadvantage: A FIFO of minimal 4 frames is needed to compensate for jitter, an average delay of 2 frames is introduced.
+//
+// Option 2 - AUDIO_FEEDBACK_METHOD_FREQUENCY_FIXED / AUDIO_FEEDBACK_METHOD_FREQUENCY_FLOAT
+// Feedback value is calculated within the audio driver by use of SOF interrupt. The driver needs information about the master clock f_m from
+// which the audio sample frequency f_s is derived, f_s itself, and the cycle count of f_m at time of the SOF interrupt (e.g. by use of a hardware counter).
+// See tud_audio_set_fb_params() and tud_audio_feedback_update()
+// Advantage: Reduced jitter in the feedback value computation, hence, the receive FIFO can be smaller and thus a smaller delay is possible.
+// Disadvantage: higher CPU load due to SOF ISR handling every frame i.e. 1ms or 125us. (The most critical point is the reading of the cycle counter value of f_m.
+// It is read from within the SOF ISR - see: audiod_sof() -, hence, the ISR must has a high priority such that no software dependent "random" delay i.e. jitter is introduced).
+// Long-term drift could occur since error is accumulated.
+//
+// Option 3 - manual
+// Determined by the user itself and set by use of tud_audio_n_fb_set(). The feedback value may be determined e.g. from some fill status of some FIFO buffer.
+// Advantage: No ISR interrupt is enabled, hence the CPU need not to handle an ISR every 1ms or 125us and thus less CPU load.
+// Disadvantage: typically a larger FIFO is needed to compensate for jitter (e.g. 6 frames), i.e. a larger delay is introduced.
 
-// Feedback value is determined by the user by use of SOF interrupt. The user may use tud_audio_sof_isr() which is called every SOF (of course only invoked when an alternate interface other than zero was set). The number of frames used to determine the feedback value for the currently active alternate setting can be get by tud_audio_get_fb_n_frames(). The feedback value must be set by use of tud_audio_n_fb_set().
 
 // This function is used to provide data rate feedback from an asynchronous sink. Feedback value will be sent at FB endpoint interval till it's changed.
 //
 // The feedback format is specified to be 16.16 for HS and 10.14 for FS devices (see Universal Serial Bus Specification Revision 2.0 5.12.4.2). By default,
-// the choice of format is left to the caller and feedback argument is sent as-is. If CFG_TUD_AUDIO_ENABLE_FEEDBACK_FORMAT_CORRECTION is set, then tinyusb
-// expects 16.16 format and handles the conversion to 10.14 on FS.
+// the choice of format is left to the caller and feedback argument is sent as-is. If CFG_TUD_AUDIO_ENABLE_FEEDBACK_FORMAT_CORRECTION is set or tud_audio_feedback_format_correction_cb()
+// return true, then tinyusb expects 16.16 format and handles the conversion to 10.14 on FS.
+//
+// Note that due to a bug in its USB Audio 2.0 driver, Windows currently requires 16.16 format for _all_ USB 2.0 devices. On Linux and it seems the
+// driver can work with either format.
 //
-// Note that due to a bug in its USB Audio 2.0 driver, Windows currently requires 16.16 format for _all_ USB 2.0 devices. On Linux and macOS it seems the 
-// driver can work with either format. So a good compromise is to keep format correction disabled and stick to 16.16 format.
-
 // Feedback value can be determined from within the SOF ISR of the audio driver. This should reduce jitter. If the feature is used, the user can not set the feedback value.
-
+//
 // Determine feedback value - The feedback method is described in 5.12.4.2 of the USB 2.0 spec
 // Boiled down, the feedback value Ff = n_samples / (micro)frame.
-// Since an accuracy of less than 1 Sample / second is desired, at least n_frames = ceil(2^K * f_s / f_m) frames need to be measured, where K = 10 for full speed and K = 13 for high speed, f_s is the sampling frequency e.g. 48 kHz and f_m is the cpu clock frequency e.g. 100 MHz (or any other master clock whose clock count is available and locked to f_s)
+// Since an accuracy of less than 1 Sample / second is desired, at least n_frames = ceil(2^K * f_s / f_m) frames need to be measured, where K = 10 for full speed and K = 13
+// for high speed, f_s is the sampling frequency e.g. 48 kHz and f_m is the cpu clock frequency e.g. 100 MHz (or any other master clock whose clock count is available and locked to f_s)
 // The update interval in the (4.10.2.1) Feedback Endpoint Descriptor must be less or equal to 2^(K - P), where P = min( ceil(log2(f_m / f_s)), K)
 // feedback = n_cycles / n_frames * f_s / f_m in 16.16 format, where n_cycles are the number of main clock cycles within fb_n_frames
-
 bool tud_audio_n_fb_set(uint8_t func_id, uint32_t feedback);
-static inline bool tud_audio_fb_set(uint32_t feedback);
 
-// Update feedback value with passed cycles since last time this update function is called.
+// Update feedback value with passed MCLK cycles since last time this update function is called.
 // Typically called within tud_audio_sof_isr(). Required tud_audio_feedback_params_cb() is implemented
 // This function will also call tud_audio_feedback_set()
 // return feedback value in 16.16 for reference (0 for error)
+// Example :
+//   binterval=3 (4ms); FS = 48kHz; MCLK = 12.288MHz
+//   In 4 SOF MCLK counted 49152 cycles
 uint32_t tud_audio_feedback_update(uint8_t func_id, uint32_t cycles);
 
 enum {
   AUDIO_FEEDBACK_METHOD_DISABLED,
   AUDIO_FEEDBACK_METHOD_FREQUENCY_FIXED,
   AUDIO_FEEDBACK_METHOD_FREQUENCY_FLOAT,
-  AUDIO_FEEDBACK_METHOD_FREQUENCY_POWER_OF_2,
-
-  // impelemnt later
-  // AUDIO_FEEDBACK_METHOD_FIFO_COUNT
+  AUDIO_FEEDBACK_METHOD_FREQUENCY_POWER_OF_2, // For driver internal use only
+  AUDIO_FEEDBACK_METHOD_FIFO_COUNT
 };
 
 typedef struct {
@@ -512,52 +537,50 @@ typedef struct {
       uint32_t mclk_freq; // Main clock frequency in Hz i.e. master clock to which sample clock is based on
     }frequency;
 
-#if 0 // implement later
-    struct {
-      uint32_t threshold_bytes; // minimum number of bytes received to be considered as filled/ready
-    }fifo_count;
-#endif
   };
 }audio_feedback_params_t;
 
 // Invoked when needed to set feedback parameters
-TU_ATTR_WEAK void tud_audio_feedback_params_cb(uint8_t func_id, uint8_t alt_itf, audio_feedback_params_t* feedback_param);
+void tud_audio_feedback_params_cb(uint8_t func_id, uint8_t alt_itf, audio_feedback_params_t* feedback_param);
 
 // Callback in ISR context, invoked periodically according to feedback endpoint bInterval.
 // Could be used to compute and update feedback value, should be placed in RAM if possible
 // frame_number  : current SOF count
 // interval_shift: number of bit shift i.e log2(interval) from Feedback endpoint descriptor
-TU_ATTR_WEAK TU_ATTR_FAST_FUNC void tud_audio_feedback_interval_isr(uint8_t func_id, uint32_t frame_number, uint8_t interval_shift);
+TU_ATTR_FAST_FUNC void tud_audio_feedback_interval_isr(uint8_t func_id, uint32_t frame_number, uint8_t interval_shift);
 
+// (Full-Speed only) Callback to set feedback format correction is applied or not,
+// default to CFG_TUD_AUDIO_ENABLE_FEEDBACK_FORMAT_CORRECTION if not implemented.
+bool tud_audio_feedback_format_correction_cb(uint8_t func_id);
 #endif // CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
 
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-TU_ATTR_WEAK bool tud_audio_int_ctr_done_cb(uint8_t rhport, uint16_t n_bytes_copied);
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+void tud_audio_int_done_cb(uint8_t rhport);
 #endif
 
 // Invoked when audio set interface request received
-TU_ATTR_WEAK bool tud_audio_set_itf_cb(uint8_t rhport, tusb_control_request_t const * p_request);
+bool tud_audio_set_itf_cb(uint8_t rhport, tusb_control_request_t const * p_request);
 
 // Invoked when audio set interface request received which closes an EP
-TU_ATTR_WEAK bool tud_audio_set_itf_close_EP_cb(uint8_t rhport, tusb_control_request_t const * p_request);
+bool tud_audio_set_itf_close_EP_cb(uint8_t rhport, tusb_control_request_t const * p_request);
 
 // Invoked when audio class specific set request received for an EP
-TU_ATTR_WEAK bool tud_audio_set_req_ep_cb(uint8_t rhport, tusb_control_request_t const * p_request, uint8_t *pBuff);
+bool tud_audio_set_req_ep_cb(uint8_t rhport, tusb_control_request_t const * p_request, uint8_t *pBuff);
 
 // Invoked when audio class specific set request received for an interface
-TU_ATTR_WEAK bool tud_audio_set_req_itf_cb(uint8_t rhport, tusb_control_request_t const * p_request, uint8_t *pBuff);
+bool tud_audio_set_req_itf_cb(uint8_t rhport, tusb_control_request_t const * p_request, uint8_t *pBuff);
 
 // Invoked when audio class specific set request received for an entity
-TU_ATTR_WEAK bool tud_audio_set_req_entity_cb(uint8_t rhport, tusb_control_request_t const * p_request, uint8_t *pBuff);
+bool tud_audio_set_req_entity_cb(uint8_t rhport, tusb_control_request_t const * p_request, uint8_t *pBuff);
 
 // Invoked when audio class specific get request received for an EP
-TU_ATTR_WEAK bool tud_audio_get_req_ep_cb(uint8_t rhport, tusb_control_request_t const * p_request);
+bool tud_audio_get_req_ep_cb(uint8_t rhport, tusb_control_request_t const * p_request);
 
 // Invoked when audio class specific get request received for an interface
-TU_ATTR_WEAK bool tud_audio_get_req_itf_cb(uint8_t rhport, tusb_control_request_t const * p_request);
+bool tud_audio_get_req_itf_cb(uint8_t rhport, tusb_control_request_t const * p_request);
 
 // Invoked when audio class specific get request received for an entity
-TU_ATTR_WEAK bool tud_audio_get_req_entity_cb(uint8_t rhport, tusb_control_request_t const * p_request);
+bool tud_audio_get_req_entity_cb(uint8_t rhport, tusb_control_request_t const * p_request);
 
 //--------------------------------------------------------------------+
 // Inline Functions
@@ -663,10 +686,10 @@ static inline tu_fifo_t* tud_audio_get_tx_support_ff(uint8_t ff_idx)
 
 #endif
 
-#if CFG_TUD_AUDIO_INT_CTR_EPSIZE_IN
-static inline uint16_t tud_audio_int_ctr_write(uint8_t const* buffer, uint16_t len)
+#if CFG_TUD_AUDIO_ENABLE_INTERRUPT_EP
+static inline bool tud_audio_int_write(const audio_interrupt_data_t * data)
 {
-  return tud_audio_int_ctr_n_write(0, buffer, len);
+  return tud_audio_int_n_write(0, data);
 }
 #endif
 
@@ -683,6 +706,7 @@ static inline bool tud_audio_fb_set(uint32_t feedback)
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     audiod_init           (void);
+bool     audiod_deinit         (void);
 void     audiod_reset          (uint8_t rhport);
 uint16_t audiod_open           (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 bool     audiod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
diff --git a/src/class/bth/bth_device.c b/src/class/bth/bth_device.c
index f96bb3552..45cbf2d98 100755
--- a/src/class/bth/bth_device.c
+++ b/src/class/bth/bth_device.c
@@ -37,25 +37,29 @@
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
-typedef struct
-{
+typedef struct {
   uint8_t itf_num;
   uint8_t ep_ev;
   uint8_t ep_acl_in;
+  uint16_t ep_acl_in_pkt_sz;
   uint8_t ep_acl_out;
   uint8_t ep_voice[2];  // Not used yet
   uint8_t ep_voice_size[2][CFG_TUD_BTH_ISO_ALT_COUNT];
 
-  // Endpoint Transfer buffer
-  CFG_TUSB_MEM_ALIGN bt_hci_cmd_t hci_cmd;
-  CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_BTH_DATA_EPSIZE];
-
+  // Previous amount of bytes sent when issuing ZLP
+  uint32_t prev_xferred_bytes;
 } btd_interface_t;
 
+typedef struct {
+  TUD_EPBUF_DEF(epout_buf, CFG_TUD_BTH_DATA_EPSIZE);
+  TUD_EPBUF_TYPE_DEF(bt_hci_cmd_t, hci_cmd);
+} btd_epbuf_t;
+
 //--------------------------------------------------------------------+
 // INTERNAL OBJECT & FUNCTION DECLARATION
 //--------------------------------------------------------------------+
-CFG_TUSB_MEM_SECTION btd_interface_t _btd_itf;
+static btd_interface_t _btd_itf;
+CFG_TUD_MEM_SECTION static btd_epbuf_t _btd_epbuf;
 
 static bool bt_tx_data(uint8_t ep, void *data, uint16_t len)
 {
@@ -91,11 +95,14 @@ bool tud_bt_acl_data_send(void *event, uint16_t event_len)
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void btd_init(void)
-{
+void btd_init(void) {
   tu_memclr(&_btd_itf, sizeof(_btd_itf));
 }
 
+bool btd_deinit(void) {
+  return true;
+}
+
 void btd_reset(uint8_t rhport)
 {
   (void)rhport;
@@ -124,14 +131,28 @@ uint16_t btd_open(uint8_t rhport, tusb_desc_interface_t const *itf_desc, uint16_
   TU_ASSERT(usbd_edpt_open(rhport, desc_ep), 0);
   _btd_itf.ep_ev = desc_ep->bEndpointAddress;
 
+  desc_ep = (tusb_desc_endpoint_t const *)tu_desc_next(desc_ep);
+
   // Open endpoint pair
-  TU_ASSERT(usbd_open_edpt_pair(rhport, tu_desc_next(desc_ep), 2, TUSB_XFER_BULK, &_btd_itf.ep_acl_out,
-                                &_btd_itf.ep_acl_in), 0);
+  TU_ASSERT(usbd_open_edpt_pair(rhport, (uint8_t const *)desc_ep, 2,
+                                TUSB_XFER_BULK, &_btd_itf.ep_acl_out,
+                                &_btd_itf.ep_acl_in),
+            0);
 
-  itf_desc = (tusb_desc_interface_t const *)tu_desc_next(tu_desc_next(tu_desc_next(desc_ep)));
+  // Save acl in endpoint max packet size
+  tusb_desc_endpoint_t const *desc_ep_acl_in = desc_ep;
+  for (size_t p = 0; p < 2; p++) {
+    if (tu_edpt_dir(desc_ep_acl_in->bEndpointAddress) == TUSB_DIR_IN) {
+      _btd_itf.ep_acl_in_pkt_sz = tu_edpt_packet_size(desc_ep_acl_in);
+      break;
+    }
+    desc_ep_acl_in = (tusb_desc_endpoint_t const *)tu_desc_next(desc_ep_acl_in);
+  }
+
+  itf_desc = (tusb_desc_interface_t const *)tu_desc_next(tu_desc_next(desc_ep));
 
   // Prepare for incoming data from host
-  TU_ASSERT(usbd_edpt_xfer(rhport, _btd_itf.ep_acl_out, _btd_itf.epout_buf, CFG_TUD_BTH_DATA_EPSIZE), 0);
+  TU_ASSERT(usbd_edpt_xfer(rhport, _btd_itf.ep_acl_out, _btd_epbuf.epout_buf, CFG_TUD_BTH_DATA_EPSIZE), 0);
 
   drv_len = hci_itf_size;
 
@@ -204,7 +225,9 @@ bool btd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t c
         request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_DEVICE)
     {
       // HCI command packet addressing for single function Primary Controllers
-      TU_VERIFY(request->bRequest == 0 && request->wValue == 0 && request->wIndex == 0);
+      // also compatible with historical mode if enabled
+      TU_VERIFY((request->bRequest == 0 && request->wValue == 0 && request->wIndex == 0) ||
+                (CFG_TUD_BTH_HISTORICAL_COMPATIBLE && request->bRequest == 0xe0));
     }
     else if (request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_INTERFACE)
     {
@@ -220,30 +243,30 @@ bool btd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t c
     }
     else return false;
 
-    return tud_control_xfer(rhport, request, &_btd_itf.hci_cmd, sizeof(_btd_itf.hci_cmd));
+    return tud_control_xfer(rhport, request, &_btd_epbuf.hci_cmd, sizeof(bt_hci_cmd_t));
   }
   else if ( stage == CONTROL_STAGE_DATA )
   {
     // Handle class request only
     TU_VERIFY(request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS);
 
-    if (tud_bt_hci_cmd_cb) tud_bt_hci_cmd_cb(&_btd_itf.hci_cmd, tu_min16(request->wLength, sizeof(_btd_itf.hci_cmd)));
+    if (tud_bt_hci_cmd_cb) {
+      tud_bt_hci_cmd_cb(&_btd_epbuf.hci_cmd, tu_min16(request->wLength, sizeof(bt_hci_cmd_t)));
+      }
   }
 
   return true;
 }
 
-bool btd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
-{
-  (void)result;
-
+bool btd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result,
+                 uint32_t xferred_bytes) {
   // received new data from host
   if (ep_addr == _btd_itf.ep_acl_out)
   {
-    if (tud_bt_acl_data_received_cb) tud_bt_acl_data_received_cb(_btd_itf.epout_buf, xferred_bytes);
+    if (tud_bt_acl_data_received_cb) tud_bt_acl_data_received_cb(_btd_epbuf.epout_buf, xferred_bytes);
 
     // prepare for next data
-    TU_ASSERT(usbd_edpt_xfer(rhport, _btd_itf.ep_acl_out, _btd_itf.epout_buf, CFG_TUD_BTH_DATA_EPSIZE));
+    TU_ASSERT(usbd_edpt_xfer(rhport, _btd_itf.ep_acl_out, _btd_epbuf.epout_buf, CFG_TUD_BTH_DATA_EPSIZE));
   }
   else if (ep_addr == _btd_itf.ep_ev)
   {
@@ -251,7 +274,20 @@ bool btd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t
   }
   else if (ep_addr == _btd_itf.ep_acl_in)
   {
-    if (tud_bt_acl_data_sent_cb) tud_bt_acl_data_sent_cb((uint16_t)xferred_bytes);
+    if ((result == XFER_RESULT_SUCCESS) && (xferred_bytes > 0) &&
+        ((xferred_bytes & (_btd_itf.ep_acl_in_pkt_sz - 1)) == 0)) {
+      // Save number of transferred bytes
+      _btd_itf.prev_xferred_bytes = xferred_bytes;
+
+      // Send zero-length packet
+      tud_bt_acl_data_send(NULL, 0);
+    } else if (tud_bt_acl_data_sent_cb) {
+      if (xferred_bytes == 0) {
+        xferred_bytes = _btd_itf.prev_xferred_bytes;
+        _btd_itf.prev_xferred_bytes = 0;
+      }
+      tud_bt_acl_data_sent_cb((uint16_t)xferred_bytes);
+    }
   }
 
   return true;
diff --git a/src/class/bth/bth_device.h b/src/class/bth/bth_device.h
index 1b90d0915..4f6350839 100755
--- a/src/class/bth/bth_device.h
+++ b/src/class/bth/bth_device.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2020 Jerzy Kasenberg
@@ -36,10 +36,17 @@
 #ifndef CFG_TUD_BTH_EVENT_EPSIZE
 #define CFG_TUD_BTH_EVENT_EPSIZE     16
 #endif
+
 #ifndef CFG_TUD_BTH_DATA_EPSIZE
 #define CFG_TUD_BTH_DATA_EPSIZE      64
 #endif
 
+// Allow BTH class to work in historically compatibility mode where the bRequest is always 0xe0.
+// See Bluetooth Core v5.3, Vol. 4, Part B, Section 2.2
+#ifndef CFG_TUD_BTH_HISTORICAL_COMPATIBLE
+#define CFG_TUD_BTH_HISTORICAL_COMPATIBLE 0
+#endif
+
 typedef struct TU_ATTR_PACKED
 {
   uint16_t op_code;
@@ -97,6 +104,7 @@ bool tud_bt_acl_data_send(void *acl_data, uint16_t data_len);
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     btd_init            (void);
+bool     btd_deinit          (void);
 void     btd_reset           (uint8_t rhport);
 uint16_t btd_open            (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 bool     btd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const *request);
diff --git a/src/class/cdc/cdc.h b/src/class/cdc/cdc.h
index e345139ea..5cbd658fe 100644
--- a/src/class/cdc/cdc.h
+++ b/src/class/cdc/cdc.h
@@ -41,16 +41,6 @@
 /** \defgroup ClassDriver_CDC_Common Common Definitions
  *  @{ */
 
-// TODO remove
-/// CDC Pipe ID, used to indicate which pipe the API is addressing to (Notification, Out, In)
-typedef enum
-{
-  CDC_PIPE_NOTIFICATION , ///< Notification pipe
-  CDC_PIPE_DATA_IN      , ///< Data in pipe
-  CDC_PIPE_DATA_OUT     , ///< Data out pipe
-  CDC_PIPE_ERROR        , ///< Invalid Pipe ID
-}cdc_pipeid_t;
-
 //--------------------------------------------------------------------+
 // CDC Communication Interface Class
 //--------------------------------------------------------------------+
@@ -146,8 +136,7 @@ typedef enum{
 //--------------------------------------------------------------------+
 
 /// Communication Interface Management Element Request Codes
-typedef enum
-{
+typedef enum {
   CDC_REQUEST_SEND_ENCAPSULATED_COMMAND                    = 0x00, ///< is used to issue a command in the format of the supported control protocol of the Communications Class interface
   CDC_REQUEST_GET_ENCAPSULATED_RESPONSE                    = 0x01, ///< is used to request a response in the format of the supported control protocol of the Communications Class interface.
   CDC_REQUEST_SET_COMM_FEATURE                             = 0x02,
@@ -190,15 +179,38 @@ typedef enum
   CDC_REQUEST_GET_ATM_VC_STATISTICS                        = 0x53,
 
   CDC_REQUEST_MDLM_SEMANTIC_MODEL                          = 0x60,
-}cdc_management_request_t;
+} cdc_management_request_t;
+
+typedef enum {
+  CDC_CONTROL_LINE_STATE_DTR = 0x01,
+  CDC_CONTROL_LINE_STATE_RTS = 0x02,
+} cdc_control_line_state_t;
+
+typedef enum {
+  CDC_LINE_CODING_STOP_BITS_1   = 0, // 1   bit
+  CDC_LINE_CODING_STOP_BITS_1_5 = 1, // 1.5 bits
+  CDC_LINE_CODING_STOP_BITS_2   = 2, // 2   bits
+} cdc_line_coding_stopbits_t;
+
+// TODO Backward compatible for typos. Maybe removed in the future release
+#define CDC_LINE_CONDING_STOP_BITS_1   CDC_LINE_CODING_STOP_BITS_1
+#define CDC_LINE_CONDING_STOP_BITS_1_5 CDC_LINE_CODING_STOP_BITS_1_5
+#define CDC_LINE_CONDING_STOP_BITS_2   CDC_LINE_CODING_STOP_BITS_2
+
+typedef enum {
+  CDC_LINE_CODING_PARITY_NONE  = 0,
+  CDC_LINE_CODING_PARITY_ODD   = 1,
+  CDC_LINE_CODING_PARITY_EVEN  = 2,
+  CDC_LINE_CODING_PARITY_MARK  = 3,
+  CDC_LINE_CODING_PARITY_SPACE = 4,
+} cdc_line_coding_parity_t;
 
 //--------------------------------------------------------------------+
-// Management Elemenent Notification (Notification Endpoint)
+// Management Element Notification (Notification Endpoint)
 //--------------------------------------------------------------------+
 
 /// 6.3 Notification Codes
-typedef enum
-{
+typedef enum {
   CDC_NOTIF_NETWORK_CONNECTION               = 0x00, ///< This notification allows the device to notify the host about network connection status.
   CDC_NOTIF_RESPONSE_AVAILABLE               = 0x01, ///< This notification allows the device to notify the hostthat a response is available. This response can be retrieved with a subsequent \ref CDC_REQUEST_GET_ENCAPSULATED_RESPONSE request.
   CDC_NOTIF_AUX_JACK_HOOK_STATE              = 0x08,
@@ -365,7 +377,9 @@ typedef struct TU_ATTR_PACKED
     uint32_t incoming_distinctive   : 1; ///< 0 : Reports only incoming ringing. 1 : Reports incoming distinctive ringing patterns.
     uint32_t dual_tone_multi_freq   : 1; ///< 0 : Cannot report dual tone multi-frequency (DTMF) digits input remotely over the telephone line. 1 : Can report DTMF digits input remotely over the telephone line.
     uint32_t line_state_change      : 1; ///< 0 : Does not support line state change notification. 1 : Does support line state change notification
-    uint32_t TU_RESERVED            : 26;
+    uint32_t TU_RESERVED0           : 2;
+    uint32_t TU_RESERVED1           : 16;
+    uint32_t TU_RESERVED2           : 8;
   } bmCapabilities;
 }cdc_desc_func_telephone_call_state_reporting_capabilities_t;
 
@@ -390,9 +404,10 @@ TU_VERIFY_STATIC(sizeof(cdc_line_coding_t) == 7, "size is not correct");
 
 typedef struct TU_ATTR_PACKED
 {
-  uint16_t dte_is_present : 1; ///< Indicates to DCE if DTE is presentor not. This signal corresponds to V.24 signal 108/2 and RS-232 signal DTR.
-  uint16_t half_duplex_carrier_control : 1;
-  uint16_t : 14;
+  uint16_t dtr : 1;
+  uint16_t rts : 1;
+  uint16_t : 6;
+  uint16_t : 8;
 } cdc_line_control_state_t;
 
 TU_VERIFY_STATIC(sizeof(cdc_line_control_state_t) == 2, "size is not correct");
diff --git a/src/class/cdc/cdc_device.c b/src/class/cdc/cdc_device.c
index fab6f0035..efb672201 100644
--- a/src/class/cdc/cdc_device.c
+++ b/src/class/cdc/cdc_device.c
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -33,16 +33,19 @@
 
 #include "cdc_device.h"
 
+// Level where CFG_TUSB_DEBUG must be at least for this driver is logged
+#ifndef CFG_TUD_CDC_LOG_LEVEL
+  #define CFG_TUD_CDC_LOG_LEVEL   CFG_TUD_LOG_LEVEL
+#endif
+
+#define TU_LOG_DRV(...)   TU_LOG(CFG_TUD_CDC_LOG_LEVEL, __VA_ARGS__)
+
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
-enum
-{
-  BULK_PACKET_SIZE = (TUD_OPT_HIGH_SPEED ? 512 : 64)
-};
+#define BULK_PACKET_SIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
 
-typedef struct
-{
+typedef struct {
   uint8_t itf_num;
   uint8_t ep_notif;
   uint8_t ep_in;
@@ -52,8 +55,8 @@ typedef struct
   uint8_t line_state;
 
   /*------------- From this point, data is not cleared by bus reset -------------*/
-  char    wanted_char;
-  cdc_line_coding_t line_coding;
+  char wanted_char;
+  TU_ATTR_ALIGNED(4) cdc_line_coding_t line_coding;
 
   // FIFO
   tu_fifo_t rx_ff;
@@ -62,34 +65,40 @@ typedef struct
   uint8_t rx_ff_buf[CFG_TUD_CDC_RX_BUFSIZE];
   uint8_t tx_ff_buf[CFG_TUD_CDC_TX_BUFSIZE];
 
-#if CFG_FIFO_MUTEX
-  osal_mutex_def_t rx_ff_mutex;
-  osal_mutex_def_t tx_ff_mutex;
-#endif
-
-  // Endpoint Transfer buffer
-  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;
+  OSAL_MUTEX_DEF(rx_ff_mutex);
+  OSAL_MUTEX_DEF(tx_ff_mutex);
+} cdcd_interface_t;
 
 #define ITF_MEM_RESET_SIZE   offsetof(cdcd_interface_t, wanted_char)
 
+typedef struct {
+  TUD_EPBUF_DEF(epout, CFG_TUD_CDC_EP_BUFSIZE);
+  TUD_EPBUF_DEF(epin, CFG_TUD_CDC_EP_BUFSIZE);
+} cdcd_epbuf_t;
+
 //--------------------------------------------------------------------+
 // INTERNAL OBJECT & FUNCTION DECLARATION
 //--------------------------------------------------------------------+
-CFG_TUSB_MEM_SECTION static cdcd_interface_t _cdcd_itf[CFG_TUD_CDC];
+static cdcd_interface_t _cdcd_itf[CFG_TUD_CDC];
+CFG_TUD_MEM_SECTION static cdcd_epbuf_t _cdcd_epbuf[CFG_TUD_CDC];
+
+static tud_cdc_configure_fifo_t _cdcd_fifo_cfg;
+
+static bool _prep_out_transaction(uint8_t itf) {
+  const uint8_t rhport = 0;
+  cdcd_interface_t* p_cdc = &_cdcd_itf[itf];
+  cdcd_epbuf_t* p_epbuf = &_cdcd_epbuf[itf];
+
+  // Skip if usb is not ready yet
+  TU_VERIFY(tud_ready() && p_cdc->ep_out);
 
-static bool _prep_out_transaction (cdcd_interface_t* p_cdc)
-{
-  uint8_t const rhport = 0;
   uint16_t available = tu_fifo_remaining(&p_cdc->rx_ff);
 
   // Prepare for incoming data but only allow what we can store in the ring buffer.
   // TODO Actually we can still carry out the transfer, keeping count of received bytes
   // and slowly move it to the FIFO when read().
   // This pre-check reduces endpoint claiming
-  TU_VERIFY(available >= sizeof(p_cdc->epout_buf));
+  TU_VERIFY(available >= CFG_TUD_CDC_EP_BUFSIZE);
 
   // claim endpoint
   TU_VERIFY(usbd_edpt_claim(rhport, p_cdc->ep_out));
@@ -97,14 +106,11 @@ static bool _prep_out_transaction (cdcd_interface_t* p_cdc)
   // fifo can be changed before endpoint is claimed
   available = tu_fifo_remaining(&p_cdc->rx_ff);
 
-  if ( available >= sizeof(p_cdc->epout_buf) )
-  {
-    return usbd_edpt_xfer(rhport, p_cdc->ep_out, p_cdc->epout_buf, sizeof(p_cdc->epout_buf));
-  }else
-  {
+  if (available >= CFG_TUD_CDC_EP_BUFSIZE) {
+    return usbd_edpt_xfer(rhport, p_cdc->ep_out, p_epbuf->epout, CFG_TUD_CDC_EP_BUFSIZE);
+  } else {
     // Release endpoint since we don't make any transfer
     usbd_edpt_release(rhport, p_cdc->ep_out);
-
     return false;
   }
 }
@@ -112,97 +118,101 @@ static bool _prep_out_transaction (cdcd_interface_t* p_cdc)
 //--------------------------------------------------------------------+
 // APPLICATION API
 //--------------------------------------------------------------------+
-bool tud_cdc_n_connected(uint8_t itf)
-{
+
+bool tud_cdc_configure_fifo(const tud_cdc_configure_fifo_t* cfg) {
+  TU_VERIFY(cfg);
+  _cdcd_fifo_cfg = (*cfg);
+  return true;
+}
+
+bool tud_cdc_n_ready(uint8_t itf) {
+  return tud_ready() && _cdcd_itf[itf].ep_in != 0 && _cdcd_itf[itf].ep_out != 0;
+}
+
+bool tud_cdc_n_connected(uint8_t itf) {
   // DTR (bit 0) active  is considered as connected
   return tud_ready() && tu_bit_test(_cdcd_itf[itf].line_state, 0);
 }
 
-uint8_t tud_cdc_n_get_line_state (uint8_t itf)
-{
+uint8_t tud_cdc_n_get_line_state(uint8_t itf) {
   return _cdcd_itf[itf].line_state;
 }
 
-void tud_cdc_n_get_line_coding (uint8_t itf, cdc_line_coding_t* coding)
-{
+void tud_cdc_n_get_line_coding(uint8_t itf, cdc_line_coding_t* coding) {
   (*coding) = _cdcd_itf[itf].line_coding;
 }
 
-void tud_cdc_n_set_wanted_char (uint8_t itf, char wanted)
-{
+void tud_cdc_n_set_wanted_char(uint8_t itf, char wanted) {
   _cdcd_itf[itf].wanted_char = wanted;
 }
 
-
 //--------------------------------------------------------------------+
 // READ API
 //--------------------------------------------------------------------+
-uint32_t tud_cdc_n_available(uint8_t itf)
-{
+uint32_t tud_cdc_n_available(uint8_t itf) {
   return tu_fifo_count(&_cdcd_itf[itf].rx_ff);
 }
 
-uint32_t tud_cdc_n_read(uint8_t itf, void* buffer, uint32_t bufsize)
-{
+uint32_t tud_cdc_n_read(uint8_t itf, void* buffer, uint32_t bufsize) {
   cdcd_interface_t* p_cdc = &_cdcd_itf[itf];
-  uint32_t num_read = tu_fifo_read_n(&p_cdc->rx_ff, buffer, (uint16_t) bufsize);
-  _prep_out_transaction(p_cdc);
+  uint32_t num_read = tu_fifo_read_n(&p_cdc->rx_ff, buffer, (uint16_t) TU_MIN(bufsize, UINT16_MAX));
+  _prep_out_transaction(itf);
   return num_read;
 }
 
-bool tud_cdc_n_peek(uint8_t itf, uint8_t* chr)
-{
+bool tud_cdc_n_peek(uint8_t itf, uint8_t* chr) {
   return tu_fifo_peek(&_cdcd_itf[itf].rx_ff, chr);
 }
 
-void tud_cdc_n_read_flush (uint8_t itf)
-{
+void tud_cdc_n_read_flush(uint8_t itf) {
   cdcd_interface_t* p_cdc = &_cdcd_itf[itf];
   tu_fifo_clear(&p_cdc->rx_ff);
-  _prep_out_transaction(p_cdc);
+  _prep_out_transaction(itf);
 }
 
 //--------------------------------------------------------------------+
 // WRITE API
 //--------------------------------------------------------------------+
-uint32_t tud_cdc_n_write(uint8_t itf, void const* buffer, uint32_t bufsize)
-{
+uint32_t tud_cdc_n_write(uint8_t itf, const void* buffer, uint32_t bufsize) {
   cdcd_interface_t* p_cdc = &_cdcd_itf[itf];
-  uint16_t ret = tu_fifo_write_n(&p_cdc->tx_ff, buffer, (uint16_t) bufsize);
+  uint16_t ret = tu_fifo_write_n(&p_cdc->tx_ff, buffer, (uint16_t) TU_MIN(bufsize, UINT16_MAX));
 
   // flush if queue more than packet size
-  if ( tu_fifo_count(&p_cdc->tx_ff) >= BULK_PACKET_SIZE )
-  {
+  if (tu_fifo_count(&p_cdc->tx_ff) >= BULK_PACKET_SIZE
+      #if CFG_TUD_CDC_TX_BUFSIZE < BULK_PACKET_SIZE
+      || tu_fifo_full(&p_cdc->tx_ff) // check full if fifo size is less than packet size
+      #endif
+      ) {
     tud_cdc_n_write_flush(itf);
   }
 
   return ret;
 }
 
-uint32_t tud_cdc_n_write_flush (uint8_t itf)
-{
+uint32_t tud_cdc_n_write_flush(uint8_t itf) {
   cdcd_interface_t* p_cdc = &_cdcd_itf[itf];
+  cdcd_epbuf_t* p_epbuf = &_cdcd_epbuf[itf];
 
   // Skip if usb is not ready yet
-  TU_VERIFY( tud_ready(), 0 );
+  TU_VERIFY(tud_ready(), 0);
 
   // No data to send
-  if ( !tu_fifo_count(&p_cdc->tx_ff) ) return 0;
+  if (!tu_fifo_count(&p_cdc->tx_ff)) {
+    return 0;
+  }
 
-  uint8_t const rhport = 0;
+  const uint8_t rhport = 0;
 
   // Claim the endpoint
-  TU_VERIFY( usbd_edpt_claim(rhport, p_cdc->ep_in), 0 );
+  TU_VERIFY(usbd_edpt_claim(rhport, p_cdc->ep_in), 0);
 
   // Pull data from FIFO
-  uint16_t const count = tu_fifo_read_n(&p_cdc->tx_ff, p_cdc->epin_buf, sizeof(p_cdc->epin_buf));
+  const uint16_t count = tu_fifo_read_n(&p_cdc->tx_ff, p_epbuf->epin, CFG_TUD_CDC_EP_BUFSIZE);
 
-  if ( count )
-  {
-    TU_ASSERT( usbd_edpt_xfer(rhport, p_cdc->ep_in, p_cdc->epin_buf, count), 0 );
+  if (count) {
+    TU_ASSERT(usbd_edpt_xfer(rhport, p_cdc->ep_in, p_epbuf->epin, count), 0);
     return count;
-  }else
-  {
+  } else {
     // Release endpoint since we don't make any transfer
     // Note: data is dropped if terminal is not connected
     usbd_edpt_release(rhport, p_cdc->ep_in);
@@ -210,33 +220,30 @@ uint32_t tud_cdc_n_write_flush (uint8_t itf)
   }
 }
 
-uint32_t tud_cdc_n_write_available (uint8_t itf)
-{
+uint32_t tud_cdc_n_write_available(uint8_t itf) {
   return tu_fifo_remaining(&_cdcd_itf[itf].tx_ff);
 }
 
-bool tud_cdc_n_write_clear (uint8_t itf)
-{
+bool tud_cdc_n_write_clear(uint8_t itf) {
   return tu_fifo_clear(&_cdcd_itf[itf].tx_ff);
 }
 
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void cdcd_init(void)
-{
+void cdcd_init(void) {
   tu_memclr(_cdcd_itf, sizeof(_cdcd_itf));
+  tu_memclr(&_cdcd_fifo_cfg, sizeof(_cdcd_fifo_cfg));
 
-  for(uint8_t i=0; i<CFG_TUD_CDC; i++)
-  {
+  for (uint8_t i = 0; i < CFG_TUD_CDC; i++) {
     cdcd_interface_t* p_cdc = &_cdcd_itf[i];
 
     p_cdc->wanted_char = (char) -1;
 
     // default line coding is : stop bit = 1, parity = none, data bits = 8
-    p_cdc->line_coding.bit_rate  = 115200;
+    p_cdc->line_coding.bit_rate = 115200;
     p_cdc->line_coding.stop_bits = 0;
-    p_cdc->line_coding.parity    = 0;
+    p_cdc->line_coding.parity = 0;
     p_cdc->line_coding.data_bits = 8;
 
     // Config RX fifo
@@ -247,87 +254,110 @@ void cdcd_init(void)
     // In this way, the most current data is prioritized.
     tu_fifo_config(&p_cdc->tx_ff, p_cdc->tx_ff_buf, TU_ARRAY_SIZE(p_cdc->tx_ff_buf), 1, true);
 
-#if CFG_FIFO_MUTEX
-    tu_fifo_config_mutex(&p_cdc->rx_ff, NULL, osal_mutex_create(&p_cdc->rx_ff_mutex));
-    tu_fifo_config_mutex(&p_cdc->tx_ff, osal_mutex_create(&p_cdc->tx_ff_mutex), NULL);
-#endif
+    #if OSAL_MUTEX_REQUIRED
+    osal_mutex_t mutex_rd = osal_mutex_create(&p_cdc->rx_ff_mutex);
+    osal_mutex_t mutex_wr = osal_mutex_create(&p_cdc->tx_ff_mutex);
+    TU_ASSERT(mutex_rd != NULL && mutex_wr != NULL, );
+
+    tu_fifo_config_mutex(&p_cdc->rx_ff, NULL, mutex_rd);
+    tu_fifo_config_mutex(&p_cdc->tx_ff, mutex_wr, NULL);
+    #endif
   }
 }
 
-void cdcd_reset(uint8_t rhport)
-{
+bool cdcd_deinit(void) {
+  #if OSAL_MUTEX_REQUIRED
+  for(uint8_t i=0; i<CFG_TUD_CDC; i++) {
+    cdcd_interface_t* p_cdc = &_cdcd_itf[i];
+    osal_mutex_t mutex_rd = p_cdc->rx_ff.mutex_rd;
+    osal_mutex_t mutex_wr = p_cdc->tx_ff.mutex_wr;
+
+    if (mutex_rd) {
+      osal_mutex_delete(mutex_rd);
+      tu_fifo_config_mutex(&p_cdc->rx_ff, NULL, NULL);
+    }
+
+    if (mutex_wr) {
+      osal_mutex_delete(mutex_wr);
+      tu_fifo_config_mutex(&p_cdc->tx_ff, NULL, NULL);
+    }
+  }
+  #endif
+
+  return true;
+}
+
+void cdcd_reset(uint8_t rhport) {
   (void) rhport;
 
-  for(uint8_t i=0; i<CFG_TUD_CDC; i++)
-  {
+  for (uint8_t i = 0; i < CFG_TUD_CDC; i++) {
     cdcd_interface_t* p_cdc = &_cdcd_itf[i];
 
     tu_memclr(p_cdc, ITF_MEM_RESET_SIZE);
-    tu_fifo_clear(&p_cdc->rx_ff);
-    tu_fifo_clear(&p_cdc->tx_ff);
+    if (!_cdcd_fifo_cfg.rx_persistent) {
+      tu_fifo_clear(&p_cdc->rx_ff);
+    }
+    if (!_cdcd_fifo_cfg.tx_persistent) {
+      tu_fifo_clear(&p_cdc->tx_ff);
+    }
     tu_fifo_set_overwritable(&p_cdc->tx_ff, true);
   }
 }
 
-uint16_t cdcd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len)
-{
+uint16_t cdcd_open(uint8_t rhport, const tusb_desc_interface_t* itf_desc, uint16_t max_len) {
   // Only support ACM subclass
   TU_VERIFY( TUSB_CLASS_CDC                           == itf_desc->bInterfaceClass &&
              CDC_COMM_SUBCLASS_ABSTRACT_CONTROL_MODEL == itf_desc->bInterfaceSubClass, 0);
 
   // Find available interface
-  cdcd_interface_t * p_cdc = NULL;
-  for(uint8_t cdc_id=0; cdc_id<CFG_TUD_CDC; cdc_id++)
-  {
-    if ( _cdcd_itf[cdc_id].ep_in == 0 )
-    {
-      p_cdc = &_cdcd_itf[cdc_id];
+  cdcd_interface_t* p_cdc;
+  uint8_t cdc_id;
+  for (cdc_id = 0; cdc_id < CFG_TUD_CDC; cdc_id++) {
+    p_cdc = &_cdcd_itf[cdc_id];
+    if (p_cdc->ep_in == 0) {
       break;
     }
   }
-  TU_ASSERT(p_cdc, 0);
+  TU_ASSERT(cdc_id < CFG_TUD_CDC, 0);
 
   //------------- Control Interface -------------//
   p_cdc->itf_num = itf_desc->bInterfaceNumber;
 
   uint16_t drv_len = sizeof(tusb_desc_interface_t);
-  uint8_t const * p_desc = tu_desc_next( itf_desc );
+  const uint8_t* p_desc = tu_desc_next(itf_desc);
 
   // Communication Functional Descriptors
-  while ( TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc) && drv_len <= max_len )
-  {
+  while (TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc) && drv_len <= max_len) {
     drv_len += tu_desc_len(p_desc);
-    p_desc   = tu_desc_next(p_desc);
+    p_desc = tu_desc_next(p_desc);
   }
 
-  if ( TUSB_DESC_ENDPOINT == tu_desc_type(p_desc) )
-  {
+  if (TUSB_DESC_ENDPOINT == tu_desc_type(p_desc)) {
     // notification endpoint
-    tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+    const tusb_desc_endpoint_t* desc_ep = (const tusb_desc_endpoint_t*) p_desc;
 
-    TU_ASSERT( usbd_edpt_open(rhport, desc_ep), 0 );
+    TU_ASSERT(usbd_edpt_open(rhport, desc_ep), 0);
     p_cdc->ep_notif = desc_ep->bEndpointAddress;
 
     drv_len += tu_desc_len(p_desc);
-    p_desc   = tu_desc_next(p_desc);
+    p_desc = tu_desc_next(p_desc);
   }
 
   //------------- Data Interface (if any) -------------//
-  if ( (TUSB_DESC_INTERFACE == tu_desc_type(p_desc)) &&
-       (TUSB_CLASS_CDC_DATA == ((tusb_desc_interface_t const *) p_desc)->bInterfaceClass) )
-  {
+  if ((TUSB_DESC_INTERFACE == tu_desc_type(p_desc)) &&
+      (TUSB_CLASS_CDC_DATA == ((const tusb_desc_interface_t*) p_desc)->bInterfaceClass)) {
     // next to endpoint descriptor
     drv_len += tu_desc_len(p_desc);
-    p_desc   = tu_desc_next(p_desc);
+    p_desc = tu_desc_next(p_desc);
 
     // Open endpoint pair
-    TU_ASSERT( usbd_open_edpt_pair(rhport, p_desc, 2, TUSB_XFER_BULK, &p_cdc->ep_out, &p_cdc->ep_in), 0 );
+    TU_ASSERT(usbd_open_edpt_pair(rhport, p_desc, 2, TUSB_XFER_BULK, &p_cdc->ep_out, &p_cdc->ep_in), 0);
 
-    drv_len += 2*sizeof(tusb_desc_endpoint_t);
+    drv_len += 2 * sizeof(tusb_desc_endpoint_t);
   }
 
   // Prepare for incoming data
-  _prep_out_transaction(p_cdc);
+  _prep_out_transaction(cdc_id);
 
   return drv_len;
 }
@@ -335,51 +365,45 @@ uint16_t cdcd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint1
 // Invoked when a control transfer occurred on an interface of this class
 // Driver response accordingly to the request and the transfer stage (setup/data/ack)
 // return false to stall control endpoint (e.g unsupported request)
-bool cdcd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
-{
+bool cdcd_control_xfer_cb(uint8_t rhport, uint8_t stage, const tusb_control_request_t* request) {
   // Handle class request only
   TU_VERIFY(request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS);
 
-  uint8_t itf = 0;
-  cdcd_interface_t* p_cdc = _cdcd_itf;
+  uint8_t itf;
+  cdcd_interface_t* p_cdc;
 
   // Identify which interface to use
-  for ( ; ; itf++, p_cdc++)
-  {
-    if (itf >= TU_ARRAY_SIZE(_cdcd_itf)) return false;
-
-    if ( p_cdc->itf_num == request->wIndex ) break;
+  for (itf = 0; itf < CFG_TUD_CDC; itf++) {
+    p_cdc = &_cdcd_itf[itf];
+    if (p_cdc->itf_num == request->wIndex) {
+      break;
+    }
   }
+  TU_VERIFY(itf < CFG_TUD_CDC);
 
-  switch ( request->bRequest )
-  {
+  switch (request->bRequest) {
     case CDC_REQUEST_SET_LINE_CODING:
-      if (stage == CONTROL_STAGE_SETUP)
-      {
-        TU_LOG2("  Set Line Coding\r\n");
+      if (stage == CONTROL_STAGE_SETUP) {
+        TU_LOG_DRV("  Set Line Coding\r\n");
         tud_control_xfer(rhport, request, &p_cdc->line_coding, sizeof(cdc_line_coding_t));
+      } else if (stage == CONTROL_STAGE_ACK) {
+        if (tud_cdc_line_coding_cb) {
+          tud_cdc_line_coding_cb(itf, &p_cdc->line_coding);
+        }
       }
-      else if ( stage == CONTROL_STAGE_ACK)
-      {
-        if ( tud_cdc_line_coding_cb ) tud_cdc_line_coding_cb(itf, &p_cdc->line_coding);
-      }
-    break;
+      break;
 
     case CDC_REQUEST_GET_LINE_CODING:
-      if (stage == CONTROL_STAGE_SETUP)
-      {
-        TU_LOG2("  Get Line Coding\r\n");
+      if (stage == CONTROL_STAGE_SETUP) {
+        TU_LOG_DRV("  Get Line Coding\r\n");
         tud_control_xfer(rhport, request, &p_cdc->line_coding, sizeof(cdc_line_coding_t));
       }
-    break;
+      break;
 
     case CDC_REQUEST_SET_CONTROL_LINE_STATE:
-      if (stage == CONTROL_STAGE_SETUP)
-      {
+      if (stage == CONTROL_STAGE_SETUP) {
         tud_control_status(rhport, request);
-      }
-      else if (stage == CONTROL_STAGE_ACK)
-      {
+      } else if (stage == CONTROL_STAGE_ACK) {
         // CDC PSTN v1.2 section 6.3.12
         // Bit 0: Indicates if DTE is present or not.
         //        This signal corresponds to V.24 signal 108/2 and RS-232 signal DTR (Data Terminal Ready)
@@ -389,89 +413,89 @@ bool cdcd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t
         bool const rts = tu_bit_test(request->wValue, 1);
 
         p_cdc->line_state = (uint8_t) request->wValue;
-        
+
         // Disable fifo overwriting if DTR bit is set
         tu_fifo_set_overwritable(&p_cdc->tx_ff, !dtr);
 
-        TU_LOG2("  Set Control Line State: DTR = %d, RTS = %d\r\n", dtr, rts);
+        TU_LOG_DRV("  Set Control Line State: DTR = %d, RTS = %d\r\n", dtr, rts);
 
         // 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;
+      break;
+
     case CDC_REQUEST_SEND_BREAK:
-      if (stage == CONTROL_STAGE_SETUP)
-      {
+      if (stage == CONTROL_STAGE_SETUP) {
         tud_control_status(rhport, request);
+      } else if (stage == CONTROL_STAGE_ACK) {
+        TU_LOG_DRV("  Send Break\r\n");
+        if (tud_cdc_send_break_cb) {
+          tud_cdc_send_break_cb(itf, request->wValue);
+        }
       }
-      else if (stage == CONTROL_STAGE_ACK)
-      {
-        TU_LOG2("  Send Break\r\n");
-        if ( tud_cdc_send_break_cb ) tud_cdc_send_break_cb(itf, request->wValue);
-      }
-    break;
+      break;
 
-    default: return false; // stall unsupported request
+    default:
+      return false; // stall unsupported request
   }
 
   return true;
 }
 
-bool cdcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
-{
+bool cdcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) {
   (void) result;
 
   uint8_t itf;
   cdcd_interface_t* p_cdc;
 
   // Identify which interface to use
-  for (itf = 0; itf < CFG_TUD_CDC; itf++)
-  {
+  for (itf = 0; itf < CFG_TUD_CDC; itf++) {
     p_cdc = &_cdcd_itf[itf];
-    if ( ( ep_addr == p_cdc->ep_out ) || ( ep_addr == p_cdc->ep_in ) ) break;
+    if ((ep_addr == p_cdc->ep_out) || (ep_addr == p_cdc->ep_in)) {
+      break;
+    }
   }
   TU_ASSERT(itf < CFG_TUD_CDC);
+  cdcd_epbuf_t* p_epbuf = &_cdcd_epbuf[itf];
 
   // Received new data
-  if ( ep_addr == p_cdc->ep_out )
-  {
-    tu_fifo_write_n(&p_cdc->rx_ff, &p_cdc->epout_buf, (uint16_t) xferred_bytes);
-    
+  if (ep_addr == p_cdc->ep_out) {
+    tu_fifo_write_n(&p_cdc->rx_ff, p_epbuf->epout, (uint16_t) xferred_bytes);
+
     // Check for wanted char and invoke callback if needed
-    if ( tud_cdc_rx_wanted_cb && (((signed char) p_cdc->wanted_char) != -1) )
-    {
-      for ( uint32_t i = 0; i < xferred_bytes; i++ )
-      {
-        if ( (p_cdc->wanted_char == p_cdc->epout_buf[i]) && !tu_fifo_empty(&p_cdc->rx_ff) )
-        {
+    if (tud_cdc_rx_wanted_cb && (((signed char) p_cdc->wanted_char) != -1)) {
+      for (uint32_t i = 0; i < xferred_bytes; i++) {
+        if ((p_cdc->wanted_char == p_epbuf->epout[i]) && !tu_fifo_empty(&p_cdc->rx_ff)) {
           tud_cdc_rx_wanted_cb(itf, p_cdc->wanted_char);
         }
       }
     }
-    
+
     // invoke receive callback (if there is still data)
-    if (tud_cdc_rx_cb && !tu_fifo_empty(&p_cdc->rx_ff) ) tud_cdc_rx_cb(itf);
-    
+    if (tud_cdc_rx_cb && !tu_fifo_empty(&p_cdc->rx_ff)) {
+      tud_cdc_rx_cb(itf);
+    }
+
     // prepare for OUT transaction
-    _prep_out_transaction(p_cdc);
+    _prep_out_transaction(itf);
   }
-  
+
   // Data sent to host, we continue to fetch from tx fifo to send.
   // Note: This will cause incorrect baudrate set in line coding.
   //       Though maybe the baudrate is not really important !!!
-  if ( ep_addr == p_cdc->ep_in )
-  {
+  if (ep_addr == p_cdc->ep_in) {
     // invoke transmit callback to possibly refill tx fifo
-    if ( tud_cdc_tx_complete_cb ) tud_cdc_tx_complete_cb(itf);
+    if (tud_cdc_tx_complete_cb) {
+      tud_cdc_tx_complete_cb(itf);
+    }
 
-    if ( 0 == tud_cdc_n_write_flush(itf) )
-    {
+    if (0 == tud_cdc_n_write_flush(itf)) {
       // If there is no data left, a ZLP should be sent if
       // xferred_bytes is multiple of EP Packet size and not zero
-      if ( !tu_fifo_count(&p_cdc->tx_ff) && xferred_bytes && (0 == (xferred_bytes & (BULK_PACKET_SIZE-1))) )
-      {
-        if ( usbd_edpt_claim(rhport, p_cdc->ep_in) )
-        {
+      if (!tu_fifo_count(&p_cdc->tx_ff) && xferred_bytes && (0 == (xferred_bytes & (BULK_PACKET_SIZE - 1)))) {
+        if (usbd_edpt_claim(rhport, p_cdc->ep_in)) {
           usbd_edpt_xfer(rhport, p_cdc->ep_in, NULL, 0);
         }
       }
diff --git a/src/class/cdc/cdc_device.h b/src/class/cdc/cdc_device.h
index fbc7162a3..2d5ba2575 100644
--- a/src/class/cdc/cdc_device.h
+++ b/src/class/cdc/cdc_device.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -24,10 +24,9 @@
  * This file is part of the TinyUSB stack.
  */
 
-#ifndef _TUSB_CDC_DEVICE_H_
-#define _TUSB_CDC_DEVICE_H_
+#ifndef TUSB_CDC_DEVICE_H_
+#define TUSB_CDC_DEVICE_H_
 
-#include "common/tusb_common.h"
 #include "cdc.h"
 
 //--------------------------------------------------------------------+
@@ -46,208 +45,175 @@
  extern "C" {
 #endif
 
-/** \addtogroup CDC_Serial Serial
- *  @{
- *  \defgroup   CDC_Serial_Device Device
- *  @{ */
+//--------------------------------------------------------------------+
+// Driver Configuration
+//--------------------------------------------------------------------+
+
+typedef struct TU_ATTR_PACKED {
+  uint8_t rx_persistent : 1; // keep rx fifo on bus reset or disconnect
+  uint8_t tx_persistent : 1; // keep tx fifo on bus reset or disconnect
+} tud_cdc_configure_fifo_t;
+
+// Configure CDC FIFOs behavior
+bool tud_cdc_configure_fifo(tud_cdc_configure_fifo_t const* cfg);
 
 //--------------------------------------------------------------------+
-// Application API (Multiple Ports)
-// CFG_TUD_CDC > 1
+// Application API (Multiple Ports) i.e. CFG_TUD_CDC > 1
 //--------------------------------------------------------------------+
 
+// Check if interface is ready
+bool tud_cdc_n_ready(uint8_t itf);
+
 // Check if terminal is connected to this port
-bool     tud_cdc_n_connected       (uint8_t itf);
+bool tud_cdc_n_connected(uint8_t itf);
 
 // Get current line state. Bit 0:  DTR (Data Terminal Ready), Bit 1: RTS (Request to Send)
-uint8_t  tud_cdc_n_get_line_state  (uint8_t itf);
+uint8_t tud_cdc_n_get_line_state(uint8_t itf);
 
 // Get current line encoding: bit rate, stop bits parity etc ..
-void     tud_cdc_n_get_line_coding (uint8_t itf, cdc_line_coding_t* coding);
+void tud_cdc_n_get_line_coding(uint8_t itf, cdc_line_coding_t* coding);
 
 // Set special character that will trigger tud_cdc_rx_wanted_cb() callback on receiving
-void     tud_cdc_n_set_wanted_char (uint8_t itf, char wanted);
+void tud_cdc_n_set_wanted_char(uint8_t itf, char wanted);
 
 // Get the number of bytes available for reading
-uint32_t tud_cdc_n_available       (uint8_t itf);
+uint32_t tud_cdc_n_available(uint8_t itf);
 
 // Read received bytes
-uint32_t tud_cdc_n_read            (uint8_t itf, void* buffer, uint32_t bufsize);
+uint32_t tud_cdc_n_read(uint8_t itf, void* buffer, uint32_t bufsize);
 
 // Read a byte, return -1 if there is none
-static inline
-int32_t  tud_cdc_n_read_char       (uint8_t itf);
+TU_ATTR_ALWAYS_INLINE static inline int32_t tud_cdc_n_read_char(uint8_t itf) {
+  uint8_t ch;
+  return tud_cdc_n_read(itf, &ch, 1) ? (int32_t) ch : -1;
+}
 
 // Clear the received FIFO
-void     tud_cdc_n_read_flush      (uint8_t itf);
+void tud_cdc_n_read_flush(uint8_t itf);
 
-// Get a byte from FIFO at the specified position without removing it
-bool     tud_cdc_n_peek            (uint8_t itf, uint8_t* ui8);
+// Get a byte from FIFO without removing it
+bool tud_cdc_n_peek(uint8_t itf, uint8_t* ui8);
 
 // Write bytes to TX FIFO, data may remain in the FIFO for a while
-uint32_t tud_cdc_n_write           (uint8_t itf, void const* buffer, uint32_t bufsize);
+uint32_t tud_cdc_n_write(uint8_t itf, void const* buffer, uint32_t bufsize);
 
 // Write a byte
-static inline
-uint32_t tud_cdc_n_write_char      (uint8_t itf, char ch);
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_cdc_n_write_char(uint8_t itf, char ch) {
+  return tud_cdc_n_write(itf, &ch, 1);
+}
 
 // Write a null-terminated string
-static inline
-uint32_t tud_cdc_n_write_str       (uint8_t itf, char const* str);
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_cdc_n_write_str(uint8_t itf, char const* str) {
+  return tud_cdc_n_write(itf, str, strlen(str));
+}
 
 // Force sending data if possible, return number of forced bytes
-uint32_t tud_cdc_n_write_flush     (uint8_t itf);
+uint32_t tud_cdc_n_write_flush(uint8_t itf);
 
 // 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);
+uint32_t tud_cdc_n_write_available(uint8_t itf);
 
 // Clear the transmit FIFO
-bool tud_cdc_n_write_clear (uint8_t itf);
+bool tud_cdc_n_write_clear(uint8_t itf);
 
 //--------------------------------------------------------------------+
 // Application API (Single Port)
 //--------------------------------------------------------------------+
-static inline bool     tud_cdc_connected       (void);
-static inline uint8_t  tud_cdc_get_line_state  (void);
-static inline void     tud_cdc_get_line_coding (cdc_line_coding_t* coding);
-static inline void     tud_cdc_set_wanted_char (char wanted);
-
-static inline uint32_t tud_cdc_available       (void);
-static inline int32_t  tud_cdc_read_char       (void);
-static inline uint32_t tud_cdc_read            (void* buffer, uint32_t bufsize);
-static inline void     tud_cdc_read_flush      (void);
-static inline bool     tud_cdc_peek            (uint8_t* ui8);
 
-static inline uint32_t tud_cdc_write_char      (char ch);
-static inline uint32_t tud_cdc_write           (void const* buffer, uint32_t bufsize);
-static inline uint32_t tud_cdc_write_str       (char const* str);
-static inline uint32_t tud_cdc_write_flush     (void);
-static inline uint32_t tud_cdc_write_available (void);
-static inline bool     tud_cdc_write_clear     (void);
-
-//--------------------------------------------------------------------+
-// Application Callback API (weak is optional)
-//--------------------------------------------------------------------+
-
-// Invoked when received new data
-TU_ATTR_WEAK void tud_cdc_rx_cb(uint8_t itf);
-
-// Invoked when received `wanted_char`
-TU_ATTR_WEAK void tud_cdc_rx_wanted_cb(uint8_t itf, char wanted_char);
-
-// Invoked when space becomes available in TX buffer
-TU_ATTR_WEAK void tud_cdc_tx_complete_cb(uint8_t itf);
-
-// Invoked when line state DTR & RTS are changed via SET_CONTROL_LINE_STATE
-TU_ATTR_WEAK void tud_cdc_line_state_cb(uint8_t itf, bool dtr, bool rts);
-
-// Invoked when line coding is change via SET_LINE_CODING
-TU_ATTR_WEAK void tud_cdc_line_coding_cb(uint8_t itf, cdc_line_coding_t const* p_line_coding);
-
-// Invoked when received send break
-TU_ATTR_WEAK void tud_cdc_send_break_cb(uint8_t itf, uint16_t duration_ms);
-
-//--------------------------------------------------------------------+
-// Inline Functions
-//--------------------------------------------------------------------+
-static inline int32_t tud_cdc_n_read_char (uint8_t itf)
-{
-  uint8_t ch;
-  return tud_cdc_n_read(itf, &ch, 1) ? (int32_t) ch : -1;
-}
-
-static inline uint32_t tud_cdc_n_write_char(uint8_t itf, char ch)
-{
-  return tud_cdc_n_write(itf, &ch, 1);
-}
-
-static inline uint32_t tud_cdc_n_write_str (uint8_t itf, char const* str)
-{
-  return tud_cdc_n_write(itf, str, strlen(str));
+TU_ATTR_ALWAYS_INLINE static inline bool tud_cdc_ready(void) {
+  return tud_cdc_n_ready(0);
 }
 
-static inline bool tud_cdc_connected (void)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool tud_cdc_connected(void) {
   return tud_cdc_n_connected(0);
 }
 
-static inline uint8_t tud_cdc_get_line_state (void)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint8_t tud_cdc_get_line_state(void) {
   return tud_cdc_n_get_line_state(0);
 }
 
-static inline void tud_cdc_get_line_coding (cdc_line_coding_t* coding)
-{
+TU_ATTR_ALWAYS_INLINE static inline void tud_cdc_get_line_coding(cdc_line_coding_t* coding) {
   tud_cdc_n_get_line_coding(0, coding);
 }
 
-static inline void tud_cdc_set_wanted_char (char wanted)
-{
+TU_ATTR_ALWAYS_INLINE static inline void tud_cdc_set_wanted_char(char wanted) {
   tud_cdc_n_set_wanted_char(0, wanted);
 }
 
-static inline uint32_t tud_cdc_available (void)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_cdc_available(void) {
   return tud_cdc_n_available(0);
 }
 
-static inline int32_t tud_cdc_read_char (void)
-{
+TU_ATTR_ALWAYS_INLINE static inline int32_t tud_cdc_read_char(void) {
   return tud_cdc_n_read_char(0);
 }
 
-static inline uint32_t tud_cdc_read (void* buffer, uint32_t bufsize)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_cdc_read(void* buffer, uint32_t bufsize) {
   return tud_cdc_n_read(0, buffer, bufsize);
 }
 
-static inline void tud_cdc_read_flush (void)
-{
+TU_ATTR_ALWAYS_INLINE static inline void tud_cdc_read_flush(void) {
   tud_cdc_n_read_flush(0);
 }
 
-static inline bool tud_cdc_peek (uint8_t* ui8)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool tud_cdc_peek(uint8_t* ui8) {
   return tud_cdc_n_peek(0, ui8);
 }
 
-static inline uint32_t tud_cdc_write_char (char ch)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_cdc_write_char(char ch) {
   return tud_cdc_n_write_char(0, ch);
 }
 
-static inline uint32_t tud_cdc_write (void const* buffer, uint32_t bufsize)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_cdc_write(void const* buffer, uint32_t bufsize) {
   return tud_cdc_n_write(0, buffer, bufsize);
 }
 
-static inline uint32_t tud_cdc_write_str (char const* str)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_cdc_write_str(char const* str) {
   return tud_cdc_n_write_str(0, str);
 }
 
-static inline uint32_t tud_cdc_write_flush (void)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_cdc_write_flush(void) {
   return tud_cdc_n_write_flush(0);
 }
 
-static inline uint32_t tud_cdc_write_available(void)
-{
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_cdc_write_available(void) {
   return tud_cdc_n_write_available(0);
 }
 
-static inline bool tud_cdc_write_clear(void)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool tud_cdc_write_clear(void) {
   return tud_cdc_n_write_clear(0);
 }
 
-/** @} */
-/** @} */
+//--------------------------------------------------------------------+
+// Application Callback API (weak is optional)
+//--------------------------------------------------------------------+
+
+// Invoked when received new data
+TU_ATTR_WEAK void tud_cdc_rx_cb(uint8_t itf);
+
+// Invoked when received `wanted_char`
+TU_ATTR_WEAK void tud_cdc_rx_wanted_cb(uint8_t itf, char wanted_char);
+
+// Invoked when a TX is complete and therefore space becomes available in TX buffer
+TU_ATTR_WEAK void tud_cdc_tx_complete_cb(uint8_t itf);
+
+// Invoked when line state DTR & RTS are changed via SET_CONTROL_LINE_STATE
+TU_ATTR_WEAK void tud_cdc_line_state_cb(uint8_t itf, bool dtr, bool rts);
+
+// Invoked when line coding is change via SET_LINE_CODING
+TU_ATTR_WEAK void tud_cdc_line_coding_cb(uint8_t itf, cdc_line_coding_t const* p_line_coding);
+
+// Invoked when received send break
+// \param[in]  itf  interface for which send break was received.
+// \param[in]  duration_ms  the length of time, in milliseconds, of the break signal. If a value of FFFFh, then the
+//                          device will send a break until another SendBreak request is received with value 0000h.
+TU_ATTR_WEAK void tud_cdc_send_break_cb(uint8_t itf, uint16_t duration_ms);
 
 //--------------------------------------------------------------------+
 // INTERNAL USBD-CLASS DRIVER API
 //--------------------------------------------------------------------+
 void     cdcd_init            (void);
+bool     cdcd_deinit          (void);
 void     cdcd_reset           (uint8_t rhport);
 uint16_t cdcd_open            (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 bool     cdcd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
diff --git a/src/class/cdc/cdc_host.c b/src/class/cdc/cdc_host.c
index ee824cb4e..e817ebc7e 100644
--- a/src/class/cdc/cdc_host.c
+++ b/src/class/cdc/cdc_host.c
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -22,6 +22,9 @@
  * THE SOFTWARE.
  *
  * This file is part of the TinyUSB stack.
+ *
+ * Contribution
+ * - Heiko Kuester: CH34x support
  */
 
 #include "tusb_option.h"
@@ -29,232 +32,1649 @@
 #if (CFG_TUH_ENABLED && CFG_TUH_CDC)
 
 #include "host/usbh.h"
-#include "host/usbh_classdriver.h"
+#include "host/usbh_pvt.h"
 
 #include "cdc_host.h"
 
+// Level where CFG_TUSB_DEBUG must be at least for this driver is logged
+#ifndef CFG_TUH_CDC_LOG_LEVEL
+  #define CFG_TUH_CDC_LOG_LEVEL   CFG_TUH_LOG_LEVEL
+#endif
+
+#define TU_LOG_DRV(...)   TU_LOG(CFG_TUH_CDC_LOG_LEVEL, __VA_ARGS__)
+
 //--------------------------------------------------------------------+
-// MACRO CONSTANT TYPEDEF
+// Host CDC Interface
 //--------------------------------------------------------------------+
+
 typedef struct {
-  uint8_t itf_num;
-  uint8_t itf_protocol;
+  uint8_t daddr;
+  uint8_t bInterfaceNumber;
+  uint8_t bInterfaceSubClass;
+  uint8_t bInterfaceProtocol;
 
   uint8_t ep_notif;
-  uint8_t ep_in;
-  uint8_t ep_out;
-
+  uint8_t serial_drid; // Serial Driver ID
+  bool mounted;        // Enumeration is complete
   cdc_acm_capability_t acm_capability;
 
-} cdch_data_t;
+  TU_ATTR_ALIGNED(4) cdc_line_coding_t line_coding; // Baudrate, stop bits, parity, data width
+  uint8_t line_state;                               // DTR (bit0), RTS (bit1)
+
+  #if CFG_TUH_CDC_FTDI || CFG_TUH_CDC_CP210X || CFG_TUH_CDC_CH34X
+  cdc_line_coding_t requested_line_coding;
+  // 1 byte padding
+  #endif
+
+  tuh_xfer_cb_t user_control_cb;
+
+  struct {
+    tu_edpt_stream_t tx;
+    tu_edpt_stream_t rx;
+
+    uint8_t tx_ff_buf[CFG_TUH_CDC_TX_BUFSIZE];
+    uint8_t rx_ff_buf[CFG_TUH_CDC_TX_BUFSIZE];
+  } stream;
+} cdch_interface_t;
+
+typedef struct {
+  TUH_EPBUF_DEF(tx, CFG_TUH_CDC_TX_EPSIZE);
+  TUH_EPBUF_DEF(rx, CFG_TUH_CDC_TX_EPSIZE);
+} cdch_epbuf_t;
+
+static cdch_interface_t cdch_data[CFG_TUH_CDC];
+CFG_TUH_MEM_SECTION static cdch_epbuf_t cdch_epbuf[CFG_TUH_CDC];
+
+//--------------------------------------------------------------------+
+// Serial Driver
+//--------------------------------------------------------------------+
+
+//------------- ACM prototypes -------------//
+static bool acm_open(uint8_t daddr, tusb_desc_interface_t const *itf_desc, uint16_t max_len);
+static void acm_process_config(tuh_xfer_t* xfer);
+
+static bool acm_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool acm_set_data_format(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool acm_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool acm_set_control_line_state(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+
+//------------- FTDI prototypes -------------//
+#if CFG_TUH_CDC_FTDI
+#include "serial/ftdi_sio.h"
+
+static uint16_t const ftdi_vid_pid_list[][2] = {CFG_TUH_CDC_FTDI_VID_PID_LIST};
+
+static bool ftdi_open(uint8_t daddr, const tusb_desc_interface_t *itf_desc, uint16_t max_len);
+static void ftdi_process_config(tuh_xfer_t* xfer);
+
+static bool ftdi_sio_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool ftdi_set_data_format(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool ftdi_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool ftdi_sio_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+#endif
+
+//------------- CP210X prototypes -------------//
+#if CFG_TUH_CDC_CP210X
+#include "serial/cp210x.h"
+
+static uint16_t const cp210x_vid_pid_list[][2] = {CFG_TUH_CDC_CP210X_VID_PID_LIST};
+
+static bool cp210x_open(uint8_t daddr, tusb_desc_interface_t const *itf_desc, uint16_t max_len);
+static void cp210x_process_config(tuh_xfer_t* xfer);
+
+static bool cp210x_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool cp210x_set_data_format(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool cp210x_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool cp210x_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+#endif
+
+//------------- CH34x prototypes -------------//
+#if CFG_TUH_CDC_CH34X
+#include "serial/ch34x.h"
+
+static uint16_t const ch34x_vid_pid_list[][2] = {CFG_TUH_CDC_CH34X_VID_PID_LIST};
+
+static bool ch34x_open(uint8_t daddr, tusb_desc_interface_t const* itf_desc, uint16_t max_len);
+static void ch34x_process_config(tuh_xfer_t* xfer);
+
+static bool ch34x_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool ch34x_set_data_format(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool ch34x_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+static bool ch34x_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+#endif
+
+//------------- Common -------------//
+enum {
+  SERIAL_DRIVER_ACM = 0,
+
+#if CFG_TUH_CDC_FTDI
+  SERIAL_DRIVER_FTDI,
+#endif
+
+#if CFG_TUH_CDC_CP210X
+  SERIAL_DRIVER_CP210X,
+#endif
+
+#if CFG_TUH_CDC_CH34X
+  SERIAL_DRIVER_CH34X,
+#endif
+
+  SERIAL_DRIVER_COUNT
+};
+
+typedef struct {
+  uint16_t const (*vid_pid_list)[2];
+  uint16_t const vid_pid_count;
+  bool (*const open)(uint8_t daddr, const tusb_desc_interface_t *itf_desc, uint16_t max_len);
+  void (*const process_set_config)(tuh_xfer_t* xfer);
+  bool (*const set_control_line_state)(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+  bool (*const set_baudrate)(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+  bool (*const set_data_format)(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+  bool (*const set_line_coding)(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+} cdch_serial_driver_t;
+
+// Note driver list must be in the same order as SERIAL_DRIVER enum
+static const cdch_serial_driver_t serial_drivers[] = {
+  {
+      .vid_pid_list           = NULL,
+      .vid_pid_count          = 0,
+      .open                   = acm_open,
+      .process_set_config     = acm_process_config,
+      .set_control_line_state = acm_set_control_line_state,
+      .set_baudrate           = acm_set_baudrate,
+      .set_data_format        = acm_set_data_format,
+      .set_line_coding        = acm_set_line_coding
+  },
+
+  #if CFG_TUH_CDC_FTDI
+  {
+      .vid_pid_list           = ftdi_vid_pid_list,
+      .vid_pid_count          = TU_ARRAY_SIZE(ftdi_vid_pid_list),
+      .open                   = ftdi_open,
+      .process_set_config     = ftdi_process_config,
+      .set_control_line_state = ftdi_sio_set_modem_ctrl,
+      .set_baudrate           = ftdi_sio_set_baudrate,
+      .set_data_format        = ftdi_set_data_format,
+      .set_line_coding        = ftdi_set_line_coding
+  },
+  #endif
+
+  #if CFG_TUH_CDC_CP210X
+  {
+      .vid_pid_list           = cp210x_vid_pid_list,
+      .vid_pid_count          = TU_ARRAY_SIZE(cp210x_vid_pid_list),
+      .open                   = cp210x_open,
+      .process_set_config     = cp210x_process_config,
+      .set_control_line_state = cp210x_set_modem_ctrl,
+      .set_baudrate           = cp210x_set_baudrate,
+      .set_data_format        = cp210x_set_data_format,
+      .set_line_coding        = cp210x_set_line_coding
+  },
+  #endif
+
+  #if CFG_TUH_CDC_CH34X
+  {
+      .vid_pid_list           = ch34x_vid_pid_list,
+      .vid_pid_count          = TU_ARRAY_SIZE(ch34x_vid_pid_list),
+      .open                   = ch34x_open,
+      .process_set_config     = ch34x_process_config,
+      .set_control_line_state = ch34x_set_modem_ctrl,
+      .set_baudrate           = ch34x_set_baudrate,
+      .set_data_format        = ch34x_set_data_format,
+      .set_line_coding        = ch34x_set_line_coding
+  },
+  #endif
+};
+
+TU_VERIFY_STATIC(TU_ARRAY_SIZE(serial_drivers) == SERIAL_DRIVER_COUNT, "Serial driver count mismatch");
 
 //--------------------------------------------------------------------+
 // INTERNAL OBJECT & FUNCTION DECLARATION
 //--------------------------------------------------------------------+
-static cdch_data_t cdch_data[CFG_TUH_DEVICE_MAX];
 
-static inline cdch_data_t* get_itf(uint8_t dev_addr)
-{
-  return &cdch_data[dev_addr-1];
-}
+static inline cdch_interface_t* get_itf(uint8_t idx) {
+  TU_ASSERT(idx < CFG_TUH_CDC, NULL);
+  cdch_interface_t* p_cdc = &cdch_data[idx];
 
-bool tuh_cdc_mounted(uint8_t dev_addr)
-{
-  cdch_data_t* cdc = get_itf(dev_addr);
-  return cdc->ep_in && cdc->ep_out;
+  return (p_cdc->daddr != 0) ? p_cdc : NULL;
 }
 
-bool tuh_cdc_is_busy(uint8_t dev_addr, cdc_pipeid_t pipeid)
-{
-  if ( !tuh_cdc_mounted(dev_addr) ) return false;
+static inline uint8_t get_idx_by_ep_addr(uint8_t daddr, uint8_t ep_addr) {
+  for(uint8_t i=0; i<CFG_TUH_CDC; i++) {
+    cdch_interface_t* p_cdc = &cdch_data[i];
+    if ( (p_cdc->daddr == daddr) &&
+         (ep_addr == p_cdc->ep_notif || ep_addr == p_cdc->stream.rx.ep_addr || ep_addr == p_cdc->stream.tx.ep_addr)) {
+      return i;
+    }
+  }
 
-  cdch_data_t const * p_cdc = get_itf(dev_addr);
+  return TUSB_INDEX_INVALID_8;
+}
 
-  switch (pipeid)
-  {
-    case CDC_PIPE_NOTIFICATION:
-      return usbh_edpt_busy(dev_addr, p_cdc->ep_notif );
+static cdch_interface_t* make_new_itf(uint8_t daddr, tusb_desc_interface_t const *itf_desc) {
+  for(uint8_t i=0; i<CFG_TUH_CDC; i++) {
+    if (cdch_data[i].daddr == 0) {
+      cdch_interface_t* p_cdc = &cdch_data[i];
+      p_cdc->daddr              = daddr;
+      p_cdc->bInterfaceNumber   = itf_desc->bInterfaceNumber;
+      p_cdc->bInterfaceSubClass = itf_desc->bInterfaceSubClass;
+      p_cdc->bInterfaceProtocol = itf_desc->bInterfaceProtocol;
+      p_cdc->line_state         = 0;
+      return p_cdc;
+    }
+  }
 
-    case CDC_PIPE_DATA_IN:
-      return usbh_edpt_busy(dev_addr, p_cdc->ep_in );
+  return NULL;
+}
 
-    case CDC_PIPE_DATA_OUT:
-      return usbh_edpt_busy(dev_addr, p_cdc->ep_out );
+static bool open_ep_stream_pair(cdch_interface_t* p_cdc , tusb_desc_endpoint_t const *desc_ep);
+static void set_config_complete(cdch_interface_t * p_cdc, uint8_t idx, uint8_t itf_num);
+static void cdch_internal_control_complete(tuh_xfer_t* xfer);
 
-    default:
-      return false;
+//--------------------------------------------------------------------+
+// APPLICATION API
+//--------------------------------------------------------------------+
+
+uint8_t tuh_cdc_itf_get_index(uint8_t daddr, uint8_t itf_num) {
+  for (uint8_t i = 0; i < CFG_TUH_CDC; i++) {
+    const cdch_interface_t* p_cdc = &cdch_data[i];
+    if (p_cdc->daddr == daddr && p_cdc->bInterfaceNumber == itf_num) return i;
   }
+
+  return TUSB_INDEX_INVALID_8;
+}
+
+bool tuh_cdc_itf_get_info(uint8_t idx, tuh_itf_info_t* info) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc && info);
+
+  info->daddr = p_cdc->daddr;
+
+  // re-construct descriptor
+  tusb_desc_interface_t* desc = &info->desc;
+  desc->bLength            = sizeof(tusb_desc_interface_t);
+  desc->bDescriptorType    = TUSB_DESC_INTERFACE;
+
+  desc->bInterfaceNumber   = p_cdc->bInterfaceNumber;
+  desc->bAlternateSetting  = 0;
+  desc->bNumEndpoints      = 2u + (p_cdc->ep_notif ? 1u : 0u);
+  desc->bInterfaceClass    = TUSB_CLASS_CDC;
+  desc->bInterfaceSubClass = p_cdc->bInterfaceSubClass;
+  desc->bInterfaceProtocol = p_cdc->bInterfaceProtocol;
+  desc->iInterface         = 0; // not used yet
+
+  return true;
+}
+
+bool tuh_cdc_mounted(uint8_t idx) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc);
+  return p_cdc->mounted;
+}
+
+bool tuh_cdc_get_dtr(uint8_t idx) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc);
+
+  return (p_cdc->line_state & CDC_CONTROL_LINE_STATE_DTR) ? true : false;
+}
+
+bool tuh_cdc_get_rts(uint8_t idx) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc);
+
+  return (p_cdc->line_state & CDC_CONTROL_LINE_STATE_RTS) ? true : false;
+}
+
+bool tuh_cdc_get_local_line_coding(uint8_t idx, cdc_line_coding_t* line_coding) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc);
+
+  *line_coding = p_cdc->line_coding;
+
+  return true;
 }
 
 //--------------------------------------------------------------------+
-// APPLICATION API (parameter validation needed)
+// Write
 //--------------------------------------------------------------------+
-bool tuh_cdc_serial_is_mounted(uint8_t dev_addr)
-{
-  // TODO consider all AT Command as serial candidate
-  return tuh_cdc_mounted(dev_addr)                                         &&
-      (cdch_data[dev_addr-1].itf_protocol <= CDC_COMM_PROTOCOL_ATCOMMAND_CDMA);
+
+uint32_t tuh_cdc_write(uint8_t idx, void const* buffer, uint32_t bufsize) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc);
+
+  return tu_edpt_stream_write(p_cdc->daddr, &p_cdc->stream.tx, buffer, bufsize);
 }
 
-bool tuh_cdc_send(uint8_t dev_addr, void const * p_data, uint32_t length, bool is_notify)
-{
-  (void) is_notify;
-  TU_VERIFY( tuh_cdc_mounted(dev_addr) );
-  TU_VERIFY( p_data != NULL && length);
+uint32_t tuh_cdc_write_flush(uint8_t idx) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc);
+
+  return tu_edpt_stream_write_xfer(p_cdc->daddr, &p_cdc->stream.tx);
+}
 
-  uint8_t const ep_out = cdch_data[dev_addr-1].ep_out;
-  if ( usbh_edpt_busy(dev_addr, ep_out) ) return false;
+bool tuh_cdc_write_clear(uint8_t idx) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc);
 
-  return usbh_edpt_xfer(dev_addr, ep_out, (void*)(uintptr_t) p_data, (uint16_t) length);
+  return tu_edpt_stream_clear(&p_cdc->stream.tx);
 }
 
-bool tuh_cdc_receive(uint8_t dev_addr, void * p_buffer, uint32_t length, bool is_notify)
-{
-  (void) is_notify;
-  TU_VERIFY( tuh_cdc_mounted(dev_addr) );
-  TU_VERIFY( p_buffer != NULL && length );
+uint32_t tuh_cdc_write_available(uint8_t idx) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc);
 
-  uint8_t const ep_in = cdch_data[dev_addr-1].ep_in;
-  if ( usbh_edpt_busy(dev_addr, ep_in) ) return false;
+  return tu_edpt_stream_write_available(p_cdc->daddr, &p_cdc->stream.tx);
+}
 
-  return usbh_edpt_xfer(dev_addr, ep_in, p_buffer, (uint16_t) length);
+//--------------------------------------------------------------------+
+// Read
+//--------------------------------------------------------------------+
+
+uint32_t tuh_cdc_read (uint8_t idx, void* buffer, uint32_t bufsize) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc);
+
+  return tu_edpt_stream_read(p_cdc->daddr, &p_cdc->stream.rx, buffer, bufsize);
 }
 
-bool tuh_cdc_set_control_line_state(uint8_t dev_addr, bool dtr, bool rts, tuh_xfer_cb_t complete_cb)
-{
-  cdch_data_t const * p_cdc = get_itf(dev_addr);
+uint32_t tuh_cdc_read_available(uint8_t idx) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc);
 
-  tusb_control_request_t const request =
-  {
-    .bmRequestType_bit =
+  return tu_edpt_stream_read_available(&p_cdc->stream.rx);
+}
+
+bool tuh_cdc_peek(uint8_t idx, uint8_t* ch) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc);
+
+  return tu_edpt_stream_peek(&p_cdc->stream.rx, ch);
+}
+
+bool tuh_cdc_read_clear (uint8_t idx) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc);
+
+  bool ret = tu_edpt_stream_clear(&p_cdc->stream.rx);
+  tu_edpt_stream_read_xfer(p_cdc->daddr, &p_cdc->stream.rx);
+  return ret;
+}
+
+//--------------------------------------------------------------------+
+// Control Endpoint API
+//--------------------------------------------------------------------+
+
+static void process_internal_control_complete(tuh_xfer_t* xfer, uint8_t itf_num) {
+  uint8_t idx = tuh_cdc_itf_get_index(xfer->daddr, itf_num);
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_ASSERT(p_cdc, );
+  uint16_t const value = tu_le16toh(xfer->setup->wValue);
+
+  if (xfer->result == XFER_RESULT_SUCCESS) {
+    switch (p_cdc->serial_drid) {
+      case SERIAL_DRIVER_ACM:
+        switch (xfer->setup->bRequest) {
+          case CDC_REQUEST_SET_CONTROL_LINE_STATE:
+            p_cdc->line_state = (uint8_t) value;
+            break;
+
+          case CDC_REQUEST_SET_LINE_CODING: {
+            uint16_t const len = tu_min16(sizeof(cdc_line_coding_t), tu_le16toh(xfer->setup->wLength));
+            memcpy(&p_cdc->line_coding, xfer->buffer, len);
+            break;
+          }
+
+          default: break;
+        }
+        break;
+
+      #if CFG_TUH_CDC_FTDI
+      case SERIAL_DRIVER_FTDI:
+        switch (xfer->setup->bRequest) {
+          case FTDI_SIO_MODEM_CTRL:
+            p_cdc->line_state = (uint8_t) value;
+            break;
+
+          case FTDI_SIO_SET_BAUD_RATE:
+            p_cdc->line_coding.bit_rate = p_cdc->requested_line_coding.bit_rate;
+            break;
+
+          default: break;
+        }
+        break;
+      #endif
+
+      #if CFG_TUH_CDC_CP210X
+      case SERIAL_DRIVER_CP210X:
+        switch(xfer->setup->bRequest) {
+          case CP210X_SET_MHS:
+            p_cdc->line_state = (uint8_t) value;
+            break;
+
+          case CP210X_SET_BAUDRATE: {
+            uint32_t baudrate;
+            memcpy(&baudrate, xfer->buffer, sizeof(uint32_t));
+            p_cdc->line_coding.bit_rate = tu_le32toh(baudrate);
+            break;
+          }
+
+          default: break;
+        }
+        break;
+      #endif
+
+      #if CFG_TUH_CDC_CH34X
+      case SERIAL_DRIVER_CH34X:
+        switch (xfer->setup->bRequest) {
+          case CH34X_REQ_WRITE_REG:
+            // register write request
+            switch (value) {
+              case CH34X_REG16_DIVISOR_PRESCALER:
+                // baudrate
+                p_cdc->line_coding.bit_rate = p_cdc->requested_line_coding.bit_rate;
+                break;
+
+              case CH32X_REG16_LCR2_LCR:
+                // data format
+                p_cdc->line_coding.stop_bits = p_cdc->requested_line_coding.stop_bits;
+                p_cdc->line_coding.parity = p_cdc->requested_line_coding.parity;
+                p_cdc->line_coding.data_bits = p_cdc->requested_line_coding.data_bits;
+                break;
+
+              default: break;
+            }
+            break;
+
+          case CH34X_REQ_MODEM_CTRL: {
+            // set modem controls RTS/DTR request. Note: signals are inverted
+            uint16_t const modem_signal = ~value;
+            if (modem_signal & CH34X_BIT_RTS) {
+              p_cdc->line_state |= CDC_CONTROL_LINE_STATE_RTS;
+            } else {
+              p_cdc->line_state &= (uint8_t) ~CDC_CONTROL_LINE_STATE_RTS;
+            }
+
+            if (modem_signal & CH34X_BIT_DTR) {
+              p_cdc->line_state |= CDC_CONTROL_LINE_STATE_DTR;
+            } else {
+              p_cdc->line_state &= (uint8_t) ~CDC_CONTROL_LINE_STATE_DTR;
+            }
+            break;
+          }
+
+          default: break;
+        }
+        break;
+      #endif
+
+      default: break;
+    }
+  }
+
+  xfer->complete_cb = p_cdc->user_control_cb;
+  if (xfer->complete_cb) {
+    xfer->complete_cb(xfer);
+  }
+}
+
+// internal control complete to update state such as line state, encoding
+static void cdch_internal_control_complete(tuh_xfer_t* xfer) {
+  uint8_t const itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
+  process_internal_control_complete(xfer, itf_num);
+}
+
+bool tuh_cdc_set_control_line_state(uint8_t idx, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc && p_cdc->serial_drid < SERIAL_DRIVER_COUNT);
+  cdch_serial_driver_t const* driver = &serial_drivers[p_cdc->serial_drid];
+
+  if (complete_cb) {
+    return driver->set_control_line_state(p_cdc, line_state, complete_cb, user_data);
+  } else {
+    // blocking
+    xfer_result_t result = XFER_RESULT_INVALID;
+    bool ret = driver->set_control_line_state(p_cdc, line_state, complete_cb, (uintptr_t) &result);
+
+    if (user_data) {
+      // user_data is not NULL, return result via user_data
+      *((xfer_result_t*) user_data) = result;
+    }
+
+    TU_VERIFY(ret && result == XFER_RESULT_SUCCESS);
+    p_cdc->line_state = (uint8_t) line_state;
+    return true;
+  }
+}
+
+bool tuh_cdc_set_baudrate(uint8_t idx, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc && p_cdc->serial_drid < SERIAL_DRIVER_COUNT);
+  cdch_serial_driver_t const* driver = &serial_drivers[p_cdc->serial_drid];
+
+  if (complete_cb) {
+    return driver->set_baudrate(p_cdc, baudrate, complete_cb, user_data);
+  } else {
+    // blocking
+    xfer_result_t result = XFER_RESULT_INVALID;
+    bool ret = driver->set_baudrate(p_cdc, baudrate, complete_cb, (uintptr_t) &result);
+
+    if (user_data) {
+      // user_data is not NULL, return result via user_data
+      *((xfer_result_t*) user_data) = result;
+    }
+
+    TU_VERIFY(ret && result == XFER_RESULT_SUCCESS);
+    p_cdc->line_coding.bit_rate = baudrate;
+    return true;
+  }
+}
+
+bool tuh_cdc_set_data_format(uint8_t idx, uint8_t stop_bits, uint8_t parity, uint8_t data_bits,
+                             tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc && p_cdc->serial_drid < SERIAL_DRIVER_COUNT);
+  cdch_serial_driver_t const* driver = &serial_drivers[p_cdc->serial_drid];
+
+  if (complete_cb) {
+    return driver->set_data_format(p_cdc, stop_bits, parity, data_bits, complete_cb, user_data);
+  } else {
+    // blocking
+    xfer_result_t result = XFER_RESULT_INVALID;
+    bool ret = driver->set_data_format(p_cdc, stop_bits, parity, data_bits, complete_cb, (uintptr_t) &result);
+
+    if (user_data) {
+      // user_data is not NULL, return result via user_data
+      *((xfer_result_t*) user_data) = result;
+    }
+
+    TU_VERIFY(ret && result == XFER_RESULT_SUCCESS);
+    p_cdc->line_coding.stop_bits = stop_bits;
+    p_cdc->line_coding.parity = parity;
+    p_cdc->line_coding.data_bits = data_bits;
+    return true;
+  }
+}
+
+bool tuh_cdc_set_line_coding(uint8_t idx, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_VERIFY(p_cdc && p_cdc->serial_drid < SERIAL_DRIVER_COUNT);
+  cdch_serial_driver_t const* driver = &serial_drivers[p_cdc->serial_drid];
+
+  if ( complete_cb ) {
+    return driver->set_line_coding(p_cdc, line_coding, complete_cb, user_data);
+  } else {
+    // blocking
+    xfer_result_t result = XFER_RESULT_INVALID;
+    bool ret = driver->set_line_coding(p_cdc, line_coding, complete_cb, (uintptr_t) &result);
+
+    if (user_data) {
+      // user_data is not NULL, return result via user_data
+      *((xfer_result_t*) user_data) = result;
+    }
+
+    TU_VERIFY(ret && result == XFER_RESULT_SUCCESS);
+    p_cdc->line_coding = *line_coding;
+    return true;
+  }
+}
+
+//--------------------------------------------------------------------+
+// CLASS-USBH API
+//--------------------------------------------------------------------+
+
+bool cdch_init(void) {
+  TU_LOG_DRV("sizeof(cdch_interface_t) = %u\r\n", sizeof(cdch_interface_t));
+  tu_memclr(cdch_data, sizeof(cdch_data));
+  for (size_t i = 0; i < CFG_TUH_CDC; i++) {
+    cdch_interface_t* p_cdc = &cdch_data[i];
+    cdch_epbuf_t* epbuf = &cdch_epbuf[i];
+    tu_edpt_stream_init(&p_cdc->stream.tx, true, true, false,
+                        p_cdc->stream.tx_ff_buf, CFG_TUH_CDC_TX_BUFSIZE,
+                        epbuf->tx, CFG_TUH_CDC_TX_EPSIZE);
+
+    tu_edpt_stream_init(&p_cdc->stream.rx, true, false, false,
+                        p_cdc->stream.rx_ff_buf, CFG_TUH_CDC_RX_BUFSIZE,
+                        epbuf->rx, CFG_TUH_CDC_RX_EPSIZE);
+  }
+
+  return true;
+}
+
+bool cdch_deinit(void) {
+  for (size_t i = 0; i < CFG_TUH_CDC; i++) {
+    cdch_interface_t* p_cdc = &cdch_data[i];
+    tu_edpt_stream_deinit(&p_cdc->stream.tx);
+    tu_edpt_stream_deinit(&p_cdc->stream.rx);
+  }
+  return true;
+}
+
+void cdch_close(uint8_t daddr) {
+  for (uint8_t idx = 0; idx < CFG_TUH_CDC; idx++) {
+    cdch_interface_t* p_cdc = &cdch_data[idx];
+    if (p_cdc->daddr == daddr) {
+      TU_LOG_DRV("  CDCh close addr = %u index = %u\r\n", daddr, idx);
+
+      // Invoke application callback
+      if (tuh_cdc_umount_cb) {
+        tuh_cdc_umount_cb(idx);
+      }
+
+      p_cdc->daddr = 0;
+      p_cdc->bInterfaceNumber = 0;
+      p_cdc->mounted = false;
+      tu_edpt_stream_close(&p_cdc->stream.tx);
+      tu_edpt_stream_close(&p_cdc->stream.rx);
+    }
+  }
+}
+
+bool cdch_xfer_cb(uint8_t daddr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes) {
+  // TODO handle stall response, retry failed transfer ...
+  TU_ASSERT(event == XFER_RESULT_SUCCESS);
+
+  uint8_t const idx = get_idx_by_ep_addr(daddr, ep_addr);
+  cdch_interface_t * p_cdc = get_itf(idx);
+  TU_ASSERT(p_cdc);
+
+  if ( ep_addr == p_cdc->stream.tx.ep_addr ) {
+    // invoke tx complete callback to possibly refill tx fifo
+    if (tuh_cdc_tx_complete_cb) {
+      tuh_cdc_tx_complete_cb(idx);
+    }
+
+    if ( 0 == tu_edpt_stream_write_xfer(daddr, &p_cdc->stream.tx) ) {
+      // If there is no data left, a ZLP should be sent if:
+      // - xferred_bytes is multiple of EP Packet size and not zero
+      tu_edpt_stream_write_zlp_if_needed(daddr, &p_cdc->stream.tx, xferred_bytes);
+    }
+  } else if ( ep_addr == p_cdc->stream.rx.ep_addr ) {
+    #if CFG_TUH_CDC_FTDI
+    if (p_cdc->serial_drid == SERIAL_DRIVER_FTDI) {
+      // FTDI reserve 2 bytes for status
+      // uint8_t status[2] = {p_cdc->stream.rx.ep_buf[0], p_cdc->stream.rx.ep_buf[1]};
+      tu_edpt_stream_read_xfer_complete_offset(&p_cdc->stream.rx, xferred_bytes, 2);
+    }else
+    #endif
     {
+      tu_edpt_stream_read_xfer_complete(&p_cdc->stream.rx, xferred_bytes);
+    }
+
+    // invoke receive callback
+    if (tuh_cdc_rx_cb) {
+      tuh_cdc_rx_cb(idx);
+    }
+
+    // prepare for next transfer if needed
+    tu_edpt_stream_read_xfer(daddr, &p_cdc->stream.rx);
+  }else if ( ep_addr == p_cdc->ep_notif ) {
+    // TODO handle notification endpoint
+  }else {
+    TU_ASSERT(false);
+  }
+
+  return true;
+}
+
+//--------------------------------------------------------------------+
+// Enumeration
+//--------------------------------------------------------------------+
+
+static bool open_ep_stream_pair(cdch_interface_t* p_cdc, tusb_desc_endpoint_t const* desc_ep) {
+  for (size_t i = 0; i < 2; i++) {
+    TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType &&
+              TUSB_XFER_BULK == desc_ep->bmAttributes.xfer);
+    TU_ASSERT(tuh_edpt_open(p_cdc->daddr, desc_ep));
+
+    if (tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN) {
+      tu_edpt_stream_open(&p_cdc->stream.rx, desc_ep);
+    } else {
+      tu_edpt_stream_open(&p_cdc->stream.tx, desc_ep);
+    }
+
+    desc_ep = (tusb_desc_endpoint_t const*) tu_desc_next(desc_ep);
+  }
+
+  return true;
+}
+
+bool cdch_open(uint8_t rhport, uint8_t daddr, tusb_desc_interface_t const *itf_desc, uint16_t max_len) {
+  (void) rhport;
+
+  // For CDC: only support ACM subclass
+  // Note: Protocol 0xFF can be RNDIS device
+  if (TUSB_CLASS_CDC                           == itf_desc->bInterfaceClass &&
+      CDC_COMM_SUBCLASS_ABSTRACT_CONTROL_MODEL == itf_desc->bInterfaceSubClass) {
+    return acm_open(daddr, itf_desc, max_len);
+  } else if (SERIAL_DRIVER_COUNT > 1 &&
+             TUSB_CLASS_VENDOR_SPECIFIC == itf_desc->bInterfaceClass) {
+    uint16_t vid, pid;
+    TU_VERIFY(tuh_vid_pid_get(daddr, &vid, &pid));
+
+    for (size_t dr = 1; dr < SERIAL_DRIVER_COUNT; dr++) {
+      cdch_serial_driver_t const* driver = &serial_drivers[dr];
+      for (size_t i = 0; i < driver->vid_pid_count; i++) {
+        if (driver->vid_pid_list[i][0] == vid && driver->vid_pid_list[i][1] == pid) {
+          return driver->open(daddr, itf_desc, max_len);
+        }
+      }
+    }
+  }
+
+  return false;
+}
+
+static void set_config_complete(cdch_interface_t * p_cdc, uint8_t idx, uint8_t itf_num) {
+  TU_LOG_DRV("CDCh Set Configure complete\r\n");
+  p_cdc->mounted = true;
+  if (tuh_cdc_mount_cb) {
+    tuh_cdc_mount_cb(idx);
+  }
+
+  // Prepare for incoming data
+  tu_edpt_stream_read_xfer(p_cdc->daddr, &p_cdc->stream.rx);
+
+  // notify usbh that driver enumeration is complete
+  usbh_driver_set_config_complete(p_cdc->daddr, itf_num);
+}
+
+bool cdch_set_config(uint8_t daddr, uint8_t itf_num) {
+  tusb_control_request_t request;
+  request.wIndex = tu_htole16((uint16_t) itf_num);
+
+  // fake transfer to kick-off process
+  tuh_xfer_t xfer;
+  xfer.daddr  = daddr;
+  xfer.result = XFER_RESULT_SUCCESS;
+  xfer.setup  = &request;
+  xfer.user_data = 0; // initial state
+
+  uint8_t const idx = tuh_cdc_itf_get_index(daddr, itf_num);
+  cdch_interface_t * p_cdc = get_itf(idx);
+  TU_ASSERT(p_cdc && p_cdc->serial_drid < SERIAL_DRIVER_COUNT);
+
+  serial_drivers[p_cdc->serial_drid].process_set_config(&xfer);
+  return true;
+}
+
+//--------------------------------------------------------------------+
+// ACM
+//--------------------------------------------------------------------+
+
+enum {
+  CONFIG_ACM_SET_CONTROL_LINE_STATE = 0,
+  CONFIG_ACM_SET_LINE_CODING,
+  CONFIG_ACM_COMPLETE,
+};
+
+static bool acm_open(uint8_t daddr, tusb_desc_interface_t const* itf_desc, uint16_t max_len) {
+  uint8_t const* p_desc_end = ((uint8_t const*) itf_desc) + max_len;
+
+  cdch_interface_t* p_cdc = make_new_itf(daddr, itf_desc);
+  TU_VERIFY(p_cdc);
+  p_cdc->serial_drid = SERIAL_DRIVER_ACM;
+
+  //------------- Control Interface -------------//
+  uint8_t const* p_desc = tu_desc_next(itf_desc);
+
+  // Communication Functional Descriptors
+  while ((p_desc < p_desc_end) && (TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc))) {
+    if (CDC_FUNC_DESC_ABSTRACT_CONTROL_MANAGEMENT == cdc_functional_desc_typeof(p_desc)) {
+      // save ACM bmCapabilities
+      p_cdc->acm_capability = ((cdc_desc_func_acm_t const*) p_desc)->bmCapabilities;
+    }
+
+    p_desc = tu_desc_next(p_desc);
+  }
+
+  // Open notification endpoint of control interface if any
+  if (itf_desc->bNumEndpoints == 1) {
+    TU_ASSERT(TUSB_DESC_ENDPOINT == tu_desc_type(p_desc));
+    tusb_desc_endpoint_t const* desc_ep = (tusb_desc_endpoint_t const*) p_desc;
+
+    TU_ASSERT(tuh_edpt_open(daddr, desc_ep));
+    p_cdc->ep_notif = desc_ep->bEndpointAddress;
+
+    p_desc = tu_desc_next(p_desc);
+  }
+
+  //------------- Data Interface (if any) -------------//
+  if ((TUSB_DESC_INTERFACE == tu_desc_type(p_desc)) &&
+      (TUSB_CLASS_CDC_DATA == ((tusb_desc_interface_t const*) p_desc)->bInterfaceClass)) {
+    // next to endpoint descriptor
+    p_desc = tu_desc_next(p_desc);
+
+    // data endpoints expected to be in pairs
+    TU_ASSERT(open_ep_stream_pair(p_cdc, (tusb_desc_endpoint_t const*) p_desc));
+  }
+
+  return true;
+}
+
+static void acm_process_config(tuh_xfer_t* xfer) {
+  uintptr_t const state = xfer->user_data;
+  uint8_t const itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
+  uint8_t const idx = tuh_cdc_itf_get_index(xfer->daddr, itf_num);
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_ASSERT(p_cdc,);
+
+  switch (state) {
+    case CONFIG_ACM_SET_CONTROL_LINE_STATE:
+      #if CFG_TUH_CDC_LINE_CONTROL_ON_ENUM
+      if (p_cdc->acm_capability.support_line_request) {
+        TU_ASSERT(acm_set_control_line_state(p_cdc, CFG_TUH_CDC_LINE_CONTROL_ON_ENUM, acm_process_config, CONFIG_ACM_SET_LINE_CODING),);
+        break;
+      }
+      #endif
+      TU_ATTR_FALLTHROUGH;
+
+    case CONFIG_ACM_SET_LINE_CODING:
+      #ifdef CFG_TUH_CDC_LINE_CODING_ON_ENUM
+      if (p_cdc->acm_capability.support_line_request) {
+        cdc_line_coding_t line_coding = CFG_TUH_CDC_LINE_CODING_ON_ENUM;
+        TU_ASSERT(acm_set_line_coding(p_cdc, &line_coding, acm_process_config, CONFIG_ACM_COMPLETE),);
+        break;
+      }
+      #endif
+      TU_ATTR_FALLTHROUGH;
+
+    case CONFIG_ACM_COMPLETE:
+      // itf_num+1 to account for data interface as well
+      set_config_complete(p_cdc, idx, itf_num + 1);
+      break;
+
+    default:
+      break;
+  }
+}
+
+static bool acm_set_control_line_state(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  TU_VERIFY(p_cdc->acm_capability.support_line_request);
+  TU_LOG_DRV("CDC ACM Set Control Line State\r\n");
+
+  tusb_control_request_t const request = {
+    .bmRequestType_bit = {
       .recipient = TUSB_REQ_RCPT_INTERFACE,
       .type      = TUSB_REQ_TYPE_CLASS,
       .direction = TUSB_DIR_OUT
     },
     .bRequest = CDC_REQUEST_SET_CONTROL_LINE_STATE,
-    .wValue   = tu_htole16((uint16_t) ((dtr ? 1u : 0u) | (rts ? 2u : 0u))),
-    .wIndex   = tu_htole16(p_cdc->itf_num),
+    .wValue   = tu_htole16(line_state),
+    .wIndex   = tu_htole16((uint16_t) p_cdc->bInterfaceNumber),
     .wLength  = 0
   };
 
-  tuh_xfer_t xfer =
-  {
-    .daddr       = dev_addr,
+  p_cdc->user_control_cb = complete_cb;
+
+  tuh_xfer_t xfer = {
+    .daddr       = p_cdc->daddr,
+    .ep_addr     = 0,
+    .setup       = &request,
+    .buffer      = NULL,
+    .complete_cb = complete_cb ? cdch_internal_control_complete : NULL, // complete_cb is NULL for sync call
+    .user_data   = user_data
+  };
+
+  TU_ASSERT(tuh_control_xfer(&xfer));
+  return true;
+}
+
+static bool acm_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  TU_LOG_DRV("CDC ACM Set Line Conding\r\n");
+
+  tusb_control_request_t const request = {
+    .bmRequestType_bit = {
+      .recipient = TUSB_REQ_RCPT_INTERFACE,
+      .type      = TUSB_REQ_TYPE_CLASS,
+      .direction = TUSB_DIR_OUT
+    },
+    .bRequest = CDC_REQUEST_SET_LINE_CODING,
+    .wValue   = 0,
+    .wIndex   = tu_htole16(p_cdc->bInterfaceNumber),
+    .wLength  = tu_htole16(sizeof(cdc_line_coding_t))
+  };
+
+  // use usbh enum buf to hold line coding since user line_coding variable does not live long enough
+  uint8_t* enum_buf = usbh_get_enum_buf();
+  memcpy(enum_buf, line_coding, sizeof(cdc_line_coding_t));
+
+  p_cdc->user_control_cb = complete_cb;
+  tuh_xfer_t xfer = {
+    .daddr       = p_cdc->daddr,
+    .ep_addr     = 0,
+    .setup       = &request,
+    .buffer      = enum_buf,
+    .complete_cb = complete_cb ? cdch_internal_control_complete : NULL, // complete_cb is NULL for sync call
+    .user_data   = user_data
+  };
+
+  TU_ASSERT(tuh_control_xfer(&xfer));
+  return true;
+}
+
+static bool acm_set_data_format(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits,
+                                tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  TU_LOG_DRV("CDC ACM Set Data Format\r\n");
+
+  cdc_line_coding_t line_coding;
+  line_coding.bit_rate = p_cdc->line_coding.bit_rate;
+  line_coding.stop_bits = stop_bits;
+  line_coding.parity = parity;
+  line_coding.data_bits = data_bits;
+
+  return acm_set_line_coding(p_cdc, &line_coding, complete_cb, user_data);
+}
+
+static bool acm_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  TU_VERIFY(p_cdc->acm_capability.support_line_request);
+  cdc_line_coding_t line_coding = p_cdc->line_coding;
+  line_coding.bit_rate = baudrate;
+  return acm_set_line_coding(p_cdc, &line_coding, complete_cb, user_data);
+}
+
+//--------------------------------------------------------------------+
+// FTDI
+//--------------------------------------------------------------------+
+#if CFG_TUH_CDC_FTDI
+
+enum {
+  CONFIG_FTDI_RESET = 0,
+  CONFIG_FTDI_MODEM_CTRL,
+  CONFIG_FTDI_SET_BAUDRATE,
+  CONFIG_FTDI_SET_DATA,
+  CONFIG_FTDI_COMPLETE
+};
+
+static bool ftdi_open(uint8_t daddr, const tusb_desc_interface_t *itf_desc, uint16_t max_len) {
+  // FTDI Interface includes 1 vendor interface + 2 bulk endpoints
+  TU_VERIFY(itf_desc->bInterfaceSubClass == 0xff && itf_desc->bInterfaceProtocol == 0xff && itf_desc->bNumEndpoints == 2);
+  TU_VERIFY(sizeof(tusb_desc_interface_t) + 2*sizeof(tusb_desc_endpoint_t) <= max_len);
+
+  cdch_interface_t * p_cdc = make_new_itf(daddr, itf_desc);
+  TU_VERIFY(p_cdc);
+
+  TU_LOG_DRV("FTDI opened\r\n");
+  p_cdc->serial_drid = SERIAL_DRIVER_FTDI;
+
+  // endpoint pair
+  tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) tu_desc_next(itf_desc);
+
+  // data endpoints expected to be in pairs
+  return open_ep_stream_pair(p_cdc, desc_ep);
+}
+
+// set request without data
+static bool ftdi_sio_set_request(cdch_interface_t* p_cdc, uint8_t command, uint16_t value, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  tusb_control_request_t const request = {
+    .bmRequestType_bit = {
+      .recipient = TUSB_REQ_RCPT_DEVICE,
+      .type      = TUSB_REQ_TYPE_VENDOR,
+      .direction = TUSB_DIR_OUT
+    },
+    .bRequest = command,
+    .wValue   = tu_htole16(value),
+    .wIndex   = 0,
+    .wLength  = 0
+  };
+
+  tuh_xfer_t xfer = {
+    .daddr       = p_cdc->daddr,
     .ep_addr     = 0,
     .setup       = &request,
     .buffer      = NULL,
     .complete_cb = complete_cb,
-    .user_data    = 0
+    .user_data   = user_data
   };
 
   return tuh_control_xfer(&xfer);
 }
 
+static bool ftdi_sio_reset(cdch_interface_t* p_cdc, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  return ftdi_sio_set_request(p_cdc, FTDI_SIO_RESET, FTDI_SIO_RESET_SIO, complete_cb, user_data);
+}
+
+static bool ftdi_set_data_format(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits,
+                                 tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  (void) p_cdc;
+  (void) stop_bits;
+  (void) parity;
+  (void) data_bits;
+  (void) complete_cb;
+  (void) user_data;
+  // TODO not implemented yet
+  return false;
+}
+
+static bool ftdi_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  (void) p_cdc;
+  (void) line_coding;
+  (void) complete_cb;
+  (void) user_data;
+  // TODO not implemented yet
+  return false;
+}
+
+static bool ftdi_sio_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  TU_LOG_DRV("CDC FTDI Set Control Line State\r\n");
+  p_cdc->user_control_cb = complete_cb;
+  TU_ASSERT(ftdi_sio_set_request(p_cdc, FTDI_SIO_MODEM_CTRL, 0x0300 | line_state,
+                                 complete_cb ? cdch_internal_control_complete : NULL, user_data));
+  return true;
+}
+
+static uint32_t ftdi_232bm_baud_base_to_divisor(uint32_t baud, uint32_t base) {
+  const uint8_t divfrac[8] = { 0, 3, 2, 4, 1, 5, 6, 7 };
+  uint32_t divisor;
+
+  /* divisor shifted 3 bits to the left */
+  uint32_t divisor3 = base / (2 * baud);
+  divisor = (divisor3 >> 3);
+  divisor |= (uint32_t) divfrac[divisor3 & 0x7] << 14;
+
+  /* Deal with special cases for highest baud rates. */
+  if (divisor == 1) { /* 1.0 */
+    divisor = 0;
+  }
+  else if (divisor == 0x4001) { /* 1.5 */
+    divisor = 1;
+  }
+
+  return divisor;
+}
+
+static uint32_t ftdi_232bm_baud_to_divisor(uint32_t baud) {
+  return ftdi_232bm_baud_base_to_divisor(baud, 48000000u);
+}
+
+static bool ftdi_sio_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  uint16_t const divisor = (uint16_t) ftdi_232bm_baud_to_divisor(baudrate);
+  TU_LOG_DRV("CDC FTDI Set BaudRate = %" PRIu32 ", divisor = 0x%04x\r\n", baudrate, divisor);
+
+  p_cdc->user_control_cb = complete_cb;
+  p_cdc->requested_line_coding.bit_rate = baudrate;
+  TU_ASSERT(ftdi_sio_set_request(p_cdc, FTDI_SIO_SET_BAUD_RATE, divisor,
+                                 complete_cb ? cdch_internal_control_complete : NULL, user_data));
+
+  return true;
+}
+
+static void ftdi_process_config(tuh_xfer_t* xfer) {
+  uintptr_t const state = xfer->user_data;
+  uint8_t const itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
+  uint8_t const idx = tuh_cdc_itf_get_index(xfer->daddr, itf_num);
+  cdch_interface_t * p_cdc = get_itf(idx);
+  TU_ASSERT(p_cdc, );
+
+  switch(state) {
+    // Note may need to read FTDI eeprom
+    case CONFIG_FTDI_RESET:
+      TU_ASSERT(ftdi_sio_reset(p_cdc, ftdi_process_config, CONFIG_FTDI_MODEM_CTRL),);
+      break;
+
+    case CONFIG_FTDI_MODEM_CTRL:
+      #if CFG_TUH_CDC_LINE_CONTROL_ON_ENUM
+      TU_ASSERT(ftdi_sio_set_modem_ctrl(p_cdc, CFG_TUH_CDC_LINE_CONTROL_ON_ENUM, ftdi_process_config, CONFIG_FTDI_SET_BAUDRATE),);
+      break;
+      #else
+      TU_ATTR_FALLTHROUGH;
+      #endif
+
+    case CONFIG_FTDI_SET_BAUDRATE: {
+      #ifdef CFG_TUH_CDC_LINE_CODING_ON_ENUM
+      cdc_line_coding_t line_coding = CFG_TUH_CDC_LINE_CODING_ON_ENUM;
+      TU_ASSERT(ftdi_sio_set_baudrate(p_cdc, line_coding.bit_rate, ftdi_process_config, CONFIG_FTDI_SET_DATA),);
+      break;
+      #else
+      TU_ATTR_FALLTHROUGH;
+      #endif
+    }
+
+    case CONFIG_FTDI_SET_DATA: {
+      #if 0 // TODO set data format
+      #ifdef CFG_TUH_CDC_LINE_CODING_ON_ENUM
+      cdc_line_coding_t line_coding = CFG_TUH_CDC_LINE_CODING_ON_ENUM;
+      TU_ASSERT(ftdi_sio_set_data(p_cdc, process_ftdi_config, CONFIG_FTDI_COMPLETE),);
+      break;
+      #endif
+      #endif
+
+      TU_ATTR_FALLTHROUGH;
+    }
+
+    case CONFIG_FTDI_COMPLETE:
+      set_config_complete(p_cdc, idx, itf_num);
+      break;
+
+    default:
+      break;
+  }
+}
+
+#endif
+
 //--------------------------------------------------------------------+
-// USBH-CLASS DRIVER API
+// CP210x
 //--------------------------------------------------------------------+
-void cdch_init(void)
-{
-  tu_memclr(cdch_data, sizeof(cdch_data));
+
+#if CFG_TUH_CDC_CP210X
+
+enum {
+  CONFIG_CP210X_IFC_ENABLE = 0,
+  CONFIG_CP210X_SET_BAUDRATE,
+  CONFIG_CP210X_SET_LINE_CTL,
+  CONFIG_CP210X_SET_DTR_RTS,
+  CONFIG_CP210X_COMPLETE
+};
+
+static bool cp210x_open(uint8_t daddr, tusb_desc_interface_t const *itf_desc, uint16_t max_len) {
+  // CP210x Interface includes 1 vendor interface + 2 bulk endpoints
+  TU_VERIFY(itf_desc->bInterfaceSubClass == 0 && itf_desc->bInterfaceProtocol == 0 && itf_desc->bNumEndpoints == 2);
+  TU_VERIFY(sizeof(tusb_desc_interface_t) + 2*sizeof(tusb_desc_endpoint_t) <= max_len);
+
+  cdch_interface_t * p_cdc = make_new_itf(daddr, itf_desc);
+  TU_VERIFY(p_cdc);
+
+  TU_LOG_DRV("CP210x opened\r\n");
+  p_cdc->serial_drid = SERIAL_DRIVER_CP210X;
+
+  // endpoint pair
+  tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) tu_desc_next(itf_desc);
+
+  // data endpoints expected to be in pairs
+  return open_ep_stream_pair(p_cdc, desc_ep);
 }
 
-bool cdch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t max_len)
-{
-  (void) rhport;
-  (void) max_len;
+static bool cp210x_set_request(cdch_interface_t* p_cdc, uint8_t command, uint16_t value, uint8_t* buffer, uint16_t length, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  tusb_control_request_t const request = {
+    .bmRequestType_bit = {
+      .recipient = TUSB_REQ_RCPT_INTERFACE,
+      .type      = TUSB_REQ_TYPE_VENDOR,
+      .direction = TUSB_DIR_OUT
+    },
+    .bRequest = command,
+    .wValue   = tu_htole16(value),
+    .wIndex   = p_cdc->bInterfaceNumber,
+    .wLength  = tu_htole16(length)
+  };
 
-  // Only support ACM subclass
-  // Protocol 0xFF can be RNDIS device for windows XP
-  TU_VERIFY( TUSB_CLASS_CDC                           == itf_desc->bInterfaceClass &&
-             CDC_COMM_SUBCLASS_ABSTRACT_CONTROL_MODEL == itf_desc->bInterfaceSubClass &&
-             0xFF                                     != itf_desc->bInterfaceProtocol);
+  // use usbh enum buf since application variable does not live long enough
+  uint8_t* enum_buf = NULL;
 
-  cdch_data_t * p_cdc = get_itf(dev_addr);
+  if (buffer && length > 0) {
+    enum_buf = usbh_get_enum_buf();
+    tu_memcpy_s(enum_buf, CFG_TUH_ENUMERATION_BUFSIZE, buffer, length);
+  }
 
-  p_cdc->itf_num      = itf_desc->bInterfaceNumber;
-  p_cdc->itf_protocol = itf_desc->bInterfaceProtocol;
+  tuh_xfer_t xfer = {
+    .daddr       = p_cdc->daddr,
+    .ep_addr     = 0,
+    .setup       = &request,
+    .buffer      = enum_buf,
+    .complete_cb = complete_cb,
+    .user_data   = user_data
+  };
 
-  //------------- Communication Interface -------------//
-  uint16_t drv_len = tu_desc_len(itf_desc);
-  uint8_t const * p_desc = tu_desc_next(itf_desc);
+  return tuh_control_xfer(&xfer);
+}
 
-  // Communication Functional Descriptors
-  while( TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc) && drv_len <= max_len )
-  {
-    if ( CDC_FUNC_DESC_ABSTRACT_CONTROL_MANAGEMENT == cdc_functional_desc_typeof(p_desc) )
-    {
-      // save ACM bmCapabilities
-      p_cdc->acm_capability = ((cdc_desc_func_acm_t const *) p_desc)->bmCapabilities;
+static bool cp210x_ifc_enable(cdch_interface_t* p_cdc, uint16_t enabled, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  return cp210x_set_request(p_cdc, CP210X_IFC_ENABLE, enabled, NULL, 0, complete_cb, user_data);
+}
+
+static bool cp210x_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  // TODO implement later
+  (void) p_cdc;
+  (void) line_coding;
+  (void) complete_cb;
+  (void) user_data;
+  return false;
+}
+
+static bool cp210x_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  TU_LOG_DRV("CDC CP210x Set BaudRate = %" PRIu32 "\r\n", baudrate);
+  uint32_t baud_le = tu_htole32(baudrate);
+  p_cdc->user_control_cb = complete_cb;
+  return cp210x_set_request(p_cdc, CP210X_SET_BAUDRATE, 0, (uint8_t *) &baud_le, 4,
+                            complete_cb ? cdch_internal_control_complete : NULL, user_data);
+}
+
+static bool cp210x_set_data_format(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits,
+                                   tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  (void) p_cdc;
+  (void) stop_bits;
+  (void) parity;
+  (void) data_bits;
+  (void) complete_cb;
+  (void) user_data;
+  // TODO not implemented yet
+  return false;
+}
+
+static bool cp210x_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  TU_LOG_DRV("CDC CP210x Set Control Line State\r\n");
+  p_cdc->user_control_cb = complete_cb;
+  return cp210x_set_request(p_cdc, CP210X_SET_MHS, 0x0300 | line_state, NULL, 0,
+                            complete_cb ? cdch_internal_control_complete : NULL, user_data);
+}
+
+static void cp210x_process_config(tuh_xfer_t* xfer) {
+  uintptr_t const state   = xfer->user_data;
+  uint8_t const   itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
+  uint8_t const   idx     = tuh_cdc_itf_get_index(xfer->daddr, itf_num);
+  cdch_interface_t *p_cdc = get_itf(idx);
+  TU_ASSERT(p_cdc,);
+
+  switch (state) {
+    case CONFIG_CP210X_IFC_ENABLE:
+      TU_ASSERT(cp210x_ifc_enable(p_cdc, 1, cp210x_process_config, CONFIG_CP210X_SET_BAUDRATE),);
+      break;
+
+    case CONFIG_CP210X_SET_BAUDRATE: {
+      #ifdef CFG_TUH_CDC_LINE_CODING_ON_ENUM
+      cdc_line_coding_t line_coding = CFG_TUH_CDC_LINE_CODING_ON_ENUM;
+      TU_ASSERT(cp210x_set_baudrate(p_cdc, line_coding.bit_rate, cp210x_process_config, CONFIG_CP210X_SET_LINE_CTL),);
+      break;
+      #else
+      TU_ATTR_FALLTHROUGH;
+      #endif
     }
 
-    drv_len += tu_desc_len(p_desc);
-    p_desc = tu_desc_next(p_desc);
+    case CONFIG_CP210X_SET_LINE_CTL: {
+      #if defined(CFG_TUH_CDC_LINE_CODING_ON_ENUM) && 0 // skip for now
+      cdc_line_coding_t line_coding = CFG_TUH_CDC_LINE_CODING_ON_ENUM;
+      break;
+      #else
+      TU_ATTR_FALLTHROUGH;
+      #endif
+    }
+
+    case CONFIG_CP210X_SET_DTR_RTS:
+      #if CFG_TUH_CDC_LINE_CONTROL_ON_ENUM
+      TU_ASSERT(cp210x_set_modem_ctrl(p_cdc, CFG_TUH_CDC_LINE_CONTROL_ON_ENUM, cp210x_process_config, CONFIG_CP210X_COMPLETE),);
+      break;
+      #else
+      TU_ATTR_FALLTHROUGH;
+      #endif
+
+    case CONFIG_CP210X_COMPLETE:
+      set_config_complete(p_cdc, idx, itf_num);
+      break;
+
+    default: break;
   }
+}
 
-  if ( TUSB_DESC_ENDPOINT == tu_desc_type(p_desc) )
-  {
-    // notification endpoint
-    tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+#endif
 
-    TU_ASSERT( tuh_edpt_open(dev_addr, desc_ep) );
-    p_cdc->ep_notif = desc_ep->bEndpointAddress;
+//--------------------------------------------------------------------+
+// CH34x (CH340 & CH341)
+//--------------------------------------------------------------------+
 
-    drv_len += tu_desc_len(p_desc);
-    p_desc = tu_desc_next(p_desc);
+#if CFG_TUH_CDC_CH34X
+
+static uint8_t ch34x_get_lcr(uint8_t stop_bits, uint8_t parity, uint8_t data_bits);
+static uint16_t ch34x_get_divisor_prescaler(uint32_t baval);
+
+//------------- control request -------------//
+
+static bool ch34x_set_request(cdch_interface_t* p_cdc, uint8_t direction, uint8_t request, uint16_t value,
+                              uint16_t index, uint8_t* buffer, uint16_t length, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  tusb_control_request_t const request_setup = {
+      .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_DEVICE,
+          .type      = TUSB_REQ_TYPE_VENDOR,
+          .direction = direction & 0x01u
+      },
+      .bRequest = request,
+      .wValue   = tu_htole16 (value),
+      .wIndex   = tu_htole16 (index),
+      .wLength  = tu_htole16 (length)
+  };
+
+  // use usbh enum buf since application variable does not live long enough
+  uint8_t* enum_buf = NULL;
+
+  if (buffer && length > 0) {
+    enum_buf = usbh_get_enum_buf();
+    if (direction == TUSB_DIR_OUT) {
+      tu_memcpy_s(enum_buf, CFG_TUH_ENUMERATION_BUFSIZE, buffer, length);
+    }
   }
 
-  //------------- Data Interface (if any) -------------//
-  if ( (TUSB_DESC_INTERFACE == tu_desc_type(p_desc)) &&
-       (TUSB_CLASS_CDC_DATA == ((tusb_desc_interface_t const *) p_desc)->bInterfaceClass) )
-  {
-    // next to endpoint descriptor
-    drv_len += tu_desc_len(p_desc);
-    p_desc = tu_desc_next(p_desc);
+  tuh_xfer_t xfer = {
+      .daddr       = p_cdc->daddr,
+      .ep_addr     = 0,
+      .setup       = &request_setup,
+      .buffer      = enum_buf,
+      .complete_cb = complete_cb,
+      .user_data   = user_data
+  };
 
-    // data endpoints expected to be in pairs
-    for(uint32_t i=0; i<2; i++)
-    {
-      tusb_desc_endpoint_t const *desc_ep = (tusb_desc_endpoint_t const *) p_desc;
-      TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType && TUSB_XFER_BULK == desc_ep->bmAttributes.xfer);
+  return tuh_control_xfer(&xfer);
+}
 
-      TU_ASSERT(tuh_edpt_open(dev_addr, desc_ep));
+static inline bool ch34x_control_out(cdch_interface_t* p_cdc, uint8_t request, uint16_t value, uint16_t index,
+                                     tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  return ch34x_set_request(p_cdc, TUSB_DIR_OUT, request, value, index, NULL, 0, complete_cb, user_data);
+}
 
-      if ( tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN )
-      {
-        p_cdc->ep_in = desc_ep->bEndpointAddress;
-      }else
-      {
-        p_cdc->ep_out = desc_ep->bEndpointAddress;
-      }
+static inline bool ch34x_control_in(cdch_interface_t* p_cdc, uint8_t request, uint16_t value, uint16_t index,
+                                    uint8_t* buffer, uint16_t buffersize, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  return ch34x_set_request(p_cdc, TUSB_DIR_IN, request, value, index, buffer, buffersize,
+                           complete_cb, user_data);
+}
 
-      drv_len += tu_desc_len(p_desc);
-      p_desc = tu_desc_next( p_desc );
+static inline bool ch34x_write_reg(cdch_interface_t* p_cdc, uint16_t reg, uint16_t reg_value, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  return ch34x_control_out(p_cdc, CH34X_REQ_WRITE_REG, reg, reg_value, complete_cb, user_data);
+}
+
+//static bool ch34x_read_reg_request ( cdch_interface_t* p_cdc, uint16_t reg,
+//                                     uint8_t *buffer, uint16_t buffersize, tuh_xfer_cb_t complete_cb, uintptr_t user_data )
+//{
+//  return ch34x_control_in ( p_cdc, CH34X_REQ_READ_REG, reg, 0, buffer, buffersize, complete_cb, user_data );
+//}
+
+static bool ch34x_write_reg_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate,
+                                     tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  uint16_t const div_ps = ch34x_get_divisor_prescaler(baudrate);
+  TU_VERIFY(div_ps);
+  TU_ASSERT(ch34x_write_reg(p_cdc, CH34X_REG16_DIVISOR_PRESCALER, div_ps,
+                            complete_cb, user_data));
+  return true;
+}
+
+//------------- Driver API -------------//
+
+// internal control complete to update state such as line state, encoding
+static void ch34x_control_complete(tuh_xfer_t* xfer) {
+  // CH34x only has 1 interface and use wIndex as payload and not for bInterfaceNumber
+  process_internal_control_complete(xfer, 0);
+}
+
+static bool ch34x_set_data_format(cdch_interface_t* p_cdc, uint8_t stop_bits, uint8_t parity, uint8_t data_bits,
+                                tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  p_cdc->requested_line_coding.stop_bits = stop_bits;
+  p_cdc->requested_line_coding.parity = parity;
+  p_cdc->requested_line_coding.data_bits = data_bits;
+
+  uint8_t const lcr = ch34x_get_lcr(stop_bits, parity, data_bits);
+  TU_VERIFY(lcr);
+  TU_ASSERT (ch34x_control_out(p_cdc, CH34X_REQ_WRITE_REG, CH32X_REG16_LCR2_LCR, lcr,
+                               complete_cb ? ch34x_control_complete : NULL, user_data));
+  return true;
+}
+
+static bool ch34x_set_baudrate(cdch_interface_t* p_cdc, uint32_t baudrate,
+                               tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  p_cdc->requested_line_coding.bit_rate = baudrate;
+  p_cdc->user_control_cb = complete_cb;
+  TU_ASSERT(ch34x_write_reg_baudrate(p_cdc, baudrate,
+                                     complete_cb ? ch34x_control_complete : NULL, user_data));
+  return true;
+}
+
+static void ch34x_set_line_coding_stage1_complete(tuh_xfer_t* xfer) {
+  // CH34x only has 1 interface and use wIndex as payload and not for bInterfaceNumber
+  uint8_t const itf_num = 0;
+  uint8_t const idx = tuh_cdc_itf_get_index(xfer->daddr, itf_num);
+  cdch_interface_t* p_cdc = get_itf(idx);
+  TU_ASSERT(p_cdc, );
+
+  if (xfer->result == XFER_RESULT_SUCCESS) {
+    // stage 1 success, continue to stage 2
+    p_cdc->line_coding.bit_rate = p_cdc->requested_line_coding.bit_rate;
+    TU_ASSERT(ch34x_set_data_format(p_cdc, p_cdc->requested_line_coding.stop_bits, p_cdc->requested_line_coding.parity,
+                                    p_cdc->requested_line_coding.data_bits, ch34x_control_complete, xfer->user_data), );
+  } else {
+    // stage 1 failed, notify user
+    xfer->complete_cb = p_cdc->user_control_cb;
+    if (xfer->complete_cb) {
+      xfer->complete_cb(xfer);
+    }
+  }
+}
+
+// 2 stages: set baudrate (stage1) + set data format (stage2)
+static bool ch34x_set_line_coding(cdch_interface_t* p_cdc, cdc_line_coding_t const* line_coding,
+                                  tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  p_cdc->requested_line_coding = *line_coding;
+  p_cdc->user_control_cb = complete_cb;
+
+  if (complete_cb) {
+    // stage 1 set baudrate
+    TU_ASSERT(ch34x_write_reg_baudrate(p_cdc, line_coding->bit_rate,
+                                       ch34x_set_line_coding_stage1_complete, user_data));
+  } else {
+    // sync call
+    xfer_result_t result;
+
+    // stage 1 set baudrate
+    TU_ASSERT(ch34x_write_reg_baudrate(p_cdc, line_coding->bit_rate, NULL, (uintptr_t) &result));
+    TU_VERIFY(result == XFER_RESULT_SUCCESS);
+    p_cdc->line_coding.bit_rate = line_coding->bit_rate;
+
+    // stage 2 set data format
+    TU_ASSERT(ch34x_set_data_format(p_cdc, line_coding->stop_bits, line_coding->parity, line_coding->data_bits,
+                                    NULL, (uintptr_t) &result));
+    TU_VERIFY(result == XFER_RESULT_SUCCESS);
+    p_cdc->line_coding.stop_bits = line_coding->stop_bits;
+    p_cdc->line_coding.parity = line_coding->parity;
+    p_cdc->line_coding.data_bits = line_coding->data_bits;
+
+    // update transfer result, user_data is expected to point to xfer_result_t
+    if (user_data) {
+      *((xfer_result_t*) user_data) = result;
     }
   }
 
   return true;
 }
 
-bool cdch_set_config(uint8_t dev_addr, uint8_t itf_num)
-{
-  (void) dev_addr; (void) itf_num;
+static bool ch34x_set_modem_ctrl(cdch_interface_t* p_cdc, uint16_t line_state,
+                                 tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  uint8_t control = 0;
+  if (line_state & CDC_CONTROL_LINE_STATE_RTS) {
+    control |= CH34X_BIT_RTS;
+  }
+  if (line_state & CDC_CONTROL_LINE_STATE_DTR) {
+    control |= CH34X_BIT_DTR;
+  }
+
+  // CH34x signals are inverted
+  control = ~control;
+
+  p_cdc->user_control_cb = complete_cb;
+  TU_ASSERT (ch34x_control_out(p_cdc, CH34X_REQ_MODEM_CTRL, control, 0,
+                               complete_cb ? ch34x_control_complete : NULL, user_data));
   return true;
 }
 
-bool cdch_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes)
-{
-  (void) ep_addr;
-  tuh_cdc_xfer_isr( dev_addr, event, 0, xferred_bytes );
+//------------- Enumeration -------------//
+enum {
+  CONFIG_CH34X_READ_VERSION = 0,
+  CONFIG_CH34X_SERIAL_INIT,
+  CONFIG_CH34X_SPECIAL_REG_WRITE,
+  CONFIG_CH34X_FLOW_CONTROL,
+  CONFIG_CH34X_MODEM_CONTROL,
+  CONFIG_CH34X_COMPLETE
+};
+
+static bool ch34x_open(uint8_t daddr, tusb_desc_interface_t const* itf_desc, uint16_t max_len) {
+  // CH34x Interface includes 1 vendor interface + 2 bulk + 1 interrupt endpoints
+  TU_VERIFY (itf_desc->bNumEndpoints == 3);
+  TU_VERIFY (sizeof(tusb_desc_interface_t) + 3 * sizeof(tusb_desc_endpoint_t) <= max_len);
+
+  cdch_interface_t* p_cdc = make_new_itf(daddr, itf_desc);
+  TU_VERIFY (p_cdc);
+
+  TU_LOG_DRV ("CH34x opened\r\n");
+  p_cdc->serial_drid = SERIAL_DRIVER_CH34X;
+
+  tusb_desc_endpoint_t const* desc_ep = (tusb_desc_endpoint_t const*) tu_desc_next(itf_desc);
+
+  // data endpoints expected to be in pairs
+  TU_ASSERT(open_ep_stream_pair(p_cdc, desc_ep));
+  desc_ep += 2;
+
+  // Interrupt endpoint: not used for now
+  TU_ASSERT(TUSB_DESC_ENDPOINT == tu_desc_type(desc_ep) &&
+            TUSB_XFER_INTERRUPT == desc_ep->bmAttributes.xfer);
+  TU_ASSERT(tuh_edpt_open(daddr, desc_ep));
+  p_cdc->ep_notif = desc_ep->bEndpointAddress;
+
   return true;
 }
 
-void cdch_close(uint8_t dev_addr)
-{
-  TU_VERIFY(dev_addr <= CFG_TUH_DEVICE_MAX, );
+static void ch34x_process_config(tuh_xfer_t* xfer) {
+  // CH34x only has 1 interface and use wIndex as payload and not for bInterfaceNumber
+  uint8_t const itf_num = 0;
+  uint8_t const idx = tuh_cdc_itf_get_index(xfer->daddr, itf_num);
+  cdch_interface_t* p_cdc = get_itf(idx);
+  uintptr_t const state = xfer->user_data;
+  uint8_t buffer[2]; // TODO remove
+  TU_ASSERT (p_cdc,);
+  TU_ASSERT (xfer->result == XFER_RESULT_SUCCESS,);
+
+  switch (state) {
+    case CONFIG_CH34X_READ_VERSION:
+      TU_LOG_DRV("[%u] CDCh CH34x attempt to read Chip Version\r\n", p_cdc->daddr);
+      TU_ASSERT (ch34x_control_in(p_cdc, CH34X_REQ_READ_VERSION, 0, 0, buffer, 2, ch34x_process_config, CONFIG_CH34X_SERIAL_INIT),);
+      break;
+
+    case CONFIG_CH34X_SERIAL_INIT: {
+      // handle version read data, set CH34x line coding (incl. baudrate)
+      uint8_t const version = xfer->buffer[0];
+      TU_LOG_DRV("[%u] CDCh CH34x Chip Version = %02x\r\n", p_cdc->daddr, version);
+      // only versions >= 0x30 are tested, below 0x30 seems having other programming, see drivers from WCH vendor, Linux kernel and FreeBSD
+      TU_ASSERT (version >= 0x30,);
+      // init CH34x with line coding
+      cdc_line_coding_t const line_coding = CFG_TUH_CDC_LINE_CODING_ON_ENUM_CH34X;
+      uint16_t const div_ps = ch34x_get_divisor_prescaler(line_coding.bit_rate);
+      TU_ASSERT(div_ps, );
+      uint8_t const lcr = ch34x_get_lcr(line_coding.stop_bits, line_coding.parity, line_coding.data_bits);
+      TU_ASSERT(lcr, );
+      TU_ASSERT (ch34x_control_out(p_cdc, CH34X_REQ_SERIAL_INIT, tu_u16(lcr, 0x9c), div_ps,
+                                   ch34x_process_config, CONFIG_CH34X_SPECIAL_REG_WRITE),);
+      break;
+    }
+
+    case CONFIG_CH34X_SPECIAL_REG_WRITE:
+      // overtake line coding and do special reg write, purpose unknown, overtaken from WCH driver
+      p_cdc->line_coding = ((cdc_line_coding_t) CFG_TUH_CDC_LINE_CODING_ON_ENUM_CH34X);
+      TU_ASSERT (ch34x_write_reg(p_cdc, TU_U16(CH341_REG_0x0F, CH341_REG_0x2C), 0x0007, ch34x_process_config, CONFIG_CH34X_FLOW_CONTROL),);
+      break;
+
+    case CONFIG_CH34X_FLOW_CONTROL:
+      // no hardware flow control
+      TU_ASSERT (ch34x_write_reg(p_cdc, TU_U16(CH341_REG_0x27, CH341_REG_0x27), 0x0000, ch34x_process_config, CONFIG_CH34X_MODEM_CONTROL),);
+      break;
+
+    case CONFIG_CH34X_MODEM_CONTROL:
+      // !always! set modem controls RTS/DTR (CH34x has no reset state after CH34X_REQ_SERIAL_INIT)
+      TU_ASSERT (ch34x_set_modem_ctrl(p_cdc, CFG_TUH_CDC_LINE_CONTROL_ON_ENUM, ch34x_process_config, CONFIG_CH34X_COMPLETE),);
+      break;
+
+    case CONFIG_CH34X_COMPLETE:
+      set_config_complete(p_cdc, idx, itf_num);
+      break;
+
+    default:
+      TU_ASSERT (false,);
+      break;
+  }
+}
+
+//------------- CH34x helper  -------------//
+
+// calculate divisor and prescaler for baudrate, return it as 16-bit combined value
+static uint16_t ch34x_get_divisor_prescaler(uint32_t baval) {
+  uint8_t a;
+  uint8_t b;
+  uint32_t c;
+
+  TU_VERIFY(baval != 0 && baval <= 2000000, 0);
+  switch (baval) {
+    case 921600:
+      a = 0xf3;
+      b = 7;
+      break;
+
+    case 307200:
+      a = 0xd9;
+      b = 7;
+      break;
+
+    default:
+      if (baval > 6000000 / 255) {
+        b = 3;
+        c = 6000000;
+      } else if (baval > 750000 / 255) {
+        b = 2;
+        c = 750000;
+      } else if (baval > 93750 / 255) {
+        b = 1;
+        c = 93750;
+      } else {
+        b = 0;
+        c = 11719;
+      }
+      a = (uint8_t) (c / baval);
+      if (a == 0 || a == 0xFF) {
+        return 0;
+      }
+      if ((c / a - baval) > (baval - c / (a + 1))) {
+        a++;
+      }
+      a = (uint8_t) (256 - a);
+      break;
+  }
+
+  // reg divisor = a, reg prescaler = b
+  // According to linux code we need to set bit 7 of UCHCOM_REG_BPS_PRE,
+  // otherwise the chip will buffer data.
+  return (uint16_t) ((uint16_t)a << 8 | 0x80 | b);
+}
+
+// calculate lcr value from data coding
+static uint8_t ch34x_get_lcr(uint8_t stop_bits, uint8_t parity, uint8_t data_bits) {
+  uint8_t lcr = CH34X_LCR_ENABLE_RX | CH34X_LCR_ENABLE_TX;
+  TU_VERIFY(data_bits >= 5 && data_bits <= 8, 0);
+  lcr |= (uint8_t) (data_bits - 5);
+
+  switch(parity) {
+    case CDC_LINE_CODING_PARITY_NONE:
+      break;
+
+    case CDC_LINE_CODING_PARITY_ODD:
+      lcr |= CH34X_LCR_ENABLE_PAR;
+      break;
+
+    case CDC_LINE_CODING_PARITY_EVEN:
+      lcr |= CH34X_LCR_ENABLE_PAR | CH34X_LCR_PAR_EVEN;
+      break;
+
+    case CDC_LINE_CODING_PARITY_MARK:
+      lcr |= CH34X_LCR_ENABLE_PAR | CH34X_LCR_MARK_SPACE;
+      break;
 
-  cdch_data_t * p_cdc = get_itf(dev_addr);
-  tu_memclr(p_cdc, sizeof(cdch_data_t));
+    case CDC_LINE_CODING_PARITY_SPACE:
+      lcr |= CH34X_LCR_ENABLE_PAR | CH34X_LCR_MARK_SPACE | CH34X_LCR_PAR_EVEN;
+      break;
+
+    default: break;
+  }
+
+  // 1.5 stop bits not supported
+  TU_VERIFY(stop_bits != CDC_LINE_CODING_STOP_BITS_1_5, 0);
+  if (stop_bits == CDC_LINE_CODING_STOP_BITS_2) {
+    lcr |= CH34X_LCR_STOP_BITS_2;
+  }
+
+  return lcr;
 }
 
+
+#endif // CFG_TUH_CDC_CH34X
+
 #endif
diff --git a/src/class/cdc/cdc_host.h b/src/class/cdc/cdc_host.h
index 33dbd2efb..df975b2f0 100644
--- a/src/class/cdc/cdc_host.h
+++ b/src/class/cdc/cdc_host.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -34,94 +34,165 @@
 #endif
 
 //--------------------------------------------------------------------+
-// CDC APPLICATION PUBLIC API
+// Class Driver Configuration
 //--------------------------------------------------------------------+
-/** \ingroup ClassDriver_CDC Communication Device Class (CDC)
- * \addtogroup CDC_Serial Serial
- * @{
- * \defgroup   CDC_Serial_Host Host
- * @{ */
 
-bool tuh_cdc_set_control_line_state(uint8_t dev_addr, bool dtr, bool rts, tuh_xfer_cb_t complete_cb);
+// Set Line Control state on enumeration/mounted: DTR ( bit 0), RTS (bit 1)
+#ifndef CFG_TUH_CDC_LINE_CONTROL_ON_ENUM
+#define CFG_TUH_CDC_LINE_CONTROL_ON_ENUM    0
+#endif
 
-static inline bool tuh_cdc_connect(uint8_t dev_addr, tuh_xfer_cb_t complete_cb)
-{
-  return tuh_cdc_set_control_line_state(dev_addr, true, true, complete_cb);
-}
+// Set Line Coding on enumeration/mounted, value for cdc_line_coding_t
+//#ifndef CFG_TUH_CDC_LINE_CODING_ON_ENUM
+//#define CFG_TUH_CDC_LINE_CODING_ON_ENUM   { 115200, CDC_LINE_CODING_STOP_BITS_1, CDC_LINE_CODING_PARITY_NONE, 8 }
+//#endif
+
+// RX FIFO size
+#ifndef CFG_TUH_CDC_RX_BUFSIZE
+#define CFG_TUH_CDC_RX_BUFSIZE USBH_EPSIZE_BULK_MAX
+#endif
+
+// RX Endpoint size
+#ifndef CFG_TUH_CDC_RX_EPSIZE
+#define CFG_TUH_CDC_RX_EPSIZE  USBH_EPSIZE_BULK_MAX
+#endif
+
+// TX FIFO size
+#ifndef CFG_TUH_CDC_TX_BUFSIZE
+#define CFG_TUH_CDC_TX_BUFSIZE USBH_EPSIZE_BULK_MAX
+#endif
+
+// TX Endpoint size
+#ifndef CFG_TUH_CDC_TX_EPSIZE
+#define CFG_TUH_CDC_TX_EPSIZE  USBH_EPSIZE_BULK_MAX
+#endif
+
+//--------------------------------------------------------------------+
+// Application API
+//--------------------------------------------------------------------+
+
+// Get Interface index from device address + interface number
+// return TUSB_INDEX_INVALID_8 (0xFF) if not found
+uint8_t tuh_cdc_itf_get_index(uint8_t daddr, uint8_t itf_num);
+
+// Get Interface information
+// return true if index is correct and interface is currently mounted
+bool tuh_cdc_itf_get_info(uint8_t idx, tuh_itf_info_t* info);
+
+// Check if a interface is mounted
+bool tuh_cdc_mounted(uint8_t idx);
+
+// Get current DTR status
+bool tuh_cdc_get_dtr(uint8_t idx);
+
+// Get current RTS status
+bool tuh_cdc_get_rts(uint8_t idx);
 
-static inline bool tuh_cdc_disconnect(uint8_t dev_addr, tuh_xfer_cb_t complete_cb)
-{
-  return tuh_cdc_set_control_line_state(dev_addr, false, false, complete_cb);
+// Check if interface is connected (DTR active)
+TU_ATTR_ALWAYS_INLINE static inline bool tuh_cdc_connected(uint8_t idx) {
+  return tuh_cdc_get_dtr(idx);
 }
 
-/** \brief 			Check if device support CDC Serial interface or not
- * \param[in]		dev_addr	device address
- * \retval      true if device supports
- * \retval      false if device does not support or is not mounted
- */
-bool tuh_cdc_serial_is_mounted(uint8_t dev_addr);
+// Get local (saved/cached) version of line coding.
+// This function should return correct values if tuh_cdc_set_line_coding() / tuh_cdc_get_line_coding()
+// are invoked previously or CFG_TUH_CDC_LINE_CODING_ON_ENUM is defined.
+// NOTE: This function does not make any USB transfer request to device.
+bool tuh_cdc_get_local_line_coding(uint8_t idx, cdc_line_coding_t* line_coding);
 
-/** \brief      Check if the interface is currently busy or not
- * \param[in]   dev_addr device address
- * \param[in]   pipeid value from \ref cdc_pipeid_t to indicate target pipe.
- * \retval      true if the interface is busy, meaning the stack is still transferring/waiting data from/to device
- * \retval      false if the interface is not busy, meaning the stack successfully transferred data from/to device
- * \note        This function is used to check if previous transfer is complete (success or error), so that the next transfer
- *              can be scheduled. User needs to make sure the corresponding interface is mounted
- *              (by \ref tuh_cdc_serial_is_mounted) before calling this function.
- */
-bool tuh_cdc_is_busy(uint8_t dev_addr, cdc_pipeid_t pipeid);
+//--------------------------------------------------------------------+
+// Write API
+//--------------------------------------------------------------------+
 
-/** \brief 			Perform USB OUT transfer to device
- * \param[in]		dev_addr	device address
- * \param[in]	  p_data    Buffer containing data. Must be accessible by USB controller (see \ref CFG_TUSB_MEM_SECTION)
- * \param[in]		length    Number of bytes to be transferred via USB bus
- * \retval      TUSB_ERROR_NONE on success
- * \retval      TUSB_ERROR_INTERFACE_IS_BUSY if the interface is already transferring data with device
- * \retval      TUSB_ERROR_DEVICE_NOT_READY if device is not yet configured (by SET CONFIGURED request)
- * \retval      TUSB_ERROR_INVALID_PARA if input parameters are not correct
- * \note        This function is non-blocking and returns immediately. The result of USB transfer will be reported by the
- *              interface's callback function. \a p_data must be declared with \ref CFG_TUSB_MEM_SECTION.
- */
-bool tuh_cdc_send(uint8_t dev_addr, void const * p_data, uint32_t length, bool is_notify);
+// Get the number of bytes available for writing
+uint32_t tuh_cdc_write_available(uint8_t idx);
 
-/** \brief 			Perform USB IN transfer to get data from device
- * \param[in]		dev_addr	device address
- * \param[in]	  p_buffer  Buffer containing received data. Must be accessible by USB controller (see \ref CFG_TUSB_MEM_SECTION)
- * \param[in]		length    Number of bytes to be transferred via USB bus
- * \retval      TUSB_ERROR_NONE on success
- * \retval      TUSB_ERROR_INTERFACE_IS_BUSY if the interface is already transferring data with device
- * \retval      TUSB_ERROR_DEVICE_NOT_READY if device is not yet configured (by SET CONFIGURED request)
- * \retval      TUSB_ERROR_INVALID_PARA if input parameters are not correct
- * \note        This function is non-blocking and returns immediately. The result of USB transfer will be reported by the
- *              interface's callback function. \a p_data must be declared with \ref CFG_TUSB_MEM_SECTION.
- */
-bool tuh_cdc_receive(uint8_t dev_addr, void * p_buffer, uint32_t length, bool is_notify);
+// Write to cdc interface
+uint32_t tuh_cdc_write(uint8_t idx, void const* buffer, uint32_t bufsize);
+
+// Force sending data if possible, return number of forced bytes
+uint32_t tuh_cdc_write_flush(uint8_t idx);
+
+// Clear the transmit FIFO
+bool tuh_cdc_write_clear(uint8_t idx);
+
+//--------------------------------------------------------------------+
+// Read API
+//--------------------------------------------------------------------+
+
+// Get the number of bytes available for reading
+uint32_t tuh_cdc_read_available(uint8_t idx);
+
+// Read from cdc interface
+uint32_t tuh_cdc_read (uint8_t idx, void* buffer, uint32_t bufsize);
+
+// Get a byte from RX FIFO without removing it
+bool tuh_cdc_peek(uint8_t idx, uint8_t* ch);
+
+// Clear the received FIFO
+bool tuh_cdc_read_clear (uint8_t idx);
+
+//--------------------------------------------------------------------+
+// Control Endpoint (Request) API
+// Each Function will make a USB control transfer request to/from device
+// - If complete_cb is provided, the function will return immediately and invoke
+// the callback when request is complete.
+// - If complete_cb is NULL, the function will block until request is complete.
+//   - In this case, user_data should be pointed to xfer_result_t to hold the transfer result.
+//   - The function will return true if transfer is successful, false otherwise.
+//--------------------------------------------------------------------+
+
+// Request to Set Control Line State: DTR (bit 0), RTS (bit 1)
+bool tuh_cdc_set_control_line_state(uint8_t idx, uint16_t line_state, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+
+// Request to set baudrate
+bool tuh_cdc_set_baudrate(uint8_t idx, uint32_t baudrate, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+
+// Request to set data format
+bool tuh_cdc_set_data_format(uint8_t idx, uint8_t stop_bits, uint8_t parity, uint8_t data_bits, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+
+// Request to Set Line Coding = baudrate + data format
+// Note: only implemented by ACM and CH34x, not supported by FTDI and CP210x yet
+bool tuh_cdc_set_line_coding(uint8_t idx, cdc_line_coding_t const* line_coding, tuh_xfer_cb_t complete_cb, uintptr_t user_data);
+
+// Request to Get Line Coding (ACM only)
+// Should only use if tuh_cdc_set_line_coding() / tuh_cdc_get_line_coding() never got invoked and
+// CFG_TUH_CDC_LINE_CODING_ON_ENUM is not defined
+// bool tuh_cdc_get_line_coding(uint8_t idx, cdc_line_coding_t* coding);
+
+// Connect by set both DTR, RTS
+TU_ATTR_ALWAYS_INLINE static inline
+bool tuh_cdc_connect(uint8_t idx, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  return tuh_cdc_set_control_line_state(idx, CDC_CONTROL_LINE_STATE_DTR | CDC_CONTROL_LINE_STATE_RTS, complete_cb, user_data);
+}
+
+// Disconnect by clear both DTR, RTS
+TU_ATTR_ALWAYS_INLINE static inline
+bool tuh_cdc_disconnect(uint8_t idx, tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  return tuh_cdc_set_control_line_state(idx, 0x00, complete_cb, user_data);
+}
 
 //--------------------------------------------------------------------+
 // CDC APPLICATION CALLBACKS
 //--------------------------------------------------------------------+
 
-/** \brief      Callback function that is invoked when an transferring event occurred
- * \param[in]		dev_addr	Address of device
- * \param[in]   event an value from \ref xfer_result_t
- * \param[in]   pipe_id value from \ref cdc_pipeid_t indicate the pipe
- * \param[in]   xferred_bytes Number of bytes transferred via USB bus
- * \note        event can be one of following
- *              - XFER_RESULT_SUCCESS : previously scheduled transfer completes successfully.
- *              - XFER_RESULT_FAILED   : previously scheduled transfer encountered a transaction error.
- *              - XFER_RESULT_STALLED : previously scheduled transfer is stalled by device.
- * \note
- */
-void tuh_cdc_xfer_isr(uint8_t dev_addr, xfer_result_t event, cdc_pipeid_t pipe_id, uint32_t xferred_bytes);
+// Invoked when a device with CDC interface is mounted
+// idx is index of cdc interface in the internal pool.
+TU_ATTR_WEAK extern void tuh_cdc_mount_cb(uint8_t idx);
+
+// Invoked when a device with CDC interface is unmounted
+TU_ATTR_WEAK extern void tuh_cdc_umount_cb(uint8_t idx);
+
+// Invoked when received new data
+TU_ATTR_WEAK extern void tuh_cdc_rx_cb(uint8_t idx);
 
-/// @} // group CDC_Serial_Host
-/// @}
+// Invoked when a TX is complete and therefore space becomes available in TX buffer
+TU_ATTR_WEAK extern void tuh_cdc_tx_complete_cb(uint8_t idx);
 
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
-void cdch_init       (void);
+bool cdch_init       (void);
+bool cdch_deinit     (void);
 bool cdch_open       (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *itf_desc, uint16_t max_len);
 bool cdch_set_config (uint8_t dev_addr, uint8_t itf_num);
 bool cdch_xfer_cb    (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
diff --git a/src/class/cdc/cdc_rndis.h b/src/class/cdc/cdc_rndis.h
index e0f129fe3..ad153e0ac 100644
--- a/src/class/cdc/cdc_rndis.h
+++ b/src/class/cdc/cdc_rndis.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
diff --git a/src/class/cdc/cdc_rndis_host.c b/src/class/cdc/cdc_rndis_host.c
index 8f28f51ac..11a5355aa 100644
--- a/src/class/cdc/cdc_rndis_host.c
+++ b/src/class/cdc/cdc_rndis_host.c
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -50,8 +50,8 @@
 //--------------------------------------------------------------------+
 #define RNDIS_MSG_PAYLOAD_MAX   (1024*4)
 
-CFG_TUSB_MEM_SECTION static uint8_t msg_notification[CFG_TUH_DEVICE_MAX][8];
-CFG_TUSB_MEM_SECTION TU_ATTR_ALIGNED(4) static uint8_t msg_payload[RNDIS_MSG_PAYLOAD_MAX];
+CFG_TUH_MEM_SECTION static uint8_t msg_notification[CFG_TUH_DEVICE_MAX][8];
+CFG_TUH_MEM_SECTION CFG_TUH_MEM_ALIGN static uint8_t msg_payload[RNDIS_MSG_PAYLOAD_MAX];
 
 static rndish_data_t rndish_data[CFG_TUH_DEVICE_MAX];
 
@@ -117,7 +117,7 @@ void rndish_init(void)
 
   //------------- Task creation -------------//
 
-  //------------- semaphore creation for notificaiton pipe -------------//
+  //------------- semaphore creation for notification pipe -------------//
   for(uint8_t i=0; i<CFG_TUH_DEVICE_MAX; i++)
   {
     rndish_data[i].sem_notification_hdl = osal_semaphore_create( OSAL_SEM_REF(rndish_data[i].semaphore_notification) );
diff --git a/src/class/cdc/cdc_rndis_host.h b/src/class/cdc/cdc_rndis_host.h
index 447cc4e97..bb431ec1f 100644
--- a/src/class/cdc/cdc_rndis_host.h
+++ b/src/class/cdc/cdc_rndis_host.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
diff --git a/src/class/cdc/serial/ch34x.h b/src/class/cdc/serial/ch34x.h
new file mode 100644
index 000000000..c18066f57
--- /dev/null
+++ b/src/class/cdc/serial/ch34x.h
@@ -0,0 +1,84 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2023 Heiko Kuester
+ *
+ * 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.
+ */
+
+#ifndef _CH34X_H_
+#define _CH34X_H_
+
+// There is no official documentation for the CH34x (CH340, CH341) chips. Reference can be found
+// - https://github.com/WCHSoftGroup/ch341ser_linux
+// - https://github.com/torvalds/linux/blob/master/drivers/usb/serial/ch341.c
+// - https://github.com/freebsd/freebsd-src/blob/main/sys/dev/usb/serial/uchcom.c
+
+// set line_coding @ enumeration
+#ifdef CFG_TUH_CDC_LINE_CODING_ON_ENUM
+#define CFG_TUH_CDC_LINE_CODING_ON_ENUM_CH34X CFG_TUH_CDC_LINE_CODING_ON_ENUM
+#else // this default is necessary to work properly
+#define CFG_TUH_CDC_LINE_CODING_ON_ENUM_CH34X { 9600, CDC_LINE_CONDING_STOP_BITS_1, CDC_LINE_CODING_PARITY_NONE, 8 }
+#endif
+
+// USB requests
+#define CH34X_REQ_READ_VERSION 0x5F // dec  95
+#define CH34X_REQ_WRITE_REG    0x9A // dec 154
+#define CH34X_REQ_READ_REG     0x95 // dec 149
+#define CH34X_REQ_SERIAL_INIT  0xA1 // dec 161
+#define CH34X_REQ_MODEM_CTRL   0xA4 // dev 164
+
+// registers
+#define CH34X_REG_BREAK        0x05
+#define CH34X_REG_PRESCALER    0x12
+#define CH34X_REG_DIVISOR      0x13
+#define CH34X_REG_LCR          0x18
+#define CH34X_REG_LCR2         0x25
+#define CH34X_REG_MCR_MSR      0x06
+#define CH34X_REG_MCR_MSR2     0x07
+#define CH34X_NBREAK_BITS      0x01
+
+#define CH341_REG_0x0F         0x0F // undocumented register
+#define CH341_REG_0x2C         0x2C // undocumented register
+#define CH341_REG_0x27         0x27 // hardware flow control (cts/rts)
+
+#define CH34X_REG16_DIVISOR_PRESCALER  TU_U16(CH34X_REG_DIVISOR, CH34X_REG_PRESCALER)
+#define CH32X_REG16_LCR2_LCR           TU_U16(CH34X_REG_LCR2, CH34X_REG_LCR)
+
+// modem control bits
+#define CH34X_BIT_RTS ( 1 << 6 )
+#define CH34X_BIT_DTR ( 1 << 5 )
+
+// line control bits
+#define CH34X_LCR_ENABLE_RX    0x80
+#define CH34X_LCR_ENABLE_TX    0x40
+#define CH34X_LCR_MARK_SPACE   0x20
+#define CH34X_LCR_PAR_EVEN     0x10
+#define CH34X_LCR_ENABLE_PAR   0x08
+#define CH34X_LCR_PAR_MASK     0x38 // all parity bits
+#define CH34X_LCR_STOP_BITS_2  0x04
+#define CH34X_LCR_CS8          0x03
+#define CH34X_LCR_CS7          0x02
+#define CH34X_LCR_CS6          0x01
+#define CH34X_LCR_CS5          0x00
+#define CH34X_LCR_CS_MASK      0x03 // all CSx bits
+
+#endif /* _CH34X_H_ */
diff --git a/src/class/cdc/serial/cp210x.h b/src/class/cdc/serial/cp210x.h
new file mode 100644
index 000000000..2c749f522
--- /dev/null
+++ b/src/class/cdc/serial/cp210x.h
@@ -0,0 +1,62 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2023 Ha Thach ([email protected]) for Adafruit Industries
+ *
+ * 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.
+ */
+
+#ifndef TUSB_CP210X_H
+#define TUSB_CP210X_H
+
+// Protocol details can be found at AN571: CP210x Virtual COM Port Interface
+// https://www.silabs.com/documents/public/application-notes/AN571.pdf
+
+#define TU_CP210X_VID 0x10C4
+
+/* Config request codes */
+#define CP210X_IFC_ENABLE      0x00
+#define CP210X_SET_BAUDDIV     0x01
+#define CP210X_GET_BAUDDIV     0x02
+#define CP210X_SET_LINE_CTL    0x03 // Set parity, data bits, stop bits
+#define CP210X_GET_LINE_CTL    0x04
+#define CP210X_SET_BREAK       0x05
+#define CP210X_IMM_CHAR        0x06
+#define CP210X_SET_MHS         0x07 // Set DTR, RTS
+#define CP210X_GET_MDMSTS      0x08 // Get modem status (DTR, RTS, CTS, DSR, RI, DCD)
+#define CP210X_SET_XON         0x09
+#define CP210X_SET_XOFF        0x0A
+#define CP210X_SET_EVENTMASK   0x0B
+#define CP210X_GET_EVENTMASK   0x0C
+#define CP210X_SET_CHAR        0x0D
+#define CP210X_GET_CHARS       0x0E
+#define CP210X_GET_PROPS       0x0F
+#define CP210X_GET_COMM_STATUS 0x10
+#define CP210X_RESET           0x11
+#define CP210X_PURGE           0x12
+#define CP210X_SET_FLOW        0x13
+#define CP210X_GET_FLOW        0x14
+#define CP210X_EMBED_EVENTS    0x15
+#define CP210X_GET_EVENTSTATE  0x16
+#define CP210X_SET_CHARS       0x19
+#define CP210X_GET_BAUDRATE    0x1D
+#define CP210X_SET_BAUDRATE    0x1E
+#define CP210X_VENDOR_SPECIFIC 0xFF // GPIO, Recipient must be Device
+
+#endif //TUSB_CP210X_H
diff --git a/src/class/cdc/serial/ftdi_sio.h b/src/class/cdc/serial/ftdi_sio.h
new file mode 100644
index 000000000..0825f0719
--- /dev/null
+++ b/src/class/cdc/serial/ftdi_sio.h
@@ -0,0 +1,246 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2023 Ha Thach ([email protected]) for Adafruit Industries
+ *
+ * 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.
+ */
+
+#ifndef TUSB_FTDI_SIO_H
+#define TUSB_FTDI_SIO_H
+
+// VID for matching FTDI devices
+#define TU_FTDI_VID 0x0403
+
+// Commands
+#define FTDI_SIO_RESET             	0    /* Reset the port */
+#define FTDI_SIO_MODEM_CTRL        	1    /* Set the modem control register */
+#define FTDI_SIO_SET_FLOW_CTRL     	2    /* Set flow control register */
+#define FTDI_SIO_SET_BAUD_RATE     	3    /* Set baud rate */
+#define FTDI_SIO_SET_DATA          	4    /* Set the data characteristics of the port */
+#define FTDI_SIO_GET_MODEM_STATUS  	5    /* Retrieve current value of modem status register */
+#define FTDI_SIO_SET_EVENT_CHAR    	6    /* Set the event character */
+#define FTDI_SIO_SET_ERROR_CHAR    	7    /* Set the error character */
+#define FTDI_SIO_SET_LATENCY_TIMER 	9    /* Set the latency timer */
+#define FTDI_SIO_GET_LATENCY_TIMER 	0x0a /* Get the latency timer */
+#define FTDI_SIO_SET_BITMODE       	0x0b /* Set bitbang mode */
+#define FTDI_SIO_READ_PINS         	0x0c /* Read immediate value of pins */
+#define FTDI_SIO_READ_EEPROM       	0x90 /* Read EEPROM */
+
+/* FTDI_SIO_RESET */
+#define FTDI_SIO_RESET_SIO 0
+#define FTDI_SIO_RESET_PURGE_RX 1
+#define FTDI_SIO_RESET_PURGE_TX 2
+
+/*
+ * BmRequestType:  0100 0000B
+ * bRequest:       FTDI_SIO_RESET
+ * wValue:         Control Value
+ *                   0 = Reset SIO
+ *                   1 = Purge RX buffer
+ *                   2 = Purge TX buffer
+ * wIndex:         Port
+ * wLength:        0
+ * Data:           None
+ *
+ * The Reset SIO command has this effect:
+ *
+ *    Sets flow control set to 'none'
+ *    Event char = $0D
+ *    Event trigger = disabled
+ *    Purge RX buffer
+ *    Purge TX buffer
+ *    Clear DTR
+ *    Clear RTS
+ *    baud and data format not reset
+ *
+ * The Purge RX and TX buffer commands affect nothing except the buffers
+ *
+   */
+
+/* FTDI_SIO_MODEM_CTRL */
+/*
+ * BmRequestType:   0100 0000B
+ * bRequest:        FTDI_SIO_MODEM_CTRL
+ * wValue:          ControlValue (see below)
+ * wIndex:          Port
+ * wLength:         0
+ * Data:            None
+ *
+ * NOTE: If the device is in RTS/CTS flow control, the RTS set by this
+ * command will be IGNORED without an error being returned
+ * Also - you can not set DTR and RTS with one control message
+ */
+
+#define FTDI_SIO_SET_DTR_MASK 0x1
+#define FTDI_SIO_SET_DTR_HIGH ((FTDI_SIO_SET_DTR_MASK << 8) | 1)
+#define FTDI_SIO_SET_DTR_LOW  ((FTDI_SIO_SET_DTR_MASK << 8) | 0)
+#define FTDI_SIO_SET_RTS_MASK 0x2
+#define FTDI_SIO_SET_RTS_HIGH ((FTDI_SIO_SET_RTS_MASK << 8) | 2)
+#define FTDI_SIO_SET_RTS_LOW  ((FTDI_SIO_SET_RTS_MASK << 8) | 0)
+
+/*
+ * ControlValue
+ * B0    DTR state
+ *          0 = reset
+ *          1 = set
+ * B1    RTS state
+ *          0 = reset
+ *          1 = set
+ * B2..7 Reserved
+ * B8    DTR state enable
+ *          0 = ignore
+ *          1 = use DTR state
+ * B9    RTS state enable
+ *          0 = ignore
+ *          1 = use RTS state
+ * B10..15 Reserved
+ */
+
+/* FTDI_SIO_SET_FLOW_CTRL */
+#define FTDI_SIO_DISABLE_FLOW_CTRL 0x0
+#define FTDI_SIO_RTS_CTS_HS (0x1 << 8)
+#define FTDI_SIO_DTR_DSR_HS (0x2 << 8)
+#define FTDI_SIO_XON_XOFF_HS (0x4 << 8)
+
+/*
+ *   BmRequestType:  0100 0000b
+ *   bRequest:       FTDI_SIO_SET_FLOW_CTRL
+ *   wValue:         Xoff/Xon
+ *   wIndex:         Protocol/Port - hIndex is protocol / lIndex is port
+ *   wLength:        0
+ *   Data:           None
+ *
+ * hIndex protocol is:
+ *   B0 Output handshaking using RTS/CTS
+ *       0 = disabled
+ *       1 = enabled
+ *   B1 Output handshaking using DTR/DSR
+ *       0 = disabled
+ *       1 = enabled
+ *   B2 Xon/Xoff handshaking
+ *       0 = disabled
+ *       1 = enabled
+ *
+ * A value of zero in the hIndex field disables handshaking
+ *
+ * If Xon/Xoff handshaking is specified, the hValue field should contain the
+ * XOFF character and the lValue field contains the XON character.
+ */
+
+/* FTDI_SIO_SET_BAUD_RATE */
+/*
+ * BmRequestType:  0100 0000B
+ * bRequest:       FTDI_SIO_SET_BAUDRATE
+ * wValue:         BaudDivisor value - see below
+ * wIndex:         Port
+ * wLength:        0
+ * Data:           None
+ * The BaudDivisor values are calculated as follows (too complicated):
+ */
+
+/* FTDI_SIO_SET_DATA */
+#define FTDI_SIO_SET_DATA_PARITY_NONE	(0x0 << 8)
+#define FTDI_SIO_SET_DATA_PARITY_ODD	(0x1 << 8)
+#define FTDI_SIO_SET_DATA_PARITY_EVEN	(0x2 << 8)
+#define FTDI_SIO_SET_DATA_PARITY_MARK	(0x3 << 8)
+#define FTDI_SIO_SET_DATA_PARITY_SPACE	(0x4 << 8)
+#define FTDI_SIO_SET_DATA_STOP_BITS_1	(0x0 << 11)
+#define FTDI_SIO_SET_DATA_STOP_BITS_15	(0x1 << 11)
+#define FTDI_SIO_SET_DATA_STOP_BITS_2	(0x2 << 11)
+#define FTDI_SIO_SET_BREAK		(0x1 << 14)
+
+/*
+ * BmRequestType:  0100 0000B
+ * bRequest:       FTDI_SIO_SET_DATA
+ * wValue:         Data characteristics (see below)
+ * wIndex:         Port
+ * wLength:        0
+ * Data:           No
+ *
+ * Data characteristics
+ *
+ *   B0..7   Number of data bits
+ *   B8..10  Parity
+ *           0 = None
+ *           1 = Odd
+ *           2 = Even
+ *           3 = Mark
+ *           4 = Space
+ *   B11..13 Stop Bits
+ *           0 = 1
+ *           1 = 1.5
+ *           2 = 2
+ *   B14
+ *           1 = TX ON (break)
+ *           0 = TX OFF (normal state)
+ *   B15 Reserved
+ *
+ */
+
+/*
+* DATA FORMAT
+*
+* IN Endpoint
+*
+* The device reserves the first two bytes of data on this endpoint to contain
+* the current values of the modem and line status registers. In the absence of
+* data, the device generates a message consisting of these two status bytes
+  * every 40 ms
+  *
+  * Byte 0: Modem Status
+*
+* Offset	Description
+* B0	Reserved - must be 1
+* B1	Reserved - must be 0
+* B2	Reserved - must be 0
+* B3	Reserved - must be 0
+* B4	Clear to Send (CTS)
+* B5	Data Set Ready (DSR)
+* B6	Ring Indicator (RI)
+* B7	Receive Line Signal Detect (RLSD)
+*
+* Byte 1: Line Status
+*
+* Offset	Description
+* B0	Data Ready (DR)
+* B1	Overrun Error (OE)
+* B2	Parity Error (PE)
+* B3	Framing Error (FE)
+* B4	Break Interrupt (BI)
+* B5	Transmitter Holding Register (THRE)
+* B6	Transmitter Empty (TEMT)
+* B7	Error in RCVR FIFO
+*
+*/
+#define FTDI_RS0_CTS	(1 << 4)
+#define FTDI_RS0_DSR	(1 << 5)
+#define FTDI_RS0_RI	(1 << 6)
+#define FTDI_RS0_RLSD	(1 << 7)
+
+#define FTDI_RS_DR	1
+#define FTDI_RS_OE	(1<<1)
+#define FTDI_RS_PE	(1<<2)
+#define FTDI_RS_FE	(1<<3)
+#define FTDI_RS_BI	(1<<4)
+#define FTDI_RS_THRE	(1<<5)
+#define FTDI_RS_TEMT	(1<<6)
+#define FTDI_RS_FIFO	(1<<7)
+
+#endif //TUSB_FTDI_SIO_H
diff --git a/src/class/dfu/dfu_device.c b/src/class/dfu/dfu_device.c
index aa5891ca9..d9e2d3f2f 100644
--- a/src/class/dfu/dfu_device.c
+++ b/src/class/dfu/dfu_device.c
@@ -37,11 +37,17 @@
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
 
+// Level where CFG_TUSB_DEBUG must be at least for this driver is logged
+#ifndef CFG_TUD_DFU_LOG_LEVEL
+  #define CFG_TUD_DFU_LOG_LEVEL   CFG_TUD_LOG_LEVEL
+#endif
+
+#define TU_LOG_DRV(...)   TU_LOG(CFG_TUD_DFU_LOG_LEVEL, __VA_ARGS__)
+
 //--------------------------------------------------------------------+
 // INTERNAL OBJECT & FUNCTION DECLARATION
 //--------------------------------------------------------------------+
-typedef struct
-{
+typedef struct {
   uint8_t attrs;
   uint8_t alt;
 
@@ -51,69 +57,65 @@ typedef struct
   bool flashing_in_progress;
   uint16_t block;
   uint16_t length;
-
-  CFG_TUSB_MEM_ALIGN uint8_t transfer_buf[CFG_TUD_DFU_XFER_BUFSIZE];
 } dfu_state_ctx_t;
 
 // Only a single dfu state is allowed
-CFG_TUSB_MEM_SECTION static dfu_state_ctx_t _dfu_ctx;
+static dfu_state_ctx_t _dfu_ctx;
+
+CFG_TUD_MEM_SECTION static struct {
+  TUD_EPBUF_DEF(transfer_buf, CFG_TUD_DFU_XFER_BUFSIZE);
+} _dfu_epbuf;
 
-static void reset_state(void)
-{
+static void reset_state(void) {
   _dfu_ctx.state = DFU_IDLE;
   _dfu_ctx.status = DFU_STATUS_OK;
   _dfu_ctx.flashing_in_progress = false;
 }
 
-static bool reply_getstatus(uint8_t rhport, tusb_control_request_t const * request, dfu_state_t state, dfu_status_t status, uint32_t timeout);
-static bool process_download_get_status(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
-static bool process_manifest_get_status(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
+static bool reply_getstatus(uint8_t rhport, const tusb_control_request_t* request, dfu_state_t state, dfu_status_t status, uint32_t timeout);
+static bool process_download_get_status(uint8_t rhport, uint8_t stage, const tusb_control_request_t* request);
+static bool process_manifest_get_status(uint8_t rhport, uint8_t stage, const tusb_control_request_t* request);
 
 //--------------------------------------------------------------------+
 // Debug
 //--------------------------------------------------------------------+
 #if CFG_TUSB_DEBUG >= 2
 
-static tu_lookup_entry_t const _dfu_request_lookup[] =
-{
-  { .key = DFU_REQUEST_DETACH         , .data = "DETACH"    },
-  { .key = DFU_REQUEST_DNLOAD         , .data = "DNLOAD"    },
-  { .key = DFU_REQUEST_UPLOAD         , .data = "UPLOAD"    },
-  { .key = DFU_REQUEST_GETSTATUS      , .data = "GETSTATUS" },
-  { .key = DFU_REQUEST_CLRSTATUS      , .data = "CLRSTATUS" },
-  { .key = DFU_REQUEST_GETSTATE       , .data = "GETSTATE"  },
-  { .key = DFU_REQUEST_ABORT          , .data = "ABORT"     },
+tu_static tu_lookup_entry_t const _dfu_request_lookup[] = {
+  { .key = DFU_REQUEST_DETACH   , .data = "DETACH"    },
+  { .key = DFU_REQUEST_DNLOAD   , .data = "DNLOAD"    },
+  { .key = DFU_REQUEST_UPLOAD   , .data = "UPLOAD"    },
+  { .key = DFU_REQUEST_GETSTATUS, .data = "GETSTATUS" },
+  { .key = DFU_REQUEST_CLRSTATUS, .data = "CLRSTATUS" },
+  { .key = DFU_REQUEST_GETSTATE , .data = "GETSTATE"  },
+  { .key = DFU_REQUEST_ABORT    , .data = "ABORT"     },
 };
 
-static tu_lookup_table_t const _dfu_request_table =
-{
+tu_static tu_lookup_table_t const _dfu_request_table = {
   .count = TU_ARRAY_SIZE(_dfu_request_lookup),
   .items = _dfu_request_lookup
 };
 
-static tu_lookup_entry_t const _dfu_state_lookup[] =
-{
-  { .key = APP_IDLE                   , .data = "APP_IDLE"                },
-  { .key = APP_DETACH                 , .data = "APP_DETACH"              },
-  { .key = DFU_IDLE                   , .data = "IDLE"                },
-  { .key = DFU_DNLOAD_SYNC            , .data = "DNLOAD_SYNC"         },
-  { .key = DFU_DNBUSY                 , .data = "DNBUSY"              },
-  { .key = DFU_DNLOAD_IDLE            , .data = "DNLOAD_IDLE"         },
-  { .key = DFU_MANIFEST_SYNC          , .data = "MANIFEST_SYNC"       },
-  { .key = DFU_MANIFEST               , .data = "MANIFEST"            },
-  { .key = DFU_MANIFEST_WAIT_RESET    , .data = "MANIFEST_WAIT_RESET" },
-  { .key = DFU_UPLOAD_IDLE            , .data = "UPLOAD_IDLE"         },
-  { .key = DFU_ERROR                  , .data = "ERROR"               },
+tu_static tu_lookup_entry_t const _dfu_state_lookup[] = {
+  { .key = APP_IDLE               , .data = "APP_IDLE"            },
+  { .key = APP_DETACH             , .data = "APP_DETACH"          },
+  { .key = DFU_IDLE               , .data = "IDLE"                },
+  { .key = DFU_DNLOAD_SYNC        , .data = "DNLOAD_SYNC"         },
+  { .key = DFU_DNBUSY             , .data = "DNBUSY"              },
+  { .key = DFU_DNLOAD_IDLE        , .data = "DNLOAD_IDLE"         },
+  { .key = DFU_MANIFEST_SYNC      , .data = "MANIFEST_SYNC"       },
+  { .key = DFU_MANIFEST           , .data = "MANIFEST"            },
+  { .key = DFU_MANIFEST_WAIT_RESET, .data = "MANIFEST_WAIT_RESET" },
+  { .key = DFU_UPLOAD_IDLE        , .data = "UPLOAD_IDLE"         },
+  { .key = DFU_ERROR              , .data = "ERROR"               },
 };
 
-static tu_lookup_table_t const _dfu_state_table =
-{
+tu_static tu_lookup_table_t const _dfu_state_table = {
   .count = TU_ARRAY_SIZE(_dfu_state_lookup),
   .items = _dfu_state_lookup
 };
 
-static tu_lookup_entry_t const _dfu_status_lookup[] =
-{
+tu_static tu_lookup_entry_t const _dfu_status_lookup[] = {
   { .key = DFU_STATUS_OK               , .data = "OK"              },
   { .key = DFU_STATUS_ERR_TARGET       , .data = "errTARGET"       },
   { .key = DFU_STATUS_ERR_FILE         , .data = "errFILE"         },
@@ -132,8 +134,7 @@ static tu_lookup_entry_t const _dfu_status_lookup[] =
   { .key = DFU_STATUS_ERR_STALLEDPKT   , .data = "errSTALLEDPKT"   },
 };
 
-static tu_lookup_table_t const _dfu_status_table =
-{
+tu_static tu_lookup_table_t const _dfu_status_table = {
   .count = TU_ARRAY_SIZE(_dfu_status_lookup),
   .items = _dfu_status_lookup
 };
@@ -143,34 +144,32 @@ static tu_lookup_table_t const _dfu_status_table =
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void dfu_moded_reset(uint8_t rhport)
-{
+void dfu_moded_reset(uint8_t rhport) {
   (void) rhport;
-
   _dfu_ctx.attrs = 0;
   _dfu_ctx.alt = 0;
-
   reset_state();
 }
 
-void dfu_moded_init(void)
-{
+void dfu_moded_init(void) {
   dfu_moded_reset(0);
 }
 
-uint16_t dfu_moded_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len)
-{
+bool dfu_moded_deinit(void) {
+  return true;
+}
+
+uint16_t dfu_moded_open(uint8_t rhport, const tusb_desc_interface_t* itf_desc, uint16_t max_len) {
   (void) rhport;
 
   //------------- Interface (with Alt) descriptor -------------//
-  uint8_t const itf_num = itf_desc->bInterfaceNumber;
+  const uint8_t itf_num = itf_desc->bInterfaceNumber;
   uint8_t alt_count = 0;
 
   uint16_t drv_len = 0;
   TU_VERIFY(itf_desc->bInterfaceSubClass == TUD_DFU_APP_SUBCLASS && itf_desc->bInterfaceProtocol == DFU_PROTOCOL_DFU, 0);
 
-  while(itf_desc->bInterfaceSubClass == TUD_DFU_APP_SUBCLASS && itf_desc->bInterfaceProtocol == DFU_PROTOCOL_DFU)
-  {
+  while(itf_desc->bInterfaceSubClass == TUD_DFU_APP_SUBCLASS && itf_desc->bInterfaceProtocol == DFU_PROTOCOL_DFU) {
     TU_ASSERT(max_len > drv_len, 0);
 
     // Alternate must have the same interface number
@@ -181,18 +180,18 @@ uint16_t dfu_moded_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc,
     alt_count++;
 
     drv_len += tu_desc_len(itf_desc);
-    itf_desc = (tusb_desc_interface_t const *) tu_desc_next(itf_desc);
+    itf_desc = (const tusb_desc_interface_t*) tu_desc_next(itf_desc);
   }
 
   //------------- DFU Functional descriptor -------------//
-  tusb_desc_dfu_functional_t const *func_desc = (tusb_desc_dfu_functional_t const *) itf_desc;
+  const tusb_desc_dfu_functional_t*func_desc = (const tusb_desc_dfu_functional_t*) itf_desc;
   TU_ASSERT(tu_desc_type(func_desc) == TUSB_DESC_FUNCTIONAL, 0);
   drv_len += sizeof(tusb_desc_dfu_functional_t);
 
   _dfu_ctx.attrs = func_desc->bAttributes;
 
   // CFG_TUD_DFU_XFER_BUFSIZE has to be set to the buffer size used in TUD_DFU_DESCRIPTOR
-  uint16_t const transfer_size = tu_le16toh( tu_unaligned_read16((uint8_t const*) func_desc + offsetof(tusb_desc_dfu_functional_t, wTransferSize)) );
+  const uint16_t transfer_size = tu_le16toh( tu_unaligned_read16((const uint8_t*) func_desc + offsetof(tusb_desc_dfu_functional_t, wTransferSize)) );
   TU_ASSERT(transfer_size <= CFG_TUD_DFU_XFER_BUFSIZE, drv_len);
 
   return drv_len;
@@ -201,99 +200,85 @@ uint16_t dfu_moded_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc,
 // Invoked when a control transfer occurred on an interface of this class
 // Driver response accordingly to the request and the transfer stage (setup/data/ack)
 // return false to stall control endpoint (e.g unsupported request)
-bool dfu_moded_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
-{
+bool dfu_moded_control_xfer_cb(uint8_t rhport, uint8_t stage, const tusb_control_request_t* request) {
   TU_VERIFY(request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_INTERFACE);
+  TU_LOG_DRV("  DFU State  : %s, Status: %s\r\n", tu_lookup_find(&_dfu_state_table, _dfu_ctx.state), tu_lookup_find(&_dfu_status_table, _dfu_ctx.status));
 
-  TU_LOG2("  DFU State  : %s, Status: %s\r\n", tu_lookup_find(&_dfu_state_table, _dfu_ctx.state), tu_lookup_find(&_dfu_status_table, _dfu_ctx.status));
-
-  if ( request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD )
-  {
+  if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD) {
     // Standard request include GET/SET_INTERFACE
-    switch ( request->bRequest )
-    {
+    switch (request->bRequest) {
       case TUSB_REQ_SET_INTERFACE:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
+        if (stage == CONTROL_STAGE_SETUP) {
           // Switch Alt interface and reset state machine
-          _dfu_ctx.alt = (uint8_t) request->wValue;
+          _dfu_ctx.alt = (uint8_t)request->wValue;
           reset_state();
           return tud_control_status(rhport, request);
         }
-      break;
+        break;
 
       case TUSB_REQ_GET_INTERFACE:
-        if(stage == CONTROL_STAGE_SETUP)
-        {
+        if (stage == CONTROL_STAGE_SETUP) {
           return tud_control_xfer(rhport, request, &_dfu_ctx.alt, 1);
         }
-      break;
+        break;
 
       // unsupported request
       default: return false;
     }
-  }
-  else if ( request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS )
-  {
-    TU_LOG2("  DFU Request: %s\r\n", tu_lookup_find(&_dfu_request_table, request->bRequest));
+  } else if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS) {
+    TU_LOG_DRV("  DFU Request: %s\r\n", tu_lookup_find(&_dfu_request_table, request->bRequest));
 
     // Class request
-    switch ( request->bRequest )
-    {
+    switch (request->bRequest) {
       case DFU_REQUEST_DETACH:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
+        if (stage == CONTROL_STAGE_SETUP) {
           tud_control_status(rhport, request);
+        } else if (stage == CONTROL_STAGE_ACK) {
+          if (tud_dfu_detach_cb) {
+            tud_dfu_detach_cb();
+          }
         }
-        else if ( stage == CONTROL_STAGE_ACK )
-        {
-          if ( tud_dfu_detach_cb ) tud_dfu_detach_cb();
-        }
-      break;
+        break;
 
       case DFU_REQUEST_CLRSTATUS:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
+        if (stage == CONTROL_STAGE_SETUP) {
           reset_state();
           tud_control_status(rhport, request);
         }
-      break;
+        break;
 
       case DFU_REQUEST_GETSTATE:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
+        if (stage == CONTROL_STAGE_SETUP) {
           tud_control_xfer(rhport, request, &_dfu_ctx.state, 1);
         }
-      break;
+        break;
 
       case DFU_REQUEST_ABORT:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
+        if (stage == CONTROL_STAGE_SETUP) {
           reset_state();
           tud_control_status(rhport, request);
+        } else if (stage == CONTROL_STAGE_ACK) {
+          if (tud_dfu_abort_cb) {
+            tud_dfu_abort_cb(_dfu_ctx.alt);
+          }
         }
-        else if ( stage == CONTROL_STAGE_ACK )
-        {
-          if ( tud_dfu_abort_cb ) tud_dfu_abort_cb(_dfu_ctx.alt);
-        }
-      break;
+        break;
 
       case DFU_REQUEST_UPLOAD:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
+        if (stage == CONTROL_STAGE_SETUP) {
           TU_VERIFY(_dfu_ctx.attrs & DFU_ATTR_CAN_UPLOAD);
           TU_VERIFY(tud_dfu_upload_cb);
           TU_VERIFY(request->wLength <= CFG_TUD_DFU_XFER_BUFSIZE);
 
-          uint16_t const xfer_len = tud_dfu_upload_cb(_dfu_ctx.alt, request->wValue, _dfu_ctx.transfer_buf, request->wLength);
+          const uint16_t xfer_len = tud_dfu_upload_cb(_dfu_ctx.alt, request->wValue, _dfu_epbuf.transfer_buf,
+                                                      request->wLength);
 
-          return tud_control_xfer(rhport, request, _dfu_ctx.transfer_buf, xfer_len);
+          return tud_control_xfer(rhport, request, _dfu_epbuf.transfer_buf, xfer_len);
         }
-      break;
+        break;
 
       case DFU_REQUEST_DNLOAD:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
+        if (stage == CONTROL_STAGE_SETUP) {
           TU_VERIFY(_dfu_ctx.attrs & DFU_ATTR_CAN_DOWNLOAD);
           TU_VERIFY(_dfu_ctx.state == DFU_IDLE || _dfu_ctx.state == DFU_DNLOAD_IDLE);
           TU_VERIFY(request->wLength <= CFG_TUD_DFU_XFER_BUFSIZE);
@@ -302,104 +287,86 @@ bool dfu_moded_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_reque
           _dfu_ctx.flashing_in_progress = true;
 
           // save block and length for flashing
-          _dfu_ctx.block  = request->wValue;
+          _dfu_ctx.block = request->wValue;
           _dfu_ctx.length = request->wLength;
 
-          if ( request->wLength )
-          {
+          if (request->wLength) {
             // Download with payload -> transition to DOWNLOAD SYNC
             _dfu_ctx.state = DFU_DNLOAD_SYNC;
-            return tud_control_xfer(rhport, request, _dfu_ctx.transfer_buf, request->wLength);
-          }
-          else
-          {
+            return tud_control_xfer(rhport, request, _dfu_epbuf.transfer_buf, request->wLength);
+          } else {
             // Download is complete -> transition to MANIFEST SYNC
             _dfu_ctx.state = DFU_MANIFEST_SYNC;
             return tud_control_status(rhport, request);
           }
         }
-      break;
+        break;
 
       case DFU_REQUEST_GETSTATUS:
-        switch ( _dfu_ctx.state )
-        {
+        switch (_dfu_ctx.state) {
           case DFU_DNLOAD_SYNC:
             return process_download_get_status(rhport, stage, request);
-          break;
+            break;
 
           case DFU_MANIFEST_SYNC:
             return process_manifest_get_status(rhport, stage, request);
-          break;
+            break;
 
           default:
-            if ( stage == CONTROL_STAGE_SETUP ) return reply_getstatus(rhport, request, _dfu_ctx.state, _dfu_ctx.status, 0);
-          break;
+            if (stage == CONTROL_STAGE_SETUP) {
+              return reply_getstatus(rhport, request, _dfu_ctx.state, _dfu_ctx.status, 0);
+            }
+            break;
         }
-      break;
+        break;
 
       default: return false; // stall unsupported request
     }
-  }else
-  {
+  } else {
     return false; // unsupported request
   }
 
   return true;
 }
 
-void tud_dfu_finish_flashing(uint8_t status)
-{
+void tud_dfu_finish_flashing(uint8_t status) {
   _dfu_ctx.flashing_in_progress = false;
 
-  if ( status == DFU_STATUS_OK )
-  {
-    if (_dfu_ctx.state == DFU_DNBUSY)
-    {
+  if (status == DFU_STATUS_OK) {
+    if (_dfu_ctx.state == DFU_DNBUSY) {
       _dfu_ctx.state = DFU_DNLOAD_SYNC;
-    }
-    else if (_dfu_ctx.state == DFU_MANIFEST)
-    {
+    } else if (_dfu_ctx.state == DFU_MANIFEST) {
       _dfu_ctx.state = (_dfu_ctx.attrs & DFU_ATTR_MANIFESTATION_TOLERANT)
-                               ? DFU_MANIFEST_SYNC : DFU_MANIFEST_WAIT_RESET;
+                         ? DFU_MANIFEST_SYNC
+                         : DFU_MANIFEST_WAIT_RESET;
     }
-  }
-  else
-  {
+  } else {
     // failed while flashing, move to dfuError
     _dfu_ctx.state = DFU_ERROR;
     _dfu_ctx.status = (dfu_status_t)status;
   }
 }
 
-static bool process_download_get_status(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
-{
-  if ( stage == CONTROL_STAGE_SETUP )
-  {
+static bool process_download_get_status(uint8_t rhport, uint8_t stage, const tusb_control_request_t* request) {
+  if (stage == CONTROL_STAGE_SETUP) {
     // only transition to next state on CONTROL_STAGE_ACK
     dfu_state_t next_state;
     uint32_t timeout;
 
-    if ( _dfu_ctx.flashing_in_progress )
-    {
+    if (_dfu_ctx.flashing_in_progress) {
       next_state = DFU_DNBUSY;
-      timeout = tud_dfu_get_timeout_cb(_dfu_ctx.alt, (uint8_t) next_state);
-    }
-    else
-    {
+      timeout = tud_dfu_get_timeout_cb(_dfu_ctx.alt, (uint8_t)next_state);
+    } else {
       next_state = DFU_DNLOAD_IDLE;
       timeout = 0;
     }
 
     return reply_getstatus(rhport, request, next_state, _dfu_ctx.status, timeout);
-  }
-  else if ( stage == CONTROL_STAGE_ACK )
-  {
-    if ( _dfu_ctx.flashing_in_progress )
-    {
+  } else if (stage == CONTROL_STAGE_ACK) {
+    if (_dfu_ctx.flashing_in_progress) {
       _dfu_ctx.state = DFU_DNBUSY;
-      tud_dfu_download_cb(_dfu_ctx.alt, _dfu_ctx.block, _dfu_ctx.transfer_buf, _dfu_ctx.length);
-    }else
-    {
+      tud_dfu_download_cb(_dfu_ctx.alt, _dfu_ctx.block, _dfu_epbuf.transfer_buf, _dfu_ctx.length);
+    } else {
       _dfu_ctx.state = DFU_DNLOAD_IDLE;
     }
   }
@@ -407,36 +374,26 @@ static bool process_download_get_status(uint8_t rhport, uint8_t stage, tusb_cont
   return true;
 }
 
-static bool process_manifest_get_status(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
-{
-  if ( stage == CONTROL_STAGE_SETUP )
-  {
+static bool process_manifest_get_status(uint8_t rhport, uint8_t stage, const tusb_control_request_t* request) {
+  if (stage == CONTROL_STAGE_SETUP) {
     // only transition to next state on CONTROL_STAGE_ACK
     dfu_state_t next_state;
     uint32_t timeout;
 
-    if ( _dfu_ctx.flashing_in_progress )
-    {
+    if (_dfu_ctx.flashing_in_progress) {
       next_state = DFU_MANIFEST;
       timeout = tud_dfu_get_timeout_cb(_dfu_ctx.alt, next_state);
-    }
-    else
-    {
+    } else {
       next_state = DFU_IDLE;
       timeout = 0;
     }
 
     return reply_getstatus(rhport, request, next_state, _dfu_ctx.status, timeout);
-  }
-  else if ( stage == CONTROL_STAGE_ACK )
-  {
-    if ( _dfu_ctx.flashing_in_progress )
-    {
+  } else if (stage == CONTROL_STAGE_ACK) {
+    if (_dfu_ctx.flashing_in_progress) {
       _dfu_ctx.state = DFU_MANIFEST;
       tud_dfu_manifest_cb(_dfu_ctx.alt);
-    }
-    else
-    {
+    } else {
       _dfu_ctx.state = DFU_IDLE;
     }
   }
@@ -444,15 +401,15 @@ static bool process_manifest_get_status(uint8_t rhport, uint8_t stage, tusb_cont
   return true;
 }
 
-static bool reply_getstatus(uint8_t rhport, tusb_control_request_t const * request, dfu_state_t state, dfu_status_t status, uint32_t timeout)
-{
+static bool reply_getstatus(uint8_t rhport, const tusb_control_request_t* request, dfu_state_t state,
+                            dfu_status_t status, uint32_t timeout) {
   dfu_status_response_t resp;
-  resp.bStatus          = (uint8_t) status;
+  resp.bStatus = (uint8_t)status;
   resp.bwPollTimeout[0] = TU_U32_BYTE0(timeout);
   resp.bwPollTimeout[1] = TU_U32_BYTE1(timeout);
   resp.bwPollTimeout[2] = TU_U32_BYTE2(timeout);
-  resp.bState           = (uint8_t) state;
-  resp.iString          = 0;
+  resp.bState = (uint8_t)state;
+  resp.iString = 0;
 
   return tud_control_xfer(rhport, request, &resp, sizeof(dfu_status_response_t));
 }
diff --git a/src/class/dfu/dfu_device.h b/src/class/dfu/dfu_device.h
index fecf8596f..00c22ea8b 100644
--- a/src/class/dfu/dfu_device.h
+++ b/src/class/dfu/dfu_device.h
@@ -86,6 +86,7 @@ TU_ATTR_WEAK void tud_dfu_abort_cb(uint8_t alt);
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     dfu_moded_init(void);
+bool     dfu_moded_deinit(void);
 void     dfu_moded_reset(uint8_t rhport);
 uint16_t dfu_moded_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 bool     dfu_moded_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
diff --git a/src/class/dfu/dfu_rt_device.c b/src/class/dfu/dfu_rt_device.c
index b9cd6096b..a63c23d9a 100644
--- a/src/class/dfu/dfu_rt_device.c
+++ b/src/class/dfu/dfu_rt_device.c
@@ -37,6 +37,13 @@
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
 
+// Level where CFG_TUSB_DEBUG must be at least for this driver is logged
+#ifndef CFG_TUD_DFU_RUNTIME_LOG_LEVEL
+  #define CFG_TUD_DFU_RUNTIME_LOG_LEVEL   CFG_TUD_LOG_LEVEL
+#endif
+
+#define TU_LOG_DRV(...)   TU_LOG(CFG_TUD_DFU_RUNTIME_LOG_LEVEL, __VA_ARGS__)
+
 //--------------------------------------------------------------------+
 // INTERNAL OBJECT & FUNCTION DECLARATION
 //--------------------------------------------------------------------+
@@ -44,13 +51,15 @@
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void dfu_rtd_init(void)
-{
+void dfu_rtd_init(void) {
 }
 
-void dfu_rtd_reset(uint8_t rhport)
-{
-    (void) rhport;
+bool dfu_rtd_deinit(void) {
+  return true;
+}
+
+void dfu_rtd_reset(uint8_t rhport) {
+  (void) rhport;
 }
 
 uint16_t dfu_rtd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len)
@@ -99,7 +108,7 @@ bool dfu_rtd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request
   {
     case DFU_REQUEST_DETACH:
     {
-      TU_LOG2("  DFU RT Request: DETACH\r\n");
+      TU_LOG_DRV("  DFU RT Request: DETACH\r\n");
       tud_control_status(rhport, request);
       tud_dfu_runtime_reboot_to_dfu_cb();
     }
@@ -107,17 +116,17 @@ bool dfu_rtd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request
 
     case DFU_REQUEST_GETSTATUS:
     {
-      TU_LOG2("  DFU RT Request: GETSTATUS\r\n");
+      TU_LOG_DRV("  DFU RT Request: GETSTATUS\r\n");
       dfu_status_response_t resp;
       // Status = OK, Poll timeout is ignored during RT, State = APP_IDLE, IString = 0
-      memset(&resp, 0x00, sizeof(dfu_status_response_t));
+      TU_VERIFY(tu_memset_s(&resp, sizeof(resp), 0x00, sizeof(resp))==0);
       tud_control_xfer(rhport, request, &resp, sizeof(dfu_status_response_t));
     }
     break;
 
     default:
     {
-      TU_LOG2("  DFU RT Unexpected Request: %d\r\n", request->bRequest);
+      TU_LOG_DRV("  DFU RT Unexpected Request: %d\r\n", request->bRequest);
       return false; // stall unsupported request
     }
   }
diff --git a/src/class/dfu/dfu_rt_device.h b/src/class/dfu/dfu_rt_device.h
index babaa8214..67eb26d95 100644
--- a/src/class/dfu/dfu_rt_device.h
+++ b/src/class/dfu/dfu_rt_device.h
@@ -43,6 +43,7 @@ void tud_dfu_runtime_reboot_to_dfu_cb(void);
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     dfu_rtd_init(void);
+bool     dfu_rtd_deinit(void);
 void     dfu_rtd_reset(uint8_t rhport);
 uint16_t dfu_rtd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 bool     dfu_rtd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
diff --git a/src/class/hid/hid.h b/src/class/hid/hid.h
index 44a464be1..c2434c20d 100644
--- a/src/class/hid/hid.h
+++ b/src/class/hid/hid.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -300,6 +300,19 @@ typedef struct TU_ATTR_PACKED
   int8_t  pan;     // using AC Pan
 } hid_mouse_report_t;
 
+
+// Absolute Mouse: same as the Standard (relative) Mouse Report but
+// with int16_t instead of int8_t for X and Y coordinates.
+typedef struct TU_ATTR_PACKED
+{
+    uint8_t buttons; /**< buttons mask for currently pressed buttons in the mouse. */
+    int16_t x;       /**< Current x position of the mouse. */
+    int16_t y;       /**< Current y position of the mouse. */
+    int8_t wheel;    /**< Current delta wheel movement on the mouse. */
+    int8_t pan;      // using AC Pan
+} hid_abs_mouse_report_t;
+
+
 /// Standard Mouse Buttons Bitmap
 typedef enum
 {
@@ -313,6 +326,29 @@ typedef enum
 /// @}
 
 //--------------------------------------------------------------------+
+// Digitizer Stylus Pen
+//--------------------------------------------------------------------+
+/** \addtogroup ClassDriver_HID_Stylus Stylus
+ *  @{ */
+
+// Standard Stylus Pen Report.
+typedef struct TU_ATTR_PACKED
+{
+  uint8_t attr;    /**< Attribute mask for describing current status of the stylus pen. */
+  uint16_t x;      /**< Current x position of the mouse. */
+  uint16_t y;      /**< Current y position of the mouse. */
+} hid_stylus_report_t;
+
+// Standard Stylus Pen Attributes Bitmap.
+typedef enum
+{
+  STYLUS_ATTR_TIP_SWITCH = TU_BIT(0), ///< Tip switch
+  STYLUS_ATTR_IN_RANGE   = TU_BIT(1), ///< In-range bit.
+} hid_stylus_attr_bm_t;
+
+/// @}
+
+//--------------------------------------------------------------------+
 // Keyboard
 //--------------------------------------------------------------------+
 /** \addtogroup ClassDriver_HID_Keyboard Keyboard
@@ -353,177 +389,224 @@ typedef enum
 //--------------------------------------------------------------------+
 // HID KEYCODE
 //--------------------------------------------------------------------+
-#define HID_KEY_NONE                      0x00
-#define HID_KEY_A                         0x04
-#define HID_KEY_B                         0x05
-#define HID_KEY_C                         0x06
-#define HID_KEY_D                         0x07
-#define HID_KEY_E                         0x08
-#define HID_KEY_F                         0x09
-#define HID_KEY_G                         0x0A
-#define HID_KEY_H                         0x0B
-#define HID_KEY_I                         0x0C
-#define HID_KEY_J                         0x0D
-#define HID_KEY_K                         0x0E
-#define HID_KEY_L                         0x0F
-#define HID_KEY_M                         0x10
-#define HID_KEY_N                         0x11
-#define HID_KEY_O                         0x12
-#define HID_KEY_P                         0x13
-#define HID_KEY_Q                         0x14
-#define HID_KEY_R                         0x15
-#define HID_KEY_S                         0x16
-#define HID_KEY_T                         0x17
-#define HID_KEY_U                         0x18
-#define HID_KEY_V                         0x19
-#define HID_KEY_W                         0x1A
-#define HID_KEY_X                         0x1B
-#define HID_KEY_Y                         0x1C
-#define HID_KEY_Z                         0x1D
-#define HID_KEY_1                         0x1E
-#define HID_KEY_2                         0x1F
-#define HID_KEY_3                         0x20
-#define HID_KEY_4                         0x21
-#define HID_KEY_5                         0x22
-#define HID_KEY_6                         0x23
-#define HID_KEY_7                         0x24
-#define HID_KEY_8                         0x25
-#define HID_KEY_9                         0x26
-#define HID_KEY_0                         0x27
-#define HID_KEY_ENTER                     0x28
-#define HID_KEY_ESCAPE                    0x29
-#define HID_KEY_BACKSPACE                 0x2A
-#define HID_KEY_TAB                       0x2B
-#define HID_KEY_SPACE                     0x2C
-#define HID_KEY_MINUS                     0x2D
-#define HID_KEY_EQUAL                     0x2E
-#define HID_KEY_BRACKET_LEFT              0x2F
-#define HID_KEY_BRACKET_RIGHT             0x30
-#define HID_KEY_BACKSLASH                 0x31
-#define HID_KEY_EUROPE_1                  0x32
-#define HID_KEY_SEMICOLON                 0x33
-#define HID_KEY_APOSTROPHE                0x34
-#define HID_KEY_GRAVE                     0x35
-#define HID_KEY_COMMA                     0x36
-#define HID_KEY_PERIOD                    0x37
-#define HID_KEY_SLASH                     0x38
-#define HID_KEY_CAPS_LOCK                 0x39
-#define HID_KEY_F1                        0x3A
-#define HID_KEY_F2                        0x3B
-#define HID_KEY_F3                        0x3C
-#define HID_KEY_F4                        0x3D
-#define HID_KEY_F5                        0x3E
-#define HID_KEY_F6                        0x3F
-#define HID_KEY_F7                        0x40
-#define HID_KEY_F8                        0x41
-#define HID_KEY_F9                        0x42
-#define HID_KEY_F10                       0x43
-#define HID_KEY_F11                       0x44
-#define HID_KEY_F12                       0x45
-#define HID_KEY_PRINT_SCREEN              0x46
-#define HID_KEY_SCROLL_LOCK               0x47
-#define HID_KEY_PAUSE                     0x48
-#define HID_KEY_INSERT                    0x49
-#define HID_KEY_HOME                      0x4A
-#define HID_KEY_PAGE_UP                   0x4B
-#define HID_KEY_DELETE                    0x4C
-#define HID_KEY_END                       0x4D
-#define HID_KEY_PAGE_DOWN                 0x4E
-#define HID_KEY_ARROW_RIGHT               0x4F
-#define HID_KEY_ARROW_LEFT                0x50
-#define HID_KEY_ARROW_DOWN                0x51
-#define HID_KEY_ARROW_UP                  0x52
-#define HID_KEY_NUM_LOCK                  0x53
-#define HID_KEY_KEYPAD_DIVIDE             0x54
-#define HID_KEY_KEYPAD_MULTIPLY           0x55
-#define HID_KEY_KEYPAD_SUBTRACT           0x56
-#define HID_KEY_KEYPAD_ADD                0x57
-#define HID_KEY_KEYPAD_ENTER              0x58
-#define HID_KEY_KEYPAD_1                  0x59
-#define HID_KEY_KEYPAD_2                  0x5A
-#define HID_KEY_KEYPAD_3                  0x5B
-#define HID_KEY_KEYPAD_4                  0x5C
-#define HID_KEY_KEYPAD_5                  0x5D
-#define HID_KEY_KEYPAD_6                  0x5E
-#define HID_KEY_KEYPAD_7                  0x5F
-#define HID_KEY_KEYPAD_8                  0x60
-#define HID_KEY_KEYPAD_9                  0x61
-#define HID_KEY_KEYPAD_0                  0x62
-#define HID_KEY_KEYPAD_DECIMAL            0x63
-#define HID_KEY_EUROPE_2                  0x64
-#define HID_KEY_APPLICATION               0x65
-#define HID_KEY_POWER                     0x66
-#define HID_KEY_KEYPAD_EQUAL              0x67
-#define HID_KEY_F13                       0x68
-#define HID_KEY_F14                       0x69
-#define HID_KEY_F15                       0x6A
-#define HID_KEY_F16                       0x6B
-#define HID_KEY_F17                       0x6C
-#define HID_KEY_F18                       0x6D
-#define HID_KEY_F19                       0x6E
-#define HID_KEY_F20                       0x6F
-#define HID_KEY_F21                       0x70
-#define HID_KEY_F22                       0x71
-#define HID_KEY_F23                       0x72
-#define HID_KEY_F24                       0x73
-#define HID_KEY_EXECUTE                   0x74
-#define HID_KEY_HELP                      0x75
-#define HID_KEY_MENU                      0x76
-#define HID_KEY_SELECT                    0x77
-#define HID_KEY_STOP                      0x78
-#define HID_KEY_AGAIN                     0x79
-#define HID_KEY_UNDO                      0x7A
-#define HID_KEY_CUT                       0x7B
-#define HID_KEY_COPY                      0x7C
-#define HID_KEY_PASTE                     0x7D
-#define HID_KEY_FIND                      0x7E
-#define HID_KEY_MUTE                      0x7F
-#define HID_KEY_VOLUME_UP                 0x80
-#define HID_KEY_VOLUME_DOWN               0x81
-#define HID_KEY_LOCKING_CAPS_LOCK         0x82
-#define HID_KEY_LOCKING_NUM_LOCK          0x83
-#define HID_KEY_LOCKING_SCROLL_LOCK       0x84
-#define HID_KEY_KEYPAD_COMMA              0x85
-#define HID_KEY_KEYPAD_EQUAL_SIGN         0x86
-#define HID_KEY_KANJI1                    0x87
-#define HID_KEY_KANJI2                    0x88
-#define HID_KEY_KANJI3                    0x89
-#define HID_KEY_KANJI4                    0x8A
-#define HID_KEY_KANJI5                    0x8B
-#define HID_KEY_KANJI6                    0x8C
-#define HID_KEY_KANJI7                    0x8D
-#define HID_KEY_KANJI8                    0x8E
-#define HID_KEY_KANJI9                    0x8F
-#define HID_KEY_LANG1                     0x90
-#define HID_KEY_LANG2                     0x91
-#define HID_KEY_LANG3                     0x92
-#define HID_KEY_LANG4                     0x93
-#define HID_KEY_LANG5                     0x94
-#define HID_KEY_LANG6                     0x95
-#define HID_KEY_LANG7                     0x96
-#define HID_KEY_LANG8                     0x97
-#define HID_KEY_LANG9                     0x98
-#define HID_KEY_ALTERNATE_ERASE           0x99
-#define HID_KEY_SYSREQ_ATTENTION          0x9A
-#define HID_KEY_CANCEL                    0x9B
-#define HID_KEY_CLEAR                     0x9C
-#define HID_KEY_PRIOR                     0x9D
-#define HID_KEY_RETURN                    0x9E
-#define HID_KEY_SEPARATOR                 0x9F
-#define HID_KEY_OUT                       0xA0
-#define HID_KEY_OPER                      0xA1
-#define HID_KEY_CLEAR_AGAIN               0xA2
-#define HID_KEY_CRSEL_PROPS               0xA3
-#define HID_KEY_EXSEL                     0xA4
-// RESERVED					                      0xA5-DF
-#define HID_KEY_CONTROL_LEFT              0xE0
-#define HID_KEY_SHIFT_LEFT                0xE1
-#define HID_KEY_ALT_LEFT                  0xE2
-#define HID_KEY_GUI_LEFT                  0xE3
-#define HID_KEY_CONTROL_RIGHT             0xE4
-#define HID_KEY_SHIFT_RIGHT               0xE5
-#define HID_KEY_ALT_RIGHT                 0xE6
-#define HID_KEY_GUI_RIGHT                 0xE7
+#define HID_KEY_NONE                        0x00
+#define HID_KEY_A                           0x04
+#define HID_KEY_B                           0x05
+#define HID_KEY_C                           0x06
+#define HID_KEY_D                           0x07
+#define HID_KEY_E                           0x08
+#define HID_KEY_F                           0x09
+#define HID_KEY_G                           0x0A
+#define HID_KEY_H                           0x0B
+#define HID_KEY_I                           0x0C
+#define HID_KEY_J                           0x0D
+#define HID_KEY_K                           0x0E
+#define HID_KEY_L                           0x0F
+#define HID_KEY_M                           0x10
+#define HID_KEY_N                           0x11
+#define HID_KEY_O                           0x12
+#define HID_KEY_P                           0x13
+#define HID_KEY_Q                           0x14
+#define HID_KEY_R                           0x15
+#define HID_KEY_S                           0x16
+#define HID_KEY_T                           0x17
+#define HID_KEY_U                           0x18
+#define HID_KEY_V                           0x19
+#define HID_KEY_W                           0x1A
+#define HID_KEY_X                           0x1B
+#define HID_KEY_Y                           0x1C
+#define HID_KEY_Z                           0x1D
+#define HID_KEY_1                           0x1E
+#define HID_KEY_2                           0x1F
+#define HID_KEY_3                           0x20
+#define HID_KEY_4                           0x21
+#define HID_KEY_5                           0x22
+#define HID_KEY_6                           0x23
+#define HID_KEY_7                           0x24
+#define HID_KEY_8                           0x25
+#define HID_KEY_9                           0x26
+#define HID_KEY_0                           0x27
+#define HID_KEY_ENTER                       0x28
+#define HID_KEY_ESCAPE                      0x29
+#define HID_KEY_BACKSPACE                   0x2A
+#define HID_KEY_TAB                         0x2B
+#define HID_KEY_SPACE                       0x2C
+#define HID_KEY_MINUS                       0x2D
+#define HID_KEY_EQUAL                       0x2E
+#define HID_KEY_BRACKET_LEFT                0x2F
+#define HID_KEY_BRACKET_RIGHT               0x30
+#define HID_KEY_BACKSLASH                   0x31
+#define HID_KEY_EUROPE_1                    0x32
+#define HID_KEY_SEMICOLON                   0x33
+#define HID_KEY_APOSTROPHE                  0x34
+#define HID_KEY_GRAVE                       0x35
+#define HID_KEY_COMMA                       0x36
+#define HID_KEY_PERIOD                      0x37
+#define HID_KEY_SLASH                       0x38
+#define HID_KEY_CAPS_LOCK                   0x39
+#define HID_KEY_F1                          0x3A
+#define HID_KEY_F2                          0x3B
+#define HID_KEY_F3                          0x3C
+#define HID_KEY_F4                          0x3D
+#define HID_KEY_F5                          0x3E
+#define HID_KEY_F6                          0x3F
+#define HID_KEY_F7                          0x40
+#define HID_KEY_F8                          0x41
+#define HID_KEY_F9                          0x42
+#define HID_KEY_F10                         0x43
+#define HID_KEY_F11                         0x44
+#define HID_KEY_F12                         0x45
+#define HID_KEY_PRINT_SCREEN                0x46
+#define HID_KEY_SCROLL_LOCK                 0x47
+#define HID_KEY_PAUSE                       0x48
+#define HID_KEY_INSERT                      0x49
+#define HID_KEY_HOME                        0x4A
+#define HID_KEY_PAGE_UP                     0x4B
+#define HID_KEY_DELETE                      0x4C
+#define HID_KEY_END                         0x4D
+#define HID_KEY_PAGE_DOWN                   0x4E
+#define HID_KEY_ARROW_RIGHT                 0x4F
+#define HID_KEY_ARROW_LEFT                  0x50
+#define HID_KEY_ARROW_DOWN                  0x51
+#define HID_KEY_ARROW_UP                    0x52
+#define HID_KEY_NUM_LOCK                    0x53
+#define HID_KEY_KEYPAD_DIVIDE               0x54
+#define HID_KEY_KEYPAD_MULTIPLY             0x55
+#define HID_KEY_KEYPAD_SUBTRACT             0x56
+#define HID_KEY_KEYPAD_ADD                  0x57
+#define HID_KEY_KEYPAD_ENTER                0x58
+#define HID_KEY_KEYPAD_1                    0x59
+#define HID_KEY_KEYPAD_2                    0x5A
+#define HID_KEY_KEYPAD_3                    0x5B
+#define HID_KEY_KEYPAD_4                    0x5C
+#define HID_KEY_KEYPAD_5                    0x5D
+#define HID_KEY_KEYPAD_6                    0x5E
+#define HID_KEY_KEYPAD_7                    0x5F
+#define HID_KEY_KEYPAD_8                    0x60
+#define HID_KEY_KEYPAD_9                    0x61
+#define HID_KEY_KEYPAD_0                    0x62
+#define HID_KEY_KEYPAD_DECIMAL              0x63
+#define HID_KEY_EUROPE_2                    0x64
+#define HID_KEY_APPLICATION                 0x65
+#define HID_KEY_POWER                       0x66
+#define HID_KEY_KEYPAD_EQUAL                0x67
+#define HID_KEY_F13                         0x68
+#define HID_KEY_F14                         0x69
+#define HID_KEY_F15                         0x6A
+#define HID_KEY_F16                         0x6B
+#define HID_KEY_F17                         0x6C
+#define HID_KEY_F18                         0x6D
+#define HID_KEY_F19                         0x6E
+#define HID_KEY_F20                         0x6F
+#define HID_KEY_F21                         0x70
+#define HID_KEY_F22                         0x71
+#define HID_KEY_F23                         0x72
+#define HID_KEY_F24                         0x73
+#define HID_KEY_EXECUTE                     0x74
+#define HID_KEY_HELP                        0x75
+#define HID_KEY_MENU                        0x76
+#define HID_KEY_SELECT                      0x77
+#define HID_KEY_STOP                        0x78
+#define HID_KEY_AGAIN                       0x79
+#define HID_KEY_UNDO                        0x7A
+#define HID_KEY_CUT                         0x7B
+#define HID_KEY_COPY                        0x7C
+#define HID_KEY_PASTE                       0x7D
+#define HID_KEY_FIND                        0x7E
+#define HID_KEY_MUTE                        0x7F
+#define HID_KEY_VOLUME_UP                   0x80
+#define HID_KEY_VOLUME_DOWN                 0x81
+#define HID_KEY_LOCKING_CAPS_LOCK           0x82
+#define HID_KEY_LOCKING_NUM_LOCK            0x83
+#define HID_KEY_LOCKING_SCROLL_LOCK         0x84
+#define HID_KEY_KEYPAD_COMMA                0x85
+#define HID_KEY_KEYPAD_EQUAL_SIGN           0x86
+#define HID_KEY_KANJI1                      0x87
+#define HID_KEY_KANJI2                      0x88
+#define HID_KEY_KANJI3                      0x89
+#define HID_KEY_KANJI4                      0x8A
+#define HID_KEY_KANJI5                      0x8B
+#define HID_KEY_KANJI6                      0x8C
+#define HID_KEY_KANJI7                      0x8D
+#define HID_KEY_KANJI8                      0x8E
+#define HID_KEY_KANJI9                      0x8F
+#define HID_KEY_LANG1                       0x90
+#define HID_KEY_LANG2                       0x91
+#define HID_KEY_LANG3                       0x92
+#define HID_KEY_LANG4                       0x93
+#define HID_KEY_LANG5                       0x94
+#define HID_KEY_LANG6                       0x95
+#define HID_KEY_LANG7                       0x96
+#define HID_KEY_LANG8                       0x97
+#define HID_KEY_LANG9                       0x98
+#define HID_KEY_ALTERNATE_ERASE             0x99
+#define HID_KEY_SYSREQ_ATTENTION            0x9A
+#define HID_KEY_CANCEL                      0x9B
+#define HID_KEY_CLEAR                       0x9C
+#define HID_KEY_PRIOR                       0x9D
+#define HID_KEY_RETURN                      0x9E
+#define HID_KEY_SEPARATOR                   0x9F
+#define HID_KEY_OUT                         0xA0
+#define HID_KEY_OPER                        0xA1
+#define HID_KEY_CLEAR_AGAIN                 0xA2
+#define HID_KEY_CRSEL_PROPS                 0xA3
+#define HID_KEY_EXSEL                       0xA4
+// RESERVED					                        0xA5-AF
+#define HID_KEY_KEYPAD_00                   0xB0
+#define HID_KEY_KEYPAD_000                  0xB1
+#define HID_KEY_THOUSANDS_SEPARATOR         0xB2
+#define HID_KEY_DECIMAL_SEPARATOR           0xB3
+#define HID_KEY_CURRENCY_UNIT               0xB4
+#define HID_KEY_CURRENCY_SUBUNIT            0xB5
+#define HID_KEY_KEYPAD_LEFT_PARENTHESIS     0xB6
+#define HID_KEY_KEYPAD_RIGHT_PARENTHESIS    0xB7
+#define HID_KEY_KEYPAD_LEFT_BRACE           0xB8
+#define HID_KEY_KEYPAD_RIGHT_BRACE          0xB9
+#define HID_KEY_KEYPAD_TAB                  0xBA
+#define HID_KEY_KEYPAD_BACKSPACE            0xBB
+#define HID_KEY_KEYPAD_A                    0xBC
+#define HID_KEY_KEYPAD_B                    0xBD
+#define HID_KEY_KEYPAD_C                    0xBE
+#define HID_KEY_KEYPAD_D                    0xBF
+#define HID_KEY_KEYPAD_E                    0xC0
+#define HID_KEY_KEYPAD_F                    0xC1
+#define HID_KEY_KEYPAD_XOR                  0xC2
+#define HID_KEY_KEYPAD_CARET                0xC3
+#define HID_KEY_KEYPAD_PERCENT              0xC4
+#define HID_KEY_KEYPAD_LESS_THAN            0xC5
+#define HID_KEY_KEYPAD_GREATER_THAN         0xC6
+#define HID_KEY_KEYPAD_AMPERSAND            0xC7
+#define HID_KEY_KEYPAD_DOUBLE_AMPERSAND     0xC8
+#define HID_KEY_KEYPAD_VERTICAL_BAR         0xC9
+#define HID_KEY_KEYPAD_DOUBLE_VERTICAL_BAR  0xCA
+#define HID_KEY_KEYPAD_COLON                0xCB
+#define HID_KEY_KEYPAD_HASH                 0xCC
+#define HID_KEY_KEYPAD_SPACE                0xCD
+#define HID_KEY_KEYPAD_AT                   0xCE
+#define HID_KEY_KEYPAD_EXCLAMATION          0xCF
+#define HID_KEY_KEYPAD_MEMORY_STORE         0xD0
+#define HID_KEY_KEYPAD_MEMORY_RECALL        0xD1
+#define HID_KEY_KEYPAD_MEMORY_CLEAR         0xD2
+#define HID_KEY_KEYPAD_MEMORY_ADD           0xD3
+#define HID_KEY_KEYPAD_MEMORY_SUBTRACT      0xD4
+#define HID_KEY_KEYPAD_MEMORY_MULTIPLY      0xD5
+#define HID_KEY_KEYPAD_MEMORY_DIVIDE        0xD6
+#define HID_KEY_KEYPAD_PLUS_MINUS           0xD7
+#define HID_KEY_KEYPAD_CLEAR                0xD8
+#define HID_KEY_KEYPAD_CLEAR_ENTRY          0xD9
+#define HID_KEY_KEYPAD_BINARY               0xDA
+#define HID_KEY_KEYPAD_OCTAL                0xDB
+#define HID_KEY_KEYPAD_DECIMAL_2            0xDC
+#define HID_KEY_KEYPAD_HEXADECIMAL          0xDD
+// RESERVED					                        0xDE-DF
+#define HID_KEY_CONTROL_LEFT                0xE0
+#define HID_KEY_SHIFT_LEFT                  0xE1
+#define HID_KEY_ALT_LEFT                    0xE2
+#define HID_KEY_GUI_LEFT                    0xE3
+#define HID_KEY_CONTROL_RIGHT               0xE4
+#define HID_KEY_SHIFT_RIGHT                 0xE5
+#define HID_KEY_ALT_RIGHT                   0xE6
+#define HID_KEY_GUI_RIGHT                   0xE7
 
 
 //--------------------------------------------------------------------+
@@ -680,35 +763,58 @@ enum {
 
 //--------------------------------------------------------------------+
 // Usage Table
+/* Usage Types Data
+    Sel  Selector               Array
+    SV   Static Value           Constant, Variable, Absolute
+    SF   Static Flag            Constant, Variable, Absolute
+    DV   Dynamic Value          Constant, Variable, Absolute
+    DF   Dynamic Flag           Constant, Variable, Absolute
+*/
+/* Usage Types Collection
+    NAry  Named Array             Logical
+    CA    Collection Application  Application
+    CL    Collection Logical      Logical
+    CP    Collection Physical     Physical
+    US    Usage Switch            Logical
+    UM    Usage Modifier          Logical
+*/
 //--------------------------------------------------------------------+
 
 /// HID Usage Table - Table 1: Usage Page Summary
 enum {
-  HID_USAGE_PAGE_DESKTOP         = 0x01,
-  HID_USAGE_PAGE_SIMULATE        = 0x02,
-  HID_USAGE_PAGE_VIRTUAL_REALITY = 0x03,
-  HID_USAGE_PAGE_SPORT           = 0x04,
-  HID_USAGE_PAGE_GAME            = 0x05,
-  HID_USAGE_PAGE_GENERIC_DEVICE  = 0x06,
-  HID_USAGE_PAGE_KEYBOARD        = 0x07,
-  HID_USAGE_PAGE_LED             = 0x08,
-  HID_USAGE_PAGE_BUTTON          = 0x09,
-  HID_USAGE_PAGE_ORDINAL         = 0x0a,
-  HID_USAGE_PAGE_TELEPHONY       = 0x0b,
-  HID_USAGE_PAGE_CONSUMER        = 0x0c,
-  HID_USAGE_PAGE_DIGITIZER       = 0x0d,
-  HID_USAGE_PAGE_PID             = 0x0f,
-  HID_USAGE_PAGE_UNICODE         = 0x10,
-  HID_USAGE_PAGE_ALPHA_DISPLAY   = 0x14,
-  HID_USAGE_PAGE_MEDICAL         = 0x40,
-  HID_USAGE_PAGE_MONITOR         = 0x80, //0x80 - 0x83
-  HID_USAGE_PAGE_POWER           = 0x84, // 0x084 - 0x87
-  HID_USAGE_PAGE_BARCODE_SCANNER = 0x8c,
-  HID_USAGE_PAGE_SCALE           = 0x8d,
-  HID_USAGE_PAGE_MSR             = 0x8e,
-  HID_USAGE_PAGE_CAMERA          = 0x90,
-  HID_USAGE_PAGE_ARCADE          = 0x91,
-  HID_USAGE_PAGE_VENDOR          = 0xFF00 // 0xFF00 - 0xFFFF
+  HID_USAGE_PAGE_DESKTOP                   = 0x01,
+  HID_USAGE_PAGE_SIMULATE                  = 0x02,
+  HID_USAGE_PAGE_VIRTUAL_REALITY           = 0x03,
+  HID_USAGE_PAGE_SPORT                     = 0x04,
+  HID_USAGE_PAGE_GAME                      = 0x05,
+  HID_USAGE_PAGE_GENERIC_DEVICE            = 0x06,
+  HID_USAGE_PAGE_KEYBOARD                  = 0x07,
+  HID_USAGE_PAGE_LED                       = 0x08,
+  HID_USAGE_PAGE_BUTTON                    = 0x09,
+  HID_USAGE_PAGE_ORDINAL                   = 0x0a,
+  HID_USAGE_PAGE_TELEPHONY                 = 0x0b,
+  HID_USAGE_PAGE_CONSUMER                  = 0x0c,
+  HID_USAGE_PAGE_DIGITIZER                 = 0x0d,
+  HID_USAGE_PAGE_PID                       = 0x0f,
+  HID_USAGE_PAGE_UNICODE                   = 0x10,
+  HID_USAGE_PAGE_SOC                       = 0x11,
+  HID_USAGE_PAGE_EYE_AND_HEAD_TRACKERS     = 0x12,
+  // 0x13 is reserved
+  HID_USAGE_PAGE_AUXILIARY_DISPLAY         = 0x14,
+  // 0x15 - 0x1f is reserved
+  HID_USAGE_PAGE_SENSORS                   = 0x20,
+  // 0x21 - 0x3f is reserved
+  HID_USAGE_PAGE_MEDICAL_INSTRUMENT        = 0x40,
+  HID_USAGE_PAGE_LIGHTING_AND_ILLUMINATION = 0x59,
+  HID_USAGE_PAGE_MONITOR                   = 0x80, // 0x80 - 0x83
+  HID_USAGE_PAGE_POWER                     = 0x84, // 0x084 - 0x87
+  HID_USAGE_PAGE_BARCODE_SCANNER           = 0x8c,
+  HID_USAGE_PAGE_SCALE                     = 0x8d,
+  HID_USAGE_PAGE_MSR                       = 0x8e,
+  HID_USAGE_PAGE_CAMERA                    = 0x90,
+  HID_USAGE_PAGE_ARCADE                    = 0x91,
+  HID_USAGE_PAGE_FIDO                      = 0xF1D0, // FIDO alliance HID usage page
+  HID_USAGE_PAGE_VENDOR                    = 0xFF00  // 0xFF00 - 0xFFFF
 };
 
 /// HID Usage Table - Table 6: Generic Desktop Page
@@ -784,11 +890,9 @@ enum {
   HID_USAGE_DESKTOP_SYSTEM_DISPLAY_LCD_AUTOSCALE          = 0xB7
 };
 
-
 /// HID Usage Table: Consumer Page (0x0C)
 /// Only contains controls that supported by Windows (whole list is too long)
-enum
-{
+enum {
   // Generic Control
   HID_USAGE_CONSUMER_CONTROL                           = 0x0001,
 
@@ -844,6 +948,281 @@ enum
   HID_USAGE_CONSUMER_AC_PAN                            = 0x0238,
 };
 
+/// HID Usage Table: Digitizer Page (0x0D)
+enum {
+  HID_USAGE_DIGITIZER_UNDEFINED                           = 0x00,
+  HID_USAGE_DIGITIZER_DIGITIZER                           = 0x01, // CA
+  HID_USAGE_DIGITIZER_PEN                                 = 0x02, // CA
+  HID_USAGE_DIGITIZER_LIGHT_PEN                           = 0x03, // CA
+  HID_USAGE_DIGITIZER_TOUCH_SCREEN                        = 0x04, // CA
+  HID_USAGE_DIGITIZER_TOUCH_PAD                           = 0x05, // CA
+  HID_USAGE_DIGITIZER_WHITEBOARD                          = 0x06, // CA
+  HID_USAGE_DIGITIZER_COORDINATE_MEASURING_MACHINE        = 0x07, // CA
+  HID_USAGE_DIGITIZER_3D_DIGITIZER                        = 0x08, // CA
+  HID_USAGE_DIGITIZER_STEREO_PLOTTER                      = 0x09, // CA
+  HID_USAGE_DIGITIZER_ARTICULATED_ARM                     = 0x0A, // CA
+  HID_USAGE_DIGITIZER_ARMATURE                            = 0x0B, // CA
+  HID_USAGE_DIGITIZER_MULTIPLE_POINT_DIGITIZER            = 0x0C, // CA
+  HID_USAGE_DIGITIZER_FREE_SPACE_WAND                     = 0x0D, // CA
+  HID_USAGE_DIGITIZER_DEVICE_CONFIGURATION                = 0x0E, // CA
+  HID_USAGE_DIGITIZER_CAPACITIVE_HEAT_MAP_DIGITIZER       = 0x0F, // CA
+  // Reserved (0x10 - 0x1F)
+  HID_USAGE_DIGITIZER_STYLUS                              = 0x20, // CA/CL
+  HID_USAGE_DIGITIZER_PUCK                                = 0x21, // CL
+  HID_USAGE_DIGITIZER_FINGER                              = 0x22, // CL
+  HID_USAGE_DIGITIZER_DEVICE_SETTINGS                     = 0x23, // CL
+  HID_USAGE_DIGITIZER_CHARACTER_GESTURE                   = 0x24, // CL
+  // Reserved (0x25 - 0x2F)
+  HID_USAGE_DIGITIZER_TIP_PRESSURE                        = 0x30, // DV
+  HID_USAGE_DIGITIZER_BARREL_PRESSURE                     = 0x31, // DV
+  HID_USAGE_DIGITIZER_IN_RANGE                            = 0x32, // MC
+  HID_USAGE_DIGITIZER_TOUCH                               = 0x33, // MC
+  HID_USAGE_DIGITIZER_UNTOUCH                             = 0x34, // OSC
+  HID_USAGE_DIGITIZER_TAP                                 = 0x35, // OSC
+  HID_USAGE_DIGITIZER_QUALITY                             = 0x36, // DV
+  HID_USAGE_DIGITIZER_DATA_VALID                          = 0x37, // MC
+  HID_USAGE_DIGITIZER_TRANSDUCER_INDEX                    = 0x38, // DV
+  HID_USAGE_DIGITIZER_TABLET_FUNCTION_KEYS                = 0x39, // CL
+  HID_USAGE_DIGITIZER_PROGRAM_CHANGE_KEYS                 = 0x3A, // CL
+  HID_USAGE_DIGITIZER_BATTERY_STRENGTH                    = 0x3B, // DV
+  HID_USAGE_DIGITIZER_INVERT                              = 0x3C, // MC
+  HID_USAGE_DIGITIZER_X_TILT                              = 0x3D, // DV
+  HID_USAGE_DIGITIZER_Y_TILT                              = 0x3E, // DV
+  HID_USAGE_DIGITIZER_AZIMUTH                             = 0x3F, // DV
+  HID_USAGE_DIGITIZER_ALTITUDE                            = 0x40, // DV
+  HID_USAGE_DIGITIZER_TWIST                               = 0x41, // DV
+  HID_USAGE_DIGITIZER_TIP_SWITCH                          = 0x42, // MC
+  HID_USAGE_DIGITIZER_SECONDARY_TIP_SWITCH                = 0x43, // MC
+  HID_USAGE_DIGITIZER_BARREL_SWITCH                       = 0x44, // MC
+  HID_USAGE_DIGITIZER_ERASER                              = 0x45, // MC
+  HID_USAGE_DIGITIZER_TABLET_PICK                         = 0x46, // MC
+  HID_USAGE_DIGITIZER_TOUCH_VALID                         = 0x47, // MC
+  HID_USAGE_DIGITIZER_WIDTH                               = 0x48, // DV
+  HID_USAGE_DIGITIZER_HEIGHT                              = 0x49, // DV
+  // Reserved (0x4A - 0x50)
+  HID_USAGE_DIGITIZER_CONTACT_IDENTIFIER                  = 0x51, // DV
+  HID_USAGE_DIGITIZER_DEVICE_MODE                         = 0x52, // DV
+  HID_USAGE_DIGITIZER_DEVICE_IDENTIFIER                   = 0x53, // DV/SV
+  HID_USAGE_DIGITIZER_CONTACT_COUNT                       = 0x54, // DV
+  HID_USAGE_DIGITIZER_CONTACT_COUNT_MAXIMUM               = 0x55, // SV
+  HID_USAGE_DIGITIZER_SCAN_TIME                           = 0x56, // DV
+  HID_USAGE_DIGITIZER_SURFACE_SWITCH                      = 0x57, // DF
+  HID_USAGE_DIGITIZER_BUTTON_SWITCH                       = 0x58, // DF
+  HID_USAGE_DIGITIZER_PAD_TYPE                            = 0x59, // SF
+  HID_USAGE_DIGITIZER_TRANSDUCER_SERIAL_NUMBER            = 0x5B, // SV
+  HID_USAGE_DIGITIZER_PREFERRED_COLOR                     = 0x5C, // DV
+  HID_USAGE_DIGITIZER_PREFERRED_COLOR_LOCKED              = 0x5D, // MC
+  HID_USAGE_DIGITIZER_PREFERRED_LINE_WIDTH                = 0x5E, // DV
+  HID_USAGE_DIGITIZER_PREFERRED_LINE_WIDTH_LOCKED         = 0x5F, // MC
+  HID_USAGE_DIGITIZER_LATENCY_MODE                        = 0x60, // DF
+  HID_USAGE_DIGITIZER_GESTURE_CHARACTER_QUALITY           = 0x61, // DV
+  HID_USAGE_DIGITIZER_CHARACTER_GESTURE_DATA_LENGTH       = 0x62, // DV
+  HID_USAGE_DIGITIZER_CHARACTER_GESTURE_DATA              = 0x63, // DV
+  HID_USAGE_DIGITIZER_GESTURE_CHARACTER_ENCODING          = 0x64, // NAry
+  HID_USAGE_DIGITIZER_UTF8_CHARACTER_GESTURE_ENCODING     = 0x65, // Sel
+  HID_USAGE_DIGITIZER_UTF16_LE_CHARACTER_GESTURE_ENCODING = 0x66, // Sel
+  HID_USAGE_DIGITIZER_UTF16_BE_CHARACTER_GESTURE_ENCODING = 0x67, // Sel
+  HID_USAGE_DIGITIZER_UTF32_LE_CHARACTER_GESTURE_ENCODING = 0x68, // Sel
+  HID_USAGE_DIGITIZER_UTF32_BE_CHARACTER_GESTURE_ENCODING = 0x69, // Sel
+  HID_USAGE_DIGITIZER_CAPACITIVE_HEAT_MAP_VENDOR_ID       = 0x6A, // SV
+  HID_USAGE_DIGITIZER_CAPACITIVE_HEAT_MAP_VERSION         = 0x6B, // SV
+  HID_USAGE_DIGITIZER_CAPACITIVE_HEAT_MAP_FRAME_DATA      = 0x6C, // DV
+  HID_USAGE_DIGITIZER_GESTURE_CHARACTER_ENABLE            = 0x6D, // DF
+  HID_USAGE_DIGITIZER_TRANSDUCER_SERIAL_NUMBER_PART2      = 0x6E, // SV
+  HID_USAGE_DIGITIZER_NO_PREFERRED_COLOR                  = 0x6F, // DF
+  HID_USAGE_DIGITIZER_PREFERRED_LINE_STYLE                = 0x70, // NAry
+  HID_USAGE_DIGITIZER_PREFERRED_LINE_STYLE_LOCKED         = 0x71, // MC
+  HID_USAGE_DIGITIZER_INK                                 = 0x72, // Sel
+  HID_USAGE_DIGITIZER_PENCIL                              = 0x73, // Sel
+  HID_USAGE_DIGITIZER_HIGHLIGHTER                         = 0x74, // Sel
+  HID_USAGE_DIGITIZER_CHISEL_MARKER                       = 0x75, // Sel
+  HID_USAGE_DIGITIZER_BRUSH                               = 0x76, // Sel
+  HID_USAGE_DIGITIZER_NO_PREFERENCE                       = 0x77, // Sel
+  // Reserved (0x78 - 0x7F)
+  HID_USAGE_DIGITIZER_DIGITIZER_DIAGNOSTIC                = 0x80, // CL
+  HID_USAGE_DIGITIZER_DIGITIZER_ERROR                     = 0x81, // NAry
+  HID_USAGE_DIGITIZER_ERR_NORMAL_STATUS                   = 0x82, // Sel
+  HID_USAGE_DIGITIZER_ERR_TRANSDUCERS_EXCEEDED            = 0x83, // Sel
+  HID_USAGE_DIGITIZER_ERR_FULL_TRANS_FEATURES_UNAVAILABLE = 0x84, // Sel
+  HID_USAGE_DIGITIZER_ERR_CHARGE_LOW                      = 0x85, // Sel
+  // Reserved (0x86 - 0x8F)
+  HID_USAGE_DIGITIZER_TRANSDUCER_SOFTWARE_INFO            = 0x90, // CL
+  HID_USAGE_DIGITIZER_TRANSDUCER_VENDOR_ID                = 0x91, // SV
+  HID_USAGE_DIGITIZER_TRANSDUCER_PRODUCT_ID               = 0x92, // SV
+  HID_USAGE_DIGITIZER_DEVICE_SUPPORTED_PROTOCOLS          = 0x93, // NAry/CL
+  HID_USAGE_DIGITIZER_TRANSDUCER_SUPPORTED_PROTOCOLS      = 0x94, // NAry/CL
+  HID_USAGE_DIGITIZER_NO_PROTOCOL                         = 0x95, // Sel
+  HID_USAGE_DIGITIZER_WACOM_AES_PROTOCOL                  = 0x96, // Sel
+  HID_USAGE_DIGITIZER_USI_PROTOCOL                        = 0x97, // Sel
+  HID_USAGE_DIGITIZER_MICROSOFT_PEN_PROTOCOL              = 0x98, // Sel
+  // Reserved (0x99 - 0x9F)
+  HID_USAGE_DIGITIZER_SUPPORTED_REPORT_RATES              = 0xA0, // SV/CL
+  HID_USAGE_DIGITIZER_REPORT_RATE                         = 0xA1, // DV
+  HID_USAGE_DIGITIZER_TRANSDUCER_CONNECTED                = 0xA2, // SF
+  HID_USAGE_DIGITIZER_SWITCH_DISABLED                     = 0xA3, // Sel
+  HID_USAGE_DIGITIZER_SWITCH_UNIMPLEMENTED                = 0xA4, // Sel
+  HID_USAGE_DIGITIZER_TRANSDUCER_SWITCHES                 = 0xA5, // CL
+  HID_USAGE_DIGITIZER_TRANSDUCER_INDEX_SELECTOR           = 0xA6, // DV
+  // Reserved (0xA7 - 0xAF)
+  HID_USAGE_DIGITIZER_BUTTON_PRESS_THRESHOLD              = 0xB0, // DV
+
+  // Reserved (0xB1 - 0xFFFF)
+};
+
+/// HID Usage Table: Physical Input Device Page (0x0F)
+enum {
+  HID_USAGE_PID_UNDEFINED                                = 0x00,
+  HID_USAGE_PID_PHYSICAL_INPUT_DEVICE                    = 0x01,
+  HID_USAGE_PID_NORMAL                                   = 0x20,
+  HID_USAGE_PID_SET_EFFECT_REPORT                        = 0x21,
+  HID_USAGE_PID_EFFECT_PARAMETER_BLOCK_INDEX             = 0x22,
+  HID_USAGE_PID_PARAMETER_BLOCK_OFFSET                   = 0x23,
+  HID_USAGE_PID_ROM_FLAG                                 = 0x24,
+  HID_USAGE_PID_EFFECT_TYPE                              = 0x25,
+  HID_USAGE_PID_ET_CONSTANTFORCE                         = 0x26,
+  HID_USAGE_PID_ET_RAMP                                  = 0x27,
+  HID_USAGE_PID_ET_CUSTOMFORCE                           = 0x28,
+  HID_USAGE_PID_ET_SQUARE                                = 0x30,
+  HID_USAGE_PID_ET_SINE                                  = 0x31,
+  HID_USAGE_PID_ET_TRIANGLE                              = 0x32,
+  HID_USAGE_PID_ET_SAWTOOTH_UP                           = 0x33,
+  HID_USAGE_PID_ET_SAWTOOTH_DOWN                         = 0x34,
+  HID_USAGE_PID_ET_SPRING                                = 0x40,
+  HID_USAGE_PID_ET_DAMPER                                = 0x41,
+  HID_USAGE_PID_ET_INERTIA                               = 0x42,
+  HID_USAGE_PID_ET_FRICTION                              = 0x43,
+  HID_USAGE_PID_DURATION                                 = 0x50,
+  HID_USAGE_PID_SAMPLE_PERIOD                            = 0x51,
+  HID_USAGE_PID_GAIN                                     = 0x52,
+  HID_USAGE_PID_TRIGGER_BUTTON                           = 0x53,
+  HID_USAGE_PID_TRIGGER_REPEAT_INTERVAL                  = 0x54,
+  HID_USAGE_PID_AXES_ENABLE                              = 0x55,
+  HID_USAGE_PID_DIRECTION_ENABLE                         = 0x56,
+  HID_USAGE_PID_DIRECTION                                = 0x57,
+  HID_USAGE_PID_TYPE_SPECIFIC_BLOCK_OFFSET               = 0x58,
+  HID_USAGE_PID_BLOCK_TYPE                               = 0x59,
+  HID_USAGE_PID_SET_ENVELOPE_REPORT                      = 0x5a,
+  HID_USAGE_PID_ATTACK_LEVEL                             = 0x5b,
+  HID_USAGE_PID_ATTACK_TIME                              = 0x5c,
+  HID_USAGE_PID_FADE_LEVEL                               = 0x5d,
+  HID_USAGE_PID_FADE_TIME                                = 0x5e,
+  HID_USAGE_PID_SET_CONDITION_REPORT                     = 0x5f,
+  HID_USAGE_PID_CENTERPOINT_OFFSET                       = 0x60,
+  HID_USAGE_PID_POSITIVE_COEFFICIENT                     = 0x61,
+  HID_USAGE_PID_NEGATIVE_COEFFICIENT                     = 0x62,
+  HID_USAGE_PID_POSITIVE_SATURATION                      = 0x63,
+  HID_USAGE_PID_NEGATIVE_SATURATION                      = 0x64,
+  HID_USAGE_PID_DEAD_BAND                                = 0x65,
+  HID_USAGE_PID_DOWNLOAD_FORCE_SAMPLE                    = 0x66,
+  HID_USAGE_PID_ISOCH_CUSTOMFORCE_ENABLE                 = 0x67,
+  HID_USAGE_PID_CUSTOMFORCE_DATA_REPORT                  = 0x68,
+  HID_USAGE_PID_CUSTOMFORCE_DATA                         = 0x69,
+  HID_USAGE_PID_CUSTOMFORCE_VENDOR_DEFINED_DATA          = 0x6a,
+  HID_USAGE_PID_SET_CUSTOMFORCE_REPORT                   = 0x6b,
+  HID_USAGE_PID_CUSTOMFORCE_DATA_OFFSET                  = 0x6c,
+  HID_USAGE_PID_SAMPLE_COUNT                             = 0x6d,
+  HID_USAGE_PID_SET_PERIODIC_REPORT                      = 0x6e,
+  HID_USAGE_PID_OFFSET                                   = 0x6f,
+  HID_USAGE_PID_MAGNITUDE                                = 0x70,
+  HID_USAGE_PID_PHASE                                    = 0x71,
+  HID_USAGE_PID_PERIOD                                   = 0x72,
+  HID_USAGE_PID_SET_CONSTANTFORCE_REPORT                 = 0x73,
+  HID_USAGE_PID_SET_RAMPFORCE_REPORT                     = 0x74,
+  HID_USAGE_PID_RAMP_START                               = 0x75,
+  HID_USAGE_PID_RAMP_END                                 = 0x76,
+  HID_USAGE_PID_EFFECT_OPERATION_REPORT                  = 0x77,
+  HID_USAGE_PID_EFFECT_OPERATION                         = 0x78,
+  HID_USAGE_PID_OP_EFFECT_START                          = 0x79,
+  HID_USAGE_PID_OP_EFFECT_START_SOLO                     = 0x7a,
+  HID_USAGE_PID_OP_EFFECT_STOP                           = 0x7b,
+  HID_USAGE_PID_LOOP_COUNT                               = 0x7c,
+  HID_USAGE_PID_DEVICE_GAIN_REPORT                       = 0x7d,
+  HID_USAGE_PID_DEVICE_GAIN                              = 0x7e,
+  HID_USAGE_PID_PARAMETER_BLOCK_POOLS_REPORT             = 0x7f,
+  HID_USAGE_PID_RAM_POOL_SIZE                            = 0x80,
+  HID_USAGE_PID_ROM_POOL_SIZE                            = 0x81,
+  HID_USAGE_PID_ROM_EFFECT_BLOCK_COUNT                   = 0x82,
+  HID_USAGE_PID_SIMULTANEOUS_EFFECTS_MAX                 = 0x83,
+  HID_USAGE_PID_POOL_ALIGNMENT                           = 0x84,
+  HID_USAGE_PID_PARAMETER_BLOCK_MOVE_REPORT              = 0x85,
+  HID_USAGE_PID_MOVE_SOURCE                              = 0x86,
+  HID_USAGE_PID_MOVE_DESTINATION                         = 0x87,
+  HID_USAGE_PID_MOVE_LENGTH                              = 0x88,
+  HID_USAGE_PID_EFFECT_PARAMETER_BLOCK_LOAD_REPORT       = 0x89,
+  HID_USAGE_PID_EFFECT_PARAMETER_BLOCK_LOAD_STATUS       = 0x8b,
+  HID_USAGE_PID_BLOCK_LOAD_SUCCESS                       = 0x8c,
+  HID_USAGE_PID_BLOCK_LOAD_FULL                          = 0x8d,
+  HID_USAGE_PID_BLOCK_LOAD_ERROR                         = 0x8e,
+  HID_USAGE_PID_BLOCK_HANDLE                             = 0x8f,
+  HID_USAGE_PID_EFFECT_PARAMETER_BLOCK_FREE_REPORT       = 0x90,
+  HID_USAGE_PID_TYPE_SPECIFIC_BLOCK_HANDLE               = 0x91,
+  HID_USAGE_PID_PID_STATE_REPORT                         = 0x92,
+  HID_USAGE_PID_EFFECT_PLAYING                           = 0x94,
+  HID_USAGE_PID_PID_DEVICE_CONTROL_REPORT                = 0x95,
+  HID_USAGE_PID_PID_DEVICE_CONTROL                       = 0x96,
+  HID_USAGE_PID_DC_ENABLE_ACTUATORS                      = 0x97,
+  HID_USAGE_PID_DC_DISABLE_ACTUATORS                     = 0x98,
+  HID_USAGE_PID_DC_STOP_ALL_EFFECTS                      = 0x99,
+  HID_USAGE_PID_DC_RESET                                 = 0x9a,
+  HID_USAGE_PID_DC_PAUSE                                 = 0x9b,
+  HID_USAGE_PID_DC_CONTINUE                              = 0x9c,
+  HID_USAGE_PID_DEVICE_PAUSED                            = 0x9f,
+  HID_USAGE_PID_ACTUATORS_ENABLED                        = 0xa0,
+  HID_USAGE_PID_SAFETY_SWITCH                            = 0xa4,
+  HID_USAGE_PID_ACTUATOR_OVERRIDE_SWITCH                 = 0xa5,
+  HID_USAGE_PID_ACTUATOR_POWER                           = 0xa6,
+  HID_USAGE_PID_START_DELAY                              = 0xa7,
+  HID_USAGE_PID_PARAMETER_BLOCK_SIZE                     = 0xa8,
+  HID_USAGE_PID_DEVICEMANAGED_POOL                       = 0xa9,
+  HID_USAGE_PID_SHARED_PARAMETER_BLOCKS                  = 0xaa,
+  HID_USAGE_PID_CREATE_NEW_EFFECT_PARAMETER_BLOCK_REPORT = 0xab,
+  HID_USAGE_PID_RAM_POOL_AVAILABLE                       = 0xac,
+};
+
+/// HID Usage Table - Lighting And Illumination Page (0x59)
+enum {
+  HID_USAGE_LIGHTING_LAMP_ARRAY                          = 0x01,
+  HID_USAGE_LIGHTING_LAMP_ARRAY_ATTRIBUTES_REPORT        = 0x02,
+  HID_USAGE_LIGHTING_LAMP_COUNT                          = 0x03,
+  HID_USAGE_LIGHTING_BOUNDING_BOX_WIDTH_IN_MICROMETERS   = 0x04,
+  HID_USAGE_LIGHTING_BOUNDING_BOX_HEIGHT_IN_MICROMETERS  = 0x05,
+  HID_USAGE_LIGHTING_BOUNDING_BOX_DEPTH_IN_MICROMETERS   = 0x06,
+  HID_USAGE_LIGHTING_LAMP_ARRAY_KIND                     = 0x07,
+  HID_USAGE_LIGHTING_MIN_UPDATE_INTERVAL_IN_MICROSECONDS = 0x08,
+  HID_USAGE_LIGHTING_LAMP_ATTRIBUTES_REQUEST_REPORT      = 0x20,
+  HID_USAGE_LIGHTING_LAMP_ID                             = 0x21,
+  HID_USAGE_LIGHTING_LAMP_ATTRIBUTES_RESPONSE_REPORT     = 0x22,
+  HID_USAGE_LIGHTING_POSITION_X_IN_MICROMETERS           = 0x23,
+  HID_USAGE_LIGHTING_POSITION_Y_IN_MICROMETERS           = 0x24,
+  HID_USAGE_LIGHTING_POSITION_Z_IN_MICROMETERS           = 0x25,
+  HID_USAGE_LIGHTING_LAMP_PURPOSES                       = 0x26,
+  HID_USAGE_LIGHTING_UPDATE_LATENCY_IN_MICROSECONDS      = 0x27,
+  HID_USAGE_LIGHTING_RED_LEVEL_COUNT                     = 0x28,
+  HID_USAGE_LIGHTING_GREEN_LEVEL_COUNT                   = 0x29,
+  HID_USAGE_LIGHTING_BLUE_LEVEL_COUNT                    = 0x2A,
+  HID_USAGE_LIGHTING_INTENSITY_LEVEL_COUNT               = 0x2B,
+  HID_USAGE_LIGHTING_IS_PROGRAMMABLE                     = 0x2C,
+  HID_USAGE_LIGHTING_INPUT_BINDING                       = 0x2D,
+  HID_USAGE_LIGHTING_LAMP_MULTI_UPDATE_REPORT            = 0x50,
+  HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL                  = 0x51,
+  HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL                = 0x52,
+  HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL                 = 0x53,
+  HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL            = 0x54,
+  HID_USAGE_LIGHTING_LAMP_UPDATE_FLAGS                   = 0x55,
+  HID_USAGE_LIGHTING_LAMP_RANGE_UPDATE_REPORT            = 0x60,
+  HID_USAGE_LIGHTING_LAMP_ID_START                       = 0x61,
+  HID_USAGE_LIGHTING_LAMP_ID_END                         = 0x62,
+  HID_USAGE_LIGHTING_LAMP_ARRAY_CONTROL_REPORT           = 0x70,
+  HID_USAGE_LIGHTING_AUTONOMOUS_MODE                     = 0x71,
+};
+
+/// HID Usage Table: FIDO Alliance Page (0xF1D0)
+enum {
+  HID_USAGE_FIDO_U2FHID   = 0x01, // U2FHID usage for top-level collection
+  HID_USAGE_FIDO_DATA_IN  = 0x20, // Raw IN data report
+  HID_USAGE_FIDO_DATA_OUT = 0x21  // Raw OUT data report
+};
+
 /*--------------------------------------------------------------------
  * ASCII to KEYCODE Conversion
  *  Expand to array of [128][2] (shift, keycode)
diff --git a/src/class/hid/hid_device.c b/src/class/hid/hid_device.c
index 8077e4deb..b4f24902b 100644
--- a/src/class/hid/hid_device.c
+++ b/src/class/hid/hid_device.c
@@ -39,96 +39,114 @@
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
-typedef struct
-{
+typedef struct {
   uint8_t itf_num;
   uint8_t ep_in;
-  uint8_t ep_out;        // optional Out endpoint
-  uint8_t itf_protocol;  // Boot mouse or keyboard
+  uint8_t ep_out;       // optional Out endpoint
+  uint8_t itf_protocol; // Boot mouse or keyboard
 
+  uint16_t report_desc_len;
   uint8_t protocol_mode; // Boot (0) or Report protocol (1)
   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_EP_BUFSIZE];
-  CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_HID_EP_BUFSIZE];
 
   // TODO save hid descriptor since host can specifically request this after enumeration
   // Note: HID descriptor may be not available from application after enumeration
-  tusb_hid_descriptor_hid_t const * hid_descriptor;
+  const tusb_hid_descriptor_hid_t*hid_descriptor;
 } hidd_interface_t;
 
-CFG_TUSB_MEM_SECTION static hidd_interface_t _hidd_itf[CFG_TUD_HID];
+typedef struct {
+  TUD_EPBUF_DEF(ctrl , CFG_TUD_HID_EP_BUFSIZE);
+  TUD_EPBUF_DEF(epin , CFG_TUD_HID_EP_BUFSIZE);
+  TUD_EPBUF_DEF(epout, CFG_TUD_HID_EP_BUFSIZE);
+} hidd_epbuf_t;
+
+static hidd_interface_t _hidd_itf[CFG_TUD_HID];
+CFG_TUD_MEM_SECTION static hidd_epbuf_t _hidd_epbuf[CFG_TUD_HID];
 
 /*------------- Helpers -------------*/
-static inline uint8_t get_index_by_itfnum(uint8_t itf_num)
-{
-	for (uint8_t i=0; i < CFG_TUD_HID; i++ )
-	{
-		if ( itf_num == _hidd_itf[i].itf_num ) return i;
-	}
+TU_ATTR_ALWAYS_INLINE static inline uint8_t get_index_by_itfnum(uint8_t itf_num) {
+  for (uint8_t i = 0; i < CFG_TUD_HID; i++) {
+    if (itf_num == _hidd_itf[i].itf_num) {
+      return i;
+    }
+  }
+  return 0xFF;
+}
+
+//--------------------------------------------------------------------+
+// Weak stubs: invoked if no strong implementation is available
+//--------------------------------------------------------------------+
+TU_ATTR_WEAK void tud_hid_set_protocol_cb(uint8_t instance, uint8_t protocol) {
+  (void) instance;
+  (void) protocol;
+}
+
+TU_ATTR_WEAK bool tud_hid_set_idle_cb(uint8_t instance, uint8_t idle_rate) {
+  (void) instance;
+  (void) idle_rate;
+  return true;
+}
+
+TU_ATTR_WEAK void tud_hid_report_complete_cb(uint8_t instance, uint8_t const* report, uint16_t len) {
+  (void) instance;
+  (void) report;
+  (void) len;
+}
 
-	return 0xFF;
+// Invoked when a transfer wasn't successful
+TU_ATTR_WEAK void tud_hid_report_failed_cb(uint8_t instance, hid_report_type_t report_type, uint8_t const* report, uint16_t xferred_bytes) {
+  (void) instance;
+  (void) report_type;
+  (void) report;
+  (void) xferred_bytes;
 }
 
 //--------------------------------------------------------------------+
 // APPLICATION API
 //--------------------------------------------------------------------+
-bool tud_hid_n_ready(uint8_t instance)
-{
+bool tud_hid_n_ready(uint8_t instance) {
   uint8_t const rhport = 0;
   uint8_t const ep_in = _hidd_itf[instance].ep_in;
   return tud_ready() && (ep_in != 0) && !usbd_edpt_busy(rhport, ep_in);
 }
 
-bool tud_hid_n_report(uint8_t instance, uint8_t report_id, void const* report, uint16_t len)
-{
-  uint8_t const rhport = 0;
-  hidd_interface_t * p_hid = &_hidd_itf[instance];
+bool tud_hid_n_report(uint8_t instance, uint8_t report_id, void const *report, uint16_t len) {
+  TU_VERIFY(instance < CFG_TUD_HID);
+  const uint8_t rhport = 0;
+  hidd_interface_t *p_hid = &_hidd_itf[instance];
+  hidd_epbuf_t *p_epbuf = &_hidd_epbuf[instance];
 
   // claim endpoint
-  TU_VERIFY( usbd_edpt_claim(rhport, p_hid->ep_in) );
+  TU_VERIFY(usbd_edpt_claim(rhport, p_hid->ep_in));
 
   // prepare data
-  if (report_id)
-  {
-    len = tu_min16(len, CFG_TUD_HID_EP_BUFSIZE-1);
-
-    p_hid->epin_buf[0] = report_id;
-    memcpy(p_hid->epin_buf+1, report, len);
+  if (report_id) {
+    p_epbuf->epin[0] = report_id;
+    TU_VERIFY(0 == tu_memcpy_s(p_epbuf->epin + 1, CFG_TUD_HID_EP_BUFSIZE - 1, report, len));
     len++;
-  }else
-  {
-    // If report id = 0, skip ID field
-    len = tu_min16(len, CFG_TUD_HID_EP_BUFSIZE);
-    memcpy(p_hid->epin_buf, report, len);
+  } else {
+    TU_VERIFY(0 == tu_memcpy_s(p_epbuf->epin, CFG_TUD_HID_EP_BUFSIZE, report, len));
   }
 
-  return usbd_edpt_xfer(rhport, p_hid->ep_in, p_hid->epin_buf, len);
+  return usbd_edpt_xfer(rhport, p_hid->ep_in, p_epbuf->epin, len);
 }
 
-uint8_t tud_hid_n_interface_protocol(uint8_t instance)
-{
+uint8_t tud_hid_n_interface_protocol(uint8_t instance) {
   return _hidd_itf[instance].itf_protocol;
 }
 
-uint8_t tud_hid_n_get_protocol(uint8_t instance)
-{
+uint8_t tud_hid_n_get_protocol(uint8_t instance) {
   return _hidd_itf[instance].protocol_mode;
 }
 
-bool tud_hid_n_keyboard_report(uint8_t instance, uint8_t report_id, uint8_t modifier, uint8_t keycode[6])
-{
+bool tud_hid_n_keyboard_report(uint8_t instance, uint8_t report_id, uint8_t modifier, const uint8_t keycode[6]) {
   hid_keyboard_report_t report;
-
   report.modifier = modifier;
   report.reserved = 0;
 
-  if ( keycode )
-  {
-    memcpy(report.keycode, keycode, 6);
-  }else
-  {
+  if (keycode) {
+    memcpy(report.keycode, keycode, sizeof(report.keycode));
+  } else {
     tu_memclr(report.keycode, 6);
   }
 
@@ -136,33 +154,51 @@ bool tud_hid_n_keyboard_report(uint8_t instance, uint8_t report_id, uint8_t modi
 }
 
 bool tud_hid_n_mouse_report(uint8_t instance, uint8_t report_id,
-                            uint8_t buttons, int8_t x, int8_t y, int8_t vertical, int8_t horizontal)
-{
-  hid_mouse_report_t report =
-  {
+                            uint8_t buttons, int8_t x, int8_t y, int8_t vertical, int8_t horizontal) {
+  hid_mouse_report_t report = {
     .buttons = buttons,
-    .x       = x,
-    .y       = y,
-    .wheel   = vertical,
-    .pan     = horizontal
+    .x = x,
+    .y = y,
+    .wheel = vertical,
+    .pan = horizontal
   };
 
   return tud_hid_n_report(instance, report_id, &report, sizeof(report));
 }
 
-bool tud_hid_n_gamepad_report(uint8_t instance, uint8_t report_id,
-                              int8_t x, int8_t y, int8_t z, int8_t rz, int8_t rx, int8_t ry, uint8_t hat, uint32_t buttons)
-{
-  hid_gamepad_report_t report =
-  {
-    .x       = x,
-    .y       = y,
-    .z       = z,
-    .rz      = rz,
-    .rx      = rx,
-    .ry      = ry,
-    .hat     = hat,
+bool tud_hid_n_abs_mouse_report(uint8_t instance, uint8_t report_id,
+                                uint8_t buttons, int16_t x, int16_t y, int8_t vertical, int8_t horizontal) {
+  hid_abs_mouse_report_t report = {
     .buttons = buttons,
+    .x = x,
+    .y = y,
+    .wheel = vertical,
+    .pan = horizontal
+  };
+  return tud_hid_n_report(instance, report_id, &report, sizeof(report));
+}
+
+bool tud_hid_n_gamepad_report(uint8_t instance, uint8_t report_id,
+                              int8_t x, int8_t y, int8_t z, int8_t rz, int8_t rx, int8_t ry, uint8_t hat, uint32_t buttons) {
+  hid_gamepad_report_t report = {
+      .x = x,
+      .y = y,
+      .z = z,
+      .rz = rz,
+      .rx = rx,
+      .ry = ry,
+      .hat = hat,
+      .buttons = buttons,
+  };
+
+  return tud_hid_n_report(instance, report_id, &report, sizeof(report));
+}
+
+bool tud_hid_n_stylus_report(uint8_t instance, uint8_t report_id, uint8_t attrs, uint16_t x, uint16_t y) {
+  hid_stylus_report_t report = {
+    .attr = attrs,
+    .x = x,
+    .y = y,
   };
 
   return tud_hid_n_report(instance, report_id, &report, sizeof(report));
@@ -171,19 +207,20 @@ bool tud_hid_n_gamepad_report(uint8_t instance, uint8_t report_id,
 //--------------------------------------------------------------------+
 // USBD-CLASS API
 //--------------------------------------------------------------------+
-void hidd_init(void)
-{
+void hidd_init(void) {
   hidd_reset(0);
 }
 
-void hidd_reset(uint8_t rhport)
-{
-  (void) rhport;
+bool hidd_deinit(void) {
+  return true;
+}
+
+void hidd_reset(uint8_t rhport) {
+  (void)rhport;
   tu_memclr(_hidd_itf, sizeof(_hidd_itf));
 }
 
-uint16_t hidd_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint16_t max_len)
-{
+uint16_t hidd_open(uint8_t rhport, tusb_desc_interface_t const *desc_itf, uint16_t max_len) {
   TU_VERIFY(TUSB_CLASS_HID == desc_itf->bInterfaceClass, 0);
 
   // len = interface + hid + n*endpoints
@@ -192,45 +229,41 @@ uint16_t hidd_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint1
   TU_ASSERT(max_len >= drv_len, 0);
 
   // Find available interface
-  hidd_interface_t * p_hid = NULL;
+  hidd_interface_t *p_hid;
   uint8_t hid_id;
-  for(hid_id=0; hid_id<CFG_TUD_HID; hid_id++)
-  {
-    if ( _hidd_itf[hid_id].ep_in == 0 )
-    {
-      p_hid = &_hidd_itf[hid_id];
+  for (hid_id = 0; hid_id < CFG_TUD_HID; hid_id++) {
+    p_hid = &_hidd_itf[hid_id];
+    if (p_hid->ep_in == 0) {
       break;
     }
   }
-  TU_ASSERT(p_hid, 0);
+  TU_ASSERT(hid_id < CFG_TUD_HID, 0);
+  hidd_epbuf_t *p_epbuf = &_hidd_epbuf[hid_id];
 
-  uint8_t const *p_desc = (uint8_t const *) desc_itf;
+  uint8_t const *p_desc = (uint8_t const *)desc_itf;
 
   //------------- HID descriptor -------------//
   p_desc = tu_desc_next(p_desc);
   TU_ASSERT(HID_DESC_TYPE_HID == tu_desc_type(p_desc), 0);
-  p_hid->hid_descriptor = (tusb_hid_descriptor_hid_t const *) p_desc;
+  p_hid->hid_descriptor = (tusb_hid_descriptor_hid_t const *)p_desc;
 
   //------------- Endpoint Descriptor -------------//
   p_desc = tu_desc_next(p_desc);
   TU_ASSERT(usbd_open_edpt_pair(rhport, p_desc, desc_itf->bNumEndpoints, TUSB_XFER_INTERRUPT, &p_hid->ep_out, &p_hid->ep_in), 0);
 
-  if ( desc_itf->bInterfaceSubClass == HID_SUBCLASS_BOOT ) p_hid->itf_protocol = desc_itf->bInterfaceProtocol;
+  if (desc_itf->bInterfaceSubClass == HID_SUBCLASS_BOOT) {
+    p_hid->itf_protocol = desc_itf->bInterfaceProtocol;
+  }
 
   p_hid->protocol_mode = HID_PROTOCOL_REPORT; // Per Specs: default is report mode
-  p_hid->itf_num       = desc_itf->bInterfaceNumber;
+  p_hid->itf_num = desc_itf->bInterfaceNumber;
 
   // Use offsetof to avoid pointer to the odd/misaligned address
-  p_hid->report_desc_len = tu_unaligned_read16((uint8_t const*) p_hid->hid_descriptor + offsetof(tusb_hid_descriptor_hid_t, wReportLength));
+  p_hid->report_desc_len = tu_unaligned_read16((uint8_t const *)p_hid->hid_descriptor + offsetof(tusb_hid_descriptor_hid_t, wReportLength));
 
   // Prepare for output endpoint
-  if (p_hid->ep_out)
-  {
-    if ( !usbd_edpt_xfer(rhport, p_hid->ep_out, p_hid->epout_buf, sizeof(p_hid->epout_buf)) )
-    {
-      TU_LOG_FAILED();
-      TU_BREAKPOINT();
-    }
+  if (p_hid->ep_out) {
+    TU_ASSERT(usbd_edpt_xfer(rhport, p_hid->ep_out, p_epbuf->epout, CFG_TUD_HID_EP_BUFSIZE), drv_len);
   }
 
   return drv_len;
@@ -239,178 +272,148 @@ uint16_t hidd_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint1
 // Invoked when a control transfer occurred on an interface of this class
 // Driver response accordingly to the request and the transfer stage (setup/data/ack)
 // return false to stall control endpoint (e.g unsupported request)
-bool hidd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
-{
+bool hidd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const *request) {
   TU_VERIFY(request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_INTERFACE);
 
-  uint8_t const hid_itf = get_index_by_itfnum((uint8_t) request->wIndex);
+  uint8_t const hid_itf = get_index_by_itfnum((uint8_t)request->wIndex);
   TU_VERIFY(hid_itf < CFG_TUD_HID);
+  hidd_interface_t *p_hid = &_hidd_itf[hid_itf];
+  hidd_epbuf_t *p_epbuf = &_hidd_epbuf[hid_itf];
 
-  hidd_interface_t* p_hid = &_hidd_itf[hid_itf];
-
-  if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD)
-  {
+  if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD) {
     //------------- STD Request -------------//
-    if ( stage == CONTROL_STAGE_SETUP )
-    {
-      uint8_t const desc_type  = tu_u16_high(request->wValue);
-      //uint8_t const desc_index = tu_u16_low (request->wValue);
+    if (stage == CONTROL_STAGE_SETUP) {
+      uint8_t const desc_type = tu_u16_high(request->wValue);
+      // uint8_t const desc_index = tu_u16_low (request->wValue);
 
-      if (request->bRequest == TUSB_REQ_GET_DESCRIPTOR && desc_type == HID_DESC_TYPE_HID)
-      {
+      if (request->bRequest == TUSB_REQ_GET_DESCRIPTOR && desc_type == HID_DESC_TYPE_HID) {
         TU_VERIFY(p_hid->hid_descriptor);
-        TU_VERIFY(tud_control_xfer(rhport, request, (void*)(uintptr_t) p_hid->hid_descriptor, p_hid->hid_descriptor->bLength));
-      }
-      else if (request->bRequest == TUSB_REQ_GET_DESCRIPTOR && desc_type == HID_DESC_TYPE_REPORT)
-      {
-        uint8_t const * desc_report = tud_hid_descriptor_report_cb(hid_itf);
-        tud_control_xfer(rhport, request, (void*)(uintptr_t) desc_report, p_hid->report_desc_len);
-      }
-      else
-      {
+        TU_VERIFY(tud_control_xfer(rhport, request, (void *)(uintptr_t)p_hid->hid_descriptor, p_hid->hid_descriptor->bLength));
+      } else if (request->bRequest == TUSB_REQ_GET_DESCRIPTOR && desc_type == HID_DESC_TYPE_REPORT) {
+        uint8_t const *desc_report = tud_hid_descriptor_report_cb(hid_itf);
+        tud_control_xfer(rhport, request, (void *)(uintptr_t)desc_report, p_hid->report_desc_len);
+      } else {
         return false; // stall unsupported request
       }
     }
-  }
-  else if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS)
-  {
+  } else if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS) {
     //------------- Class Specific Request -------------//
-    switch( request->bRequest )
-    {
+    switch (request->bRequest) {
       case HID_REQ_CONTROL_GET_REPORT:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
+        if (stage == CONTROL_STAGE_SETUP) {
           uint8_t const report_type = tu_u16_high(request->wValue);
-          uint8_t const report_id   = tu_u16_low(request->wValue);
+          uint8_t const report_id = tu_u16_low(request->wValue);
 
-          uint8_t* report_buf = p_hid->epin_buf;
+          uint8_t* report_buf = p_epbuf->ctrl;
           uint16_t req_len = tu_min16(request->wLength, CFG_TUD_HID_EP_BUFSIZE);
-
           uint16_t xferlen = 0;
 
           // If host request a specific Report ID, add ID to as 1 byte of response
-          if ( (report_id != HID_REPORT_TYPE_INVALID) && (req_len > 1) )
-          {
+          if ((report_id != HID_REPORT_TYPE_INVALID) && (req_len > 1)) {
             *report_buf++ = report_id;
             req_len--;
-
             xferlen++;
           }
 
           xferlen += tud_hid_get_report_cb(hid_itf, report_id, (hid_report_type_t) report_type, report_buf, req_len);
-          TU_ASSERT( xferlen > 0 );
+          TU_ASSERT(xferlen > 0);
 
-          tud_control_xfer(rhport, request, p_hid->epin_buf, xferlen);
+          tud_control_xfer(rhport, request, p_epbuf->ctrl, xferlen);
         }
-      break;
+        break;
 
-      case  HID_REQ_CONTROL_SET_REPORT:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
-          TU_VERIFY(request->wLength <= sizeof(p_hid->epout_buf));
-          tud_control_xfer(rhport, request, p_hid->epout_buf, request->wLength);
-        }
-        else if ( stage == CONTROL_STAGE_ACK )
-        {
+      case HID_REQ_CONTROL_SET_REPORT:
+        if (stage == CONTROL_STAGE_SETUP) {
+          TU_VERIFY(request->wLength <= CFG_TUD_HID_EP_BUFSIZE);
+          tud_control_xfer(rhport, request, p_epbuf->ctrl, request->wLength);
+        } else if (stage == CONTROL_STAGE_ACK) {
           uint8_t const report_type = tu_u16_high(request->wValue);
-          uint8_t const report_id   = tu_u16_low(request->wValue);
+          uint8_t const report_id = tu_u16_low(request->wValue);
 
-          uint8_t const* report_buf = p_hid->epout_buf;
+          uint8_t const* report_buf = p_epbuf->ctrl;
           uint16_t report_len = tu_min16(request->wLength, CFG_TUD_HID_EP_BUFSIZE);
 
           // If host request a specific Report ID, extract report ID in buffer before invoking callback
-          if ( (report_id != HID_REPORT_TYPE_INVALID) && (report_len > 1) && (report_id == report_buf[0]) )
-          {
+          if ((report_id != HID_REPORT_TYPE_INVALID) && (report_len > 1) && (report_id == report_buf[0])) {
             report_buf++;
             report_len--;
           }
 
           tud_hid_set_report_cb(hid_itf, report_id, (hid_report_type_t) report_type, report_buf, report_len);
         }
-      break;
+        break;
 
       case HID_REQ_CONTROL_SET_IDLE:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
+        if (stage == CONTROL_STAGE_SETUP) {
           p_hid->idle_rate = tu_u16_high(request->wValue);
-          if ( tud_hid_set_idle_cb )
-          {
-            // stall request if callback return false
-            TU_VERIFY( tud_hid_set_idle_cb( hid_itf, p_hid->idle_rate) );
-          }
-
+          TU_VERIFY(tud_hid_set_idle_cb(hid_itf, p_hid->idle_rate)); // stall if false
           tud_control_status(rhport, request);
         }
-      break;
+        break;
 
       case HID_REQ_CONTROL_GET_IDLE:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
+        if (stage == CONTROL_STAGE_SETUP) {
           // TODO idle rate of report
           tud_control_xfer(rhport, request, &p_hid->idle_rate, 1);
         }
-      break;
+        break;
 
       case HID_REQ_CONTROL_GET_PROTOCOL:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
+        if (stage == CONTROL_STAGE_SETUP) {
           tud_control_xfer(rhport, request, &p_hid->protocol_mode, 1);
         }
-      break;
+        break;
 
       case HID_REQ_CONTROL_SET_PROTOCOL:
-        if ( stage == CONTROL_STAGE_SETUP )
-        {
+        if (stage == CONTROL_STAGE_SETUP) {
           tud_control_status(rhport, request);
-        }
-        else if ( stage == CONTROL_STAGE_ACK )
-        {
+        } else if (stage == CONTROL_STAGE_ACK) {
           p_hid->protocol_mode = (uint8_t) request->wValue;
-          if (tud_hid_set_protocol_cb)
-          {
-            tud_hid_set_protocol_cb(hid_itf, p_hid->protocol_mode);
-          }
+          tud_hid_set_protocol_cb(hid_itf, p_hid->protocol_mode);
         }
-      break;
+        break;
 
-      default: return false; // stall unsupported request
+      default:
+        return false; // stall unsupported request
     }
-  }else
-  {
+  } else {
     return false; // stall unsupported request
   }
 
   return true;
 }
 
-bool hidd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
-{
-  (void) result;
-
-  uint8_t instance = 0;
-  hidd_interface_t * p_hid = _hidd_itf;
+bool hidd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) {
+  uint8_t instance;
+  hidd_interface_t *p_hid;
 
   // Identify which interface to use
-  for (instance = 0; instance < CFG_TUD_HID; instance++)
-  {
+  for (instance = 0; instance < CFG_TUD_HID; instance++) {
     p_hid = &_hidd_itf[instance];
-    if ( (ep_addr == p_hid->ep_out) || (ep_addr == p_hid->ep_in) ) break;
+    if ((ep_addr == p_hid->ep_out) || (ep_addr == p_hid->ep_in)) {
+      break;
+    }
   }
   TU_ASSERT(instance < CFG_TUD_HID);
+  hidd_epbuf_t *p_epbuf = &_hidd_epbuf[instance];
 
-  // Sent report successfully
-  if (ep_addr == p_hid->ep_in)
-  {
-    if (tud_hid_report_complete_cb)
-    {
-      tud_hid_report_complete_cb(instance, p_hid->epin_buf, (/*uint16_t*/ uint8_t) xferred_bytes);
+  if (ep_addr == p_hid->ep_in) {
+    // Input report
+    if (XFER_RESULT_SUCCESS == result) {
+      tud_hid_report_complete_cb(instance, p_epbuf->epin, (uint16_t) xferred_bytes);
+    } else {
+      tud_hid_report_failed_cb(instance, HID_REPORT_TYPE_INPUT, p_epbuf->epin, (uint16_t) xferred_bytes);
     }
-  }
-  // Received report
-  else if (ep_addr == p_hid->ep_out)
-  {
-    tud_hid_set_report_cb(instance, 0, HID_REPORT_TYPE_INVALID, p_hid->epout_buf, (uint16_t) xferred_bytes);
-    TU_ASSERT(usbd_edpt_xfer(rhport, p_hid->ep_out, p_hid->epout_buf, sizeof(p_hid->epout_buf)));
+  } else {
+    // Output report
+    if (XFER_RESULT_SUCCESS == result) {
+      tud_hid_set_report_cb(instance, 0, HID_REPORT_TYPE_OUTPUT, p_epbuf->epout, (uint16_t)xferred_bytes);
+    } else {
+      tud_hid_report_failed_cb(instance, HID_REPORT_TYPE_OUTPUT, p_epbuf->epout, (uint16_t) xferred_bytes);
+    }
+
+    // prepare for new transfer
+    TU_ASSERT(usbd_edpt_xfer(rhport, p_hid->ep_out, p_epbuf->epout, CFG_TUD_HID_EP_BUFSIZE));
   }
 
   return true;
diff --git a/src/class/hid/hid_device.h b/src/class/hid/hid_device.h
index 3143b1024..fc1dbcbd8 100644
--- a/src/class/hid/hid_device.h
+++ b/src/class/hid/hid_device.h
@@ -24,8 +24,8 @@
  * This file is part of the TinyUSB stack.
  */
 
-#ifndef _TUSB_HID_DEVICE_H_
-#define _TUSB_HID_DEVICE_H_
+#ifndef TUSB_HID_DEVICE_H_
+#define TUSB_HID_DEVICE_H_
 
 #include "hid.h"
 
@@ -48,8 +48,7 @@
 #endif
 
 //--------------------------------------------------------------------+
-// Application API (Multiple Instances)
-// CFG_TUD_HID > 1
+// Application API (Multiple Instances) i.e. CFG_TUD_HID > 1
 //--------------------------------------------------------------------+
 
 // Check if the interface is ready to use
@@ -66,29 +65,64 @@ bool tud_hid_n_report(uint8_t instance, uint8_t report_id, void const* report, u
 
 // KEYBOARD: convenient helper to send keyboard report if application
 // use template layout report as defined by hid_keyboard_report_t
-bool tud_hid_n_keyboard_report(uint8_t instance, uint8_t report_id, uint8_t modifier, uint8_t keycode[6]);
+bool tud_hid_n_keyboard_report(uint8_t instance, uint8_t report_id, uint8_t modifier, const uint8_t keycode[6]);
 
 // MOUSE: convenient helper to send mouse report if application
 // use template layout report as defined by hid_mouse_report_t
 bool tud_hid_n_mouse_report(uint8_t instance, uint8_t report_id, uint8_t buttons, int8_t x, int8_t y, int8_t vertical, int8_t horizontal);
 
+// ABSOLUTE MOUSE: convenient helper to send absolute mouse report if application
+// use template layout report as defined by hid_abs_mouse_report_t
+bool tud_hid_n_abs_mouse_report(uint8_t instance, uint8_t report_id, uint8_t buttons, int16_t x, int16_t y, int8_t vertical, int8_t horizontal);
+
 // Gamepad: convenient helper to send gamepad report if application
 // use template layout report TUD_HID_REPORT_DESC_GAMEPAD
 bool tud_hid_n_gamepad_report(uint8_t instance, uint8_t report_id, int8_t x, int8_t y, int8_t z, int8_t rz, int8_t rx, int8_t ry, uint8_t hat, uint32_t buttons);
 
+// STYLUS PEN: convenient helper to send absolute stylus pen report if application
+bool tud_hid_n_stylus_report(uint8_t instance, uint8_t report_id, uint8_t attrs, uint16_t x, uint16_t y);
+
 //--------------------------------------------------------------------+
 // Application API (Single Port)
 //--------------------------------------------------------------------+
-static inline bool    tud_hid_ready(void);
-static inline uint8_t tud_hid_interface_protocol(void);
-static inline uint8_t tud_hid_get_protocol(void);
-static inline bool    tud_hid_report(uint8_t report_id, void const* report, uint16_t len);
-static inline bool    tud_hid_keyboard_report(uint8_t report_id, uint8_t modifier, uint8_t keycode[6]);
-static inline bool    tud_hid_mouse_report(uint8_t report_id, uint8_t buttons, int8_t x, int8_t y, int8_t vertical, int8_t horizontal);
-static inline bool    tud_hid_gamepad_report(uint8_t report_id, int8_t x, int8_t y, int8_t z, int8_t rz, int8_t rx, int8_t ry, uint8_t hat, uint32_t buttons);
+TU_ATTR_ALWAYS_INLINE static inline bool tud_hid_ready(void) {
+  return tud_hid_n_ready(0);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline uint8_t tud_hid_interface_protocol(void) {
+  return tud_hid_n_interface_protocol(0);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline uint8_t tud_hid_get_protocol(void) {
+  return tud_hid_n_get_protocol(0);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool tud_hid_report(uint8_t report_id, void const* report, uint16_t len) {
+  return tud_hid_n_report(0, report_id, report, len);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool tud_hid_keyboard_report(uint8_t report_id, uint8_t modifier, const uint8_t keycode[6]) {
+  return tud_hid_n_keyboard_report(0, report_id, modifier, keycode);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool tud_hid_mouse_report(uint8_t report_id, uint8_t buttons, int8_t x, int8_t y, int8_t vertical, int8_t horizontal) {
+  return tud_hid_n_mouse_report(0, report_id, buttons, x, y, vertical, horizontal);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool tud_hid_abs_mouse_report(uint8_t report_id, uint8_t buttons, int16_t x, int16_t y, int8_t vertical, int8_t horizontal) {
+  return tud_hid_n_abs_mouse_report(0, report_id, buttons, x, y, vertical, horizontal);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool tud_hid_gamepad_report(uint8_t report_id, int8_t x, int8_t y, int8_t z, int8_t rz, int8_t rx, int8_t ry, uint8_t hat, uint32_t buttons) {
+  return tud_hid_n_gamepad_report(0, report_id, x, y, z, rz, rx, ry, hat, buttons);
+}
+
+TU_ATTR_ALWAYS_INLINE static inline bool tud_hid_stylus_report(uint8_t report_id, uint8_t attrs, uint16_t x, uint16_t y) {
+  return tud_hid_n_stylus_report(0, report_id, attrs, x, y);
+}
 
 //--------------------------------------------------------------------+
-// Callbacks (Weak is optional)
+// Application Callbacks
 //--------------------------------------------------------------------+
 
 // Invoked when received GET HID REPORT DESCRIPTOR request
@@ -101,61 +135,25 @@ uint8_t const * tud_hid_descriptor_report_cb(uint8_t instance);
 uint16_t tud_hid_get_report_cb(uint8_t instance, uint8_t report_id, hid_report_type_t report_type, uint8_t* buffer, uint16_t reqlen);
 
 // Invoked when received SET_REPORT control request or
-// received data on OUT endpoint ( Report ID = 0, Type = 0 )
+// received data on OUT endpoint (Report ID = 0, Type = OUTPUT)
 void tud_hid_set_report_cb(uint8_t instance, uint8_t report_id, hid_report_type_t report_type, uint8_t const* buffer, uint16_t bufsize);
 
 // Invoked when received SET_PROTOCOL request
 // protocol is either HID_PROTOCOL_BOOT (0) or HID_PROTOCOL_REPORT (1)
-TU_ATTR_WEAK void tud_hid_set_protocol_cb(uint8_t instance, uint8_t protocol);
+void tud_hid_set_protocol_cb(uint8_t instance, uint8_t protocol);
 
 // Invoked when received SET_IDLE request. return false will stall the request
-// - Idle Rate = 0 : only send report if there is changes, i.e skip duplication
+// - Idle Rate = 0 : only send report if there is changes, i.e. skip duplication
 // - Idle Rate > 0 : skip duplication, but send at least 1 report every idle rate (in unit of 4 ms).
-TU_ATTR_WEAK bool tud_hid_set_idle_cb(uint8_t instance, uint8_t idle_rate);
+bool tud_hid_set_idle_cb(uint8_t instance, uint8_t idle_rate);
 
 // Invoked when sent REPORT successfully to host
 // Application can use this to send the next report
 // Note: For composite reports, report[0] is report ID
-TU_ATTR_WEAK void tud_hid_report_complete_cb(uint8_t instance, uint8_t const* report, /*uint16_t*/ uint8_t len );
+void tud_hid_report_complete_cb(uint8_t instance, uint8_t const* report, uint16_t len);
 
-
-//--------------------------------------------------------------------+
-// Inline Functions
-//--------------------------------------------------------------------+
-static inline bool tud_hid_ready(void)
-{
-  return tud_hid_n_ready(0);
-}
-
-static inline uint8_t tud_hid_interface_protocol(void)
-{
-  return tud_hid_n_interface_protocol(0);
-}
-
-static inline uint8_t tud_hid_get_protocol(void)
-{
-  return tud_hid_n_get_protocol(0);
-}
-
-static inline bool tud_hid_report(uint8_t report_id, void const* report, uint16_t len)
-{
-  return tud_hid_n_report(0, report_id, report, len);
-}
-
-static inline bool tud_hid_keyboard_report(uint8_t report_id, uint8_t modifier, uint8_t keycode[6])
-{
-  return tud_hid_n_keyboard_report(0, report_id, modifier, keycode);
-}
-
-static inline bool tud_hid_mouse_report(uint8_t report_id, uint8_t buttons, int8_t x, int8_t y, int8_t vertical, int8_t horizontal)
-{
-  return tud_hid_n_mouse_report(0, report_id, buttons, x, y, vertical, horizontal);
-}
-
-static inline bool  tud_hid_gamepad_report(uint8_t report_id, int8_t x, int8_t y, int8_t z, int8_t rz, int8_t rx, int8_t ry, uint8_t hat, uint32_t buttons)
-{
-  return tud_hid_n_gamepad_report(0, report_id, x, y, z, rz, rx, ry, hat, buttons);
-}
+// Invoked when a transfer wasn't successful
+void tud_hid_report_failed_cb(uint8_t instance, hid_report_type_t report_type, uint8_t const* report, uint16_t xferred_bytes);
 
 /* --------------------------------------------------------------------+
  * HID Report Descriptor Template
@@ -182,7 +180,7 @@ static inline bool  tud_hid_gamepad_report(uint8_t report_id, int8_t x, int8_t y
   HID_COLLECTION ( HID_COLLECTION_APPLICATION )                    ,\
     /* Report ID if any */\
     __VA_ARGS__ \
-    /* 8 bits Modifier Keys (Shfit, Control, Alt) */ \
+    /* 8 bits Modifier Keys (Shift, Control, Alt) */ \
     HID_USAGE_PAGE ( HID_USAGE_PAGE_KEYBOARD )                     ,\
       HID_USAGE_MIN    ( 224                                    )  ,\
       HID_USAGE_MAX    ( 231                                    )  ,\
@@ -266,6 +264,90 @@ static inline bool  tud_hid_gamepad_report(uint8_t report_id, int8_t x, int8_t y
     HID_COLLECTION_END                                            , \
   HID_COLLECTION_END \
 
+// Stylus Pen Report Descriptor Template
+#define TUD_HID_REPORT_DESC_STYLUS_PEN(...) \
+  HID_USAGE_PAGE ( HID_USAGE_PAGE_DIGITIZER )                     , \
+  HID_USAGE      ( HID_USAGE_DIGITIZER_TOUCH_SCREEN )             , \
+  HID_COLLECTION ( HID_COLLECTION_APPLICATION  )                  , \
+    /* Report ID if any */\
+    __VA_ARGS__ \
+    HID_USAGE    ( HID_USAGE_DIGITIZER_STYLUS )                   , \
+    HID_COLLECTION ( HID_COLLECTION_PHYSICAL   )                  , \
+      HID_USAGE_PAGE  ( HID_USAGE_DIGITIZER_TIP_SWITCH )          , \
+      HID_USAGE_PAGE  ( HID_USAGE_DIGITIZER_IN_RANGE )            , \
+        HID_LOGICAL_MIN ( 0                                      ), \
+        HID_LOGICAL_MAX ( 1                                      ), \
+        HID_REPORT_SIZE ( 1                                      ), \
+        HID_REPORT_COUNT( 2                                      ), \
+        HID_INPUT       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ), \
+        HID_REPORT_SIZE ( 1                                      ), \
+        HID_REPORT_COUNT( 6                                      ), \
+        HID_INPUT       ( HID_CONSTANT | HID_ARRAY | HID_ABSOLUTE), \
+      HID_USAGE_PAGE    ( HID_USAGE_PAGE_DESKTOP                 ), \
+        HID_PHYSICAL_MAX_N( 0x7fff, 2                            ), \
+        HID_LOGICAL_MAX_N ( 0x7fff, 2                            ), \
+        HID_REPORT_SIZE ( 16                                     ), \
+        HID_REPORT_COUNT( 1                                      ), \
+        HID_UNIT_EXPONENT( 0x0f                                  ), \
+        HID_UNIT        ( HID_VARIABLE | HID_NONLINEAR           ), \
+        HID_PHYSICAL_MIN( 0                                      ), \
+        HID_PHYSICAL_MAX( 0                                      ), \
+        HID_USAGE       ( HID_USAGE_DESKTOP_X                    ), \
+        HID_INPUT       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ), \
+        HID_USAGE       ( HID_USAGE_DESKTOP_Y                    ), \
+        HID_INPUT       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ), \
+    HID_COLLECTION_END                                          , \
+  HID_COLLECTION_END \
+
+// Absolute Mouse Report Descriptor Template
+#define TUD_HID_REPORT_DESC_ABSMOUSE(...) \
+  HID_USAGE_PAGE ( HID_USAGE_PAGE_DESKTOP      )                   ,\
+  HID_USAGE      ( HID_USAGE_DESKTOP_MOUSE     )                   ,\
+  HID_COLLECTION ( HID_COLLECTION_APPLICATION  )                   ,\
+    /* Report ID if any */\
+    __VA_ARGS__ \
+    HID_USAGE      ( HID_USAGE_DESKTOP_POINTER )                   ,\
+    HID_COLLECTION ( HID_COLLECTION_PHYSICAL   )                   ,\
+      HID_USAGE_PAGE  ( HID_USAGE_PAGE_BUTTON  )                   ,\
+        HID_USAGE_MIN   ( 1                                      ) ,\
+        HID_USAGE_MAX   ( 5                                      ) ,\
+        HID_LOGICAL_MIN ( 0                                      ) ,\
+        HID_LOGICAL_MAX ( 1                                      ) ,\
+        /* Left, Right, Middle, Backward, Forward buttons */ \
+        HID_REPORT_COUNT( 5                                      ) ,\
+        HID_REPORT_SIZE ( 1                                      ) ,\
+        HID_INPUT       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ) ,\
+        /* 3 bit padding */ \
+        HID_REPORT_COUNT( 1                                      ) ,\
+        HID_REPORT_SIZE ( 3                                      ) ,\
+        HID_INPUT       ( HID_CONSTANT                           ) ,\
+      HID_USAGE_PAGE  ( HID_USAGE_PAGE_DESKTOP )                   ,\
+        /* X, Y absolute position [0, 32767] */ \
+        HID_USAGE       ( HID_USAGE_DESKTOP_X                    ) ,\
+        HID_USAGE       ( HID_USAGE_DESKTOP_Y                    ) ,\
+        HID_LOGICAL_MIN  ( 0x00                                ) ,\
+        HID_LOGICAL_MAX_N( 0x7FFF, 2                           ) ,\
+        HID_REPORT_SIZE  ( 16                                  ) ,\
+        HID_REPORT_COUNT ( 2                                   ) ,\
+        HID_INPUT       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ) ,\
+        /* Vertical wheel scroll [-127, 127] */ \
+        HID_USAGE       ( HID_USAGE_DESKTOP_WHEEL                )  ,\
+        HID_LOGICAL_MIN ( 0x81                                   )  ,\
+        HID_LOGICAL_MAX ( 0x7f                                   )  ,\
+        HID_REPORT_COUNT( 1                                      )  ,\
+        HID_REPORT_SIZE ( 8                                      )  ,\
+        HID_INPUT       ( HID_DATA | HID_VARIABLE | HID_RELATIVE )  ,\
+      HID_USAGE_PAGE  ( HID_USAGE_PAGE_CONSUMER ), \
+       /* Horizontal wheel scroll [-127, 127] */ \
+        HID_USAGE_N     ( HID_USAGE_CONSUMER_AC_PAN, 2           ), \
+        HID_LOGICAL_MIN ( 0x81                                   ), \
+        HID_LOGICAL_MAX ( 0x7f                                   ), \
+        HID_REPORT_COUNT( 1                                      ), \
+        HID_REPORT_SIZE ( 8                                      ), \
+        HID_INPUT       ( HID_DATA | HID_VARIABLE | HID_RELATIVE ), \
+    HID_COLLECTION_END                                            , \
+  HID_COLLECTION_END \
+
 // Consumer Control Report Descriptor Template
 #define TUD_HID_REPORT_DESC_CONSUMER(...) \
   HID_USAGE_PAGE ( HID_USAGE_PAGE_CONSUMER    )              ,\
@@ -352,6 +434,31 @@ static inline bool  tud_hid_gamepad_report(uint8_t report_id, int8_t x, int8_t y
     HID_INPUT          ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ) ,\
   HID_COLLECTION_END \
 
+// FIDO U2F Authenticator Descriptor Template
+// - 1st parameter is report size, which is 64 bytes maximum in U2F
+// - 2nd parameter is HID_REPORT_ID(n) (optional)
+#define TUD_HID_REPORT_DESC_FIDO_U2F(report_size, ...) \
+  HID_USAGE_PAGE_N ( HID_USAGE_PAGE_FIDO, 2                    ) ,\
+  HID_USAGE      ( HID_USAGE_FIDO_U2FHID                       ) ,\
+  HID_COLLECTION ( HID_COLLECTION_APPLICATION                  ) ,\
+    /* Report ID if any */ \
+    __VA_ARGS__ \
+    /* Usage Data In */ \
+    HID_USAGE         ( HID_USAGE_FIDO_DATA_IN                 ) ,\
+    HID_LOGICAL_MIN   ( 0                                      ) ,\
+    HID_LOGICAL_MAX_N ( 0xff, 2                                ) ,\
+    HID_REPORT_SIZE   ( 8                                      ) ,\
+    HID_REPORT_COUNT  ( report_size                            ) ,\
+    HID_INPUT         ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ) ,\
+    /* Usage Data Out */ \
+    HID_USAGE         ( HID_USAGE_FIDO_DATA_OUT                ) ,\
+    HID_LOGICAL_MIN   ( 0                                      ) ,\
+    HID_LOGICAL_MAX_N ( 0xff, 2                                ) ,\
+    HID_REPORT_SIZE   ( 8                                      ) ,\
+    HID_REPORT_COUNT  ( report_size                            ) ,\
+    HID_OUTPUT        ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ) ,\
+  HID_COLLECTION_END \
+
 // HID Generic Input & Output
 // - 1st parameter is report size (mandatory)
 // - 2nd parameter is report id HID_REPORT_ID(n) (optional)
@@ -377,10 +484,183 @@ static inline bool  tud_hid_gamepad_report(uint8_t report_id, int8_t x, int8_t y
       HID_OUTPUT      ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE  ),\
     HID_COLLECTION_END \
 
+// HID Lighting and Illumination Report Descriptor Template
+// - 1st parameter is report id (required)
+//   Creates 6 report ids for lighting HID usages in the following order:
+//     report_id+0: HID_USAGE_LIGHTING_LAMP_ARRAY_ATTRIBUTES_REPORT
+//     report_id+1: HID_USAGE_LIGHTING_LAMP_ATTRIBUTES_REQUEST_REPORT
+//     report_id+2: HID_USAGE_LIGHTING_LAMP_ATTRIBUTES_RESPONSE_REPORT
+//     report_id+3: HID_USAGE_LIGHTING_LAMP_MULTI_UPDATE_REPORT
+//     report_id+4: HID_USAGE_LIGHTING_LAMP_RANGE_UPDATE_REPORT
+//     report_id+5: HID_USAGE_LIGHTING_LAMP_ARRAY_CONTROL_REPORT
+#define TUD_HID_REPORT_DESC_LIGHTING(report_id) \
+  HID_USAGE_PAGE ( HID_USAGE_PAGE_LIGHTING_AND_ILLUMINATION ),\
+  HID_USAGE      ( HID_USAGE_LIGHTING_LAMP_ARRAY            ),\
+  HID_COLLECTION ( HID_COLLECTION_APPLICATION               ),\
+    /* Lamp Array Attributes Report */ \
+    HID_REPORT_ID (report_id                                    ) \
+    HID_USAGE ( HID_USAGE_LIGHTING_LAMP_ARRAY_ATTRIBUTES_REPORT ),\
+    HID_COLLECTION ( HID_COLLECTION_LOGICAL                     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_COUNT                          ),\
+      HID_LOGICAL_MIN   ( 0                                                      ),\
+      HID_LOGICAL_MAX_N ( 65535, 3                                               ),\
+      HID_REPORT_SIZE   ( 16                                                     ),\
+      HID_REPORT_COUNT  ( 1                                                      ),\
+      HID_FEATURE       ( HID_CONSTANT | HID_VARIABLE | HID_ABSOLUTE             ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BOUNDING_BOX_WIDTH_IN_MICROMETERS   ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BOUNDING_BOX_HEIGHT_IN_MICROMETERS  ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BOUNDING_BOX_DEPTH_IN_MICROMETERS   ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_ARRAY_KIND                     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_MIN_UPDATE_INTERVAL_IN_MICROSECONDS ),\
+      HID_LOGICAL_MIN   ( 0                                                      ),\
+      HID_LOGICAL_MAX_N ( 2147483647, 3                                          ),\
+      HID_REPORT_SIZE   ( 32                                                     ),\
+      HID_REPORT_COUNT  ( 5                                                      ),\
+      HID_FEATURE       ( HID_CONSTANT | HID_VARIABLE | HID_ABSOLUTE             ),\
+    HID_COLLECTION_END ,\
+    /* Lamp Attributes Request Report */ \
+    HID_REPORT_ID       ( report_id + 1                                     ) \
+    HID_USAGE           ( HID_USAGE_LIGHTING_LAMP_ATTRIBUTES_REQUEST_REPORT ),\
+    HID_COLLECTION      ( HID_COLLECTION_LOGICAL                            ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_ID             ),\
+      HID_LOGICAL_MIN   ( 0                                      ),\
+      HID_LOGICAL_MAX_N ( 65535, 3                               ),\
+      HID_REPORT_SIZE   ( 16                                     ),\
+      HID_REPORT_COUNT  ( 1                                      ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ),\
+    HID_COLLECTION_END ,\
+    /* Lamp Attributes Response Report */ \
+    HID_REPORT_ID       ( report_id + 2                                      ) \
+    HID_USAGE           ( HID_USAGE_LIGHTING_LAMP_ATTRIBUTES_RESPONSE_REPORT ),\
+    HID_COLLECTION      ( HID_COLLECTION_LOGICAL                             ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_ID                        ),\
+      HID_LOGICAL_MIN   ( 0                                                 ),\
+      HID_LOGICAL_MAX_N ( 65535, 3                                          ),\
+      HID_REPORT_SIZE   ( 16                                                ),\
+      HID_REPORT_COUNT  ( 1                                                 ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE            ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_POSITION_X_IN_MICROMETERS      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_POSITION_Y_IN_MICROMETERS      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_POSITION_Z_IN_MICROMETERS      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_UPDATE_LATENCY_IN_MICROSECONDS ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_PURPOSES                  ),\
+      HID_LOGICAL_MIN   ( 0                                                 ),\
+      HID_LOGICAL_MAX_N ( 2147483647, 3                                     ),\
+      HID_REPORT_SIZE   ( 32                                                ),\
+      HID_REPORT_COUNT  ( 5                                                 ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE            ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_LEVEL_COUNT                ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_LEVEL_COUNT              ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_LEVEL_COUNT               ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_LEVEL_COUNT          ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_IS_PROGRAMMABLE                ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INPUT_BINDING                  ),\
+      HID_LOGICAL_MIN   ( 0                                                 ),\
+      HID_LOGICAL_MAX_N ( 255, 2                                            ),\
+      HID_REPORT_SIZE   ( 8                                                 ),\
+      HID_REPORT_COUNT  ( 6                                                 ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE            ),\
+    HID_COLLECTION_END ,\
+    /* Lamp Multi-Update Report */ \
+    HID_REPORT_ID       ( report_id + 3                               ) \
+    HID_USAGE           ( HID_USAGE_LIGHTING_LAMP_MULTI_UPDATE_REPORT ),\
+    HID_COLLECTION      ( HID_COLLECTION_LOGICAL                      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_COUNT               ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_UPDATE_FLAGS        ),\
+      HID_LOGICAL_MIN   ( 0                                           ),\
+      HID_LOGICAL_MAX   ( 8                                           ),\
+      HID_REPORT_SIZE   ( 8                                           ),\
+      HID_REPORT_COUNT  ( 2                                           ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_ID                  ),\
+      HID_LOGICAL_MIN   ( 0                                           ),\
+      HID_LOGICAL_MAX_N ( 65535, 3                                    ),\
+      HID_REPORT_SIZE   ( 16                                          ),\
+      HID_REPORT_COUNT  ( 8                                           ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_LOGICAL_MIN   ( 0                                           ),\
+      HID_LOGICAL_MAX_N ( 255, 2                                      ),\
+      HID_REPORT_SIZE   ( 8                                           ),\
+      HID_REPORT_COUNT  ( 32                                          ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE      ),\
+    HID_COLLECTION_END ,\
+    /* Lamp Range Update Report */ \
+    HID_REPORT_ID       ( report_id + 4 ) \
+    HID_USAGE           ( HID_USAGE_LIGHTING_LAMP_RANGE_UPDATE_REPORT ),\
+    HID_COLLECTION      ( HID_COLLECTION_LOGICAL                      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_UPDATE_FLAGS        ),\
+      HID_LOGICAL_MIN   ( 0                                           ),\
+      HID_LOGICAL_MAX   ( 8                                           ),\
+      HID_REPORT_SIZE   ( 8                                           ),\
+      HID_REPORT_COUNT  ( 1                                           ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_ID_START            ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_LAMP_ID_END              ),\
+      HID_LOGICAL_MIN   ( 0                                           ),\
+      HID_LOGICAL_MAX_N ( 65535, 3                                    ),\
+      HID_REPORT_SIZE   ( 16                                          ),\
+      HID_REPORT_COUNT  ( 2                                           ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_RED_UPDATE_CHANNEL       ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_GREEN_UPDATE_CHANNEL     ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_BLUE_UPDATE_CHANNEL      ),\
+      HID_USAGE         ( HID_USAGE_LIGHTING_INTENSITY_UPDATE_CHANNEL ),\
+      HID_LOGICAL_MIN   ( 0                                           ),\
+      HID_LOGICAL_MAX_N ( 255, 2                                      ),\
+      HID_REPORT_SIZE   ( 8                                           ),\
+      HID_REPORT_COUNT  ( 4                                           ),\
+      HID_FEATURE       ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE      ),\
+    HID_COLLECTION_END ,\
+    /* Lamp Array Control Report */ \
+    HID_REPORT_ID      ( report_id + 5                                ) \
+    HID_USAGE          ( HID_USAGE_LIGHTING_LAMP_ARRAY_CONTROL_REPORT ),\
+    HID_COLLECTION     ( HID_COLLECTION_LOGICAL                       ),\
+      HID_USAGE        ( HID_USAGE_LIGHTING_AUTONOMOUS_MODE     ),\
+      HID_LOGICAL_MIN  ( 0                                      ),\
+      HID_LOGICAL_MAX  ( 1                                      ),\
+      HID_REPORT_SIZE  ( 8                                      ),\
+      HID_REPORT_COUNT ( 1                                      ),\
+      HID_FEATURE      ( HID_DATA | HID_VARIABLE | HID_ABSOLUTE ),\
+    HID_COLLECTION_END ,\
+  HID_COLLECTION_END \
+
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     hidd_init            (void);
+bool     hidd_deinit          (void);
 void     hidd_reset           (uint8_t rhport);
 uint16_t hidd_open            (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 bool     hidd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
@@ -390,4 +670,4 @@ bool     hidd_xfer_cb         (uint8_t rhport, uint8_t ep_addr, xfer_result_t ev
  }
 #endif
 
-#endif /* _TUSB_HID_DEVICE_H_ */
+#endif
diff --git a/src/class/hid/hid_host.c b/src/class/hid/hid_host.c
index ca745464c..a3cc7d6d7 100644
--- a/src/class/hid/hid_host.c
+++ b/src/class/hid/hid_host.c
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -29,362 +29,501 @@
 #if (CFG_TUH_ENABLED && CFG_TUH_HID)
 
 #include "host/usbh.h"
-#include "host/usbh_classdriver.h"
+#include "host/usbh_pvt.h"
 
 #include "hid_host.h"
 
+// Level where CFG_TUSB_DEBUG must be at least for this driver is logged
+#ifndef CFG_TUH_HID_LOG_LEVEL
+  #define CFG_TUH_HID_LOG_LEVEL   CFG_TUH_LOG_LEVEL
+#endif
+
+#define TU_LOG_DRV(...)   TU_LOG(CFG_TUH_HID_LOG_LEVEL, __VA_ARGS__)
+
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
-
-typedef struct
-{
+typedef struct {
+  uint8_t daddr;
   uint8_t itf_num;
   uint8_t ep_in;
   uint8_t ep_out;
 
+  bool mounted;           // Enumeration is complete
   uint8_t itf_protocol;   // None, Keyboard, Mouse
   uint8_t protocol_mode;  // Boot (0) or Report protocol (1)
 
-  uint8_t  report_desc_type;
+  uint8_t report_desc_type;
   uint16_t report_desc_len;
 
   uint16_t epin_size;
   uint16_t epout_size;
-
-  uint8_t epin_buf[CFG_TUH_HID_EPIN_BUFSIZE];
-  uint8_t epout_buf[CFG_TUH_HID_EPOUT_BUFSIZE];
 } hidh_interface_t;
 
-typedef struct
-{
-  uint8_t inst_count;
-  hidh_interface_t instances[CFG_TUH_HID];
-} hidh_device_t;
+typedef struct {
+  TUH_EPBUF_DEF(epin, CFG_TUH_HID_EPIN_BUFSIZE);
+  TUH_EPBUF_DEF(epout, CFG_TUH_HID_EPOUT_BUFSIZE);
+} hidh_epbuf_t;
+
+static hidh_interface_t _hidh_itf[CFG_TUH_HID];
+CFG_TUH_MEM_SECTION static hidh_epbuf_t _hidh_epbuf[CFG_TUH_HID];
 
-static hidh_device_t _hidh_dev[CFG_TUH_DEVICE_MAX];
+static uint8_t _hidh_default_protocol = HID_PROTOCOL_BOOT;
+
+//--------------------------------------------------------------------+
+// Helper
+//--------------------------------------------------------------------+
+TU_ATTR_ALWAYS_INLINE static inline hidh_interface_t* get_hid_itf(uint8_t daddr, uint8_t idx) {
+  TU_ASSERT(daddr > 0 && idx < CFG_TUH_HID, NULL);
+  hidh_interface_t* p_hid = &_hidh_itf[idx];
+  return (p_hid->daddr == daddr) ? p_hid : NULL;
+}
 
-//------------- Internal prototypes -------------//
+TU_ATTR_ALWAYS_INLINE static inline hidh_epbuf_t* get_hid_epbuf(uint8_t idx) {
+  return &_hidh_epbuf[idx];
+}
 
-// Get HID device & interface
-TU_ATTR_ALWAYS_INLINE static inline hidh_device_t* get_dev(uint8_t dev_addr);
-TU_ATTR_ALWAYS_INLINE static inline hidh_interface_t* get_instance(uint8_t dev_addr, uint8_t instance);
-static uint8_t get_instance_id_by_itfnum(uint8_t dev_addr, uint8_t itf);
-static uint8_t get_instance_id_by_epaddr(uint8_t dev_addr, uint8_t ep_addr);
+// Get instance ID by endpoint address
+static uint8_t get_idx_by_epaddr(uint8_t daddr, uint8_t ep_addr) {
+  for (uint8_t idx = 0; idx < CFG_TUH_HID; idx++) {
+    hidh_interface_t const* p_hid = &_hidh_itf[idx];
+    if (p_hid->daddr == daddr &&
+        (p_hid->ep_in == ep_addr || p_hid->ep_out == ep_addr)) {
+      return idx;
+    }
+  }
+  return TUSB_INDEX_INVALID_8;
+}
+
+static hidh_interface_t* find_new_itf(void) {
+  for (uint8_t i = 0; i < CFG_TUH_HID; i++) {
+    if (_hidh_itf[i].daddr == 0) return &_hidh_itf[i];
+  }
+  return NULL;
+}
 
 //--------------------------------------------------------------------+
 // Interface API
 //--------------------------------------------------------------------+
+uint8_t tuh_hid_itf_get_count(uint8_t daddr) {
+  uint8_t count = 0;
+  for (uint8_t i = 0; i < CFG_TUH_HID; i++) {
+    if (_hidh_itf[i].daddr == daddr) count++;
+  }
+  return count;
+}
+
+uint8_t tuh_hid_itf_get_total_count(void) {
+  uint8_t count = 0;
+  for (uint8_t i = 0; i < CFG_TUH_HID; i++) {
+    if (_hidh_itf[i].daddr != 0) count++;
+  }
+  return count;
+}
 
-uint8_t tuh_hid_instance_count(uint8_t dev_addr)
-{
-  return get_dev(dev_addr)->inst_count;
+bool tuh_hid_mounted(uint8_t daddr, uint8_t idx) {
+  hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
+  TU_VERIFY(p_hid);
+  return p_hid->mounted;
+}
+
+bool tuh_hid_itf_get_info(uint8_t daddr, uint8_t idx, tuh_itf_info_t* info) {
+  hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
+  TU_VERIFY(p_hid && info);
+
+  info->daddr = daddr;
+
+  // re-construct descriptor
+  tusb_desc_interface_t* desc = &info->desc;
+  desc->bLength = sizeof(tusb_desc_interface_t);
+  desc->bDescriptorType = TUSB_DESC_INTERFACE;
+
+  desc->bInterfaceNumber = p_hid->itf_num;
+  desc->bAlternateSetting = 0;
+  desc->bNumEndpoints = (uint8_t) ((p_hid->ep_in ? 1u : 0u) + (p_hid->ep_out ? 1u : 0u));
+  desc->bInterfaceClass = TUSB_CLASS_HID;
+  desc->bInterfaceSubClass = (p_hid->itf_protocol ? HID_SUBCLASS_BOOT : HID_SUBCLASS_NONE);
+  desc->bInterfaceProtocol = p_hid->itf_protocol;
+  desc->iInterface = 0; // not used yet
+
+  return true;
 }
 
-bool tuh_hid_mounted(uint8_t dev_addr, uint8_t instance)
-{
-  hidh_interface_t* hid_itf = get_instance(dev_addr, instance);
-  return (hid_itf->ep_in != 0) || (hid_itf->ep_out != 0);
+uint8_t tuh_hid_itf_get_index(uint8_t daddr, uint8_t itf_num) {
+  for (uint8_t idx = 0; idx < CFG_TUH_HID; idx++) {
+    hidh_interface_t const* p_hid = &_hidh_itf[idx];
+    if (p_hid->daddr == daddr && p_hid->itf_num == itf_num) return idx;
+  }
+
+  return TUSB_INDEX_INVALID_8;
 }
 
-uint8_t tuh_hid_interface_protocol(uint8_t dev_addr, uint8_t instance)
-{
-  hidh_interface_t* hid_itf = get_instance(dev_addr, instance);
-  return hid_itf->itf_protocol;
+uint8_t tuh_hid_interface_protocol(uint8_t daddr, uint8_t idx) {
+  hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
+  return p_hid ? p_hid->itf_protocol : 0;
 }
 
 //--------------------------------------------------------------------+
 // Control Endpoint API
 //--------------------------------------------------------------------+
-
-uint8_t tuh_hid_get_protocol(uint8_t dev_addr, uint8_t instance)
-{
-  hidh_interface_t* hid_itf = get_instance(dev_addr, instance);
-  return hid_itf->protocol_mode;
+uint8_t tuh_hid_get_protocol(uint8_t daddr, uint8_t idx) {
+  hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
+  return p_hid ? p_hid->protocol_mode : 0;
 }
 
-static void set_protocol_complete(tuh_xfer_t* xfer)
-{
-  uint8_t const itf_num     = (uint8_t) tu_le16toh(xfer->setup->wIndex);
-  uint8_t const daddr       = xfer->daddr;
-  uint8_t const instance    = get_instance_id_by_itfnum(daddr, itf_num);
-  hidh_interface_t* hid_itf = get_instance(daddr, instance);
+static void set_protocol_complete(tuh_xfer_t* xfer) {
+  uint8_t const itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
+  uint8_t const daddr = xfer->daddr;
+  uint8_t const idx = tuh_hid_itf_get_index(daddr, itf_num);
+
+  hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
+  TU_VERIFY(p_hid,);
 
-  if (XFER_RESULT_SUCCESS == xfer->result)
-  {
-    hid_itf->protocol_mode = (uint8_t) tu_le16toh(xfer->setup->wValue);
+  if (XFER_RESULT_SUCCESS == xfer->result) {
+    p_hid->protocol_mode = (uint8_t) tu_le16toh(xfer->setup->wValue);
   }
 
-  if (tuh_hid_set_protocol_complete_cb)
-  {
-    tuh_hid_set_protocol_complete_cb(daddr, instance, hid_itf->protocol_mode);
+  if (tuh_hid_set_protocol_complete_cb) {
+    tuh_hid_set_protocol_complete_cb(daddr, idx, p_hid->protocol_mode);
   }
 }
 
+void tuh_hid_set_default_protocol(uint8_t protocol) {
+  _hidh_default_protocol = protocol;
+}
 
-static bool _hidh_set_protocol(uint8_t dev_addr, uint8_t itf_num, uint8_t protocol, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
-  TU_LOG2("HID Set Protocol = %d\r\n", protocol);
+static bool _hidh_set_protocol(uint8_t daddr, uint8_t itf_num, uint8_t protocol,
+                               tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  TU_LOG_DRV("HID Set Protocol = %d\r\n", protocol);
 
-  tusb_control_request_t const request =
-  {
-    .bmRequestType_bit =
-    {
-      .recipient = TUSB_REQ_RCPT_INTERFACE,
-      .type      = TUSB_REQ_TYPE_CLASS,
-      .direction = TUSB_DIR_OUT
-    },
-    .bRequest = HID_REQ_CONTROL_SET_PROTOCOL,
-    .wValue   = protocol,
-    .wIndex   = itf_num,
-    .wLength  = 0
+  tusb_control_request_t const request = {
+      .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_INTERFACE,
+          .type      = TUSB_REQ_TYPE_CLASS,
+          .direction = TUSB_DIR_OUT
+      },
+      .bRequest = HID_REQ_CONTROL_SET_PROTOCOL,
+      .wValue   = protocol,
+      .wIndex   = itf_num,
+      .wLength  = 0
   };
 
-  tuh_xfer_t xfer =
-  {
-    .daddr       = dev_addr,
-    .ep_addr     = 0,
-    .setup       = &request,
-    .buffer      = NULL,
-    .complete_cb = complete_cb,
-    .user_data   = user_data
+  tuh_xfer_t xfer = {
+      .daddr       = daddr,
+      .ep_addr     = 0,
+      .setup       = &request,
+      .buffer      = NULL,
+      .complete_cb = complete_cb,
+      .user_data   = user_data
   };
 
-  TU_ASSERT( tuh_control_xfer(&xfer) );
-  return true;
+  return tuh_control_xfer(&xfer);
 }
 
-bool tuh_hid_set_protocol(uint8_t dev_addr, uint8_t instance, uint8_t protocol)
-{
-  hidh_interface_t* hid_itf = get_instance(dev_addr, instance);
-  TU_VERIFY(hid_itf->itf_protocol != HID_ITF_PROTOCOL_NONE);
+bool tuh_hid_set_protocol(uint8_t daddr, uint8_t idx, uint8_t protocol) {
+  hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
+  TU_VERIFY(p_hid && p_hid->itf_protocol != HID_ITF_PROTOCOL_NONE);
 
-  return _hidh_set_protocol(dev_addr, hid_itf->itf_num, protocol, set_protocol_complete, 0);
+  return _hidh_set_protocol(daddr, p_hid->itf_num, protocol, set_protocol_complete, 0);
 }
 
-static void set_report_complete(tuh_xfer_t* xfer)
-{
-  TU_LOG2("HID Set Report complete\r\n");
+static void get_report_complete(tuh_xfer_t* xfer) {
+  TU_LOG_DRV("HID Get Report complete\r\n");
 
-  if (tuh_hid_set_report_complete_cb)
-  {
-    uint8_t const itf_num     = (uint8_t) tu_le16toh(xfer->setup->wIndex);
-    uint8_t const instance    = get_instance_id_by_itfnum(xfer->daddr, itf_num);
+  if (tuh_hid_get_report_complete_cb) {
+    uint8_t const itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
+    uint8_t const idx = tuh_hid_itf_get_index(xfer->daddr, itf_num);
 
     uint8_t const report_type = tu_u16_high(xfer->setup->wValue);
-    uint8_t const report_id   = tu_u16_low(xfer->setup->wValue);
+    uint8_t const report_id = tu_u16_low(xfer->setup->wValue);
 
-    tuh_hid_set_report_complete_cb(xfer->daddr, instance, report_id, report_type,
+    tuh_hid_get_report_complete_cb(xfer->daddr, idx, report_id, report_type,
                                    (xfer->result == XFER_RESULT_SUCCESS) ? xfer->setup->wLength : 0);
   }
 }
 
-bool tuh_hid_set_report(uint8_t dev_addr, uint8_t instance, uint8_t report_id, uint8_t report_type, void* report, uint16_t len)
-{
-  hidh_interface_t* hid_itf = get_instance(dev_addr, instance);
-  TU_LOG2("HID Set Report: id = %u, type = %u, len = %u\r\n", report_id, report_type, len);
+bool tuh_hid_get_report(uint8_t daddr, uint8_t idx, uint8_t report_id, uint8_t report_type, void* report, uint16_t len) {
+  hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
+  TU_VERIFY(p_hid);
+  TU_LOG_DRV("HID Get Report: id = %u, type = %u, len = %u\r\n", report_id, report_type, len);
 
-  tusb_control_request_t const request =
-  {
-    .bmRequestType_bit =
-    {
-      .recipient = TUSB_REQ_RCPT_INTERFACE,
-      .type      = TUSB_REQ_TYPE_CLASS,
-      .direction = TUSB_DIR_OUT
-    },
-    .bRequest = HID_REQ_CONTROL_SET_REPORT,
-    .wValue   = tu_u16(report_type, report_id),
-    .wIndex   = hid_itf->itf_num,
-    .wLength  = len
+  tusb_control_request_t const request = {
+      .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_INTERFACE,
+          .type      = TUSB_REQ_TYPE_CLASS,
+          .direction = TUSB_DIR_IN
+      },
+      .bRequest = HID_REQ_CONTROL_GET_REPORT,
+      .wValue   = tu_htole16(tu_u16(report_type, report_id)),
+      .wIndex   = tu_htole16((uint16_t) p_hid->itf_num),
+      .wLength  = len
   };
 
-  tuh_xfer_t xfer =
-  {
-    .daddr       = dev_addr,
-    .ep_addr     = 0,
-    .setup       = &request,
-    .buffer      = report,
-    .complete_cb = set_report_complete,
-    .user_data   = 0
+  tuh_xfer_t xfer = {
+      .daddr       = daddr,
+      .ep_addr     = 0,
+      .setup       = &request,
+      .buffer      = report,
+      .complete_cb = get_report_complete,
+      .user_data   = 0
   };
 
-  TU_ASSERT( tuh_control_xfer(&xfer) );
-  return true;
+  return tuh_control_xfer(&xfer);
 }
 
-static bool _hidh_set_idle(uint8_t dev_addr, uint8_t itf_num, uint16_t idle_rate, tuh_xfer_cb_t complete_cb, uintptr_t user_data)
-{
-  // SET IDLE request, device can stall if not support this request
-  TU_LOG2("HID Set Idle \r\n");
-  tusb_control_request_t const request =
-  {
-    .bmRequestType_bit =
-    {
-      .recipient = TUSB_REQ_RCPT_INTERFACE,
-      .type      = TUSB_REQ_TYPE_CLASS,
-      .direction = TUSB_DIR_OUT
-    },
-    .bRequest = HID_REQ_CONTROL_SET_IDLE,
-    .wValue   = idle_rate,
-    .wIndex   = itf_num,
-    .wLength  = 0
+static void set_report_complete(tuh_xfer_t* xfer) {
+  TU_LOG_DRV("HID Set Report complete\r\n");
+
+  if (tuh_hid_set_report_complete_cb) {
+    uint8_t const itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
+    uint8_t const idx = tuh_hid_itf_get_index(xfer->daddr, itf_num);
+
+    uint8_t const report_type = tu_u16_high(xfer->setup->wValue);
+    uint8_t const report_id = tu_u16_low(xfer->setup->wValue);
+
+    tuh_hid_set_report_complete_cb(xfer->daddr, idx, report_id, report_type,
+                                   (xfer->result == XFER_RESULT_SUCCESS) ? xfer->setup->wLength : 0);
+  }
+}
+
+bool tuh_hid_set_report(uint8_t daddr, uint8_t idx, uint8_t report_id, uint8_t report_type, void* report, uint16_t len) {
+  hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
+  TU_VERIFY(p_hid);
+  TU_LOG_DRV("HID Set Report: id = %u, type = %u, len = %u\r\n", report_id, report_type, len);
+
+  tusb_control_request_t const request = {
+      .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_INTERFACE,
+          .type      = TUSB_REQ_TYPE_CLASS,
+          .direction = TUSB_DIR_OUT
+      },
+      .bRequest = HID_REQ_CONTROL_SET_REPORT,
+      .wValue   = tu_htole16(tu_u16(report_type, report_id)),
+      .wIndex   = tu_htole16((uint16_t) p_hid->itf_num),
+      .wLength  = len
   };
 
-  tuh_xfer_t xfer =
-  {
-    .daddr       = dev_addr,
-    .ep_addr     = 0,
-    .setup       = &request,
-    .buffer      = NULL,
-    .complete_cb = complete_cb,
-    .user_data   = user_data
+  tuh_xfer_t xfer = {
+      .daddr       = daddr,
+      .ep_addr     = 0,
+      .setup       = &request,
+      .buffer      = report,
+      .complete_cb = set_report_complete,
+      .user_data   = 0
   };
 
-  TU_ASSERT( tuh_control_xfer(&xfer) );
+  return tuh_control_xfer(&xfer);
+}
 
-  return true;
+static bool _hidh_set_idle(uint8_t daddr, uint8_t itf_num, uint16_t idle_rate,
+                           tuh_xfer_cb_t complete_cb, uintptr_t user_data) {
+  // SET IDLE request, device can stall if not support this request
+  TU_LOG_DRV("HID Set Idle \r\n");
+
+  tusb_control_request_t const request = {
+      .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_INTERFACE,
+          .type      = TUSB_REQ_TYPE_CLASS,
+          .direction = TUSB_DIR_OUT
+      },
+      .bRequest = HID_REQ_CONTROL_SET_IDLE,
+      .wValue   = tu_htole16(idle_rate),
+      .wIndex   = tu_htole16((uint16_t) itf_num),
+      .wLength  = 0
+  };
+
+  tuh_xfer_t xfer = {
+      .daddr       = daddr,
+      .ep_addr     = 0,
+      .setup       = &request,
+      .buffer      = NULL,
+      .complete_cb = complete_cb,
+      .user_data   = user_data
+  };
+
+  return tuh_control_xfer(&xfer);
 }
 
 //--------------------------------------------------------------------+
 // Interrupt Endpoint API
 //--------------------------------------------------------------------+
 
-bool tuh_hid_receive_report(uint8_t dev_addr, uint8_t instance)
-{
-  hidh_interface_t* hid_itf = get_instance(dev_addr, instance);
+// Check if HID interface is ready to receive report
+bool tuh_hid_receive_ready(uint8_t dev_addr, uint8_t idx) {
+  hidh_interface_t* p_hid = get_hid_itf(dev_addr, idx);
+  TU_VERIFY(p_hid);
+  return !usbh_edpt_busy(dev_addr, p_hid->ep_in);
+}
+
+bool tuh_hid_receive_report(uint8_t daddr, uint8_t idx) {
+  hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
+  TU_VERIFY(p_hid);
+  hidh_epbuf_t* epbuf = get_hid_epbuf(idx);
 
   // claim endpoint
-  TU_VERIFY( usbh_edpt_claim(dev_addr, hid_itf->ep_in) );
+  TU_VERIFY(usbh_edpt_claim(daddr, p_hid->ep_in));
 
-  if ( !usbh_edpt_xfer(dev_addr, hid_itf->ep_in, hid_itf->epin_buf, hid_itf->epin_size) )
-  {
-    usbh_edpt_claim(dev_addr, hid_itf->ep_in);
+  if (!usbh_edpt_xfer(daddr, p_hid->ep_in, epbuf->epin, p_hid->epin_size)) {
+    usbh_edpt_release(daddr, p_hid->ep_in);
     return false;
   }
 
   return true;
 }
+bool tuh_hid_receive_abort(uint8_t dev_addr, uint8_t idx) {
+  hidh_interface_t* p_hid = get_hid_itf(dev_addr, idx);
+  TU_VERIFY(p_hid);
+  return tuh_edpt_abort_xfer(dev_addr, p_hid->ep_in);
+}
+
+bool tuh_hid_send_ready(uint8_t dev_addr, uint8_t idx) {
+  hidh_interface_t* p_hid = get_hid_itf(dev_addr, idx);
+  TU_VERIFY(p_hid);
+  return !usbh_edpt_busy(dev_addr, p_hid->ep_out);
+}
 
-//bool tuh_n_hid_n_ready(uint8_t dev_addr, uint8_t instance)
-//{
-//  TU_VERIFY(tuh_n_hid_n_mounted(dev_addr, instance));
-//
-//  hidh_interface_t* hid_itf = get_instance(dev_addr, instance);
-//  return !usbh_edpt_busy(dev_addr, hid_itf->ep_in);
-//}
+bool tuh_hid_send_report(uint8_t daddr, uint8_t idx, uint8_t report_id, const void* report, uint16_t len) {
+  TU_LOG_DRV("HID Send Report %d\r\n", report_id);
 
-//void tuh_hid_send_report(uint8_t dev_addr, uint8_t instance, uint8_t report_id, uint8_t const* report, uint16_t len);
+  hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
+  TU_VERIFY(p_hid);
+  hidh_epbuf_t* epbuf = get_hid_epbuf(idx);
+
+  if (p_hid->ep_out == 0) {
+    // This HID does not have an out endpoint (other than control)
+    return false;
+  } else if (len > CFG_TUH_HID_EPOUT_BUFSIZE ||
+             (report_id != 0 && len > (CFG_TUH_HID_EPOUT_BUFSIZE - 1))) {
+    // ep_out buffer is not large enough to hold contents
+    return false;
+  }
+
+  // claim endpoint
+  TU_VERIFY(usbh_edpt_claim(daddr, p_hid->ep_out));
+
+  if (report_id == 0) {
+    // No report ID in transmission
+    memcpy(&epbuf->epout[0], report, len);
+  } else {
+    epbuf->epout[0] = report_id;
+    memcpy(&epbuf->epout[1], report, len);
+    ++len; // 1 more byte for report_id
+  }
+
+  TU_LOG3_MEM(epbuf->epout, len, 2);
+
+  if (!usbh_edpt_xfer(daddr, p_hid->ep_out, epbuf->epout, len)) {
+    usbh_edpt_release(daddr, p_hid->ep_out);
+    return false;
+  }
+
+  return true;
+}
 
 //--------------------------------------------------------------------+
 // USBH API
 //--------------------------------------------------------------------+
-void hidh_init(void)
-{
-  tu_memclr(_hidh_dev, sizeof(_hidh_dev));
+bool hidh_init(void) {
+  TU_LOG_DRV("sizeof(hidh_interface_t) = %u\r\n", sizeof(hidh_interface_t));
+  tu_memclr(_hidh_itf, sizeof(_hidh_itf));
+  return true;
 }
 
-bool hidh_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
-{
+bool hidh_deinit(void) {
+  return true;
+}
+
+bool hidh_xfer_cb(uint8_t daddr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) {
   (void) result;
 
   uint8_t const dir = tu_edpt_dir(ep_addr);
-  uint8_t const instance = get_instance_id_by_epaddr(dev_addr, ep_addr);
-  hidh_interface_t* hid_itf = get_instance(dev_addr, instance);
+  uint8_t const idx = get_idx_by_epaddr(daddr, ep_addr);
+
+  hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
+  TU_VERIFY(p_hid);
+  hidh_epbuf_t* epbuf = get_hid_epbuf(idx);
 
-  if ( dir == TUSB_DIR_IN )
-  {
-    TU_LOG2("  Get Report callback (%u, %u)\r\n", dev_addr, instance);
-    TU_LOG3_MEM(hid_itf->epin_buf, xferred_bytes, 2);
-    tuh_hid_report_received_cb(dev_addr, instance, hid_itf->epin_buf, (uint16_t) xferred_bytes);
-  }else
-  {
-    if (tuh_hid_report_sent_cb) tuh_hid_report_sent_cb(dev_addr, instance, hid_itf->epout_buf, (uint16_t) xferred_bytes);
+  if (dir == TUSB_DIR_IN) {
+    TU_LOG_DRV("  Get Report callback (%u, %u)\r\n", daddr, idx);
+    TU_LOG3_MEM(epbuf->epin, xferred_bytes, 2);
+    tuh_hid_report_received_cb(daddr, idx, epbuf->epin, (uint16_t) xferred_bytes);
+  } else {
+    if (tuh_hid_report_sent_cb) {
+      tuh_hid_report_sent_cb(daddr, idx, epbuf->epout, (uint16_t) xferred_bytes);
+    }
   }
 
   return true;
 }
 
-void hidh_close(uint8_t dev_addr)
-{
-  TU_VERIFY(dev_addr <= CFG_TUH_DEVICE_MAX, );
-
-  hidh_device_t* hid_dev = get_dev(dev_addr);
-
-  if (tuh_hid_umount_cb)
-  {
-    for (uint8_t inst = 0; inst < hid_dev->inst_count; inst++ ) tuh_hid_umount_cb(dev_addr, inst);
+void hidh_close(uint8_t daddr) {
+  for (uint8_t i = 0; i < CFG_TUH_HID; i++) {
+    hidh_interface_t* p_hid = &_hidh_itf[i];
+    if (p_hid->daddr == daddr) {
+      TU_LOG_DRV("  HIDh close addr = %u index = %u\r\n", daddr, i);
+      if (tuh_hid_umount_cb) tuh_hid_umount_cb(daddr, i);
+      tu_memclr(p_hid, sizeof(hidh_interface_t));
+    }
   }
-
-  tu_memclr(hid_dev, sizeof(hidh_device_t));
 }
 
 //--------------------------------------------------------------------+
 // Enumeration
 //--------------------------------------------------------------------+
 
-bool hidh_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len)
-{
+bool hidh_open(uint8_t rhport, uint8_t daddr, tusb_desc_interface_t const* desc_itf, uint16_t max_len) {
   (void) rhport;
   (void) max_len;
 
   TU_VERIFY(TUSB_CLASS_HID == desc_itf->bInterfaceClass);
-
-  TU_LOG2("[%u] HID opening Interface %u\r\n", dev_addr, desc_itf->bInterfaceNumber);
+  TU_LOG_DRV("[%u] HID opening Interface %u\r\n", daddr, desc_itf->bInterfaceNumber);
 
   // len = interface + hid + n*endpoints
   uint16_t const drv_len = (uint16_t) (sizeof(tusb_desc_interface_t) + sizeof(tusb_hid_descriptor_hid_t) +
                                        desc_itf->bNumEndpoints * sizeof(tusb_desc_endpoint_t));
   TU_ASSERT(max_len >= drv_len);
-
-  uint8_t const *p_desc = (uint8_t const *) desc_itf;
+  uint8_t const* p_desc = (uint8_t const*) desc_itf;
 
   //------------- HID descriptor -------------//
   p_desc = tu_desc_next(p_desc);
-  tusb_hid_descriptor_hid_t const *desc_hid = (tusb_hid_descriptor_hid_t const *) p_desc;
+  tusb_hid_descriptor_hid_t const* desc_hid = (tusb_hid_descriptor_hid_t const*) p_desc;
   TU_ASSERT(HID_DESC_TYPE_HID == desc_hid->bDescriptorType);
 
-  // not enough interface, try to increase CFG_TUH_HID
-  // TODO multiple devices
-  hidh_device_t* hid_dev = get_dev(dev_addr);
-  TU_ASSERT(hid_dev->inst_count < CFG_TUH_HID, 0);
-
-  hidh_interface_t* hid_itf = get_instance(dev_addr, hid_dev->inst_count);  
+  hidh_interface_t* p_hid = find_new_itf();
+  TU_ASSERT(p_hid); // not enough interface, try to increase CFG_TUH_HID
+  p_hid->daddr = daddr;
 
   //------------- Endpoint Descriptors -------------//
   p_desc = tu_desc_next(p_desc);
-  tusb_desc_endpoint_t const * desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+  tusb_desc_endpoint_t const* desc_ep = (tusb_desc_endpoint_t const*) p_desc;
 
-  for(int i = 0; i < desc_itf->bNumEndpoints; i++)
-  {
+  for (int i = 0; i < desc_itf->bNumEndpoints; i++) {
     TU_ASSERT(TUSB_DESC_ENDPOINT == desc_ep->bDescriptorType);
-    TU_ASSERT( tuh_edpt_open(dev_addr, desc_ep) );
+    TU_ASSERT(tuh_edpt_open(daddr, desc_ep));
 
-    if(tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN)
-    {
-      hid_itf->ep_in     = desc_ep->bEndpointAddress;
-      hid_itf->epin_size = tu_edpt_packet_size(desc_ep);
-    }
-    else
-    {
-      hid_itf->ep_out     = desc_ep->bEndpointAddress;
-      hid_itf->epout_size = tu_edpt_packet_size(desc_ep);
+    if (tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN) {
+      p_hid->ep_in = desc_ep->bEndpointAddress;
+      p_hid->epin_size = tu_edpt_packet_size(desc_ep);
+    } else {
+      p_hid->ep_out = desc_ep->bEndpointAddress;
+      p_hid->epout_size = tu_edpt_packet_size(desc_ep);
     }
 
     p_desc = tu_desc_next(p_desc);
-    desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+    desc_ep = (tusb_desc_endpoint_t const*) p_desc;
   }
 
-  hid_dev->inst_count++;
-
-  hid_itf->itf_num   = desc_itf->bInterfaceNumber;
+  p_hid->itf_num = desc_itf->bInterfaceNumber;
 
   // Assume bNumDescriptors = 1
-  hid_itf->report_desc_type = desc_hid->bReportType;
-  hid_itf->report_desc_len  = tu_unaligned_read16(&desc_hid->wReportLength);
+  p_hid->report_desc_type = desc_hid->bReportType;
+  p_hid->report_desc_len = tu_unaligned_read16(&desc_hid->wReportLength);
 
   // Per HID Specs: default is Report protocol, though we will force Boot protocol when set_config
-  hid_itf->protocol_mode = HID_PROTOCOL_BOOT;
-  if ( HID_SUBCLASS_BOOT == desc_itf->bInterfaceSubClass ) hid_itf->itf_protocol = desc_itf->bInterfaceProtocol;
+  p_hid->protocol_mode = _hidh_default_protocol;
+  if (HID_SUBCLASS_BOOT == desc_itf->bInterfaceSubClass) {
+    p_hid->itf_protocol = desc_itf->bInterfaceProtocol;
+  }
 
   return true;
 }
@@ -400,18 +539,17 @@ enum {
   CONFIG_COMPLETE
 };
 
-static void config_driver_mount_complete(uint8_t dev_addr, uint8_t instance, uint8_t const* desc_report, uint16_t desc_len);
+static void config_driver_mount_complete(uint8_t daddr, uint8_t idx, uint8_t const* desc_report, uint16_t desc_len);
 static void process_set_config(tuh_xfer_t* xfer);
 
-bool hidh_set_config(uint8_t dev_addr, uint8_t itf_num)
-{
+bool hidh_set_config(uint8_t daddr, uint8_t itf_num) {
   tusb_control_request_t request;
   request.wIndex = tu_htole16((uint16_t) itf_num);
 
   tuh_xfer_t xfer;
-  xfer.daddr     = dev_addr;
-  xfer.result    = XFER_RESULT_SUCCESS;
-  xfer.setup     = &request;
+  xfer.daddr = daddr;
+  xfer.result = XFER_RESULT_SUCCESS;
+  xfer.setup = &request;
   xfer.user_data = CONFG_SET_IDLE;
 
   // fake request to kick-off the set config process
@@ -420,94 +558,93 @@ bool hidh_set_config(uint8_t dev_addr, uint8_t itf_num)
   return true;
 }
 
-static void process_set_config(tuh_xfer_t* xfer)
-{
-  // Stall is a valid response for SET_IDLE, therefore we could ignore its result
-  if ( xfer->setup->bRequest != HID_REQ_CONTROL_SET_IDLE )
-  {
-    TU_ASSERT(xfer->result == XFER_RESULT_SUCCESS, );
+static void process_set_config(tuh_xfer_t* xfer) {
+  // Stall is a valid response for SET_IDLE, sometime SET_PROTOCOL as well
+  // therefore we could ignore its result
+  if (!(xfer->setup->bRequest == HID_REQ_CONTROL_SET_IDLE ||
+        xfer->setup->bRequest == HID_REQ_CONTROL_SET_PROTOCOL)) {
+    TU_ASSERT(xfer->result == XFER_RESULT_SUCCESS,);
   }
 
   uintptr_t const state = xfer->user_data;
   uint8_t const itf_num = (uint8_t) tu_le16toh(xfer->setup->wIndex);
-  uint8_t const daddr   = xfer->daddr;
+  uint8_t const daddr = xfer->daddr;
 
-  uint8_t const instance    = get_instance_id_by_itfnum(daddr, itf_num);
-  hidh_interface_t* hid_itf = get_instance(daddr, instance);
+  uint8_t const idx = tuh_hid_itf_get_index(daddr, itf_num);
+  hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
+  TU_VERIFY(p_hid,);
 
-  switch(state)
-  {
-    case CONFG_SET_IDLE:
-    {
+  switch (state) {
+    case CONFG_SET_IDLE: {
       // Idle rate = 0 mean only report when there is changes
       const uint16_t idle_rate = 0;
-      const uintptr_t next_state = (hid_itf->itf_protocol != HID_ITF_PROTOCOL_NONE) ? CONFIG_SET_PROTOCOL : CONFIG_GET_REPORT_DESC;
+      const uintptr_t next_state = (p_hid->itf_protocol != HID_ITF_PROTOCOL_NONE)
+                                   ? CONFIG_SET_PROTOCOL : CONFIG_GET_REPORT_DESC;
       _hidh_set_idle(daddr, itf_num, idle_rate, process_set_config, next_state);
+      break;
     }
-    break;
 
     case CONFIG_SET_PROTOCOL:
-      _hidh_set_protocol(daddr, hid_itf->itf_num, HID_PROTOCOL_BOOT, process_set_config, CONFIG_GET_REPORT_DESC);
-    break;
+      _hidh_set_protocol(daddr, p_hid->itf_num, _hidh_default_protocol, process_set_config, CONFIG_GET_REPORT_DESC);
+      break;
 
     case CONFIG_GET_REPORT_DESC:
       // Get Report Descriptor if possible
       // using usbh enumeration buffer since report descriptor can be very long
-      if( hid_itf->report_desc_len > CFG_TUH_ENUMERATION_BUFSIZE )
-      {
-        TU_LOG2("HID Skip Report Descriptor since it is too large %u bytes\r\n", hid_itf->report_desc_len);
+      if (p_hid->report_desc_len > CFG_TUH_ENUMERATION_BUFSIZE) {
+        TU_LOG_DRV("HID Skip Report Descriptor since it is too large %u bytes\r\n", p_hid->report_desc_len);
 
         // Driver is mounted without report descriptor
-        config_driver_mount_complete(daddr, instance, NULL, 0);
-      }else
-      {
-        tuh_descriptor_get_hid_report(daddr, itf_num, hid_itf->report_desc_type, 0, usbh_get_enum_buf(), hid_itf->report_desc_len, process_set_config, CONFIG_COMPLETE);
+        config_driver_mount_complete(daddr, idx, NULL, 0);
+      } else {
+        tuh_descriptor_get_hid_report(daddr, itf_num, p_hid->report_desc_type, 0,
+                                      usbh_get_enum_buf(), p_hid->report_desc_len,
+                                      process_set_config, CONFIG_COMPLETE);
       }
       break;
 
-    case CONFIG_COMPLETE:
-    {
+    case CONFIG_COMPLETE: {
       uint8_t const* desc_report = usbh_get_enum_buf();
-      uint16_t const desc_len    = tu_le16toh(xfer->setup->wLength);
+      uint16_t const desc_len = tu_le16toh(xfer->setup->wLength);
 
-      config_driver_mount_complete(daddr, instance, desc_report, desc_len);
+      config_driver_mount_complete(daddr, idx, desc_report, desc_len);
+      break;
     }
-    break;
 
-    default: break;
+    default:
+      break;
   }
 }
 
-static void config_driver_mount_complete(uint8_t dev_addr, uint8_t instance, uint8_t const* desc_report, uint16_t desc_len)
-{
-  hidh_interface_t* hid_itf = get_instance(dev_addr, instance);
+static void config_driver_mount_complete(uint8_t daddr, uint8_t idx, uint8_t const* desc_report, uint16_t desc_len) {
+  hidh_interface_t* p_hid = get_hid_itf(daddr, idx);
+  TU_VERIFY(p_hid,);
+  p_hid->mounted = true;
 
   // enumeration is complete
-  tuh_hid_mount_cb(dev_addr, instance, desc_report, desc_len);
+  if (tuh_hid_mount_cb) tuh_hid_mount_cb(daddr, idx, desc_report, desc_len);
 
   // notify usbh that driver enumeration is complete
-  usbh_driver_set_config_complete(dev_addr, hid_itf->itf_num);
+  usbh_driver_set_config_complete(daddr, p_hid->itf_num);
 }
 
 //--------------------------------------------------------------------+
 // Report Descriptor Parser
 //--------------------------------------------------------------------+
 
-uint8_t tuh_hid_parse_report_descriptor(tuh_hid_report_info_t* report_info_arr, uint8_t arr_count, uint8_t const* desc_report, uint16_t desc_len)
-{
+uint8_t tuh_hid_parse_report_descriptor(tuh_hid_report_info_t* report_info_arr, uint8_t arr_count,
+                                        uint8_t const* desc_report, uint16_t desc_len) {
   // Report Item 6.2.2.2 USB HID 1.11
-  union TU_ATTR_PACKED
-  {
+  union TU_ATTR_PACKED {
     uint8_t byte;
-    struct TU_ATTR_PACKED
-    {
-        uint8_t size : 2;
-        uint8_t type : 2;
-        uint8_t tag  : 4;
+    struct TU_ATTR_PACKED {
+      uint8_t size : 2;
+      uint8_t type : 2;
+      uint8_t tag : 4;
     };
   } header;
 
-  tu_memclr(report_info_arr, arr_count*sizeof(tuh_hid_report_info_t));
+  tu_memclr(report_info_arr, arr_count * sizeof(tuh_hid_report_info_t));
 
   uint8_t report_num = 0;
   tuh_hid_report_info_t* info = report_info_arr;
@@ -517,160 +654,111 @@ uint8_t tuh_hid_parse_report_descriptor(tuh_hid_report_info_t* report_info_arr,
 //  uint8_t ri_report_size = 0;
 
   uint8_t ri_collection_depth = 0;
-
-  while(desc_len && report_num < arr_count)
-  {
+  while (desc_len && report_num < arr_count) {
     header.byte = *desc_report++;
     desc_len--;
 
-    uint8_t const tag  = header.tag;
+    uint8_t const tag = header.tag;
     uint8_t const type = header.type;
     uint8_t const size = header.size;
 
     uint8_t const data8 = desc_report[0];
 
     TU_LOG(3, "tag = %d, type = %d, size = %d, data = ", tag, type, size);
-    for(uint32_t i=0; i<size; i++) TU_LOG(3, "%02X ", desc_report[i]);
+    for (uint32_t i = 0; i < size; i++) {
+      TU_LOG(3, "%02X ", desc_report[i]);
+    }
     TU_LOG(3, "\r\n");
 
-    switch(type)
-    {
+    switch (type) {
       case RI_TYPE_MAIN:
-        switch (tag)
-        {
+        switch (tag) {
           case RI_MAIN_INPUT: break;
           case RI_MAIN_OUTPUT: break;
           case RI_MAIN_FEATURE: break;
-
           case RI_MAIN_COLLECTION:
             ri_collection_depth++;
-          break;
+            break;
 
           case RI_MAIN_COLLECTION_END:
             ri_collection_depth--;
-            if (ri_collection_depth == 0)
-            {
+            if (ri_collection_depth == 0) {
               info++;
               report_num++;
             }
-          break;
+            break;
 
-          default: break;
+          default:break;
         }
-      break;
+        break;
 
       case RI_TYPE_GLOBAL:
-        switch(tag)
-        {
+        switch (tag) {
           case RI_GLOBAL_USAGE_PAGE:
             // only take in account the "usage page" before REPORT ID
-            if ( ri_collection_depth == 0 ) memcpy(&info->usage_page, desc_report, size);
-          break;
+            if (ri_collection_depth == 0) memcpy(&info->usage_page, desc_report, size);
+            break;
 
-          case RI_GLOBAL_LOGICAL_MIN   : break;
-          case RI_GLOBAL_LOGICAL_MAX   : break;
-          case RI_GLOBAL_PHYSICAL_MIN  : break;
-          case RI_GLOBAL_PHYSICAL_MAX  : break;
+          case RI_GLOBAL_LOGICAL_MIN: break;
+          case RI_GLOBAL_LOGICAL_MAX: break;
+          case RI_GLOBAL_PHYSICAL_MIN: break;
+          case RI_GLOBAL_PHYSICAL_MAX: break;
 
           case RI_GLOBAL_REPORT_ID:
             info->report_id = data8;
-          break;
+            break;
 
           case RI_GLOBAL_REPORT_SIZE:
 //            ri_report_size = data8;
-          break;
+            break;
 
           case RI_GLOBAL_REPORT_COUNT:
 //            ri_report_count = data8;
-          break;
+            break;
 
-          case RI_GLOBAL_UNIT_EXPONENT : break;
-          case RI_GLOBAL_UNIT          : break;
-          case RI_GLOBAL_PUSH          : break;
-          case RI_GLOBAL_POP           : break;
+          case RI_GLOBAL_UNIT_EXPONENT: break;
+          case RI_GLOBAL_UNIT: break;
+          case RI_GLOBAL_PUSH: break;
+          case RI_GLOBAL_POP: break;
 
           default: break;
         }
-      break;
+        break;
 
       case RI_TYPE_LOCAL:
-        switch(tag)
-        {
+        switch (tag) {
           case RI_LOCAL_USAGE:
             // only take in account the "usage" before starting REPORT ID
-            if ( ri_collection_depth == 0 ) info->usage = data8;
-          break;
+            if (ri_collection_depth == 0) info->usage = data8;
+            break;
 
-          case RI_LOCAL_USAGE_MIN        : break;
-          case RI_LOCAL_USAGE_MAX        : break;
-          case RI_LOCAL_DESIGNATOR_INDEX : break;
-          case RI_LOCAL_DESIGNATOR_MIN   : break;
-          case RI_LOCAL_DESIGNATOR_MAX   : break;
-          case RI_LOCAL_STRING_INDEX     : break;
-          case RI_LOCAL_STRING_MIN       : break;
-          case RI_LOCAL_STRING_MAX       : break;
-          case RI_LOCAL_DELIMITER        : break;
+          case RI_LOCAL_USAGE_MIN: break;
+          case RI_LOCAL_USAGE_MAX: break;
+          case RI_LOCAL_DESIGNATOR_INDEX: break;
+          case RI_LOCAL_DESIGNATOR_MIN: break;
+          case RI_LOCAL_DESIGNATOR_MAX: break;
+          case RI_LOCAL_STRING_INDEX: break;
+          case RI_LOCAL_STRING_MIN: break;
+          case RI_LOCAL_STRING_MAX: break;
+          case RI_LOCAL_DELIMITER: break;
           default: break;
         }
-      break;
+        break;
 
-      // error
+        // error
       default: break;
     }
 
     desc_report += size;
-    desc_len    -= size;
+    desc_len -= size;
   }
 
-  for ( uint8_t i = 0; i < report_num; i++ )
-  {
-    info = report_info_arr+i;
-    TU_LOG2("%u: id = %u, usage_page = %u, usage = %u\r\n", i, info->report_id, info->usage_page, info->usage);
+  for (uint8_t i = 0; i < report_num; i++) {
+    info = report_info_arr + i;
+    TU_LOG_DRV("%u: id = %u, usage_page = %u, usage = %u\r\n", i, info->report_id, info->usage_page, info->usage);
   }
 
   return report_num;
 }
 
-//--------------------------------------------------------------------+
-// Helper
-//--------------------------------------------------------------------+
-
-// Get Device by address
-TU_ATTR_ALWAYS_INLINE static inline hidh_device_t* get_dev(uint8_t dev_addr)
-{
-  return &_hidh_dev[dev_addr-1];
-}
-
-// Get Interface by instance number
-TU_ATTR_ALWAYS_INLINE static inline hidh_interface_t* get_instance(uint8_t dev_addr, uint8_t instance)
-{
-  return &_hidh_dev[dev_addr-1].instances[instance];
-}
-
-// Get instance ID by interface number
-static uint8_t get_instance_id_by_itfnum(uint8_t dev_addr, uint8_t itf)
-{
-  for ( uint8_t inst = 0; inst < CFG_TUH_HID; inst++ )
-  {
-    hidh_interface_t *hid = get_instance(dev_addr, inst);
-
-    if ( (hid->itf_num == itf) && (hid->ep_in || hid->ep_out) ) return inst;
-  }
-
-  return 0xff;
-}
-
-// Get instance ID by endpoint address
-static uint8_t get_instance_id_by_epaddr(uint8_t dev_addr, uint8_t ep_addr)
-{
-  for ( uint8_t inst = 0; inst < CFG_TUH_HID; inst++ )
-  {
-    hidh_interface_t *hid = get_instance(dev_addr, inst);
-
-    if ( (ep_addr == hid->ep_in) || ( ep_addr == hid->ep_out) ) return inst;
-  }
-
-  return 0xff;
-}
-
 #endif
diff --git a/src/class/hid/hid_host.h b/src/class/hid/hid_host.h
index fe09b03b2..9681c704b 100644
--- a/src/class/hid/hid_host.h
+++ b/src/class/hid/hid_host.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -30,7 +30,7 @@
 #include "hid.h"
 
 #ifdef __cplusplus
- extern "C" {
+extern "C" {
 #endif
 
 //--------------------------------------------------------------------+
@@ -47,10 +47,9 @@
 #endif
 
 
-typedef struct
-{
-  uint8_t  report_id;
-  uint8_t  usage;
+typedef struct {
+  uint8_t report_id;
+  uint8_t usage;
   uint16_t usage_page;
 
   // TODO still use the endpoint size for now
@@ -62,18 +61,32 @@ typedef struct
 // Interface API
 //--------------------------------------------------------------------+
 
-// Get the number of HID instances
-uint8_t tuh_hid_instance_count(uint8_t dev_addr);
+// Get the total number of mounted HID interfaces of a device
+uint8_t tuh_hid_itf_get_count(uint8_t dev_addr);
 
-// Check if HID instance is mounted
-bool tuh_hid_mounted(uint8_t dev_addr, uint8_t instance);
+// Get all mounted interfaces across devices
+uint8_t tuh_hid_itf_get_total_count(void);
+
+// backward compatible rename
+#define tuh_hid_instance_count    tuh_hid_itf_get_count
+
+// Get Interface information
+bool tuh_hid_itf_get_info(uint8_t daddr, uint8_t idx, tuh_itf_info_t* itf_info);
+
+// Get Interface index from device address + interface number
+// return TUSB_INDEX_INVALID_8 (0xFF) if not found
+uint8_t tuh_hid_itf_get_index(uint8_t daddr, uint8_t itf_num);
 
 // Get interface supported protocol (bInterfaceProtocol) check out hid_interface_protocol_enum_t for possible values
-uint8_t tuh_hid_interface_protocol(uint8_t dev_addr, uint8_t instance);
+uint8_t tuh_hid_interface_protocol(uint8_t dev_addr, uint8_t idx);
+
+// Check if HID interface is mounted
+bool tuh_hid_mounted(uint8_t dev_addr, uint8_t idx);
 
 // Parse report descriptor into array of report_info struct and return number of reports.
 // For complicated report, application should write its own parser.
-uint8_t tuh_hid_parse_report_descriptor(tuh_hid_report_info_t* reports_info_arr, uint8_t arr_count, uint8_t const* desc_report, uint16_t desc_len) TU_ATTR_UNUSED;
+TU_ATTR_UNUSED uint8_t tuh_hid_parse_report_descriptor(tuh_hid_report_info_t* reports_info_arr, uint8_t arr_count,
+                                                       uint8_t const* desc_report, uint16_t desc_len);
 
 //--------------------------------------------------------------------+
 // Control Endpoint API
@@ -82,31 +95,46 @@ uint8_t tuh_hid_parse_report_descriptor(tuh_hid_report_info_t* reports_info_arr,
 // Get current protocol: HID_PROTOCOL_BOOT (0) or HID_PROTOCOL_REPORT (1)
 // Note: Device will be initialized in Boot protocol for simplicity.
 //       Application can use set_protocol() to switch back to Report protocol.
-uint8_t tuh_hid_get_protocol(uint8_t dev_addr, uint8_t instance);
+uint8_t tuh_hid_get_protocol(uint8_t dev_addr, uint8_t idx);
+
+// Device by default is enumerated in Boot protocol for simplicity. Application
+// can use this to modify the default protocol for next enumeration.
+void tuh_hid_set_default_protocol(uint8_t protocol);
 
 // Set protocol to HID_PROTOCOL_BOOT (0) or HID_PROTOCOL_REPORT (1)
 // This function is only supported by Boot interface (tuh_n_hid_interface_protocol() != NONE)
-bool tuh_hid_set_protocol(uint8_t dev_addr, uint8_t instance, uint8_t protocol);
+bool tuh_hid_set_protocol(uint8_t dev_addr, uint8_t idx, uint8_t protocol);
+
+// Get Report using control endpoint
+// report_type is either Input, Output or Feature, (value from hid_report_type_t)
+bool tuh_hid_get_report(uint8_t dev_addr, uint8_t idx, uint8_t report_id, uint8_t report_type, void* report, uint16_t len);
 
 // Set Report using control endpoint
-// report_type is either Intput, Output or Feature, (value from hid_report_type_t)
-bool tuh_hid_set_report(uint8_t dev_addr, uint8_t instance, uint8_t report_id, uint8_t report_type, void* report, uint16_t len);
+// report_type is either Input, Output or Feature, (value from hid_report_type_t)
+bool tuh_hid_set_report(uint8_t dev_addr, uint8_t idx, uint8_t report_id, uint8_t report_type,
+                        void* report, uint16_t len);
 
 //--------------------------------------------------------------------+
 // Interrupt Endpoint API
 //--------------------------------------------------------------------+
 
-// Check if the interface is ready to use
-//bool tuh_n_hid_n_ready(uint8_t dev_addr, uint8_t instance);
+// Check if HID interface is ready to receive report
+bool tuh_hid_receive_ready(uint8_t dev_addr, uint8_t idx);
 
 // Try to receive next report on Interrupt Endpoint. Immediately return
 // - true If succeeded, tuh_hid_report_received_cb() callback will be invoked when report is available
 // - false if failed to queue the transfer e.g endpoint is busy
-bool tuh_hid_receive_report(uint8_t dev_addr, uint8_t instance);
+bool tuh_hid_receive_report(uint8_t dev_addr, uint8_t idx);
+
+// Abort receiving report on Interrupt Endpoint
+bool tuh_hid_receive_abort(uint8_t dev_addr, uint8_t idx);
+
+// Check if HID interface is ready to send report
+bool tuh_hid_send_ready(uint8_t dev_addr, uint8_t idx);
 
 // Send report using interrupt endpoint
 // If report_id > 0 (composite), it will be sent as 1st byte, then report contents. Otherwise only report content is sent.
-//void tuh_hid_send_report(uint8_t dev_addr, uint8_t instance, uint8_t report_id, uint8_t const* report, uint16_t len);
+bool tuh_hid_send_report(uint8_t dev_addr, uint8_t idx, uint8_t report_id, const void* report, uint16_t len);
 
 //--------------------------------------------------------------------+
 // Callbacks (Weak is optional)
@@ -117,33 +145,38 @@ bool tuh_hid_receive_report(uint8_t dev_addr, uint8_t instance);
 // can be used to parse common/simple enough descriptor.
 // Note: if report descriptor length > CFG_TUH_ENUMERATION_BUFSIZE, it will be skipped
 // therefore report_desc = NULL, desc_len = 0
-void tuh_hid_mount_cb(uint8_t dev_addr, uint8_t instance, uint8_t const* report_desc, uint16_t desc_len);
+TU_ATTR_WEAK void tuh_hid_mount_cb(uint8_t dev_addr, uint8_t idx, uint8_t const* report_desc, uint16_t desc_len);
 
 // Invoked when device with hid interface is un-mounted
-TU_ATTR_WEAK void tuh_hid_umount_cb(uint8_t dev_addr, uint8_t instance);
+TU_ATTR_WEAK void tuh_hid_umount_cb(uint8_t dev_addr, uint8_t idx);
 
 // Invoked when received report from device via interrupt endpoint
 // Note: if there is report ID (composite), it is 1st byte of report
-void tuh_hid_report_received_cb(uint8_t dev_addr, uint8_t instance, uint8_t const* report, uint16_t len);
+void tuh_hid_report_received_cb(uint8_t dev_addr, uint8_t idx, uint8_t const* report, uint16_t len);
 
 // Invoked when sent report to device successfully via interrupt endpoint
-TU_ATTR_WEAK void tuh_hid_report_sent_cb(uint8_t dev_addr, uint8_t instance, uint8_t const* report, uint16_t len);
+TU_ATTR_WEAK void tuh_hid_report_sent_cb(uint8_t dev_addr, uint8_t idx, uint8_t const* report, uint16_t len);
+
+// Invoked when Get Report to device via either control endpoint
+// len = 0 indicate there is error in the transfer e.g stalled response
+TU_ATTR_WEAK void tuh_hid_get_report_complete_cb(uint8_t dev_addr, uint8_t idx, uint8_t report_id, uint8_t report_type, uint16_t len);
 
 // Invoked when Sent Report to device via either control endpoint
 // len = 0 indicate there is error in the transfer e.g stalled response
-TU_ATTR_WEAK void tuh_hid_set_report_complete_cb(uint8_t dev_addr, uint8_t instance, uint8_t report_id, uint8_t report_type, uint16_t len);
+TU_ATTR_WEAK void tuh_hid_set_report_complete_cb(uint8_t dev_addr, uint8_t idx, uint8_t report_id, uint8_t report_type, uint16_t len);
 
 // Invoked when Set Protocol request is complete
-TU_ATTR_WEAK void tuh_hid_set_protocol_complete_cb(uint8_t dev_addr, uint8_t instance, uint8_t protocol);
+TU_ATTR_WEAK void tuh_hid_set_protocol_complete_cb(uint8_t dev_addr, uint8_t idx, uint8_t protocol);
 
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
-void hidh_init       (void);
-bool hidh_open       (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len);
-bool hidh_set_config (uint8_t dev_addr, uint8_t itf_num);
-bool hidh_xfer_cb    (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes);
-void hidh_close      (uint8_t dev_addr);
+bool hidh_init(void);
+bool hidh_deinit(void);
+bool hidh_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const* desc_itf, uint16_t max_len);
+bool hidh_set_config(uint8_t dev_addr, uint8_t itf_num);
+bool hidh_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes);
+void hidh_close(uint8_t dev_addr);
 
 #ifdef __cplusplus
 }
diff --git a/src/class/midi/midi.h b/src/class/midi/midi.h
index ea2cfec75..68a784c60 100644
--- a/src/class/midi/midi.h
+++ b/src/class/midi/midi.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -71,8 +71,8 @@ typedef enum
   MIDI_CIN_SYSEX_END_1BYTE   = 5, // SysEx ends with 1 data, or 1 byte system common message
   MIDI_CIN_SYSEX_END_2BYTE   = 6, // SysEx ends with 2 data
   MIDI_CIN_SYSEX_END_3BYTE   = 7, // SysEx ends with 3 data
-  MIDI_CIN_NOTE_ON           = 9,
   MIDI_CIN_NOTE_OFF          = 8,
+  MIDI_CIN_NOTE_ON           = 9,
   MIDI_CIN_POLY_KEYPRESS     = 10,
   MIDI_CIN_CONTROL_CHANGE    = 11,
   MIDI_CIN_PROGRAM_CHANGE    = 12,
diff --git a/src/class/midi/midi_device.c b/src/class/midi/midi_device.c
index c6c99db29..4ae8ea8fa 100644
--- a/src/class/midi/midi_device.c
+++ b/src/class/midi/midi_device.c
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -40,15 +40,13 @@
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
 
-typedef struct
-{
+typedef struct {
   uint8_t buffer[4];
   uint8_t index;
   uint8_t total;
-}midid_stream_t;
+} midid_stream_t;
 
-typedef struct
-{
+typedef struct {
   uint8_t itf_num;
   uint8_t ep_in;
   uint8_t ep_out;
@@ -70,36 +68,36 @@ typedef struct
   osal_mutex_def_t rx_ff_mutex;
   osal_mutex_def_t tx_ff_mutex;
   #endif
-
-  // Endpoint Transfer buffer
-  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;
 
 #define ITF_MEM_RESET_SIZE   offsetof(midid_interface_t, rx_ff)
 
+// Endpoint Transfer buffer
+CFG_TUD_MEM_SECTION static struct {
+  TUD_EPBUF_DEF(epin, CFG_TUD_MIDI_EP_BUFSIZE);
+  TUD_EPBUF_DEF(epout, CFG_TUD_MIDI_EP_BUFSIZE);
+} _midid_epbuf[CFG_TUD_MIDI];
+
 //--------------------------------------------------------------------+
 // INTERNAL OBJECT & FUNCTION DECLARATION
 //--------------------------------------------------------------------+
-CFG_TUSB_MEM_SECTION midid_interface_t _midid_itf[CFG_TUD_MIDI];
+static midid_interface_t _midid_itf[CFG_TUD_MIDI];
 
-bool tud_midi_n_mounted (uint8_t itf)
-{
+bool tud_midi_n_mounted (uint8_t itf) {
   midid_interface_t* midi = &_midid_itf[itf];
   return midi->ep_in && midi->ep_out;
 }
 
-static void _prep_out_transaction (midid_interface_t* p_midi)
-{
-  uint8_t const rhport = 0;
+static void _prep_out_transaction(uint8_t idx) {
+  const uint8_t rhport = 0;
+  midid_interface_t* p_midi = &_midid_itf[idx];
   uint16_t available = tu_fifo_remaining(&p_midi->rx_ff);
 
   // Prepare for incoming data but only allow what we can store in the ring buffer.
   // TODO Actually we can still carry out the transfer, keeping count of received bytes
   // and slowly move it to the FIFO when read().
   // This pre-check reduces endpoint claiming
-  TU_VERIFY(available >= sizeof(p_midi->epout_buf), );
+  TU_VERIFY(available >= CFG_TUD_MIDI_EP_BUFSIZE, );
 
   // claim endpoint
   TU_VERIFY(usbd_edpt_claim(rhport, p_midi->ep_out), );
@@ -107,8 +105,8 @@ static void _prep_out_transaction (midid_interface_t* p_midi)
   // fifo can be changed before endpoint is claimed
   available = tu_fifo_remaining(&p_midi->rx_ff);
 
-  if ( available >= sizeof(p_midi->epout_buf) )  {
-    usbd_edpt_xfer(rhport, p_midi->ep_out, p_midi->epout_buf, sizeof(p_midi->epout_buf));
+  if ( available >= CFG_TUD_MIDI_EP_BUFSIZE )  {
+    usbd_edpt_xfer(rhport, p_midi->ep_out, _midid_epbuf[idx].epout, CFG_TUD_MIDI_EP_BUFSIZE);
   }else
   {
     // Release endpoint since we don't make any transfer
@@ -124,7 +122,7 @@ uint32_t tud_midi_n_available(uint8_t itf, uint8_t cable_num)
   (void) cable_num;
 
   midid_interface_t* midi = &_midid_itf[itf];
-  midid_stream_t const* stream = &midi->stream_read;
+  const midid_stream_t* stream = &midi->stream_read;
 
   // when using with packet API stream total & index are both zero
   return tu_fifo_count(&midi->rx_ff) + (uint8_t) (stream->total - stream->index);
@@ -182,7 +180,7 @@ uint32_t tud_midi_n_stream_read(uint8_t itf, uint8_t cable_num, void* buffer, ui
     uint8_t const count = (uint8_t) tu_min32(stream->total - stream->index, bufsize);
 
     // Skip the header (1st byte) in the buffer
-    memcpy(buf8, stream->buffer + 1 + stream->index, count);
+    TU_VERIFY(0 == tu_memcpy_s(buf8, bufsize, stream->buffer + 1 + stream->index, count));
 
     total_read += count;
     stream->index += count;
@@ -205,8 +203,8 @@ bool tud_midi_n_packet_read (uint8_t itf, uint8_t packet[4])
   midid_interface_t* midi = &_midid_itf[itf];
   TU_VERIFY(midi->ep_out);
 
-  uint32_t const num_read = tu_fifo_read_n(&midi->rx_ff, packet, 4);
-  _prep_out_transaction(midi);
+  const uint32_t num_read = tu_fifo_read_n(&midi->rx_ff, packet, 4);
+  _prep_out_transaction(itf);
   return (num_read == 4);
 }
 
@@ -214,31 +212,31 @@ bool tud_midi_n_packet_read (uint8_t itf, uint8_t packet[4])
 // WRITE API
 //--------------------------------------------------------------------+
 
-static uint32_t write_flush(midid_interface_t* midi)
-{
-  // No data to send
-  if ( !tu_fifo_count(&midi->tx_ff) ) return 0;
+static uint32_t write_flush(uint8_t idx) {
+  midid_interface_t* midi = &_midid_itf[idx];
 
-  uint8_t const rhport = 0;
+  if (!tu_fifo_count(&midi->tx_ff)) {
+    return 0; // No data to send
+  }
+
+  const uint8_t rhport = 0;
 
   // skip if previous transfer not complete
   TU_VERIFY( usbd_edpt_claim(rhport, midi->ep_in), 0 );
 
-  uint16_t count = tu_fifo_read_n(&midi->tx_ff, midi->epin_buf, CFG_TUD_MIDI_EP_BUFSIZE);
+  uint16_t count = tu_fifo_read_n(&midi->tx_ff, _midid_epbuf[idx].epin, CFG_TUD_MIDI_EP_BUFSIZE);
 
-  if (count)
-  {
-    TU_ASSERT( usbd_edpt_xfer(rhport, midi->ep_in, midi->epin_buf, count), 0 );
+  if (count) {
+    TU_ASSERT( usbd_edpt_xfer(rhport, midi->ep_in, _midid_epbuf[idx].epin, count), 0 );
     return count;
-  }else
-  {
+  }else {
     // Release endpoint since we don't make any transfer
     usbd_edpt_release(rhport, midi->ep_in);
     return 0;
   }
 }
 
-uint32_t tud_midi_n_stream_write(uint8_t itf, uint8_t cable_num, uint8_t const* buffer, uint32_t bufsize)
+uint32_t tud_midi_n_stream_write(uint8_t itf, uint8_t cable_num, const uint8_t* buffer, uint32_t bufsize)
 {
   midid_interface_t* midi = &_midid_itf[itf];
   TU_VERIFY(midi->ep_in, 0);
@@ -248,24 +246,23 @@ uint32_t tud_midi_n_stream_write(uint8_t itf, uint8_t cable_num, uint8_t const*
   uint32_t i = 0;
   while ( (i < bufsize) && (tu_fifo_remaining(&midi->tx_ff) >= 4) )
   {
-    uint8_t const data = buffer[i];
+    const uint8_t data = buffer[i];
     i++;
 
     if ( stream->index == 0 )
     {
       //------------- New event packet -------------//
-
-      uint8_t const msg = data >> 4;
+      const uint8_t msg = data >> 4;
 
       stream->index = 2;
       stream->buffer[1] = data;
 
       // Check to see if we're still in a SysEx transmit.
-      if ( stream->buffer[0] == MIDI_CIN_SYSEX_START )
+      if ( ((stream->buffer[0]) & 0xF) == MIDI_CIN_SYSEX_START )
       {
         if ( data == MIDI_STATUS_SYSEX_END )
         {
-          stream->buffer[0] = MIDI_CIN_SYSEX_END_1BYTE;
+          stream->buffer[0] = (uint8_t) ((cable_num << 4) | MIDI_CIN_SYSEX_END_1BYTE);
           stream->total = 2;
         }
         else
@@ -308,6 +305,7 @@ uint32_t tud_midi_n_stream_write(uint8_t itf, uint8_t cable_num, uint8_t const*
           stream->buffer[0] = MIDI_CIN_SYSEX_END_1BYTE;
           stream->total = 2;
         }
+        stream->buffer[0] |= (uint8_t)(cable_num << 4);
       }
       else
       {
@@ -328,9 +326,9 @@ uint32_t tud_midi_n_stream_write(uint8_t itf, uint8_t cable_num, uint8_t const*
       stream->index++;
 
       // See if this byte ends a SysEx.
-      if ( stream->buffer[0] == MIDI_CIN_SYSEX_START && data == MIDI_STATUS_SYSEX_END )
+      if ( (stream->buffer[0] & 0xF) == MIDI_CIN_SYSEX_START && data == MIDI_STATUS_SYSEX_END )
       {
-        stream->buffer[0] = MIDI_CIN_SYSEX_START + (stream->index - 1);
+        stream->buffer[0] = (uint8_t) ((cable_num << 4) | (MIDI_CIN_SYSEX_START + (stream->index - 1)));
         stream->total = stream->index;
       }
     }
@@ -339,9 +337,11 @@ uint32_t tud_midi_n_stream_write(uint8_t itf, uint8_t cable_num, uint8_t const*
     if ( stream->index == stream->total )
     {
       // zeroes unused bytes
-      for(uint8_t idx = stream->total; idx < 4; idx++) stream->buffer[idx] = 0;
+      for (uint8_t idx = stream->total; idx < 4; idx++) {
+        stream->buffer[idx] = 0;
+      }
 
-      uint16_t const count = tu_fifo_write_n(&midi->tx_ff, stream->buffer, 4);
+      const uint16_t count = tu_fifo_write_n(&midi->tx_ff, stream->buffer, 4);
 
       // complete current event packet, reset stream
       stream->index = stream->total = 0;
@@ -351,20 +351,21 @@ uint32_t tud_midi_n_stream_write(uint8_t itf, uint8_t cable_num, uint8_t const*
     }
   }
 
-  write_flush(midi);
+  write_flush(itf);
 
   return i;
 }
 
-bool tud_midi_n_packet_write (uint8_t itf, uint8_t const packet[4])
-{
+bool tud_midi_n_packet_write (uint8_t itf, const uint8_t packet[4]) {
   midid_interface_t* midi = &_midid_itf[itf];
   TU_VERIFY(midi->ep_in);
 
-  if (tu_fifo_remaining(&midi->tx_ff) < 4) return false;
+  if (tu_fifo_remaining(&midi->tx_ff) < 4) {
+    return false;
+  }
 
   tu_fifo_write_n(&midi->tx_ff, packet, 4);
-  write_flush(midi);
+  write_flush(itf);
 
   return true;
 }
@@ -372,12 +373,10 @@ bool tud_midi_n_packet_write (uint8_t itf, uint8_t const packet[4])
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void midid_init(void)
-{
+void midid_init(void) {
   tu_memclr(_midid_itf, sizeof(_midid_itf));
 
-  for(uint8_t i=0; i<CFG_TUD_MIDI; i++)
-  {
+  for (uint8_t i = 0; i < CFG_TUD_MIDI; i++) {
     midid_interface_t* midi = &_midid_itf[i];
 
     // config fifo
@@ -385,12 +384,38 @@ void midid_init(void)
     tu_fifo_config(&midi->tx_ff, midi->tx_ff_buf, CFG_TUD_MIDI_TX_BUFSIZE, 1, false); // OBVS.
 
     #if CFG_FIFO_MUTEX
-    tu_fifo_config_mutex(&midi->rx_ff, NULL, osal_mutex_create(&midi->rx_ff_mutex));
-    tu_fifo_config_mutex(&midi->tx_ff, osal_mutex_create(&midi->tx_ff_mutex), NULL);
+    osal_mutex_t mutex_rd = osal_mutex_create(&midi->rx_ff_mutex);
+    osal_mutex_t mutex_wr = osal_mutex_create(&midi->tx_ff_mutex);
+    TU_ASSERT(mutex_wr != NULL && mutex_wr != NULL, );
+
+    tu_fifo_config_mutex(&midi->rx_ff, NULL, mutex_rd);
+    tu_fifo_config_mutex(&midi->tx_ff, mutex_wr, NULL);
     #endif
   }
 }
 
+bool midid_deinit(void) {
+  #if CFG_FIFO_MUTEX
+  for(uint8_t i=0; i<CFG_TUD_MIDI; i++) {
+    midid_interface_t* midi = &_midid_itf[i];
+    osal_mutex_t mutex_rd = midi->rx_ff.mutex_rd;
+    osal_mutex_t mutex_wr = midi->tx_ff.mutex_wr;
+
+    if (mutex_rd) {
+      osal_mutex_delete(mutex_rd);
+      tu_fifo_config_mutex(&midi->rx_ff, NULL, NULL);
+    }
+
+    if (mutex_wr) {
+      osal_mutex_delete(mutex_wr);
+      tu_fifo_config_mutex(&midi->tx_ff, NULL, NULL);
+    }
+  }
+  #endif
+
+  return true;
+}
+
 void midid_reset(uint8_t rhport)
 {
   (void) rhport;
@@ -404,32 +429,28 @@ void midid_reset(uint8_t rhport)
   }
 }
 
-uint16_t midid_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint16_t max_len)
-{
+uint16_t midid_open(uint8_t rhport, const tusb_desc_interface_t* desc_itf, uint16_t max_len) {
   uint16_t drv_len = 0;
   uint8_t const * p_desc = (uint8_t const *)desc_itf;
-  // 1st Interface is Audio Control v1
+
+  // 1st Interface is Audio Control v1 (optional)
   if (TUSB_CLASS_AUDIO               == desc_itf->bInterfaceClass    &&
-            AUDIO_SUBCLASS_CONTROL         == desc_itf->bInterfaceSubClass &&
-            AUDIO_FUNC_PROTOCOL_CODE_UNDEF == desc_itf->bInterfaceProtocol)
-  {
+      AUDIO_SUBCLASS_CONTROL         == desc_itf->bInterfaceSubClass &&
+      AUDIO_FUNC_PROTOCOL_CODE_UNDEF == desc_itf->bInterfaceProtocol) {
     drv_len = tu_desc_len(desc_itf);
     p_desc = tu_desc_next(desc_itf);
     // Skip Class Specific descriptors
-    while ( TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc) && drv_len <= max_len )
-    {
-        drv_len += tu_desc_len(p_desc);
-        p_desc   = tu_desc_next(p_desc);
+    while ( TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc) && drv_len <= max_len ) {
+      drv_len += tu_desc_len(p_desc);
+      p_desc   = tu_desc_next(p_desc);
     }
-  }
-  else
-  {
-    TU_LOG1("Warning: MIDI Device has no Audio Control Interface");
+  } else {
+    TU_LOG2("Warning: MIDI Device has no Audio Control Interface");
   }
 
   // 2nd Interface is MIDI Streaming
   TU_VERIFY(TUSB_DESC_INTERFACE == tu_desc_type(p_desc), 0);
-  tusb_desc_interface_t const * desc_midi = (tusb_desc_interface_t const *) p_desc;
+  const tusb_desc_interface_t* desc_midi = (const tusb_desc_interface_t*) p_desc;
 
   TU_VERIFY(TUSB_CLASS_AUDIO               == desc_midi->bInterfaceClass    &&
             AUDIO_SUBCLASS_MIDI_STREAMING  == desc_midi->bInterfaceSubClass &&
@@ -437,11 +458,10 @@ uint16_t midid_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint
 
   // Find available interface
   midid_interface_t * p_midi = NULL;
-  for(uint8_t i=0; i<CFG_TUD_MIDI; i++)
-  {
-    if ( _midid_itf[i].ep_in == 0 && _midid_itf[i].ep_out == 0 )
-    {
-      p_midi = &_midid_itf[i];
+  uint8_t idx;
+  for(idx=0; idx<CFG_TUD_MIDI; idx++) {
+    if ( _midid_itf[idx].ep_in == 0 && _midid_itf[idx].ep_out == 0 ) {
+      p_midi = &_midid_itf[idx];
       break;
     }
   }
@@ -460,8 +480,8 @@ uint16_t midid_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint
   {
     if ( TUSB_DESC_ENDPOINT == tu_desc_type(p_desc) )
     {
-      TU_ASSERT(usbd_edpt_open(rhport, (tusb_desc_endpoint_t const *) p_desc), 0);
-      uint8_t ep_addr = ((tusb_desc_endpoint_t const *) p_desc)->bEndpointAddress;
+      TU_ASSERT(usbd_edpt_open(rhport, (const tusb_desc_endpoint_t*) p_desc), 0);
+      uint8_t ep_addr = ((const tusb_desc_endpoint_t*) p_desc)->bEndpointAddress;
 
       if (tu_edpt_dir(ep_addr) == TUSB_DIR_IN)
       {
@@ -482,7 +502,7 @@ uint16_t midid_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint
   }
 
   // Prepare for incoming data
-  _prep_out_transaction(p_midi);
+  _prep_out_transaction(idx);
 
   return drv_len;
 }
@@ -490,14 +510,9 @@ uint16_t midid_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint
 // Invoked when a control transfer occurred on an interface of this class
 // Driver response accordingly to the request and the transfer stage (setup/data/ack)
 // return false to stall control endpoint (e.g unsupported request)
-bool midid_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
-{
-  (void) rhport;
-  (void) stage;
-  (void) request;
-
-  // driver doesn't support any request yet
-  return false;
+bool midid_control_xfer_cb(uint8_t rhport, uint8_t stage, const tusb_control_request_t* request) {
+  (void) rhport; (void) stage; (void) request;
+  return false; // driver doesn't support any request yet
 }
 
 bool midid_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
@@ -505,40 +520,37 @@ bool midid_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32
   (void) result;
   (void) rhport;
 
-  uint8_t itf;
+  uint8_t idx;
   midid_interface_t* p_midi;
 
   // Identify which interface to use
-  for (itf = 0; itf < CFG_TUD_MIDI; itf++)
-  {
-    p_midi = &_midid_itf[itf];
-    if ( ( ep_addr == p_midi->ep_out ) || ( ep_addr == p_midi->ep_in ) ) break;
+  for (idx = 0; idx < CFG_TUD_MIDI; idx++) {
+    p_midi = &_midid_itf[idx];
+    if ((ep_addr == p_midi->ep_out) || (ep_addr == p_midi->ep_in)) {
+      break;
+    }
   }
-  TU_ASSERT(itf < CFG_TUD_MIDI);
+  TU_ASSERT(idx < CFG_TUD_MIDI);
 
   // receive new data
-  if ( ep_addr == p_midi->ep_out )
-  {
-    tu_fifo_write_n(&p_midi->rx_ff, p_midi->epout_buf, (uint16_t) xferred_bytes);
+  if (ep_addr == p_midi->ep_out) {
+    tu_fifo_write_n(&p_midi->rx_ff, _midid_epbuf[idx].epout, (uint16_t)xferred_bytes);
 
     // invoke receive callback if available
-    if (tud_midi_rx_cb) tud_midi_rx_cb(itf);
+    if (tud_midi_rx_cb) {
+      tud_midi_rx_cb(idx);
+    }
 
     // prepare for next
     // TODO for now ep_out is not used by public API therefore there is no race condition,
     // and does not need to claim like ep_in
-    _prep_out_transaction(p_midi);
-  }
-  else if ( ep_addr == p_midi->ep_in )
-  {
-    if (0 == write_flush(p_midi))
-    {
+    _prep_out_transaction(idx);
+  } else if (ep_addr == p_midi->ep_in) {
+    if (0 == write_flush(idx)) {
       // If there is no data left, a ZLP should be sent if
       // xferred_bytes is multiple of EP size and not zero
-      if ( !tu_fifo_count(&p_midi->tx_ff) && xferred_bytes && (0 == (xferred_bytes % CFG_TUD_MIDI_EP_BUFSIZE)) )
-      {
-        if ( usbd_edpt_claim(rhport, p_midi->ep_in) )
-        {
+      if (!tu_fifo_count(&p_midi->tx_ff) && xferred_bytes && (0 == (xferred_bytes % CFG_TUD_MIDI_EP_BUFSIZE))) {
+        if (usbd_edpt_claim(rhport, p_midi->ep_in)) {
           usbd_edpt_xfer(rhport, p_midi->ep_in, NULL, 0);
         }
       }
diff --git a/src/class/midi/midi_device.h b/src/class/midi/midi_device.h
index 211edc8d1..3e89cc0a3 100644
--- a/src/class/midi/midi_device.h
+++ b/src/class/midi/midi_device.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -158,6 +158,7 @@ static inline bool tud_midi_packet_write (uint8_t const packet[4])
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     midid_init            (void);
+bool     midid_deinit          (void);
 void     midid_reset           (uint8_t rhport);
 uint16_t midid_open            (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 bool     midid_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
diff --git a/src/class/msc/msc.h b/src/class/msc/msc.h
index 84b6e4d79..bbfd35a43 100644
--- a/src/class/msc/msc.h
+++ b/src/class/msc/msc.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -53,7 +53,7 @@ enum {
 };
 
 /// \brief MassStorage Protocol.
-/// \details CBI only approved to use with full-speed floopy disk & should not used with highspeed or device other than floopy
+/// \details CBI only approved to use with full-speed floppy disk & should not used with highspeed or device other than floppy
 typedef enum
 {
   MSC_PROTOCOL_CBI              = 0 ,  ///< Control/Bulk/Interrupt protocol (with command completion interrupt)
@@ -97,7 +97,7 @@ typedef struct TU_ATTR_PACKED
 {
   uint32_t signature    ; ///< Signature that helps identify this data packet as a CSW. The signature field shall contain the value 53425355h (little endian), indicating CSW.
   uint32_t tag          ; ///< The device shall set this field to the value received in the dCBWTag of the associated CBW.
-  uint32_t data_residue ; ///< For Data-Out the device shall report in the dCSWDataResiduethe difference between the amount of data expected as stated in the dCBWDataTransferLength, and the actual amount of data processed by the device. For Data-In the device shall report in the dCSWDataResiduethe difference between the amount of data expected as stated in the dCBWDataTransferLengthand the actual amount of relevant data sent by the device
+  uint32_t data_residue ; ///< For Data-Out the device shall report in the dCSWDataResidue the difference between the amount of data expected as stated in the dCBWDataTransferLength, and the actual amount of data processed by the device. For Data-In the device shall report in the dCSWDataResiduethe difference between the amount of data expected as stated in the dCBWDataTransferLengthand the actual amount of relevant data sent by the device
   uint8_t  status       ; ///< indicates the success or failure of the command. Values from \ref msc_csw_status_t
 }msc_csw_t;
 
@@ -120,14 +120,14 @@ typedef enum
   SCSI_CMD_REQUEST_SENSE                = 0x03, ///< The SCSI Request Sense command is part of the SCSI computer protocol standard. This command is used to obtain sense data -- status/error information -- from a target device.
   SCSI_CMD_READ_FORMAT_CAPACITY         = 0x23, ///< The command allows the Host to request a list of the possible format capacities for an installed writable media. This command also has the capability to report the writable capacity for a media when it is installed
   SCSI_CMD_READ_10                      = 0x28, ///< The READ (10) command requests that the device server read the specified logical block(s) and transfer them to the data-in buffer.
-  SCSI_CMD_WRITE_10                     = 0x2A, ///< The WRITE (10) command requests thatthe device server transfer the specified logical block(s) from the data-out buffer and write them.
+  SCSI_CMD_WRITE_10                     = 0x2A, ///< The WRITE (10) command requests that the device server transfer the specified logical block(s) from the data-out buffer and write them.
 }scsi_cmd_type_t;
 
 /// SCSI Sense Key
 typedef enum
 {
   SCSI_SENSE_NONE            = 0x00, ///< no specific Sense Key. This would be the case for a successful command
-  SCSI_SENSE_RECOVERED_ERROR = 0x01, ///< ndicates the last command completed successfully with some recovery action performed by the disc drive.
+  SCSI_SENSE_RECOVERED_ERROR = 0x01, ///< Indicates the last command completed successfully with some recovery action performed by the disc drive.
   SCSI_SENSE_NOT_READY       = 0x02, ///< Indicates the logical unit addressed cannot be accessed.
   SCSI_SENSE_MEDIUM_ERROR    = 0x03, ///< Indicates the command terminated with a non-recovered error condition.
   SCSI_SENSE_HARDWARE_ERROR  = 0x04, ///< Indicates the disc drive detected a nonrecoverable hardware failure while performing the command or during a self test.
@@ -138,7 +138,7 @@ typedef enum
   SCSI_SENSE_ABORTED_COMMAND = 0x0b, ///< Indicates the disc drive aborted the command.
   SCSI_SENSE_EQUAL           = 0x0c, ///< Indicates a SEARCH DATA command has satisfied an equal comparison.
   SCSI_SENSE_VOLUME_OVERFLOW = 0x0d, ///< Indicates a buffered peripheral device has reached the end of medium partition and data remains in the buffer that has not been written to the medium.
-  SCSI_SENSE_MISCOMPARE      = 0x0e  ///< ndicates that the source data did not match the data read from the medium.
+  SCSI_SENSE_MISCOMPARE      = 0x0e  ///< Indicates that the source data did not match the data read from the medium.
 }scsi_sense_key_type_t;
 
 //--------------------------------------------------------------------+
diff --git a/src/class/msc/msc_device.c b/src/class/msc/msc_device.c
index 00b0a1d06..6670045aa 100644
--- a/src/class/msc/msc_device.c
+++ b/src/class/msc/msc_device.c
@@ -34,15 +34,17 @@
 
 #include "msc_device.h"
 
+// Level where CFG_TUSB_DEBUG must be at least for this driver is logged
+#ifndef CFG_TUD_MSC_LOG_LEVEL
+  #define CFG_TUD_MSC_LOG_LEVEL   CFG_TUD_LOG_LEVEL
+#endif
+
+#define TU_LOG_DRV(...)   TU_LOG(CFG_TUD_MSC_LOG_LEVEL, __VA_ARGS__)
+
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
-
-// Can be selectively disabled to reduce logging when troubleshooting other driver
-#define MSC_DEBUG   2
-
-enum
-{
+enum {
   MSC_STAGE_CMD  = 0,
   MSC_STAGE_DATA,
   MSC_STAGE_STATUS,
@@ -50,11 +52,9 @@ enum
   MSC_STAGE_NEED_RESET,
 };
 
-typedef struct
-{
-  // TODO optimize alignment
-  CFG_TUSB_MEM_ALIGN msc_cbw_t cbw;
-  CFG_TUSB_MEM_ALIGN msc_csw_t csw;
+typedef struct {
+  TU_ATTR_ALIGNED(4) msc_cbw_t cbw;
+  TU_ATTR_ALIGNED(4) msc_csw_t csw;
 
   uint8_t  itf_num;
   uint8_t  ep_in;
@@ -62,6 +62,7 @@ typedef struct
 
   // Bulk Only Transfer (BOT) Protocol
   uint8_t  stage;
+
   uint32_t total_len;   // byte to be transferred, can be smaller than total_bytes in cbw
   uint32_t xferred_len; // numbered of bytes transferred so far in the Data Stage
 
@@ -71,8 +72,11 @@ typedef struct
   uint8_t add_sense_qualifier;
 }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_EP_BUFSIZE];
+static mscd_interface_t _mscd_itf;
+
+CFG_TUD_MEM_SECTION static struct {
+  TUD_EPBUF_DEF(buf, CFG_TUD_MSC_EP_BUFSIZE);
+} _mscd_epbuf;
 
 //--------------------------------------------------------------------+
 // INTERNAL OBJECT & FUNCTION DECLARATION
@@ -83,28 +87,24 @@ static void proc_read10_cmd(uint8_t rhport, mscd_interface_t* p_msc);
 static void proc_write10_cmd(uint8_t rhport, mscd_interface_t* p_msc);
 static void proc_write10_new_data(uint8_t rhport, mscd_interface_t* p_msc, uint32_t xferred_bytes);
 
-TU_ATTR_ALWAYS_INLINE static inline bool is_data_in(uint8_t dir)
-{
+TU_ATTR_ALWAYS_INLINE static inline bool is_data_in(uint8_t dir) {
   return tu_bit_test(dir, 7);
 }
 
-static inline bool send_csw(uint8_t rhport, mscd_interface_t* p_msc)
-{
+static inline bool send_csw(uint8_t rhport, mscd_interface_t* p_msc) {
   // Data residue is always = host expect - actual transferred
   p_msc->csw.data_residue = p_msc->cbw.total_bytes - p_msc->xferred_len;
-
   p_msc->stage = MSC_STAGE_STATUS_SENT;
-  return usbd_edpt_xfer(rhport, p_msc->ep_in , (uint8_t*) &p_msc->csw, sizeof(msc_csw_t));
+  memcpy(_mscd_epbuf.buf, &p_msc->csw, sizeof(msc_csw_t));
+  return usbd_edpt_xfer(rhport, p_msc->ep_in , _mscd_epbuf.buf, sizeof(msc_csw_t));
 }
 
-static inline bool prepare_cbw(uint8_t rhport, mscd_interface_t* p_msc)
-{
+static inline bool prepare_cbw(uint8_t rhport, mscd_interface_t* p_msc) {
   p_msc->stage = MSC_STAGE_CMD;
-  return usbd_edpt_xfer(rhport, p_msc->ep_out, (uint8_t*) &p_msc->cbw, sizeof(msc_cbw_t));
+  return usbd_edpt_xfer(rhport, p_msc->ep_out,  _mscd_epbuf.buf, sizeof(msc_cbw_t));
 }
 
-static void fail_scsi_op(uint8_t rhport, mscd_interface_t* p_msc, uint8_t status)
-{
+static void fail_scsi_op(uint8_t rhport, mscd_interface_t* p_msc, uint8_t status) {
   msc_cbw_t const * p_cbw = &p_msc->cbw;
   msc_csw_t       * p_csw = &p_msc->csw;
 
@@ -113,83 +113,63 @@ static void fail_scsi_op(uint8_t rhport, mscd_interface_t* p_msc, uint8_t status
   p_msc->stage        = MSC_STAGE_STATUS;
 
   // failed but sense key is not set: default to Illegal Request
-  if ( p_msc->sense_key == 0 ) tud_msc_set_sense(p_cbw->lun, SCSI_SENSE_ILLEGAL_REQUEST, 0x20, 0x00);
+  if (p_msc->sense_key == 0) {
+    tud_msc_set_sense(p_cbw->lun, SCSI_SENSE_ILLEGAL_REQUEST, 0x20, 0x00);
+  }
 
   // If there is data stage and not yet complete, stall it
-  if ( p_cbw->total_bytes && p_csw->data_residue )
-  {
-    if ( is_data_in(p_cbw->dir) )
-    {
+  if (p_cbw->total_bytes && p_csw->data_residue) {
+    if (is_data_in(p_cbw->dir)) {
       usbd_edpt_stall(rhport, p_msc->ep_in);
-    }
-    else
-    {
+    } else {
       usbd_edpt_stall(rhport, p_msc->ep_out);
     }
   }
 }
 
-static inline uint32_t rdwr10_get_lba(uint8_t const command[])
-{
+static inline uint32_t rdwr10_get_lba(uint8_t const command[]) {
   // use offsetof to avoid pointer to the odd/unaligned address
-  uint32_t const lba = tu_unaligned_read32(command + offsetof(scsi_write10_t, lba));
-
-  // lba is in Big Endian
-  return tu_ntohl(lba);
+  const uint32_t lba = tu_unaligned_read32(command + offsetof(scsi_write10_t, lba));
+  return tu_ntohl(lba); // lba is in Big Endian
 }
 
-static inline uint16_t rdwr10_get_blockcount(msc_cbw_t const* cbw)
-{
+static inline uint16_t rdwr10_get_blockcount(msc_cbw_t const* cbw) {
   uint16_t const block_count = tu_unaligned_read16(cbw->command + offsetof(scsi_write10_t, block_count));
   return tu_ntohs(block_count);
 }
 
-static inline uint16_t rdwr10_get_blocksize(msc_cbw_t const* cbw)
-{
+static inline uint16_t rdwr10_get_blocksize(msc_cbw_t const* cbw) {
   // first extract block count in the command
   uint16_t const block_count = rdwr10_get_blockcount(cbw);
-
-  // invalid block count
-  if (block_count == 0) return 0;
-
+  if (block_count == 0) {
+    return 0; // invalid block count
+  }
   return (uint16_t) (cbw->total_bytes / block_count);
 }
 
-uint8_t rdwr10_validate_cmd(msc_cbw_t const* cbw)
-{
+static uint8_t rdwr10_validate_cmd(msc_cbw_t const* cbw) {
   uint8_t status = MSC_CSW_STATUS_PASSED;
   uint16_t const block_count = rdwr10_get_blockcount(cbw);
 
-  if ( cbw->total_bytes == 0 )
-  {
-    if ( block_count )
-    {
-      TU_LOG(MSC_DEBUG, "  SCSI case 2 (Hn < Di) or case 3 (Hn < Do) \r\n");
+  if (cbw->total_bytes == 0) {
+    if (block_count) {
+      TU_LOG_DRV("  SCSI case 2 (Hn < Di) or case 3 (Hn < Do) \r\n");
       status = MSC_CSW_STATUS_PHASE_ERROR;
-    }else
-    {
+    } else {
       // no data transfer, only exist in complaint test suite
     }
-  }else
-  {
-    if ( SCSI_CMD_READ_10 == cbw->command[0] && !is_data_in(cbw->dir) )
-    {
-      TU_LOG(MSC_DEBUG, "  SCSI case 10 (Ho <> Di)\r\n");
+  } else {
+    if (SCSI_CMD_READ_10 == cbw->command[0] && !is_data_in(cbw->dir)) {
+      TU_LOG_DRV("  SCSI case 10 (Ho <> Di)\r\n");
       status = MSC_CSW_STATUS_PHASE_ERROR;
-    }
-    else if ( SCSI_CMD_WRITE_10 == cbw->command[0] && is_data_in(cbw->dir) )
-    {
-      TU_LOG(MSC_DEBUG, "  SCSI case 8 (Hi <> Do)\r\n");
+    } else if (SCSI_CMD_WRITE_10 == cbw->command[0] && is_data_in(cbw->dir)) {
+      TU_LOG_DRV("  SCSI case 8 (Hi <> Do)\r\n");
       status = MSC_CSW_STATUS_PHASE_ERROR;
-    }
-    else if ( 0 == block_count )
-    {
-      TU_LOG(MSC_DEBUG, "  SCSI case 4 Hi > Dn (READ10) or case 9 Ho > Dn (WRITE10) \r\n");
-      status =  MSC_CSW_STATUS_FAILED;
-    }
-    else if ( cbw->total_bytes / block_count == 0 )
-    {
-      TU_LOG(MSC_DEBUG, " Computed block size = 0. SCSI case 7 Hi < Di (READ10) or case 13 Ho < Do (WRIT10)\r\n");
+    } else if (0 == block_count) {
+      TU_LOG_DRV("  SCSI case 4 Hi > Dn (READ10) or case 9 Ho > Dn (WRITE10) \r\n");
+      status = MSC_CSW_STATUS_FAILED;
+    } else if (cbw->total_bytes / block_count == 0) {
+      TU_LOG_DRV(" Computed block size = 0. SCSI case 7 Hi < Di (READ10) or case 13 Ho < Do (WRIT10)\r\n");
       status = MSC_CSW_STATUS_PHASE_ERROR;
     }
   }
@@ -200,10 +180,9 @@ uint8_t rdwr10_validate_cmd(msc_cbw_t const* cbw)
 //--------------------------------------------------------------------+
 // Debug
 //--------------------------------------------------------------------+
-#if CFG_TUSB_DEBUG >= 2
+#if CFG_TUSB_DEBUG >= CFG_TUD_MSC_LOG_LEVEL
 
-TU_ATTR_UNUSED static tu_lookup_entry_t const _msc_scsi_cmd_lookup[] =
-{
+TU_ATTR_UNUSED tu_static tu_lookup_entry_t const _msc_scsi_cmd_lookup[] = {
   { .key = SCSI_CMD_TEST_UNIT_READY              , .data = "Test Unit Ready" },
   { .key = SCSI_CMD_INQUIRY                      , .data = "Inquiry" },
   { .key = SCSI_CMD_MODE_SELECT_6                , .data = "Mode_Select 6" },
@@ -217,8 +196,7 @@ TU_ATTR_UNUSED static tu_lookup_entry_t const _msc_scsi_cmd_lookup[] =
   { .key = SCSI_CMD_WRITE_10                     , .data = "Write10" }
 };
 
-TU_ATTR_UNUSED static tu_lookup_table_t const _msc_scsi_cmd_table =
-{
+TU_ATTR_UNUSED tu_static tu_lookup_table_t const _msc_scsi_cmd_table = {
   .count = TU_ARRAY_SIZE(_msc_scsi_cmd_lookup),
   .items = _msc_scsi_cmd_lookup
 };
@@ -228,19 +206,15 @@ TU_ATTR_UNUSED static tu_lookup_table_t const _msc_scsi_cmd_table =
 //--------------------------------------------------------------------+
 // APPLICATION API
 //--------------------------------------------------------------------+
-bool tud_msc_set_sense(uint8_t lun, uint8_t sense_key, uint8_t add_sense_code, uint8_t add_sense_qualifier)
-{
+bool tud_msc_set_sense(uint8_t lun, uint8_t sense_key, uint8_t add_sense_code, uint8_t add_sense_qualifier) {
   (void) lun;
-
   _mscd_itf.sense_key           = sense_key;
   _mscd_itf.add_sense_code      = add_sense_code;
   _mscd_itf.add_sense_qualifier = add_sense_qualifier;
-
   return true;
 }
 
-static inline void set_sense_medium_not_present(uint8_t lun)
-{
+static inline void set_sense_medium_not_present(uint8_t lun) {
   // default sense is NOT READY, MEDIUM NOT PRESENT
   tud_msc_set_sense(lun, SCSI_SENSE_NOT_READY, 0x3A, 0x00);
 }
@@ -248,48 +222,43 @@ static inline void set_sense_medium_not_present(uint8_t lun)
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void mscd_init(void)
-{
+void mscd_init(void) {
   tu_memclr(&_mscd_itf, sizeof(mscd_interface_t));
 }
 
-void mscd_reset(uint8_t rhport)
-{
+bool mscd_deinit(void) {
+  return true; // nothing to do
+}
+
+void mscd_reset(uint8_t rhport) {
   (void) rhport;
   tu_memclr(&_mscd_itf, sizeof(mscd_interface_t));
 }
 
-uint16_t mscd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len)
-{
+uint16_t mscd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len) {
   // only support SCSI's BOT protocol
   TU_VERIFY(TUSB_CLASS_MSC    == itf_desc->bInterfaceClass &&
             MSC_SUBCLASS_SCSI == itf_desc->bInterfaceSubClass &&
             MSC_PROTOCOL_BOT  == itf_desc->bInterfaceProtocol, 0);
-
-  // msc driver length is fixed
   uint16_t const drv_len = sizeof(tusb_desc_interface_t) + 2*sizeof(tusb_desc_endpoint_t);
-
-  // Max length must be at least 1 interface + 2 endpoints
-  TU_ASSERT(max_len >= drv_len, 0);
+  TU_ASSERT(max_len >= drv_len, 0); // Max length must be at least 1 interface + 2 endpoints
 
   mscd_interface_t * p_msc = &_mscd_itf;
   p_msc->itf_num = itf_desc->bInterfaceNumber;
 
   // Open endpoint pair
-  TU_ASSERT( usbd_open_edpt_pair(rhport, tu_desc_next(itf_desc), 2, TUSB_XFER_BULK, &p_msc->ep_out, &p_msc->ep_in), 0 );
+  TU_ASSERT(usbd_open_edpt_pair(rhport, tu_desc_next(itf_desc), 2, TUSB_XFER_BULK, &p_msc->ep_out, &p_msc->ep_in), 0);
 
   // Prepare for Command Block Wrapper
-  TU_ASSERT( prepare_cbw(rhport, p_msc), drv_len);
+  TU_ASSERT(prepare_cbw(rhport, p_msc), drv_len);
 
   return drv_len;
 }
 
-static void proc_bot_reset(mscd_interface_t* p_msc)
-{
+static void proc_bot_reset(mscd_interface_t* p_msc) {
   p_msc->stage       = MSC_STAGE_CMD;
   p_msc->total_len   = 0;
   p_msc->xferred_len = 0;
-
   p_msc->sense_key           = 0;
   p_msc->add_sense_code      = 0;
   p_msc->add_sense_qualifier = 0;
@@ -298,10 +267,10 @@ static void proc_bot_reset(mscd_interface_t* p_msc)
 // Invoked when a control transfer occurred on an interface of this class
 // Driver response accordingly to the request and the transfer stage (setup/data/ack)
 // return false to stall control endpoint (e.g unsupported request)
-bool mscd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
-{
-  // nothing to do with DATA & ACK stage
-  if (stage != CONTROL_STAGE_SETUP) return true;
+bool mscd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) {
+  if (stage != CONTROL_STAGE_SETUP) {
+    return true; // nothing to do with DATA & ACK stage
+  }
 
   mscd_interface_t* p_msc = &_mscd_itf;
 
@@ -309,35 +278,25 @@ bool mscd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t
   if ( TUSB_REQ_TYPE_STANDARD     == request->bmRequestType_bit.type      &&
        TUSB_REQ_RCPT_ENDPOINT     == request->bmRequestType_bit.recipient &&
        TUSB_REQ_CLEAR_FEATURE     == request->bRequest                    &&
-       TUSB_REQ_FEATURE_EDPT_HALT == request->wValue )
-  {
+       TUSB_REQ_FEATURE_EDPT_HALT == request->wValue ) {
     uint8_t const ep_addr = tu_u16_low(request->wIndex);
 
-    if ( p_msc->stage == MSC_STAGE_NEED_RESET )
-    {
+    if (p_msc->stage == MSC_STAGE_NEED_RESET) {
       // reset recovery is required to recover from this stage
       // Clear Stall request cannot resolve this -> continue to stall endpoint
       usbd_edpt_stall(rhport, ep_addr);
-    }
-    else
-    {
-      if ( ep_addr == p_msc->ep_in )
-      {
-        if ( p_msc->stage == MSC_STAGE_STATUS )
-        {
+    } else {
+      if (ep_addr == p_msc->ep_in) {
+        if (p_msc->stage == MSC_STAGE_STATUS) {
           // resume sending SCSI status if we are in this stage previously before stalled
-          TU_ASSERT( send_csw(rhport, p_msc) );
+          TU_ASSERT(send_csw(rhport, p_msc));
         }
-      }
-      else if ( ep_addr == p_msc->ep_out )
-      {
-        if ( p_msc->stage == MSC_STAGE_CMD )
-        {
+      } else if (ep_addr == p_msc->ep_out) {
+        if (p_msc->stage == MSC_STAGE_CMD) {
           // part of reset recovery (probably due to invalid CBW) -> prepare for new command
           // Note: skip if already queued previously
-          if ( usbd_edpt_ready(rhport, p_msc->ep_out) )
-          {
-            TU_ASSERT( prepare_cbw(rhport, p_msc) );
+          if (usbd_edpt_ready(rhport, p_msc->ep_out)) {
+            TU_ASSERT(prepare_cbw(rhport, p_msc));
           }
         }
       }
@@ -349,33 +308,27 @@ bool mscd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t
   // From this point only handle class request only
   TU_VERIFY(request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS);
 
-  switch ( request->bRequest )
-  {
+  switch ( request->bRequest ) {
     case MSC_REQ_RESET:
-      TU_LOG(MSC_DEBUG, "  MSC BOT Reset\r\n");
+      TU_LOG_DRV("  MSC BOT Reset\r\n");
       TU_VERIFY(request->wValue == 0 && request->wLength == 0);
-
-      // driver state reset
-      proc_bot_reset(p_msc);
-
+      proc_bot_reset(p_msc); // driver state reset
       tud_control_status(rhport, request);
     break;
 
-    case MSC_REQ_GET_MAX_LUN:
-    {
-      TU_LOG(MSC_DEBUG, "  MSC Get Max Lun\r\n");
+    case MSC_REQ_GET_MAX_LUN: {
+      TU_LOG_DRV("  MSC Get Max Lun\r\n");
       TU_VERIFY(request->wValue == 0 && request->wLength == 1);
 
       uint8_t maxlun = 1;
-      if (tud_msc_get_maxlun_cb) maxlun = tud_msc_get_maxlun_cb();
+      if (tud_msc_get_maxlun_cb) {
+        maxlun = tud_msc_get_maxlun_cb();
+      }
       TU_VERIFY(maxlun);
-
-      // MAX LUN is minus 1 by specs
-      maxlun--;
-
+      maxlun--; // MAX LUN is minus 1 by specs
       tud_control_xfer(rhport, request, &maxlun, 1);
+      break;
     }
-    break;
 
     default: return false; // stall unsupported request
   }
@@ -383,37 +336,36 @@ bool mscd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t
   return true;
 }
 
-bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes)
-{
+bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes) {
   (void) event;
 
   mscd_interface_t* p_msc = &_mscd_itf;
-  msc_cbw_t const * p_cbw = &p_msc->cbw;
-  msc_csw_t       * p_csw = &p_msc->csw;
+  msc_cbw_t * p_cbw = &p_msc->cbw;
+  msc_csw_t * p_csw = &p_msc->csw;
 
-  switch (p_msc->stage)
-  {
+  switch (p_msc->stage) {
     case MSC_STAGE_CMD:
       //------------- new CBW received -------------//
       // Complete IN while waiting for CMD is usually Status of previous SCSI op, ignore it
-      if(ep_addr != p_msc->ep_out) return true;
+      if (ep_addr != p_msc->ep_out) {
+        return true;
+      }
 
-      if ( !(xferred_bytes == sizeof(msc_cbw_t) && p_cbw->signature == MSC_CBW_SIGNATURE) )
-      {
-        TU_LOG(MSC_DEBUG, "  SCSI CBW is not valid\r\n");
+      const uint32_t signature = tu_le32toh(tu_unaligned_read32(_mscd_epbuf.buf));
 
+      if (!(xferred_bytes == sizeof(msc_cbw_t) && signature == MSC_CBW_SIGNATURE)) {
         // BOT 6.6.1 If CBW is not valid stall both endpoints until reset recovery
+        TU_LOG_DRV("  SCSI CBW is not valid\r\n");
         p_msc->stage = MSC_STAGE_NEED_RESET;
-
-        // invalid CBW stall both endpoints
         usbd_edpt_stall(rhport, p_msc->ep_in);
         usbd_edpt_stall(rhport, p_msc->ep_out);
-
         return false;
       }
 
-      TU_LOG(MSC_DEBUG, "  SCSI Command [Lun%u]: %s\r\n", p_cbw->lun, tu_lookup_find(&_msc_scsi_cmd_table, p_cbw->command[0]));
-      //TU_LOG_MEM(MSC_DEBUG, p_cbw, xferred_bytes, 2);
+      memcpy(p_cbw, _mscd_epbuf.buf, sizeof(msc_cbw_t));
+
+      TU_LOG_DRV("  SCSI Command [Lun%u]: %s\r\n", p_cbw->lun, tu_lookup_find(&_msc_scsi_cmd_table, p_cbw->command[0]));
+      // TU_LOG_MEM(CFG_TUD_MSC_LOG_LEVEL, p_cbw, xferred_bytes, 2);
 
       p_csw->signature    = MSC_CSW_SIGNATURE;
       p_csw->tag          = p_cbw->tag;
@@ -426,87 +378,65 @@ bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
       p_msc->xferred_len = 0;
 
       // Read10 or Write10
-      if ( (SCSI_CMD_READ_10 == p_cbw->command[0]) || (SCSI_CMD_WRITE_10 == p_cbw->command[0]) )
-      {
+      if ((SCSI_CMD_READ_10 == p_cbw->command[0]) || (SCSI_CMD_WRITE_10 == p_cbw->command[0])) {
         uint8_t const status = rdwr10_validate_cmd(p_cbw);
 
-        if ( status != MSC_CSW_STATUS_PASSED)
-        {
+        if (status != MSC_CSW_STATUS_PASSED) {
           fail_scsi_op(rhport, p_msc, status);
-        }else if ( p_cbw->total_bytes )
-        {
-          if (SCSI_CMD_READ_10 == p_cbw->command[0])
-          {
+        } else if (p_cbw->total_bytes) {
+          if (SCSI_CMD_READ_10 == p_cbw->command[0]) {
             proc_read10_cmd(rhport, p_msc);
-          }else
-          {
+          } else {
             proc_write10_cmd(rhport, p_msc);
           }
-        }else
-        {
+        } else {
           // no data transfer, only exist in complaint test suite
           p_msc->stage = MSC_STAGE_STATUS;
         }
-      }
-      else
-      {
+      } else {
         // For other SCSI commands
         // 1. OUT : queue transfer (invoke app callback after done)
         // 2. IN & Zero: Process if is built-in, else Invoke app callback. Skip DATA if zero length
-        if ( (p_cbw->total_bytes > 0 ) && !is_data_in(p_cbw->dir) )
-        {
-          if (p_cbw->total_bytes > sizeof(_mscd_buf))
-          {
-            TU_LOG(MSC_DEBUG, "  SCSI reject non READ10/WRITE10 with large data\r\n");
+        if ((p_cbw->total_bytes > 0) && !is_data_in(p_cbw->dir)) {
+          if (p_cbw->total_bytes > CFG_TUD_MSC_EP_BUFSIZE) {
+            TU_LOG_DRV("  SCSI reject non READ10/WRITE10 with large data\r\n");
             fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED);
-          }else
-          {
+          } else {
             // Didn't check for case 9 (Ho > Dn), which requires examining scsi command first
             // but it is OK to just receive data then responded with failed status
-            TU_ASSERT( usbd_edpt_xfer(rhport, p_msc->ep_out, _mscd_buf, (uint16_t) p_msc->total_len) );
+            TU_ASSERT(usbd_edpt_xfer(rhport, p_msc->ep_out, _mscd_epbuf.buf, (uint16_t) p_msc->total_len));
           }
-        }else
-        {
+        } else {
           // First process if it is a built-in commands
-          int32_t resplen = proc_builtin_scsi(p_cbw->lun, p_cbw->command, _mscd_buf, sizeof(_mscd_buf));
+          int32_t resplen = proc_builtin_scsi(p_cbw->lun, p_cbw->command, _mscd_epbuf.buf, CFG_TUD_MSC_EP_BUFSIZE);
 
           // Invoke user callback if not built-in
-          if ( (resplen < 0) && (p_msc->sense_key == 0) )
-          {
-            resplen = tud_msc_scsi_cb(p_cbw->lun, p_cbw->command, _mscd_buf, (uint16_t) p_msc->total_len);
+          if ((resplen < 0) && (p_msc->sense_key == 0)) {
+            resplen = tud_msc_scsi_cb(p_cbw->lun, p_cbw->command, _mscd_epbuf.buf, (uint16_t)p_msc->total_len);
           }
 
-          if ( resplen < 0 )
-          {
+          if (resplen < 0) {
             // unsupported command
-            TU_LOG(MSC_DEBUG, "  SCSI unsupported or failed command\r\n");
+            TU_LOG_DRV("  SCSI unsupported or failed command\r\n");
             fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED);
-          }
-          else if (resplen == 0)
-          {
-            if (p_cbw->total_bytes)
-            {
+          } else if (resplen == 0) {
+            if (p_cbw->total_bytes) {
               // 6.7 The 13 Cases: case 4 (Hi > Dn)
-              // TU_LOG(MSC_DEBUG, "  SCSI case 4 (Hi > Dn): %lu\r\n", p_cbw->total_bytes);
+              // TU_LOG_DRV("  SCSI case 4 (Hi > Dn): %lu\r\n", p_cbw->total_bytes);
               fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED);
-            }else
-            {
+            } else {
               // case 1 Hn = Dn: all good
               p_msc->stage = MSC_STAGE_STATUS;
             }
-          }
-          else
-          {
-            if ( p_cbw->total_bytes == 0 )
-            {
+          } else {
+            if (p_cbw->total_bytes == 0) {
               // 6.7 The 13 Cases: case 2 (Hn < Di)
-              // TU_LOG(MSC_DEBUG, "  SCSI case 2 (Hn < Di): %lu\r\n", p_cbw->total_bytes);
+              // TU_LOG_DRV("  SCSI case 2 (Hn < Di): %lu\r\n", p_cbw->total_bytes);
               fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED);
-            }else
-            {
+            } else {
               // cannot return more than host expect
-              p_msc->total_len = tu_min32((uint32_t) resplen, p_cbw->total_bytes);
-              TU_ASSERT( usbd_edpt_xfer(rhport, p_msc->ep_in, _mscd_buf, (uint16_t) p_msc->total_len) );
+              p_msc->total_len = tu_min32((uint32_t)resplen, p_cbw->total_bytes);
+              TU_ASSERT(usbd_edpt_xfer(rhport, p_msc->ep_in, _mscd_epbuf.buf, (uint16_t) p_msc->total_len));
             }
           }
         }
@@ -514,53 +444,41 @@ bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
     break;
 
     case MSC_STAGE_DATA:
-      TU_LOG(MSC_DEBUG, "  SCSI Data [Lun%u]\r\n", p_cbw->lun);
-      //TU_LOG_MEM(MSC_DEBUG, _mscd_buf, xferred_bytes, 2);
+      TU_LOG_DRV("  SCSI Data [Lun%u]\r\n", p_cbw->lun);
+      TU_ASSERT(xferred_bytes <= CFG_TUD_MSC_EP_BUFSIZE); // sanity check to avoid buffer overflow
+      // TU_LOG_MEM(CFG_TUD_MSC_LOG_LEVEL, _mscd_epbuf.buf, xferred_bytes, 2);
 
-      if (SCSI_CMD_READ_10 == p_cbw->command[0])
-      {
+      if (SCSI_CMD_READ_10 == p_cbw->command[0]) {
         p_msc->xferred_len += xferred_bytes;
 
-        if ( p_msc->xferred_len >= p_msc->total_len )
-        {
+        if ( p_msc->xferred_len >= p_msc->total_len ) {
           // Data Stage is complete
           p_msc->stage = MSC_STAGE_STATUS;
-        }else
-        {
+        }else {
           proc_read10_cmd(rhport, p_msc);
         }
-      }
-      else if (SCSI_CMD_WRITE_10 == p_cbw->command[0])
-      {
+      } else if (SCSI_CMD_WRITE_10 == p_cbw->command[0]) {
         proc_write10_new_data(rhport, p_msc, xferred_bytes);
-      }
-      else
-      {
+      } else {
         p_msc->xferred_len += xferred_bytes;
 
         // OUT transfer, invoke callback if needed
-        if ( !is_data_in(p_cbw->dir) )
-        {
-          int32_t cb_result = tud_msc_scsi_cb(p_cbw->lun, p_cbw->command, _mscd_buf, (uint16_t) p_msc->total_len);
+        if ( !is_data_in(p_cbw->dir) ) {
+          int32_t cb_result = tud_msc_scsi_cb(p_cbw->lun, p_cbw->command, _mscd_epbuf.buf, (uint16_t) p_msc->total_len);
 
-          if ( cb_result < 0 )
-          {
+          if ( cb_result < 0 ) {
             // unsupported command
-            TU_LOG(MSC_DEBUG, "  SCSI unsupported command\r\n");
+            TU_LOG_DRV("  SCSI unsupported command\r\n");
             fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED);
-          }else
-          {
+          }else {
             // TODO haven't implement this scenario any further yet
           }
         }
 
-        if ( p_msc->xferred_len >= p_msc->total_len )
-        {
+        if ( p_msc->xferred_len >= p_msc->total_len ) {
           // Data Stage is complete
           p_msc->stage = MSC_STAGE_STATUS;
-        }
-        else
-        {
+        } else {
           // This scenario with command that take more than one transfer is already rejected at Command stage
           TU_BREAKPOINT();
         }
@@ -573,53 +491,52 @@ bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
 
     case MSC_STAGE_STATUS_SENT:
       // Wait for the Status phase to complete
-      if( (ep_addr == p_msc->ep_in) && (xferred_bytes == sizeof(msc_csw_t)) )
-      {
-        TU_LOG(MSC_DEBUG, "  SCSI Status [Lun%u] = %u\r\n", p_cbw->lun, p_csw->status);
-        // TU_LOG_MEM(MSC_DEBUG, p_csw, xferred_bytes, 2);
+      if ((ep_addr == p_msc->ep_in) && (xferred_bytes == sizeof(msc_csw_t))) {
+        TU_LOG_DRV("  SCSI Status [Lun%u] = %u\r\n", p_cbw->lun, p_csw->status);
+        // TU_LOG_MEM(CFG_TUD_MSC_LOG_LEVEL, p_csw, xferred_bytes, 2);
 
         // Invoke complete callback if defined
         // Note: There is racing issue with samd51 + qspi flash testing with arduino
         // if complete_cb() is invoked after queuing the status.
-        switch(p_cbw->command[0])
-        {
+        switch (p_cbw->command[0]) {
           case SCSI_CMD_READ_10:
-            if ( tud_msc_read10_complete_cb ) tud_msc_read10_complete_cb(p_cbw->lun);
-          break;
+            if (tud_msc_read10_complete_cb) {
+              tud_msc_read10_complete_cb(p_cbw->lun);
+            }
+            break;
 
           case SCSI_CMD_WRITE_10:
-            if ( tud_msc_write10_complete_cb ) tud_msc_write10_complete_cb(p_cbw->lun);
-          break;
+            if (tud_msc_write10_complete_cb) {
+              tud_msc_write10_complete_cb(p_cbw->lun);
+            }
+            break;
 
           default:
-            if ( tud_msc_scsi_complete_cb ) tud_msc_scsi_complete_cb(p_cbw->lun, p_cbw->command);
-          break;
+            if (tud_msc_scsi_complete_cb) {
+              tud_msc_scsi_complete_cb(p_cbw->lun, p_cbw->command);
+            }
+            break;
         }
 
-        TU_ASSERT( prepare_cbw(rhport, p_msc) );
-      }else
-      {
+        TU_ASSERT(prepare_cbw(rhport, p_msc));
+      } else {
         // Any xfer ended here is consider unknown error, ignore it
         TU_LOG1("  Warning expect SCSI Status but received unknown data\r\n");
       }
-    break;
+      break;
 
-    default : break;
+    default: break;
   }
 
-  if ( p_msc->stage == MSC_STAGE_STATUS )
-  {
+  if (p_msc->stage == MSC_STAGE_STATUS) {
     // skip status if epin is currently stalled, will do it when received Clear Stall request
-    if ( !usbd_edpt_stalled(rhport,  p_msc->ep_in) )
-    {
-      if ( (p_cbw->total_bytes > p_msc->xferred_len) && is_data_in(p_cbw->dir) )
-      {
+    if (!usbd_edpt_stalled(rhport, p_msc->ep_in)) {
+      if ((p_cbw->total_bytes > p_msc->xferred_len) && is_data_in(p_cbw->dir)) {
         // 6.7 The 13 Cases: case 5 (Hi > Di): STALL before status
-        // TU_LOG(MSC_DEBUG, "  SCSI case 5 (Hi > Di): %lu > %lu\r\n", p_cbw->total_bytes, p_msc->xferred_len);
+        // TU_LOG_DRV("  SCSI case 5 (Hi > Di): %lu > %lu\r\n", p_cbw->total_bytes, p_msc->xferred_len);
         usbd_edpt_stall(rhport, p_msc->ep_in);
-      }else
-      {
-        TU_ASSERT( send_csw(rhport, p_msc) );
+      } else {
+        TU_ASSERT(send_csw(rhport, p_msc));
       }
     }
 
@@ -627,8 +544,7 @@ bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
     // WORKAROUND: cxd56 has its own nuttx usb stack which does not forward Set/ClearFeature(Endpoint) to DCD.
     // There is no way for us to know when EP is un-stall, therefore we will unconditionally un-stall here and
     // hope everything will work
-    if ( usbd_edpt_stalled(rhport, p_msc->ep_in) )
-    {
+    if ( usbd_edpt_stalled(rhport, p_msc->ep_in) ) {
       usbd_edpt_clear_stall(rhport, p_msc->ep_in);
       send_csw(rhport, p_msc);
     }
@@ -644,82 +560,97 @@ bool mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t
 
 // return response's length (copied to buffer). Negative if it is not an built-in command or indicate Failed status (CSW)
 // In case of a failed status, sense key must be set for reason of failure
-static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_t* buffer, uint32_t bufsize)
-{
-  (void) bufsize; // TODO refractor later
+static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_t* buffer, uint32_t bufsize) {
+  (void)bufsize; // TODO refractor later
   int32_t resplen;
 
   mscd_interface_t* p_msc = &_mscd_itf;
 
-  switch ( scsi_cmd[0] )
-  {
+  switch (scsi_cmd[0]) {
     case SCSI_CMD_TEST_UNIT_READY:
       resplen = 0;
-      if ( !tud_msc_test_unit_ready_cb(lun) )
-      {
+      if (!tud_msc_test_unit_ready_cb(lun)) {
         // Failed status response
-        resplen = - 1;
+        resplen = -1;
 
         // set default sense if not set by callback
-        if ( p_msc->sense_key == 0 ) set_sense_medium_not_present(lun);
+        if (p_msc->sense_key == 0) {
+          set_sense_medium_not_present(lun);
+        }
       }
-    break;
+      break;
 
     case SCSI_CMD_START_STOP_UNIT:
       resplen = 0;
 
-      if (tud_msc_start_stop_cb)
-      {
-        scsi_start_stop_unit_t const * start_stop = (scsi_start_stop_unit_t const *) scsi_cmd;
-        if ( !tud_msc_start_stop_cb(lun, start_stop->power_condition, start_stop->start, start_stop->load_eject) )
-        {
+      if (tud_msc_start_stop_cb) {
+        scsi_start_stop_unit_t const* start_stop = (scsi_start_stop_unit_t const*)scsi_cmd;
+        if (!tud_msc_start_stop_cb(lun, start_stop->power_condition, start_stop->start, start_stop->load_eject)) {
           // Failed status response
-          resplen = - 1;
+          resplen = -1;
 
           // set default sense if not set by callback
-          if ( p_msc->sense_key == 0 ) set_sense_medium_not_present(lun);
+          if (p_msc->sense_key == 0) {
+            set_sense_medium_not_present(lun);
+          }
         }
       }
-    break;
+      break;
+
+    case SCSI_CMD_PREVENT_ALLOW_MEDIUM_REMOVAL:
+      resplen = 0;
+
+      if (tud_msc_prevent_allow_medium_removal_cb) {
+        scsi_prevent_allow_medium_removal_t const* prevent_allow = (scsi_prevent_allow_medium_removal_t const*)scsi_cmd;
+        if (!tud_msc_prevent_allow_medium_removal_cb(lun, prevent_allow->prohibit_removal, prevent_allow->control)) {
+          // Failed status response
+          resplen = -1;
 
-    case SCSI_CMD_READ_CAPACITY_10:
-    {
+          // set default sense if not set by callback
+          if (p_msc->sense_key == 0) {
+            set_sense_medium_not_present(lun);
+          }
+        }
+      }
+      break;
+
+
+    case SCSI_CMD_READ_CAPACITY_10: {
       uint32_t block_count;
       uint32_t block_size;
       uint16_t block_size_u16;
 
       tud_msc_capacity_cb(lun, &block_count, &block_size_u16);
-      block_size = (uint32_t) block_size_u16;
+      block_size = (uint32_t)block_size_u16;
 
       // Invalid block size/count from callback, possibly unit is not ready
       // stall this request, set sense key to NOT READY
-      if (block_count == 0 || block_size == 0)
-      {
+      if (block_count == 0 || block_size == 0) {
         resplen = -1;
 
         // set default sense if not set by callback
-        if ( p_msc->sense_key == 0 ) set_sense_medium_not_present(lun);
-      }else
-      {
+        if (p_msc->sense_key == 0) {
+          set_sense_medium_not_present(lun);
+        }
+      } else {
         scsi_read_capacity10_resp_t read_capa10;
 
-        read_capa10.last_lba   = tu_htonl(block_count-1);
+        read_capa10.last_lba = tu_htonl(block_count-1);
         read_capa10.block_size = tu_htonl(block_size);
 
         resplen = sizeof(read_capa10);
-        memcpy(buffer, &read_capa10, (size_t) resplen);
+        TU_VERIFY(0 == tu_memcpy_s(buffer, bufsize, &read_capa10, (size_t) resplen));
       }
     }
     break;
 
-    case SCSI_CMD_READ_FORMAT_CAPACITY:
-    {
+    case SCSI_CMD_READ_FORMAT_CAPACITY: {
       scsi_read_format_capacity_data_t read_fmt_capa =
       {
-          .list_length     = 8,
-          .block_num       = 0,
-          .descriptor_type = 2, // formatted media
-          .block_size_u16  = 0
+        .list_length = 8,
+        .block_num = 0,
+        .descriptor_type = 2, // formatted media
+        .block_size_u16 = 0
       };
 
       uint32_t block_count;
@@ -729,31 +660,30 @@ static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_
 
       // Invalid block size/count from callback, possibly unit is not ready
       // stall this request, set sense key to NOT READY
-      if (block_count == 0 || block_size == 0)
-      {
+      if (block_count == 0 || block_size == 0) {
         resplen = -1;
 
         // set default sense if not set by callback
-        if ( p_msc->sense_key == 0 ) set_sense_medium_not_present(lun);
-      }else
-      {
+        if (p_msc->sense_key == 0) {
+          set_sense_medium_not_present(lun);
+        }
+      } else {
         read_fmt_capa.block_num = tu_htonl(block_count);
         read_fmt_capa.block_size_u16 = tu_htons(block_size);
 
         resplen = sizeof(read_fmt_capa);
-        memcpy(buffer, &read_fmt_capa, (size_t) resplen);
+        TU_VERIFY(0 == tu_memcpy_s(buffer, bufsize, &read_fmt_capa, (size_t) resplen));
       }
     }
     break;
 
-    case SCSI_CMD_INQUIRY:
-    {
+    case SCSI_CMD_INQUIRY: {
       scsi_inquiry_resp_t inquiry_rsp =
       {
-          .is_removable         = 1,
-          .version              = 2,
-          .response_data_format = 2,
-          .additional_length    = sizeof(scsi_inquiry_resp_t) - 5,
+        .is_removable = 1,
+        .version = 2,
+        .response_data_format = 2,
+        .additional_length = sizeof(scsi_inquiry_resp_t) - 5,
       };
 
       // vendor_id, product_id, product_rev is space padded string
@@ -764,54 +694,50 @@ static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_
       tud_msc_inquiry_cb(lun, inquiry_rsp.vendor_id, inquiry_rsp.product_id, inquiry_rsp.product_rev);
 
       resplen = sizeof(inquiry_rsp);
-      memcpy(buffer, &inquiry_rsp, (size_t) resplen);
+      TU_VERIFY(0 == tu_memcpy_s(buffer, bufsize, &inquiry_rsp, (size_t) resplen));
     }
     break;
 
-    case SCSI_CMD_MODE_SENSE_6:
-    {
+    case SCSI_CMD_MODE_SENSE_6: {
       scsi_mode_sense6_resp_t mode_resp =
       {
-          .data_len             = 3,
-          .medium_type          = 0,
-          .write_protected      = false,
-          .reserved             = 0,
-          .block_descriptor_len = 0  // no block descriptor are included
+        .data_len = 3,
+        .medium_type = 0,
+        .write_protected = false,
+        .reserved = 0,
+        .block_descriptor_len = 0 // no block descriptor are included
       };
 
       bool writable = true;
-      if ( tud_msc_is_writable_cb )
-      {
+      if (tud_msc_is_writable_cb) {
         writable = tud_msc_is_writable_cb(lun);
       }
 
       mode_resp.write_protected = !writable;
 
       resplen = sizeof(mode_resp);
-      memcpy(buffer, &mode_resp, (size_t) resplen);
+      TU_VERIFY(0 == tu_memcpy_s(buffer, bufsize, &mode_resp, (size_t) resplen));
     }
     break;
 
-    case SCSI_CMD_REQUEST_SENSE:
-    {
+    case SCSI_CMD_REQUEST_SENSE: {
       scsi_sense_fixed_resp_t sense_rsp =
       {
-          .response_code = 0x70, // current, fixed format
-          .valid         = 1
+        .response_code = 0x70, // current, fixed format
+        .valid = 1
       };
 
-      sense_rsp.add_sense_len       = sizeof(scsi_sense_fixed_resp_t) - 8;
-      sense_rsp.sense_key           = (uint8_t) (p_msc->sense_key & 0x0F);
-      sense_rsp.add_sense_code      = p_msc->add_sense_code;
+      sense_rsp.add_sense_len = sizeof(scsi_sense_fixed_resp_t) - 8;
+      sense_rsp.sense_key = (uint8_t)(p_msc->sense_key & 0x0F);
+      sense_rsp.add_sense_code = p_msc->add_sense_code;
       sense_rsp.add_sense_qualifier = p_msc->add_sense_qualifier;
 
       resplen = sizeof(sense_rsp);
-      memcpy(buffer, &sense_rsp, (size_t) resplen);
+      TU_VERIFY(0 == tu_memcpy_s(buffer, bufsize, &sense_rsp, (size_t) resplen));
 
       // request sense callback could overwrite the sense data
-      if (tud_msc_request_sense_cb)
-      {
-        resplen = tud_msc_request_sense_cb(lun, buffer, (uint16_t) bufsize);
+      if (tud_msc_request_sense_cb) {
+        resplen = tud_msc_request_sense_cb(lun, buffer, (uint16_t)bufsize);
       }
 
       // Clear sense data after copy
@@ -819,15 +745,15 @@ static int32_t proc_builtin_scsi(uint8_t lun, uint8_t const scsi_cmd[16], uint8_
     }
     break;
 
-    default: resplen = -1; break;
+    default: resplen = -1;
+      break;
   }
 
   return resplen;
 }
 
-static void proc_read10_cmd(uint8_t rhport, mscd_interface_t* p_msc)
-{
-  msc_cbw_t const * p_cbw = &p_msc->cbw;
+static void proc_read10_cmd(uint8_t rhport, mscd_interface_t* p_msc) {
+  msc_cbw_t const* p_cbw = &p_msc->cbw;
 
   // block size already verified not zero
   uint16_t const block_sz = rdwr10_get_blocksize(p_cbw);
@@ -836,45 +762,37 @@ static void proc_read10_cmd(uint8_t rhport, mscd_interface_t* p_msc)
   uint32_t const lba = rdwr10_get_lba(p_cbw->command) + (p_msc->xferred_len / block_sz);
 
   // remaining bytes capped at class buffer
-  int32_t nbytes = (int32_t) tu_min32(sizeof(_mscd_buf), p_cbw->total_bytes-p_msc->xferred_len);
+  int32_t nbytes = (int32_t)tu_min32(CFG_TUD_MSC_EP_BUFSIZE, p_cbw->total_bytes - p_msc->xferred_len);
 
   // Application can consume smaller bytes
   uint32_t const offset = p_msc->xferred_len % block_sz;
-  nbytes = tud_msc_read10_cb(p_cbw->lun, lba, offset, _mscd_buf, (uint32_t) nbytes);
+  nbytes = tud_msc_read10_cb(p_cbw->lun, lba, offset, _mscd_epbuf.buf, (uint32_t)nbytes);
 
-  if ( nbytes < 0 )
-  {
+  if (nbytes < 0) {
     // negative means error -> endpoint is stalled & status in CSW set to failed
-    TU_LOG(MSC_DEBUG, "  tud_msc_read10_cb() return -1\r\n");
+    TU_LOG_DRV("  tud_msc_read10_cb() return -1\r\n");
 
     // set sense
     set_sense_medium_not_present(p_cbw->lun);
 
     fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED);
-  }
-  else if ( nbytes == 0 )
-  {
+  } else if (nbytes == 0) {
     // zero means not ready -> simulate an transfer complete so that this driver callback will fired again
     dcd_event_xfer_complete(rhport, p_msc->ep_in, 0, XFER_RESULT_SUCCESS, false);
-  }
-  else
-  {
-    TU_ASSERT( usbd_edpt_xfer(rhport, p_msc->ep_in, _mscd_buf, (uint16_t) nbytes), );
+  } else {
+    TU_ASSERT(usbd_edpt_xfer(rhport, p_msc->ep_in, _mscd_epbuf.buf, (uint16_t) nbytes),);
   }
 }
 
-static void proc_write10_cmd(uint8_t rhport, mscd_interface_t* p_msc)
-{
-  msc_cbw_t const * p_cbw = &p_msc->cbw;
+static void proc_write10_cmd(uint8_t rhport, mscd_interface_t* p_msc) {
+  msc_cbw_t const* p_cbw = &p_msc->cbw;
   bool writable = true;
 
-  if ( tud_msc_is_writable_cb )
-  {
+  if (tud_msc_is_writable_cb) {
     writable = tud_msc_is_writable_cb(p_cbw->lun);
   }
 
-  if ( !writable )
-  {
+  if (!writable) {
     // Not writable, complete this SCSI op with error
     // Sense = Write protected
     tud_msc_set_sense(p_cbw->lun, SCSI_SENSE_DATA_PROTECT, 0x27, 0x00);
@@ -883,16 +801,15 @@ static void proc_write10_cmd(uint8_t rhport, mscd_interface_t* p_msc)
   }
 
   // remaining bytes capped at class buffer
-  uint16_t nbytes = (uint16_t) tu_min32(sizeof(_mscd_buf), p_cbw->total_bytes-p_msc->xferred_len);
+  uint16_t nbytes = (uint16_t)tu_min32(CFG_TUD_MSC_EP_BUFSIZE, p_cbw->total_bytes - p_msc->xferred_len);
 
   // Write10 callback will be called later when usb transfer complete
-  TU_ASSERT( usbd_edpt_xfer(rhport, p_msc->ep_out, _mscd_buf, nbytes), );
+  TU_ASSERT(usbd_edpt_xfer(rhport, p_msc->ep_out, _mscd_epbuf.buf, nbytes),);
 }
 
 // process new data arrived from WRITE10
-static void proc_write10_new_data(uint8_t rhport, mscd_interface_t* p_msc, uint32_t xferred_bytes)
-{
-  msc_cbw_t const * p_cbw = &p_msc->cbw;
+static void proc_write10_new_data(uint8_t rhport, mscd_interface_t* p_msc, uint32_t xferred_bytes) {
+  msc_cbw_t const* p_cbw = &p_msc->cbw;
 
   // block size already verified not zero
   uint16_t const block_sz = rdwr10_get_blocksize(p_cbw);
@@ -902,46 +819,36 @@ static void proc_write10_new_data(uint8_t rhport, mscd_interface_t* p_msc, uint3
 
   // Invoke callback to consume new data
   uint32_t const offset = p_msc->xferred_len % block_sz;
-  int32_t nbytes = tud_msc_write10_cb(p_cbw->lun, lba, offset, _mscd_buf, xferred_bytes);
+  int32_t nbytes = tud_msc_write10_cb(p_cbw->lun, lba, offset, _mscd_epbuf.buf, xferred_bytes);
 
-  if ( nbytes < 0 )
-  {
+  if (nbytes < 0) {
     // negative means error -> failed this scsi op
-    TU_LOG(MSC_DEBUG, "  tud_msc_write10_cb() return -1\r\n");
+    TU_LOG_DRV("  tud_msc_write10_cb() return -1\r\n");
 
     // update actual byte before failed
     p_msc->xferred_len += xferred_bytes;
 
-    // Set sense
     set_sense_medium_not_present(p_cbw->lun);
-
     fail_scsi_op(rhport, p_msc, MSC_CSW_STATUS_FAILED);
-  }else
-  {
-    // Application consume less than what we got (including zero)
-    if ( (uint32_t) nbytes < xferred_bytes )
-    {
-      uint32_t const left_over = xferred_bytes - (uint32_t) nbytes;
-      if ( nbytes > 0 )
-      {
-        p_msc->xferred_len += (uint16_t) nbytes;
-        memmove(_mscd_buf, _mscd_buf+nbytes, left_over);
+  } else {
+    if ((uint32_t)nbytes < xferred_bytes) {
+      // Application consume less than what we got (including zero)
+      const uint32_t left_over = xferred_bytes - (uint32_t)nbytes;
+      if (nbytes > 0) {
+        p_msc->xferred_len += (uint16_t)nbytes;
+        memmove(_mscd_epbuf.buf, _mscd_epbuf.buf + nbytes, left_over);
       }
 
-      // simulate an transfer complete with adjusted parameters --> callback will be invoked with adjusted parameter
+      // simulate a transfer complete with adjusted parameters --> callback will be invoked with adjusted parameter
       dcd_event_xfer_complete(rhport, p_msc->ep_out, left_over, XFER_RESULT_SUCCESS, false);
-    }
-    else
-    {
+    } else {
       // Application consume all bytes in our buffer
       p_msc->xferred_len += xferred_bytes;
 
-      if ( p_msc->xferred_len >= p_msc->total_len )
-      {
+      if (p_msc->xferred_len >= p_msc->total_len) {
         // Data Stage is complete
         p_msc->stage = MSC_STAGE_STATUS;
-      }else
-      {
+      } else {
         // prepare to receive more data from host
         proc_write10_cmd(rhport, p_msc);
       }
diff --git a/src/class/msc/msc_device.h b/src/class/msc/msc_device.h
index 5839b168d..29acd280a 100644
--- a/src/class/msc/msc_device.h
+++ b/src/class/msc/msc_device.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -131,6 +131,9 @@ TU_ATTR_WEAK uint8_t tud_msc_get_maxlun_cb(void);
 // - Start = 1 : active mode, if load_eject = 1 : load disk storage
 TU_ATTR_WEAK bool tud_msc_start_stop_cb(uint8_t lun, uint8_t power_condition, bool start, bool load_eject);
 
+//Invoked when we receive the Prevent / Allow Medium Removal command
+TU_ATTR_WEAK bool tud_msc_prevent_allow_medium_removal_cb(uint8_t lun, uint8_t prohibit_removal, uint8_t control);
+
 // Invoked when received REQUEST_SENSE
 TU_ATTR_WEAK int32_t tud_msc_request_sense_cb(uint8_t lun, void* buffer, uint16_t bufsize);
 
@@ -150,6 +153,7 @@ TU_ATTR_WEAK bool tud_msc_is_writable_cb(uint8_t lun);
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     mscd_init            (void);
+bool     mscd_deinit          (void);
 void     mscd_reset           (uint8_t rhport);
 uint16_t mscd_open            (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 bool     mscd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * p_request);
diff --git a/src/class/msc/msc_host.c b/src/class/msc/msc_host.c
index 934f79ff7..ef0635bbe 100644
--- a/src/class/msc/msc_host.c
+++ b/src/class/msc/msc_host.c
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -29,124 +29,129 @@
 #if CFG_TUH_ENABLED && CFG_TUH_MSC
 
 #include "host/usbh.h"
-#include "host/usbh_classdriver.h"
+#include "host/usbh_pvt.h"
 
 #include "msc_host.h"
 
+// Level where CFG_TUSB_DEBUG must be at least for this driver is logged
+#ifndef CFG_TUH_MSC_LOG_LEVEL
+  #define CFG_TUH_MSC_LOG_LEVEL   CFG_TUH_LOG_LEVEL
+#endif
+
+#define TU_LOG_DRV(...)   TU_LOG(CFG_TUH_MSC_LOG_LEVEL, __VA_ARGS__)
+
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
-enum
-{
+enum {
   MSC_STAGE_IDLE = 0,
   MSC_STAGE_CMD,
   MSC_STAGE_DATA,
   MSC_STAGE_STATUS,
 };
 
-typedef struct
-{
+typedef struct {
   uint8_t itf_num;
   uint8_t ep_in;
   uint8_t ep_out;
-
   uint8_t max_lun;
 
   volatile bool configured; // Receive SET_CONFIGURE
   volatile bool mounted;    // Enumeration is complete
 
+  // SCSI command data
+  uint8_t stage;
+  void* buffer;
+  tuh_msc_complete_cb_t complete_cb;
+  uintptr_t complete_arg;
+
   struct {
     uint32_t block_size;
     uint32_t block_count;
   } capacity[CFG_TUH_MSC_MAXLUN];
+} msch_interface_t;
 
-  //------------- SCSI -------------//
-  uint8_t stage;
-  void*   buffer;
-  tuh_msc_complete_cb_t complete_cb;
-
-  msc_cbw_t cbw;
-  msc_csw_t csw;
-}msch_interface_t;
+typedef struct {
+  TUH_EPBUF_TYPE_DEF(msc_cbw_t, cbw);
+  TUH_EPBUF_TYPE_DEF(msc_csw_t, csw);
+} msch_epbuf_t;
 
-CFG_TUSB_MEM_SECTION static msch_interface_t _msch_itf[CFG_TUH_DEVICE_MAX];
+static msch_interface_t _msch_itf[CFG_TUH_DEVICE_MAX];
+CFG_TUH_MEM_SECTION static msch_epbuf_t _msch_epbuf[CFG_TUH_DEVICE_MAX];
 
-// buffer used to read scsi information when mounted
-// largest response data currently is inquiry TODO Inquiry is not part of enum anymore
-CFG_TUSB_MEM_SECTION TU_ATTR_ALIGNED(4)
-static uint8_t _msch_buffer[sizeof(scsi_inquiry_resp_t)];
+TU_ATTR_ALWAYS_INLINE static inline msch_interface_t* get_itf(uint8_t daddr) {
+  return &_msch_itf[daddr - 1];
+}
 
-TU_ATTR_ALWAYS_INLINE
-static inline msch_interface_t* get_itf(uint8_t dev_addr)
-{
-  return &_msch_itf[dev_addr-1];
+TU_ATTR_ALWAYS_INLINE static inline msch_epbuf_t* get_epbuf(uint8_t daddr) {
+  return &_msch_epbuf[daddr - 1];
 }
 
 //--------------------------------------------------------------------+
 // PUBLIC API
 //--------------------------------------------------------------------+
-uint8_t tuh_msc_get_maxlun(uint8_t dev_addr)
-{
+uint8_t tuh_msc_get_maxlun(uint8_t dev_addr) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   return p_msc->max_lun;
 }
 
-uint32_t tuh_msc_get_block_count(uint8_t dev_addr, uint8_t lun)
-{
+uint32_t tuh_msc_get_block_count(uint8_t dev_addr, uint8_t lun) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   return p_msc->capacity[lun].block_count;
 }
 
-uint32_t tuh_msc_get_block_size(uint8_t dev_addr, uint8_t lun)
-{
+uint32_t tuh_msc_get_block_size(uint8_t dev_addr, uint8_t lun) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   return p_msc->capacity[lun].block_size;
 }
 
-bool tuh_msc_mounted(uint8_t dev_addr)
-{
+bool tuh_msc_mounted(uint8_t dev_addr) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   return p_msc->mounted;
 }
 
-bool tuh_msc_ready(uint8_t dev_addr)
-{
+bool tuh_msc_ready(uint8_t dev_addr) {
   msch_interface_t* p_msc = get_itf(dev_addr);
-  return p_msc->mounted && !usbh_edpt_busy(dev_addr, p_msc->ep_in);
+  return p_msc->mounted && !usbh_edpt_busy(dev_addr, p_msc->ep_in) && !usbh_edpt_busy(dev_addr, p_msc->ep_out);
 }
 
 //--------------------------------------------------------------------+
 // PUBLIC API: SCSI COMMAND
 //--------------------------------------------------------------------+
-static inline void cbw_init(msc_cbw_t *cbw, uint8_t lun)
-{
+static inline void cbw_init(msc_cbw_t* cbw, uint8_t lun) {
   tu_memclr(cbw, sizeof(msc_cbw_t));
   cbw->signature = MSC_CBW_SIGNATURE;
   cbw->tag       = 0x54555342; // TUSB
   cbw->lun       = lun;
 }
 
-bool tuh_msc_scsi_command(uint8_t dev_addr, msc_cbw_t const* cbw, void* data, tuh_msc_complete_cb_t complete_cb)
-{
-  msch_interface_t* p_msc = get_itf(dev_addr);
+bool tuh_msc_scsi_command(uint8_t daddr, msc_cbw_t const* cbw, void* data,
+                          tuh_msc_complete_cb_t complete_cb, uintptr_t arg) {
+  msch_interface_t* p_msc = get_itf(daddr);
   TU_VERIFY(p_msc->configured);
 
-  // TODO claim endpoint
+  // claim endpoint
+  TU_VERIFY(usbh_edpt_claim(daddr, p_msc->ep_out));
+  msch_epbuf_t* epbuf = get_epbuf(daddr);
 
-  p_msc->cbw = *cbw;
-  p_msc->stage = MSC_STAGE_CMD;
+  epbuf->cbw = *cbw;
   p_msc->buffer = data;
   p_msc->complete_cb = complete_cb;
+  p_msc->complete_arg = arg;
+  p_msc->stage = MSC_STAGE_CMD;
 
-  TU_ASSERT(usbh_edpt_xfer(dev_addr, p_msc->ep_out, (uint8_t*) &p_msc->cbw, sizeof(msc_cbw_t)));
+  if (!usbh_edpt_xfer(daddr, p_msc->ep_out, (uint8_t*) &epbuf->cbw, sizeof(msc_cbw_t))) {
+    usbh_edpt_release(daddr, p_msc->ep_out);
+    return false;
+  }
 
   return true;
 }
 
-bool tuh_msc_read_capacity(uint8_t dev_addr, uint8_t lun, scsi_read_capacity10_resp_t* response, tuh_msc_complete_cb_t complete_cb)
-{
-   msch_interface_t* p_msc = get_itf(dev_addr);
-   TU_VERIFY(p_msc->configured);
+bool tuh_msc_read_capacity(uint8_t dev_addr, uint8_t lun, scsi_read_capacity10_resp_t* response,
+                           tuh_msc_complete_cb_t complete_cb, uintptr_t arg) {
+  msch_interface_t* p_msc = get_itf(dev_addr);
+  TU_VERIFY(p_msc->configured);
 
   msc_cbw_t cbw;
   cbw_init(&cbw, lun);
@@ -156,11 +161,11 @@ bool tuh_msc_read_capacity(uint8_t dev_addr, uint8_t lun, scsi_read_capacity10_r
   cbw.cmd_len    = sizeof(scsi_read_capacity10_t);
   cbw.command[0] = SCSI_CMD_READ_CAPACITY_10;
 
-  return tuh_msc_scsi_command(dev_addr, &cbw, response, complete_cb);
+  return tuh_msc_scsi_command(dev_addr, &cbw, response, complete_cb, arg);
 }
 
-bool tuh_msc_inquiry(uint8_t dev_addr, uint8_t lun, scsi_inquiry_resp_t* response, tuh_msc_complete_cb_t complete_cb)
-{
+bool tuh_msc_inquiry(uint8_t dev_addr, uint8_t lun, scsi_inquiry_resp_t* response,
+                     tuh_msc_complete_cb_t complete_cb, uintptr_t arg) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   TU_VERIFY(p_msc->mounted);
 
@@ -171,18 +176,16 @@ bool tuh_msc_inquiry(uint8_t dev_addr, uint8_t lun, scsi_inquiry_resp_t* respons
   cbw.dir         = TUSB_DIR_IN_MASK;
   cbw.cmd_len     = sizeof(scsi_inquiry_t);
 
-  scsi_inquiry_t const cmd_inquiry =
-  {
-    .cmd_code     = SCSI_CMD_INQUIRY,
-    .alloc_length = sizeof(scsi_inquiry_resp_t)
+  scsi_inquiry_t const cmd_inquiry = {
+      .cmd_code     = SCSI_CMD_INQUIRY,
+      .alloc_length = sizeof(scsi_inquiry_resp_t)
   };
   memcpy(cbw.command, &cmd_inquiry, cbw.cmd_len);
 
-  return tuh_msc_scsi_command(dev_addr, &cbw, response, complete_cb);
+  return tuh_msc_scsi_command(dev_addr, &cbw, response, complete_cb, arg);
 }
 
-bool tuh_msc_test_unit_ready(uint8_t dev_addr, uint8_t lun, tuh_msc_complete_cb_t complete_cb)
-{
+bool tuh_msc_test_unit_ready(uint8_t dev_addr, uint8_t lun, tuh_msc_complete_cb_t complete_cb, uintptr_t arg) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   TU_VERIFY(p_msc->configured);
 
@@ -190,16 +193,16 @@ bool tuh_msc_test_unit_ready(uint8_t dev_addr, uint8_t lun, tuh_msc_complete_cb_
   cbw_init(&cbw, lun);
 
   cbw.total_bytes = 0;
-  cbw.dir         = TUSB_DIR_OUT;
-  cbw.cmd_len     = sizeof(scsi_test_unit_ready_t);
-  cbw.command[0]  = SCSI_CMD_TEST_UNIT_READY;
-  cbw.command[1]  = lun; // according to wiki TODO need verification
+  cbw.dir        = TUSB_DIR_OUT;
+  cbw.cmd_len    = sizeof(scsi_test_unit_ready_t);
+  cbw.command[0] = SCSI_CMD_TEST_UNIT_READY;
+  cbw.command[1] = lun; // according to wiki TODO need verification
 
-  return tuh_msc_scsi_command(dev_addr, &cbw, NULL, complete_cb);
+  return tuh_msc_scsi_command(dev_addr, &cbw, NULL, complete_cb, arg);
 }
 
-bool tuh_msc_request_sense(uint8_t dev_addr, uint8_t lun, void *resposne, tuh_msc_complete_cb_t complete_cb)
-{
+bool tuh_msc_request_sense(uint8_t dev_addr, uint8_t lun, void* response,
+                           tuh_msc_complete_cb_t complete_cb, uintptr_t arg) {
   msc_cbw_t cbw;
   cbw_init(&cbw, lun);
 
@@ -207,73 +210,64 @@ bool tuh_msc_request_sense(uint8_t dev_addr, uint8_t lun, void *resposne, tuh_ms
   cbw.dir         = TUSB_DIR_IN_MASK;
   cbw.cmd_len     = sizeof(scsi_request_sense_t);
 
-  scsi_request_sense_t const cmd_request_sense =
-  {
-    .cmd_code     = SCSI_CMD_REQUEST_SENSE,
-    .alloc_length = 18
+  scsi_request_sense_t const cmd_request_sense = {
+      .cmd_code     = SCSI_CMD_REQUEST_SENSE,
+      .alloc_length = 18
   };
-
   memcpy(cbw.command, &cmd_request_sense, cbw.cmd_len);
 
-  return tuh_msc_scsi_command(dev_addr, &cbw, resposne, complete_cb);
+  return tuh_msc_scsi_command(dev_addr, &cbw, response, complete_cb, arg);
 }
 
-bool tuh_msc_read10(uint8_t dev_addr, uint8_t lun, void * buffer, uint32_t lba, uint16_t block_count, tuh_msc_complete_cb_t complete_cb)
-{
+bool tuh_msc_read10(uint8_t dev_addr, uint8_t lun, void* buffer, uint32_t lba, uint16_t block_count,
+                    tuh_msc_complete_cb_t complete_cb, uintptr_t arg) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   TU_VERIFY(p_msc->mounted);
 
   msc_cbw_t cbw;
   cbw_init(&cbw, lun);
- 
-  cbw.total_bytes = block_count*p_msc->capacity[lun].block_size;
-  cbw.dir         = TUSB_DIR_IN_MASK;
-  cbw.cmd_len     = sizeof(scsi_read10_t);
- 
-  scsi_read10_t const cmd_read10 =
-  {
-    .cmd_code    = SCSI_CMD_READ_10,
-    .lba         = tu_htonl(lba),
-    .block_count = tu_htons(block_count)
+
+  cbw.total_bytes = block_count * p_msc->capacity[lun].block_size;
+  cbw.dir = TUSB_DIR_IN_MASK;
+  cbw.cmd_len = sizeof(scsi_read10_t);
+
+  scsi_read10_t const cmd_read10 = {
+      .cmd_code    = SCSI_CMD_READ_10,
+      .lba         = tu_htonl(lba),
+      .block_count = tu_htons(block_count)
   };
- 
   memcpy(cbw.command, &cmd_read10, cbw.cmd_len);
- 
-  return tuh_msc_scsi_command(dev_addr, &cbw, buffer, complete_cb);
+
+  return tuh_msc_scsi_command(dev_addr, &cbw, buffer, complete_cb, arg);
 }
- 
-bool tuh_msc_write10(uint8_t dev_addr, uint8_t lun, void const * buffer, uint32_t lba, uint16_t block_count, tuh_msc_complete_cb_t complete_cb)
-{
+
+bool tuh_msc_write10(uint8_t dev_addr, uint8_t lun, void const* buffer, uint32_t lba, uint16_t block_count,
+                     tuh_msc_complete_cb_t complete_cb, uintptr_t arg) {
   msch_interface_t* p_msc = get_itf(dev_addr);
   TU_VERIFY(p_msc->mounted);
 
   msc_cbw_t cbw;
   cbw_init(&cbw, lun);
 
-  cbw.total_bytes = block_count*p_msc->capacity[lun].block_size;
+  cbw.total_bytes = block_count * p_msc->capacity[lun].block_size;
   cbw.dir         = TUSB_DIR_OUT;
   cbw.cmd_len     = sizeof(scsi_write10_t);
 
-  scsi_write10_t const cmd_write10 =
-  {
-    .cmd_code    = SCSI_CMD_WRITE_10,
-    .lba         = tu_htonl(lba),
-    .block_count = tu_htons(block_count)
+  scsi_write10_t const cmd_write10 = {
+      .cmd_code    = SCSI_CMD_WRITE_10,
+      .lba         = tu_htonl(lba),
+      .block_count = tu_htons(block_count)
   };
-
   memcpy(cbw.command, &cmd_write10, cbw.cmd_len);
 
-  return tuh_msc_scsi_command(dev_addr, &cbw, (void*)(uintptr_t) buffer, complete_cb);
+  return tuh_msc_scsi_command(dev_addr, &cbw, (void*) (uintptr_t) buffer, complete_cb, arg);
 }
 
 #if 0
 // MSC interface Reset (not used now)
-bool tuh_msc_reset(uint8_t dev_addr)
-{
-  tusb_control_request_t const new_request =
-  {
-    .bmRequestType_bit =
-    {
+bool tuh_msc_reset(uint8_t dev_addr) {
+  tusb_control_request_t const new_request = {
+    .bmRequestType_bit = {
       .recipient = TUSB_REQ_RCPT_INTERFACE,
       .type      = TUSB_REQ_TYPE_CLASS,
       .direction = TUSB_DIR_OUT
@@ -290,65 +284,78 @@ bool tuh_msc_reset(uint8_t dev_addr)
 //--------------------------------------------------------------------+
 // CLASS-USBH API
 //--------------------------------------------------------------------+
-void msch_init(void)
-{
+bool msch_init(void) {
+  TU_LOG_DRV("sizeof(msch_interface_t) = %u\r\n", sizeof(msch_interface_t));
+  TU_LOG_DRV("sizeof(msch_epbuf_t) = %u\r\n", sizeof(msch_epbuf_t));
   tu_memclr(_msch_itf, sizeof(_msch_itf));
+  return true;
 }
 
-void msch_close(uint8_t dev_addr)
-{
-  TU_VERIFY(dev_addr <= CFG_TUH_DEVICE_MAX, );
+bool msch_deinit(void) {
+  return true;
+}
 
+void msch_close(uint8_t dev_addr) {
+  TU_VERIFY(dev_addr <= CFG_TUH_DEVICE_MAX,);
   msch_interface_t* p_msc = get_itf(dev_addr);
+  TU_VERIFY(p_msc->configured,);
+
+  TU_LOG_DRV("  MSCh close addr = %d\r\n", dev_addr);
 
   // invoke Application Callback
-  if (p_msc->mounted && tuh_msc_umount_cb) tuh_msc_umount_cb(dev_addr);
+  if (p_msc->mounted) {
+    if (tuh_msc_umount_cb) {
+      tuh_msc_umount_cb(dev_addr);
+    }
+  }
 
   tu_memclr(p_msc, sizeof(msch_interface_t));
 }
 
-bool msch_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes)
-{
+bool msch_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes) {
   msch_interface_t* p_msc = get_itf(dev_addr);
-  msc_cbw_t const * cbw = &p_msc->cbw;
-  msc_csw_t       * csw = &p_msc->csw;
+  msch_epbuf_t* epbuf = get_epbuf(dev_addr);
+  msc_cbw_t const * cbw = &epbuf->cbw;
+  msc_csw_t       * csw = &epbuf->csw;
 
-  switch (p_msc->stage)
-  {
+  switch (p_msc->stage) {
     case MSC_STAGE_CMD:
       // Must be Command Block
-      TU_ASSERT(ep_addr == p_msc->ep_out &&  event == XFER_RESULT_SUCCESS && xferred_bytes == sizeof(msc_cbw_t));
-
-      if ( cbw->total_bytes && p_msc->buffer )
-      {
+      TU_ASSERT(ep_addr == p_msc->ep_out && event == XFER_RESULT_SUCCESS && xferred_bytes == sizeof(msc_cbw_t));
+      if (cbw->total_bytes && p_msc->buffer) {
         // Data stage if any
         p_msc->stage = MSC_STAGE_DATA;
-
         uint8_t const ep_data = (cbw->dir & TUSB_DIR_IN_MASK) ? p_msc->ep_in : p_msc->ep_out;
         TU_ASSERT(usbh_edpt_xfer(dev_addr, ep_data, p_msc->buffer, (uint16_t) cbw->total_bytes));
-      }else
-      {
-        // Status stage
-        p_msc->stage = MSC_STAGE_STATUS;
-        TU_ASSERT(usbh_edpt_xfer(dev_addr, p_msc->ep_in, (uint8_t*) &p_msc->csw, (uint16_t) sizeof(msc_csw_t)));
+        break;
       }
-    break;
+
+      TU_ATTR_FALLTHROUGH; // fallthrough to status stage
 
     case MSC_STAGE_DATA:
       // Status stage
       p_msc->stage = MSC_STAGE_STATUS;
-      TU_ASSERT(usbh_edpt_xfer(dev_addr, p_msc->ep_in, (uint8_t*) &p_msc->csw, (uint16_t) sizeof(msc_csw_t)));
-    break;
+      TU_ASSERT(usbh_edpt_xfer(dev_addr, p_msc->ep_in, (uint8_t*) csw, (uint16_t) sizeof(msc_csw_t)));
+      break;
 
     case MSC_STAGE_STATUS:
       // SCSI op is complete
       p_msc->stage = MSC_STAGE_IDLE;
 
-      if (p_msc->complete_cb) p_msc->complete_cb(dev_addr, cbw, csw);
-    break;
+      if (p_msc->complete_cb) {
+        tuh_msc_complete_data_t const cb_data = {
+            .cbw = cbw,
+            .csw = csw,
+            .scsi_data = p_msc->buffer,
+            .user_arg = p_msc->complete_arg
+        };
+        p_msc->complete_cb(dev_addr, &cb_data);
+      }
+      break;
 
-    // unknown state
-    default: break;
+      // unknown state
+    default:
+      break;
   }
 
   return true;
@@ -357,39 +364,35 @@ bool msch_xfer_cb(uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32
 //--------------------------------------------------------------------+
 // MSC Enumeration
 //--------------------------------------------------------------------+
+static void config_get_maxlun_complete(tuh_xfer_t* xfer);
+static bool config_test_unit_ready_complete(uint8_t dev_addr, tuh_msc_complete_data_t const* cb_data);
+static bool config_request_sense_complete(uint8_t dev_addr, tuh_msc_complete_data_t const* cb_data);
+static bool config_read_capacity_complete(uint8_t dev_addr, tuh_msc_complete_data_t const* cb_data);
 
-static void config_get_maxlun_complete (tuh_xfer_t* xfer);
-static bool config_test_unit_ready_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw);
-static bool config_request_sense_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw);
-static bool config_read_capacity_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw);
-
-bool msch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len)
-{
+bool msch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const* desc_itf, uint16_t max_len) {
   (void) rhport;
   TU_VERIFY (MSC_SUBCLASS_SCSI == desc_itf->bInterfaceSubClass &&
-             MSC_PROTOCOL_BOT  == desc_itf->bInterfaceProtocol);
+             MSC_PROTOCOL_BOT == desc_itf->bInterfaceProtocol);
 
   // msc driver length is fixed
-  uint16_t const drv_len = (uint16_t) (sizeof(tusb_desc_interface_t) + desc_itf->bNumEndpoints * sizeof(tusb_desc_endpoint_t));
+  uint16_t const drv_len = (uint16_t) (sizeof(tusb_desc_interface_t) +
+                                       desc_itf->bNumEndpoints * sizeof(tusb_desc_endpoint_t));
   TU_ASSERT(drv_len <= max_len);
 
   msch_interface_t* p_msc = get_itf(dev_addr);
-  tusb_desc_endpoint_t const * ep_desc = (tusb_desc_endpoint_t const *) tu_desc_next(desc_itf);
+  tusb_desc_endpoint_t const* ep_desc = (tusb_desc_endpoint_t const*) tu_desc_next(desc_itf);
 
-  for(uint32_t i=0; i<2; i++)
-  {
+  for (uint32_t i = 0; i < 2; i++) {
     TU_ASSERT(TUSB_DESC_ENDPOINT == ep_desc->bDescriptorType && TUSB_XFER_BULK == ep_desc->bmAttributes.xfer);
     TU_ASSERT(tuh_edpt_open(dev_addr, ep_desc));
 
-    if ( tu_edpt_dir(ep_desc->bEndpointAddress) == TUSB_DIR_IN )
-    {
+    if (TUSB_DIR_IN == tu_edpt_dir(ep_desc->bEndpointAddress)) {
       p_msc->ep_in = ep_desc->bEndpointAddress;
-    }else
-    {
+    } else {
       p_msc->ep_out = ep_desc->bEndpointAddress;
     }
 
-    ep_desc = (tusb_desc_endpoint_t const *) tu_desc_next(ep_desc);
+    ep_desc = (tusb_desc_endpoint_t const*) tu_desc_next(ep_desc);
   }
 
   p_msc->itf_num = desc_itf->bInterfaceNumber;
@@ -397,98 +400,106 @@ bool msch_open(uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *de
   return true;
 }
 
-bool msch_set_config(uint8_t dev_addr, uint8_t itf_num)
-{
-  msch_interface_t* p_msc = get_itf(dev_addr);
+bool msch_set_config(uint8_t daddr, uint8_t itf_num) {
+  msch_interface_t* p_msc = get_itf(daddr);
   TU_ASSERT(p_msc->itf_num == itf_num);
-
   p_msc->configured = true;
 
   //------------- Get Max Lun -------------//
-  TU_LOG2("MSC Get Max Lun\r\n");
-  tusb_control_request_t const request =
-  {
-    .bmRequestType_bit =
-    {
-      .recipient = TUSB_REQ_RCPT_INTERFACE,
-      .type      = TUSB_REQ_TYPE_CLASS,
-      .direction = TUSB_DIR_IN
-    },
-    .bRequest = MSC_REQ_GET_MAX_LUN,
-    .wValue   = 0,
-    .wIndex   = itf_num,
-    .wLength  = 1
+  TU_LOG_DRV("MSC Get Max Lun\r\n");
+  tusb_control_request_t const request = {
+      .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_INTERFACE,
+          .type      = TUSB_REQ_TYPE_CLASS,
+          .direction = TUSB_DIR_IN
+      },
+      .bRequest = MSC_REQ_GET_MAX_LUN,
+      .wValue   = 0,
+      .wIndex   = itf_num,
+      .wLength  = 1
   };
 
-  tuh_xfer_t xfer =
-  {
-    .daddr       = dev_addr,
-    .ep_addr     = 0,
-    .setup       = &request,
-    .buffer      = &p_msc->max_lun,
-    .complete_cb = config_get_maxlun_complete,
-    .user_data    = 0
+  uint8_t* enum_buf = usbh_get_enum_buf();
+  tuh_xfer_t xfer = {
+      .daddr       = daddr,
+      .ep_addr     = 0,
+      .setup       = &request,
+      .buffer      = enum_buf,
+      .complete_cb = config_get_maxlun_complete,
+      .user_data    = 0
   };
   TU_ASSERT(tuh_control_xfer(&xfer));
 
   return true;
 }
 
-static void config_get_maxlun_complete (tuh_xfer_t* xfer)
-{
+static void config_get_maxlun_complete(tuh_xfer_t* xfer) {
   uint8_t const daddr = xfer->daddr;
   msch_interface_t* p_msc = get_itf(daddr);
 
-  // STALL means zero
-  p_msc->max_lun = (XFER_RESULT_SUCCESS == xfer->result) ? _msch_buffer[0] : 0;
-  p_msc->max_lun++; // MAX LUN is minus 1 by specs
+  // MAXLUN's response is minus 1 by specs, STALL means 1
+  if (XFER_RESULT_SUCCESS == xfer->result) {
+    uint8_t* enum_buf = usbh_get_enum_buf();
+    p_msc->max_lun = enum_buf[0] + 1;
+  } else {
+    p_msc->max_lun = 1;
+  }
+
+  TU_LOG_DRV("  Max LUN = %u\r\n", p_msc->max_lun);
 
   // TODO multiple LUN support
-  TU_LOG2("SCSI Test Unit Ready\r\n");
+  TU_LOG_DRV("SCSI Test Unit Ready\r\n");
   uint8_t const lun = 0;
-  tuh_msc_test_unit_ready(daddr, lun, config_test_unit_ready_complete);
+  tuh_msc_test_unit_ready(daddr, lun, config_test_unit_ready_complete, 0);
 }
 
-static bool config_test_unit_ready_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw)
-{
-  if (csw->status == 0)
-  {
+static bool config_test_unit_ready_complete(uint8_t dev_addr, tuh_msc_complete_data_t const* cb_data) {
+  msc_cbw_t const* cbw = cb_data->cbw;
+  msc_csw_t const* csw = cb_data->csw;
+  uint8_t* enum_buf = usbh_get_enum_buf();
+
+  if (csw->status == 0) {
     // Unit is ready, read its capacity
-    TU_LOG2("SCSI Read Capacity\r\n");
-    tuh_msc_read_capacity(dev_addr, cbw->lun, (scsi_read_capacity10_resp_t*) ((void*) _msch_buffer), config_read_capacity_complete);
-  }else
-  {
+    TU_LOG_DRV("SCSI Read Capacity\r\n");
+    tuh_msc_read_capacity(dev_addr, cbw->lun, (scsi_read_capacity10_resp_t*) (uintptr_t) enum_buf,
+                          config_read_capacity_complete, 0);
+  } else {
     // Note: During enumeration, some device fails Test Unit Ready and require a few retries
     // with Request Sense to start working !!
     // TODO limit number of retries
-    TU_LOG2("SCSI Request Sense\r\n");
-    TU_ASSERT(tuh_msc_request_sense(dev_addr, cbw->lun, _msch_buffer, config_request_sense_complete));
+    TU_LOG_DRV("SCSI Request Sense\r\n");
+    TU_ASSERT(tuh_msc_request_sense(dev_addr, cbw->lun, enum_buf, config_request_sense_complete, 0));
   }
 
   return true;
 }
 
-static bool config_request_sense_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw)
-{
+static bool config_request_sense_complete(uint8_t dev_addr, tuh_msc_complete_data_t const* cb_data) {
+  msc_cbw_t const* cbw = cb_data->cbw;
+  msc_csw_t const* csw = cb_data->csw;
+
   TU_ASSERT(csw->status == 0);
-  TU_ASSERT(tuh_msc_test_unit_ready(dev_addr, cbw->lun, config_test_unit_ready_complete));
+  TU_ASSERT(tuh_msc_test_unit_ready(dev_addr, cbw->lun, config_test_unit_ready_complete, 0));
   return true;
 }
 
-static bool config_read_capacity_complete(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw)
-{
+static bool config_read_capacity_complete(uint8_t dev_addr, tuh_msc_complete_data_t const* cb_data) {
+  msc_cbw_t const* cbw = cb_data->cbw;
+  msc_csw_t const* csw = cb_data->csw;
   TU_ASSERT(csw->status == 0);
-
   msch_interface_t* p_msc = get_itf(dev_addr);
+  uint8_t* enum_buf = usbh_get_enum_buf();
 
   // Capacity response field: Block size and Last LBA are both Big-Endian
-  scsi_read_capacity10_resp_t* resp = (scsi_read_capacity10_resp_t*) ((void*) _msch_buffer);
+  scsi_read_capacity10_resp_t* resp = (scsi_read_capacity10_resp_t*) (uintptr_t) enum_buf;
   p_msc->capacity[cbw->lun].block_count = tu_ntohl(resp->last_lba) + 1;
   p_msc->capacity[cbw->lun].block_size  = tu_ntohl(resp->block_size);
 
   // Mark enumeration is complete
   p_msc->mounted = true;
-  if (tuh_msc_mount_cb) tuh_msc_mount_cb(dev_addr);
+  if (tuh_msc_mount_cb) {
+    tuh_msc_mount_cb(dev_addr);
+  }
 
   // notify usbh that driver enumeration is complete
   usbh_driver_set_config_complete(dev_addr, p_msc->itf_num);
diff --git a/src/class/msc/msc_host.h b/src/class/msc/msc_host.h
index 8d95980f9..09d777066 100644
--- a/src/class/msc/msc_host.h
+++ b/src/class/msc/msc_host.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -24,8 +24,8 @@
  * This file is part of the TinyUSB stack.
  */
 
-#ifndef _TUSB_MSC_HOST_H_
-#define _TUSB_MSC_HOST_H_
+#ifndef TUSB_MSC_HOST_H_
+#define TUSB_MSC_HOST_H_
 
 #include "msc.h"
 
@@ -41,7 +41,14 @@
 #define CFG_TUH_MSC_MAXLUN  4
 #endif
 
-typedef bool (*tuh_msc_complete_cb_t)(uint8_t dev_addr, msc_cbw_t const* cbw, msc_csw_t const* csw);
+typedef struct {
+  msc_cbw_t const* cbw; // SCSI command
+  msc_csw_t const* csw; // SCSI status
+  void* scsi_data;      // SCSI Data
+  uintptr_t user_arg;   // user argument
+}tuh_msc_complete_data_t;
+
+typedef bool (*tuh_msc_complete_cb_t)(uint8_t dev_addr, tuh_msc_complete_data_t const* cb_data);
 
 //--------------------------------------------------------------------+
 // Application API
@@ -66,33 +73,38 @@ uint32_t tuh_msc_get_block_size(uint8_t dev_addr, uint8_t lun);
 // Perform a full SCSI command (cbw, data, csw) in non-blocking manner.
 // Complete callback is invoked when SCSI op is complete.
 // return true if success, false if there is already pending operation.
-bool tuh_msc_scsi_command(uint8_t dev_addr, msc_cbw_t const* cbw, void* data, tuh_msc_complete_cb_t complete_cb);
+// NOTE: buffer must be accessible by USB/DMA controller, aligned correctly and multiple of cache line if enabled
+bool tuh_msc_scsi_command(uint8_t daddr, msc_cbw_t const* cbw, void* data, tuh_msc_complete_cb_t complete_cb, uintptr_t arg);
 
 // Perform SCSI Inquiry command
 // Complete callback is invoked when SCSI op is complete.
-bool tuh_msc_inquiry(uint8_t dev_addr, uint8_t lun, scsi_inquiry_resp_t* response, tuh_msc_complete_cb_t complete_cb);
+// NOTE: response must be accessible by USB/DMA controller, aligned correctly and multiple of cache line if enabled
+bool tuh_msc_inquiry(uint8_t dev_addr, uint8_t lun, scsi_inquiry_resp_t* response, tuh_msc_complete_cb_t complete_cb, uintptr_t arg);
 
 // Perform SCSI Test Unit Ready command
 // Complete callback is invoked when SCSI op is complete.
-bool tuh_msc_test_unit_ready(uint8_t dev_addr, uint8_t lun, tuh_msc_complete_cb_t complete_cb);
+bool tuh_msc_test_unit_ready(uint8_t dev_addr, uint8_t lun, tuh_msc_complete_cb_t complete_cb, uintptr_t arg);
 
 // Perform SCSI Request Sense 10 command
 // Complete callback is invoked when SCSI op is complete.
-bool tuh_msc_request_sense(uint8_t dev_addr, uint8_t lun, void *resposne, tuh_msc_complete_cb_t complete_cb);
+// NOTE: response must be accessible by USB/DMA controller, aligned correctly and multiple of cache line if enabled
+bool tuh_msc_request_sense(uint8_t dev_addr, uint8_t lun, void *response, tuh_msc_complete_cb_t complete_cb, uintptr_t arg);
 
 // Perform SCSI Read 10 command. Read n blocks starting from LBA to buffer
 // Complete callback is invoked when SCSI op is complete.
-bool tuh_msc_read10(uint8_t dev_addr, uint8_t lun, void * buffer, uint32_t lba, uint16_t block_count, tuh_msc_complete_cb_t complete_cb);
+// NOTE: buffer must be accessible by USB/DMA controller, aligned correctly and multiple of cache line if enabled
+bool tuh_msc_read10(uint8_t dev_addr, uint8_t lun, void * buffer, uint32_t lba, uint16_t block_count, tuh_msc_complete_cb_t complete_cb, uintptr_t arg);
 
 // Perform SCSI Write 10 command. Write n blocks starting from LBA to device
 // Complete callback is invoked when SCSI op is complete.
-bool tuh_msc_write10(uint8_t dev_addr, uint8_t lun, void const * buffer, uint32_t lba, uint16_t block_count, tuh_msc_complete_cb_t complete_cb);
+// NOTE: buffer must be accessible by USB/DMA controller, aligned correctly and multiple of cache line if enabled
+bool tuh_msc_write10(uint8_t dev_addr, uint8_t lun, void const * buffer, uint32_t lba, uint16_t block_count, tuh_msc_complete_cb_t complete_cb, uintptr_t arg);
 
 // Perform SCSI Read Capacity 10 command
 // Complete callback is invoked when SCSI op is complete.
 // Note: during enumeration, host stack already carried out this request. Application can retrieve capacity by
 // simply call tuh_msc_get_block_count() and tuh_msc_get_block_size()
-bool tuh_msc_read_capacity(uint8_t dev_addr, uint8_t lun, scsi_read_capacity10_resp_t* response, tuh_msc_complete_cb_t complete_cb);
+bool tuh_msc_read_capacity(uint8_t dev_addr, uint8_t lun, scsi_read_capacity10_resp_t* response, tuh_msc_complete_cb_t complete_cb, uintptr_t arg);
 
 //------------- Application Callback -------------//
 
@@ -106,9 +118,10 @@ TU_ATTR_WEAK void tuh_msc_umount_cb(uint8_t dev_addr);
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 
-void msch_init       (void);
+bool msch_init       (void);
+bool msch_deinit     (void);
 bool msch_open       (uint8_t rhport, uint8_t dev_addr, tusb_desc_interface_t const *desc_itf, uint16_t max_len);
-bool msch_set_config (uint8_t dev_addr, uint8_t itf_num);
+bool msch_set_config (uint8_t daddr, uint8_t itf_num);
 void msch_close      (uint8_t dev_addr);
 bool msch_xfer_cb    (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
 
@@ -116,4 +129,4 @@ bool msch_xfer_cb    (uint8_t dev_addr, uint8_t ep_addr, xfer_result_t event, ui
  }
 #endif
 
-#endif /* _TUSB_MSC_HOST_H_ */
+#endif
diff --git a/src/class/net/ecm_rndis_device.c b/src/class/net/ecm_rndis_device.c
index 5f316762f..a54e6d662 100644
--- a/src/class/net/ecm_rndis_device.c
+++ b/src/class/net/ecm_rndis_device.c
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2020 Peter Lawrence
@@ -35,13 +35,18 @@
 #include "net_device.h"
 #include "rndis_protocol.h"
 
-void rndis_class_set_handler(uint8_t *data, int size); /* found in ./misc/networking/rndis_reports.c */
+extern void rndis_class_set_handler(uint8_t *data, int size); /* found in ./misc/networking/rndis_reports.c */
+
+#define CFG_TUD_NET_PACKET_PREFIX_LEN sizeof(rndis_data_packet_t)
+#define CFG_TUD_NET_PACKET_SUFFIX_LEN 0
+
+#define NETD_PACKET_SIZE  (CFG_TUD_NET_PACKET_PREFIX_LEN + CFG_TUD_NET_MTU + CFG_TUD_NET_PACKET_PREFIX_LEN)
+#define NETD_CONTROL_SIZE 120
 
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
-typedef struct
-{
+typedef struct {
   uint8_t itf_num;      // Index number of Management Interface, +1 for Data Interface
   uint8_t itf_data_alt; // Alternate setting of Data Interface. 0 : inactive, 1 : active
 
@@ -51,98 +56,67 @@ typedef struct
 
   bool ecm_mode;
 
-  // Endpoint descriptor use to open/close when receving SetInterface
+  // Endpoint descriptor use to open/close when receiving SetInterface
   // TODO since configuration descriptor may not be long-lived memory, we should
   // keep a copy of endpoint attribute instead
   uint8_t const * ecm_desc_epdata;
-
 } netd_interface_t;
 
-#define CFG_TUD_NET_PACKET_PREFIX_LEN sizeof(rndis_data_packet_t)
-#define CFG_TUD_NET_PACKET_SUFFIX_LEN 0
-
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static uint8_t received[CFG_TUD_NET_PACKET_PREFIX_LEN + CFG_TUD_NET_MTU + CFG_TUD_NET_PACKET_PREFIX_LEN];
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static uint8_t transmitted[CFG_TUD_NET_PACKET_PREFIX_LEN + CFG_TUD_NET_MTU + CFG_TUD_NET_PACKET_PREFIX_LEN];
-
-struct ecm_notify_struct
-{
+typedef struct ecm_notify_struct {
   tusb_control_request_t header;
   uint32_t downlink, uplink;
-};
-
-static const struct ecm_notify_struct ecm_notify_nc =
-{
-  .header = {
-    .bmRequestType = 0xA1,
-    .bRequest = 0 /* NETWORK_CONNECTION aka NetworkConnection */,
-    .wValue = 1 /* Connected */,
-    .wLength = 0,
-  },
-};
+} ecm_notify_t;
 
-static const struct ecm_notify_struct ecm_notify_csc =
-{
-  .header = {
-    .bmRequestType = 0xA1,
-    .bRequest = 0x2A /* CONNECTION_SPEED_CHANGE aka ConnectionSpeedChange */,
-    .wLength = 8,
-  },
-  .downlink = 9728000,
-  .uplink = 9728000,
-};
+typedef struct {
+  TUD_EPBUF_DEF(rx, NETD_PACKET_SIZE);
+  TUD_EPBUF_DEF(tx, NETD_PACKET_SIZE);
 
-// TODO remove CFG_TUSB_MEM_SECTION, control internal buffer is already in this special section
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static union
-{
-  uint8_t rndis_buf[120];
-  struct ecm_notify_struct ecm_buf;
-} notify;
+  TUD_EPBUF_DEF(notify, sizeof(ecm_notify_t));
+  TUD_EPBUF_DEF(ctrl, NETD_CONTROL_SIZE);
+} netd_epbuf_t;
 
 //--------------------------------------------------------------------+
 // INTERNAL OBJECT & FUNCTION DECLARATION
 //--------------------------------------------------------------------+
-// TODO remove CFG_TUSB_MEM_SECTION
-CFG_TUSB_MEM_SECTION static netd_interface_t _netd_itf;
-
+static netd_interface_t _netd_itf;
+CFG_TUD_MEM_SECTION static netd_epbuf_t _netd_epbuf;
 static bool can_xmit;
 
-void tud_network_recv_renew(void)
-{
-  usbd_edpt_xfer(0, _netd_itf.ep_out, received, sizeof(received));
+void tud_network_recv_renew(void) {
+  usbd_edpt_xfer(0, _netd_itf.ep_out, _netd_epbuf.rx, NETD_PACKET_SIZE);
 }
 
-static void do_in_xfer(uint8_t *buf, uint16_t len)
-{
+static void do_in_xfer(uint8_t *buf, uint16_t len) {
   can_xmit = false;
   usbd_edpt_xfer(0, _netd_itf.ep_in, buf, len);
 }
 
-void netd_report(uint8_t *buf, uint16_t len)
-{
-  uint8_t const rhport = 0;
+void netd_report(uint8_t *buf, uint16_t len) {
+  const uint8_t rhport = 0;
+  len = tu_min16(len, sizeof(ecm_notify_t));
 
-  // skip if previous report not yet acknowledged by host
-  if ( usbd_edpt_busy(rhport, _netd_itf.ep_notif) ) return;
-  usbd_edpt_xfer(rhport, _netd_itf.ep_notif, buf, len);
+  TU_VERIFY(usbd_edpt_claim(rhport, _netd_itf.ep_notif), );
+  memcpy(_netd_epbuf.notify, buf, len);
+  usbd_edpt_xfer(rhport, _netd_itf.ep_notif, _netd_epbuf.notify, len);
 }
 
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void netd_init(void)
-{
+void netd_init(void) {
   tu_memclr(&_netd_itf, sizeof(_netd_itf));
 }
 
-void netd_reset(uint8_t rhport)
-{
-  (void) rhport;
+bool netd_deinit(void) {
+  return true;
+}
 
+void netd_reset(uint8_t rhport) {
+  (void) rhport;
   netd_init();
 }
 
-uint16_t netd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len)
-{
+uint16_t netd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len) {
   bool const is_rndis = (TUD_RNDIS_ITF_CLASS    == itf_desc->bInterfaceClass    &&
                          TUD_RNDIS_ITF_SUBCLASS == itf_desc->bInterfaceSubClass &&
                          TUD_RNDIS_ITF_PROTOCOL == itf_desc->bInterfaceProtocol);
@@ -166,21 +140,19 @@ uint16_t netd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint1
   uint8_t const * p_desc = tu_desc_next( itf_desc );
 
   // Communication Functional Descriptors
-  while ( TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc) && drv_len <= max_len )
-  {
+  while (TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc) && drv_len <= max_len) {
     drv_len += tu_desc_len(p_desc);
     p_desc   = tu_desc_next(p_desc);
   }
 
   // notification endpoint (if any)
-  if ( TUSB_DESC_ENDPOINT == tu_desc_type(p_desc) )
-  {
-    TU_ASSERT( usbd_edpt_open(rhport, (tusb_desc_endpoint_t const *) p_desc), 0 );
+  if (TUSB_DESC_ENDPOINT == tu_desc_type(p_desc)) {
+    TU_ASSERT(usbd_edpt_open(rhport, (tusb_desc_endpoint_t const *) p_desc), 0);
 
-    _netd_itf.ep_notif = ((tusb_desc_endpoint_t const *) p_desc)->bEndpointAddress;
+    _netd_itf.ep_notif = ((tusb_desc_endpoint_t const*)p_desc)->bEndpointAddress;
 
     drv_len += tu_desc_len(p_desc);
-    p_desc   = tu_desc_next(p_desc);
+    p_desc = tu_desc_next(p_desc);
   }
 
   //------------- Data Interface -------------//
@@ -190,27 +162,24 @@ uint16_t netd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint1
   //   - 1 : IN & OUT endpoints for active networking
   TU_ASSERT(TUSB_DESC_INTERFACE == tu_desc_type(p_desc), 0);
 
-  do
-  {
+  do {
     tusb_desc_interface_t const * data_itf_desc = (tusb_desc_interface_t const *) p_desc;
     TU_ASSERT(TUSB_CLASS_CDC_DATA == data_itf_desc->bInterfaceClass, 0);
 
     drv_len += tu_desc_len(p_desc);
     p_desc   = tu_desc_next(p_desc);
-  }while( _netd_itf.ecm_mode && (TUSB_DESC_INTERFACE == tu_desc_type(p_desc)) && (drv_len <= max_len) );
+  } while (_netd_itf.ecm_mode && (TUSB_DESC_INTERFACE == tu_desc_type(p_desc)) && (drv_len <= max_len));
 
   // Pair of endpoints
   TU_ASSERT(TUSB_DESC_ENDPOINT == tu_desc_type(p_desc), 0);
 
-  if ( _netd_itf.ecm_mode )
-  {
+  if (_netd_itf.ecm_mode) {
     // ECM by default is in-active, save the endpoint attribute
     // to open later when received setInterface
     _netd_itf.ecm_desc_epdata = p_desc;
-  }else
-  {
+  } else {
     // Open endpoint pair for RNDIS
-    TU_ASSERT( usbd_open_edpt_pair(rhport, p_desc, 2, TUSB_XFER_BULK, &_netd_itf.ep_out, &_netd_itf.ep_in), 0 );
+    TU_ASSERT(usbd_open_edpt_pair(rhport, p_desc, 2, TUSB_XFER_BULK, &_netd_itf.ep_out, &_netd_itf.ep_in), 0);
 
     tud_network_init_cb();
 
@@ -226,38 +195,50 @@ uint16_t netd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint1
   return drv_len;
 }
 
-static void ecm_report(bool nc)
-{
-  notify.ecm_buf = (nc) ? ecm_notify_nc : ecm_notify_csc;
-  notify.ecm_buf.header.wIndex = _netd_itf.itf_num;
-  netd_report((uint8_t *)&notify.ecm_buf, (nc) ? sizeof(notify.ecm_buf.header) : sizeof(notify.ecm_buf));
+static void ecm_report(bool nc) {
+  const ecm_notify_t ecm_notify_nc = {
+    .header = {
+      .bmRequestType = 0xA1,
+      .bRequest = 0, /* NETWORK_CONNECTION aka NetworkConnection */
+      .wValue = 1,   /* Connected */
+      .wLength = 0,
+    },
+  };
+
+  const ecm_notify_t ecm_notify_csc = {
+    .header = {
+      .bmRequestType = 0xA1,
+      .bRequest = 0x2A, /* CONNECTION_SPEED_CHANGE aka ConnectionSpeedChange */
+      .wLength = 8,
+    },
+    .downlink = 9728000,
+    .uplink = 9728000,
+  };
+
+  ecm_notify_t notify = (nc) ? ecm_notify_nc : ecm_notify_csc;
+  notify.header.wIndex = _netd_itf.itf_num;
+  netd_report((uint8_t *)&notify, (nc) ? sizeof(notify.header) : sizeof(notify));
 }
 
 // Invoked when a control transfer occurred on an interface of this class
 // Driver response accordingly to the request and the transfer stage (setup/data/ack)
 // return false to stall control endpoint (e.g unsupported request)
-bool netd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
-{
-  if ( stage == CONTROL_STAGE_SETUP )
-  {
-    switch ( request->bmRequestType_bit.type )
-    {
+bool netd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) {
+  if (stage == CONTROL_STAGE_SETUP) {
+    switch (request->bmRequestType_bit.type) {
       case TUSB_REQ_TYPE_STANDARD:
-        switch ( request->bRequest )
-        {
-          case TUSB_REQ_GET_INTERFACE:
-          {
-            uint8_t const req_itfnum = (uint8_t) request->wIndex;
+        switch (request->bRequest) {
+          case TUSB_REQ_GET_INTERFACE: {
+            uint8_t const req_itfnum = (uint8_t)request->wIndex;
             TU_VERIFY(_netd_itf.itf_num+1 == req_itfnum);
 
             tud_control_xfer(rhport, request, &_netd_itf.itf_data_alt, 1);
           }
           break;
 
-          case TUSB_REQ_SET_INTERFACE:
-          {
-            uint8_t const req_itfnum = (uint8_t) request->wIndex;
-            uint8_t const req_alt    = (uint8_t) request->wValue;
+          case TUSB_REQ_SET_INTERFACE: {
+            uint8_t const req_itfnum = (uint8_t)request->wIndex;
+            uint8_t const req_alt = (uint8_t)request->wValue;
 
             // Only valid for Data Interface with Alternate is either 0 or 1
             TU_VERIFY(_netd_itf.itf_num+1 == req_itfnum && req_alt < 2);
@@ -267,14 +248,14 @@ bool netd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t
 
             _netd_itf.itf_data_alt = req_alt;
 
-            if ( _netd_itf.itf_data_alt )
-            {
+            if (_netd_itf.itf_data_alt) {
               // TODO since we don't actually close endpoint
               // hack here to not re-open it
-              if ( _netd_itf.ep_in == 0 && _netd_itf.ep_out == 0 )
-              {
+              if (_netd_itf.ep_in == 0 && _netd_itf.ep_out == 0) {
                 TU_ASSERT(_netd_itf.ecm_desc_epdata);
-                TU_ASSERT( usbd_open_edpt_pair(rhport, _netd_itf.ecm_desc_epdata, 2, TUSB_XFER_BULK, &_netd_itf.ep_out, &_netd_itf.ep_in) );
+                TU_ASSERT(
+                  usbd_open_edpt_pair(rhport, _netd_itf.ecm_desc_epdata, 2, TUSB_XFER_BULK, &_netd_itf.ep_out, &
+                    _netd_itf.ep_in));
 
                 // TODO should be merge with RNDIS's after endpoint opened
                 // Also should have opposite callback for application to disable network !!
@@ -282,8 +263,7 @@ bool netd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t
                 can_xmit = true; // we are ready to transmit a packet
                 tud_network_recv_renew(); // prepare for incoming packets
               }
-            }else
-            {
+            } else {
               // TODO close the endpoint pair
               // For now pretend that we did, this should have no harm since host won't try to
               // communicate with the endpoints again
@@ -297,50 +277,39 @@ bool netd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t
           // unsupported request
           default: return false;
         }
-      break;
+        break;
 
       case TUSB_REQ_TYPE_CLASS:
-        TU_VERIFY (_netd_itf.itf_num == request->wIndex);
+        TU_VERIFY(_netd_itf.itf_num == request->wIndex);
 
-        if (_netd_itf.ecm_mode)
-        {
+        if (_netd_itf.ecm_mode) {
           /* the only required CDC-ECM Management Element Request is SetEthernetPacketFilter */
-          if (0x43 /* SET_ETHERNET_PACKET_FILTER */ == request->bRequest)
-          {
+          if (0x43 /* SET_ETHERNET_PACKET_FILTER */ == request->bRequest) {
             tud_control_xfer(rhport, request, NULL, 0);
             ecm_report(true);
           }
-        }
-        else
-        {
-          if (request->bmRequestType_bit.direction == TUSB_DIR_IN)
-          {
-            rndis_generic_msg_t *rndis_msg = (rndis_generic_msg_t *) ((void*) notify.rndis_buf);
+        } else {
+          if (request->bmRequestType_bit.direction == TUSB_DIR_IN) {
+            rndis_generic_msg_t* rndis_msg = (rndis_generic_msg_t*)((void*)_netd_epbuf.ctrl);
             uint32_t msglen = tu_le32toh(rndis_msg->MessageLength);
-            TU_ASSERT(msglen <= sizeof(notify.rndis_buf));
-            tud_control_xfer(rhport, request, notify.rndis_buf, (uint16_t) msglen);
-          }
-          else
-          {
-            tud_control_xfer(rhport, request, notify.rndis_buf, (uint16_t) sizeof(notify.rndis_buf));
+            TU_ASSERT(msglen <= NETD_CONTROL_SIZE);
+            tud_control_xfer(rhport, request, _netd_epbuf.ctrl, (uint16_t)msglen);
+          } else {
+            tud_control_xfer(rhport, request, _netd_epbuf.ctrl, NETD_CONTROL_SIZE);
           }
         }
-      break;
+        break;
 
       // unsupported request
       default: return false;
     }
-  }
-  else if ( stage == CONTROL_STAGE_DATA )
-  {
+  } else if (stage == CONTROL_STAGE_DATA) {
     // Handle RNDIS class control OUT only
     if (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS &&
-        request->bmRequestType_bit.direction == TUSB_DIR_OUT   &&
-        _netd_itf.itf_num == request->wIndex)
-    {
-      if ( !_netd_itf.ecm_mode )
-      {
-        rndis_class_set_handler(notify.rndis_buf, request->wLength);
+        request->bmRequestType_bit.direction == TUSB_DIR_OUT &&
+        _netd_itf.itf_num == request->wIndex) {
+      if (!_netd_itf.ecm_mode) {
+        rndis_class_set_handler(_netd_epbuf.ctrl, request->wLength);
       }
     }
   }
@@ -348,92 +317,77 @@ bool netd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t
   return true;
 }
 
-static void handle_incoming_packet(uint32_t len)
-{
-  uint8_t *pnt = received;
+static void handle_incoming_packet(uint32_t len) {
+  uint8_t* pnt = _netd_epbuf.rx;
   uint32_t size = 0;
 
-  if (_netd_itf.ecm_mode)
-  {
+  if (_netd_itf.ecm_mode) {
     size = len;
-  }
-  else
-  {
-    rndis_data_packet_t *r = (rndis_data_packet_t *) ((void*) pnt);
-    if (len >= sizeof(rndis_data_packet_t))
-      if ( (r->MessageType == REMOTE_NDIS_PACKET_MSG) && (r->MessageLength <= len))
-        if ( (r->DataOffset + offsetof(rndis_data_packet_t, DataOffset) + r->DataLength) <= len)
-        {
-          pnt = &received[r->DataOffset + offsetof(rndis_data_packet_t, DataOffset)];
+  } else {
+    rndis_data_packet_t* r = (rndis_data_packet_t*)((void*)pnt);
+    if (len >= sizeof(rndis_data_packet_t)) {
+      if ((r->MessageType == REMOTE_NDIS_PACKET_MSG) && (r->MessageLength <= len)) {
+        if ((r->DataOffset + offsetof(rndis_data_packet_t, DataOffset) + r->DataLength) <= len) {
+          pnt = &_netd_epbuf.rx[r->DataOffset + offsetof(rndis_data_packet_t, DataOffset)];
           size = r->DataLength;
         }
+      }
+    }
   }
 
-  if (!tud_network_recv_cb(pnt, (uint16_t) size))
-  {
+  if (!tud_network_recv_cb(pnt, (uint16_t)size)) {
     /* if a buffer was never handled by user code, we must renew on the user's behalf */
     tud_network_recv_renew();
   }
 }
 
-bool netd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
-{
-  (void) rhport;
-  (void) result;
+bool netd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) {
+  (void)rhport;
+  (void)result;
 
   /* new packet received */
-  if ( ep_addr == _netd_itf.ep_out )
-  {
+  if (ep_addr == _netd_itf.ep_out) {
     handle_incoming_packet(xferred_bytes);
   }
 
   /* data transmission finished */
-  if ( ep_addr == _netd_itf.ep_in )
-  {
+  if (ep_addr == _netd_itf.ep_in) {
     /* TinyUSB requires the class driver to implement ZLP (since ZLP usage is class-specific) */
 
-    if ( xferred_bytes && (0 == (xferred_bytes % CFG_TUD_NET_ENDPOINT_SIZE)) )
-    {
+    if (xferred_bytes && (0 == (xferred_bytes % CFG_TUD_NET_ENDPOINT_SIZE))) {
       do_in_xfer(NULL, 0); /* a ZLP is needed */
-    }
-    else
-    {
+    } else {
       /* we're finally finished */
       can_xmit = true;
     }
   }
 
-  if ( _netd_itf.ecm_mode && (ep_addr == _netd_itf.ep_notif) )
-  {
-    if (sizeof(notify.ecm_buf.header) == xferred_bytes) ecm_report(false);
+  if (_netd_itf.ecm_mode && (ep_addr == _netd_itf.ep_notif)) {
+    if (sizeof(tusb_control_request_t) == xferred_bytes) {
+      ecm_report(false);
+    }
   }
 
   return true;
 }
 
-bool tud_network_can_xmit(uint16_t size)
-{
+bool tud_network_can_xmit(uint16_t size) {
   (void)size;
-
   return can_xmit;
 }
 
-void tud_network_xmit(void *ref, uint16_t arg)
-{
-  uint8_t *data;
-  uint16_t len;
-
-  if (!can_xmit)
+void tud_network_xmit(void *ref, uint16_t arg) {
+  if (!can_xmit) {
     return;
+  }
 
-  len = (_netd_itf.ecm_mode) ? 0 : CFG_TUD_NET_PACKET_PREFIX_LEN;
-  data = transmitted + len;
+  uint16_t len = (_netd_itf.ecm_mode) ? 0 : CFG_TUD_NET_PACKET_PREFIX_LEN;
+  uint8_t* data = _netd_epbuf.tx + len;
 
   len += tud_network_xmit_cb(data, ref, arg);
 
-  if (!_netd_itf.ecm_mode)
-  {
-    rndis_data_packet_t *hdr = (rndis_data_packet_t *) ((void*) transmitted);
+  if (!_netd_itf.ecm_mode) {
+    rndis_data_packet_t *hdr = (rndis_data_packet_t *) ((void*) _netd_epbuf.tx);
     memset(hdr, 0, sizeof(rndis_data_packet_t));
     hdr->MessageType = REMOTE_NDIS_PACKET_MSG;
     hdr->MessageLength = len;
@@ -441,7 +395,7 @@ void tud_network_xmit(void *ref, uint16_t arg)
     hdr->DataLength = len - sizeof(rndis_data_packet_t);
   }
 
-  do_in_xfer(transmitted, len);
+  do_in_xfer(_netd_epbuf.tx, len);
 }
 
 #endif
diff --git a/src/class/net/ncm.h b/src/class/net/ncm.h
index 96ba11fbc..0245a87f2 100644
--- a/src/class/net/ncm.h
+++ b/src/class/net/ncm.h
@@ -2,6 +2,7 @@
  * The MIT License (MIT)
  *
  * Copyright (c) 2021, Ha Thach (tinyusb.org)
+ * Copyright (c) 2024, Hardy Griech
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -24,22 +25,58 @@
  * This file is part of the TinyUSB stack.
  */
 
-
 #ifndef _TUSB_NCM_H_
 #define _TUSB_NCM_H_
 
 #include "common/tusb_common.h"
 
-#ifdef __cplusplus
- extern "C" {
+// NTB buffers size for reception side, must be >> MTU to avoid TCP retransmission (driver issue ?)
+// Linux use 2048 as minimal size
+#ifndef CFG_TUD_NCM_OUT_NTB_MAX_SIZE
+  #define CFG_TUD_NCM_OUT_NTB_MAX_SIZE 3200
 #endif
 
-// Table 4.3 Data Class Interface Protocol Codes
-typedef enum
-{
-  NCM_DATA_PROTOCOL_NETWORK_TRANSFER_BLOCK = 0x01
-} ncm_data_interface_protocol_code_t;
+// NTB buffers size for reception side, must be > MTU
+// Linux use 2048 as minimal size
+#ifndef CFG_TUD_NCM_IN_NTB_MAX_SIZE
+  #define CFG_TUD_NCM_IN_NTB_MAX_SIZE 3200
+#endif
+
+// Number of NTB buffers for reception side
+// Depending on the configuration, this parameter could be increased with the cost of additional RAM requirements
+// On Full-Speed (RP2040) :
+//    1  - good performance
+//    2  - up to 30% more performance with iperf with small packets
+//    >2 - no performance gain
+// On High-Speed (STM32F7) :
+//    No performance gain
+#ifndef CFG_TUD_NCM_OUT_NTB_N
+  #define CFG_TUD_NCM_OUT_NTB_N 1
+#endif
 
+// Number of NTB buffers for transmission side
+// Depending on the configuration, this parameter could be increased with the cost of additional RAM requirements
+// On Full-Speed (RP2040) :
+//    1 - good performance but SystemView shows lost events (on load test)
+//    2 - up to 50% more performance with iperf with small packets, "tud_network_can_xmit: request blocked"
+//        happens from time to time with SystemView
+//    3 - "tud_network_can_xmit: request blocked" never happens
+//    >3 - no performance gain
+// On High-Speed (STM32F7) :
+//    No performance gain
+#ifndef CFG_TUD_NCM_IN_NTB_N
+  #define CFG_TUD_NCM_IN_NTB_N 1
+#endif
+
+// How many datagrams it is allowed to put into an NTB for transmission side
+#ifndef CFG_TUD_NCM_IN_MAX_DATAGRAMS_PER_NTB
+  #define CFG_TUD_NCM_IN_MAX_DATAGRAMS_PER_NTB 8
+#endif
+
+// This tells the host how many datagrams it is allowed to put into an NTB
+#ifndef CFG_TUD_NCM_OUT_MAX_DATAGRAMS_PER_NTB
+  #define CFG_TUD_NCM_OUT_MAX_DATAGRAMS_PER_NTB 6
+#endif
 
 // Table 6.2 Class-Specific Request Codes for Network Control Model subclass
 typedef enum
@@ -62,8 +99,66 @@ typedef enum
   NCM_SET_CRC_MODE                                 = 0x8A,
 } ncm_request_code_t;
 
-#ifdef __cplusplus
- }
-#endif
+#define NTH16_SIGNATURE 0x484D434E
+#define NDP16_SIGNATURE_NCM0 0x304D434E
+#define NDP16_SIGNATURE_NCM1 0x314D434E
+
+typedef struct TU_ATTR_PACKED {
+  uint16_t wLength;
+  uint16_t bmNtbFormatsSupported;
+  uint32_t dwNtbInMaxSize;
+  uint16_t wNdbInDivisor;
+  uint16_t wNdbInPayloadRemainder;
+  uint16_t wNdbInAlignment;
+  uint16_t wReserved;
+  uint32_t dwNtbOutMaxSize;
+  uint16_t wNdbOutDivisor;
+  uint16_t wNdbOutPayloadRemainder;
+  uint16_t wNdbOutAlignment;
+  uint16_t wNtbOutMaxDatagrams;
+} ntb_parameters_t;
+
+typedef struct TU_ATTR_PACKED {
+  uint32_t dwSignature;
+  uint16_t wHeaderLength;
+  uint16_t wSequence;
+  uint16_t wBlockLength;
+  uint16_t wNdpIndex;
+} nth16_t;
+
+typedef struct TU_ATTR_PACKED {
+  uint16_t wDatagramIndex;
+  uint16_t wDatagramLength;
+} ndp16_datagram_t;
+
+typedef struct TU_ATTR_PACKED {
+  uint32_t dwSignature;
+  uint16_t wLength;
+  uint16_t wNextNdpIndex;
+  //ndp16_datagram_t datagram[];
+} ndp16_t;
+
+typedef union TU_ATTR_PACKED {
+  struct {
+    nth16_t nth;
+    ndp16_t ndp;
+    ndp16_datagram_t ndp_datagram[CFG_TUD_NCM_IN_MAX_DATAGRAMS_PER_NTB + 1];
+  };
+  uint8_t data[CFG_TUD_NCM_IN_NTB_MAX_SIZE];
+} xmit_ntb_t;
+
+typedef union TU_ATTR_PACKED {
+  struct {
+    nth16_t nth;
+    // only the header is at a guaranteed position
+  };
+  uint8_t data[CFG_TUD_NCM_OUT_NTB_MAX_SIZE];
+} recv_ntb_t;
+
+typedef struct {
+  tusb_control_request_t header;
+  uint32_t downlink;
+  uint32_t uplink;
+} ncm_notify_t;
 
 #endif
diff --git a/src/class/net/ncm_device.c b/src/class/net/ncm_device.c
index 00892b49c..f9fda0698 100644
--- a/src/class/net/ncm_device.c
+++ b/src/class/net/ncm_device.c
@@ -1,9 +1,10 @@
 /*
  * The MIT License (MIT)
  *
+ * Copyright (c) 2019 Ha Thach (tinyusb.org)
+ * Copyright (c) 2024 Hardy Griech
  * Copyright (c) 2020 Jacob Berg Potter
  * Copyright (c) 2020 Peter Lawrence
- * Copyright (c) 2019 Ha Thach (tinyusb.org)
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -26,486 +27,918 @@
  * This file is part of the TinyUSB stack.
  */
 
+/**
+ * Small Glossary (from the spec)
+ * --------------
+ * Datagram - A collection of bytes forming a single item of information, passed as a unit from source to destination.
+ * NCM      - Network Control Model
+ * NDP      - NCM Datagram Pointer: NTB structure that delineates Datagrams (typically Ethernet frames) within an NTB
+ * NTB      - NCM Transfer Block: a data structure for efficient USB encapsulation of one or more datagrams
+ *            Each NTB is designed to be a single USB transfer
+ * NTH      - NTB Header: a data structure at the front of each NTB, which provides the information needed to validate
+ *            the NTB and begin decoding
+ *
+ * Some explanations
+ * -----------------
+ * - rhport        is the USB port of the device, in most cases "0"
+ * - itf_data_alt  if != 0 -> data xmit/recv are allowed (see spec)
+ * - ep_in         IN endpoints take data from the device intended to go in to the host (the device transmits)
+ * - ep_out        OUT endpoints send data out of the host to the device (the device receives)
+ */
+
 #include "tusb_option.h"
 
-#if ( CFG_TUD_ENABLED && CFG_TUD_NCM )
+#if (CFG_TUD_ENABLED && CFG_TUD_NCM)
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
 
 #include "device/usbd.h"
 #include "device/usbd_pvt.h"
+
+#include "ncm.h"
 #include "net_device.h"
 
-//--------------------------------------------------------------------+
-// MACRO CONSTANT TYPEDEF
-//--------------------------------------------------------------------+
+// Level where CFG_TUSB_DEBUG must be at least for this driver is logged
+#ifndef CFG_TUD_NCM_LOG_LEVEL
+  #define CFG_TUD_NCM_LOG_LEVEL   CFG_TUD_LOG_LEVEL
+#endif
+
+#define TU_LOG_DRV(...)   TU_LOG(CFG_TUD_NCM_LOG_LEVEL, __VA_ARGS__)
 
-#define NTH16_SIGNATURE      0x484D434E
-#define NDP16_SIGNATURE_NCM0 0x304D434E
-#define NDP16_SIGNATURE_NCM1 0x314D434E
+// Alignment must be 4
+#define TUD_NCM_ALIGNMENT   4
+// calculate alignment of xmit datagrams within an NTB
+#define XMIT_ALIGN_OFFSET(x) ((TUD_NCM_ALIGNMENT - ((x) & (TUD_NCM_ALIGNMENT - 1))) & (TUD_NCM_ALIGNMENT - 1))
 
-typedef struct TU_ATTR_PACKED
-{
-  uint16_t wLength;
-  uint16_t bmNtbFormatsSupported;
-  uint32_t dwNtbInMaxSize;
-  uint16_t wNdbInDivisor;
-  uint16_t wNdbInPayloadRemainder;
-  uint16_t wNdbInAlignment;
-  uint16_t wReserved;
-  uint32_t dwNtbOutMaxSize;
-  uint16_t wNdbOutDivisor;
-  uint16_t wNdbOutPayloadRemainder;
-  uint16_t wNdbOutAlignment;
-  uint16_t wNtbOutMaxDatagrams;
-} ntb_parameters_t;
+//-----------------------------------------------------------------------------
+//
+// Module global things
+//
+#define XMIT_NTB_N CFG_TUD_NCM_IN_NTB_N
+#define RECV_NTB_N CFG_TUD_NCM_OUT_NTB_N
 
-typedef struct TU_ATTR_PACKED
-{
-  uint32_t dwSignature;
-  uint16_t wHeaderLength;
-  uint16_t wSequence;
-  uint16_t wBlockLength;
-  uint16_t wNdpIndex;
-} nth16_t;
+typedef struct {
+  // general
+  uint8_t ep_in;        // endpoint for outgoing datagrams (naming is a little bit confusing)
+  uint8_t ep_out;       // endpoint for incoming datagrams (naming is a little bit confusing)
+  uint8_t ep_notif;     // endpoint for notifications
+  uint8_t itf_num;      // interface number
+  uint8_t itf_data_alt; // ==0 -> no endpoints, i.e. no network traffic, ==1 -> normal operation with two endpoints (spec, chapter 5.3)
+  uint8_t rhport;       // storage of \a rhport because some callbacks are done without it
 
-typedef struct TU_ATTR_PACKED
-{
-  uint16_t wDatagramIndex;
-  uint16_t wDatagramLength;
-} ndp16_datagram_t;
+  // recv handling
+  recv_ntb_t *recv_free_ntb[RECV_NTB_N];                // free list of recv NTBs
+  recv_ntb_t *recv_ready_ntb[RECV_NTB_N];               // NTBs waiting for transmission to glue logic
+  recv_ntb_t *recv_tinyusb_ntb;                         // buffer for the running transfer TinyUSB -> driver
+  recv_ntb_t *recv_glue_ntb;                            // buffer for the running transfer driver -> glue logic
+  uint16_t recv_glue_ntb_datagram_ndx;                  // index into \a recv_glue_ntb_datagram
 
-typedef struct TU_ATTR_PACKED
-{
-  uint32_t dwSignature;
-  uint16_t wLength;
-  uint16_t wNextNdpIndex;
-  ndp16_datagram_t datagram[];
-} ndp16_t;
+  // xmit handling
+  xmit_ntb_t *xmit_free_ntb[XMIT_NTB_N];                // free list of xmit NTBs
+  xmit_ntb_t *xmit_ready_ntb[XMIT_NTB_N];               // NTBs waiting for transmission to TinyUSB
+  xmit_ntb_t *xmit_tinyusb_ntb;                         // buffer for the running transfer driver -> TinyUSB
+  xmit_ntb_t *xmit_glue_ntb;                            // buffer for the running transfer glue logic -> driver
+  uint16_t xmit_sequence;                               // NTB sequence counter
+  uint16_t xmit_glue_ntb_datagram_ndx;                  // index into \a xmit_glue_ntb_datagram
 
-typedef union TU_ATTR_PACKED {
+  // notification handling
+  enum {
+    NOTIFICATION_SPEED,
+    NOTIFICATION_CONNECTED,
+    NOTIFICATION_DONE
+  } notification_xmit_state;                            // state of notification transmission
+  bool notification_xmit_is_running;                    // notification is currently transmitted
+
+  // misc
+  bool tud_network_recv_renew_active;                   // tud_network_recv_renew() is active (avoid recursive invocations)
+  bool tud_network_recv_renew_process_again;            // tud_network_recv_renew() should process again
+} ncm_interface_t;
+
+typedef struct {
   struct {
-    nth16_t nth;
-    ndp16_t ndp;
-  };
-  uint8_t data[CFG_TUD_NCM_IN_NTB_MAX_SIZE];
-} transmit_ntb_t;
+    TUD_EPBUF_TYPE_DEF(recv_ntb_t, ntb);
+  } recv[RECV_NTB_N];
+
+  struct {
+    TUD_EPBUF_TYPE_DEF(xmit_ntb_t, ntb);
+  } xmit[XMIT_NTB_N];
+
+  TUD_EPBUF_TYPE_DEF(ncm_notify_t, epnotif);
+} ncm_epbuf_t;
+
+static ncm_interface_t ncm_interface;
+CFG_TUD_MEM_SECTION static ncm_epbuf_t ncm_epbuf;
 
-struct ecm_notify_struct
-{
-  tusb_control_request_t header;
-  uint32_t downlink, uplink;
+/**
+ * This is the NTB parameter structure
+ *
+ * \attention
+ *     We are lucky, that byte order is correct
+ */
+TU_ATTR_ALIGNED(4) static const ntb_parameters_t ntb_parameters = {
+  .wLength                  = sizeof(ntb_parameters_t),
+  .bmNtbFormatsSupported    = 0x01,// 16-bit NTB supported
+  .dwNtbInMaxSize           = CFG_TUD_NCM_IN_NTB_MAX_SIZE,
+  .wNdbInDivisor            = 1,
+  .wNdbInPayloadRemainder   = 0,
+  .wNdbInAlignment          = TUD_NCM_ALIGNMENT,
+  .wReserved                = 0,
+  .dwNtbOutMaxSize          = CFG_TUD_NCM_OUT_NTB_MAX_SIZE,
+  .wNdbOutDivisor           = 1,
+  .wNdbOutPayloadRemainder  = 0,
+  .wNdbOutAlignment         = TUD_NCM_ALIGNMENT,
+  .wNtbOutMaxDatagrams      = CFG_TUD_NCM_OUT_MAX_DATAGRAMS_PER_NTB,
 };
 
-typedef struct
-{
-  uint8_t itf_num;      // Index number of Management Interface, +1 for Data Interface
-  uint8_t itf_data_alt; // Alternate setting of Data Interface. 0 : inactive, 1 : active
+// Some confusing remarks about wNtbOutMaxDatagrams...
+//      ==1 -> SystemView packets/s goes up to 2000 and events are lost during startup
+//      ==0 -> SystemView runs fine, iperf shows in wireshark a lot of error
+//      ==6 -> SystemView runs fine, iperf also
+//      >6  -> iperf starts to show errors
+//      -> 6 seems to be the best value.  Why?  Don't know, perhaps only on my system?
+//
+//      iperf:    for MSS in 100 200 400 800 1200 1450 1500; do iperf -c 192.168.14.1 -e -i 1 -M $MSS -l 8192 -P 1; sleep 2; done
+//      sysview:  SYSTICKS_PER_SEC=35000, IDLE_US=1000, PRINT_MOD=1000
+//
 
-  uint8_t ep_notif;
-  uint8_t ep_in;
-  uint8_t ep_out;
+//-----------------------------------------------------------------------------
+//
+// everything about notifications
+//
 
-  const ndp16_t *ndp;
-  uint8_t num_datagrams, current_datagram_index;
+/**
+ * Transmit next notification to the host (if appropriate).
+ * Notifications are transferred to the host once during connection setup.
+ */
+static void notification_xmit(uint8_t rhport, bool force_next) {
+  TU_LOG_DRV("notification_xmit(%d, %d) - %d %d\n", force_next, rhport, ncm_interface.notification_xmit_state, ncm_interface.notification_xmit_is_running);
 
-  enum {
-    REPORT_SPEED,
-    REPORT_CONNECTED,
-    REPORT_DONE
-  } report_state;
-  bool report_pending;
+  if (!force_next && ncm_interface.notification_xmit_is_running) {
+    return;
+  }
+
+  if (ncm_interface.notification_xmit_state == NOTIFICATION_SPEED) {
+    TU_LOG_DRV("  NOTIFICATION_SPEED\n");
+    ncm_notify_t notify_speed_change = {
+      .header = {
+        .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_INTERFACE,
+          .type = TUSB_REQ_TYPE_CLASS,
+          .direction = TUSB_DIR_IN
+        },
+        .bRequest = CDC_NOTIF_CONNECTION_SPEED_CHANGE,
+        .wValue = 0,
+        .wIndex = ncm_interface.itf_num,
+        .wLength = 8
+      }
+    };
+    if (tud_speed_get() == TUSB_SPEED_HIGH) {
+      notify_speed_change.downlink = 480000000;
+      notify_speed_change.uplink = 480000000;
+    } else {
+      notify_speed_change.downlink = 12000000;
+      notify_speed_change.uplink = 12000000;
+    }
+
+    uint16_t notif_len = sizeof(notify_speed_change.header) + notify_speed_change.header.wLength;
+    ncm_epbuf.epnotif = notify_speed_change;
+    usbd_edpt_xfer(rhport, ncm_interface.ep_notif, (uint8_t*) &ncm_epbuf.epnotif, notif_len);
+
+    ncm_interface.notification_xmit_state = NOTIFICATION_CONNECTED;
+    ncm_interface.notification_xmit_is_running = true;
+  } else if (ncm_interface.notification_xmit_state == NOTIFICATION_CONNECTED) {
+    TU_LOG_DRV("  NOTIFICATION_CONNECTED\n");
+    ncm_notify_t notify_connected = {
+      .header = {
+        .bmRequestType_bit = {
+          .recipient = TUSB_REQ_RCPT_INTERFACE,
+          .type = TUSB_REQ_TYPE_CLASS,
+          .direction = TUSB_DIR_IN
+        },
+        .bRequest = CDC_NOTIF_NETWORK_CONNECTION,
+        .wValue = 1 /* Connected */,
+        .wIndex = ncm_interface.itf_num,
+        .wLength = 0,
+      },
+    };
 
-  uint8_t  current_ntb;           // Index in transmit_ntb[] that is currently being filled with datagrams
-  uint8_t  datagram_count;        // Number of datagrams in transmit_ntb[current_ntb]
-  uint16_t next_datagram_offset;  // Offset in transmit_ntb[current_ntb].data to place the next datagram
-  uint16_t ntb_in_size;           // Maximum size of transmitted (IN to host) NTBs; initially CFG_TUD_NCM_IN_NTB_MAX_SIZE
-  uint8_t  max_datagrams_per_ntb; // Maximum number of datagrams per NTB; initially CFG_TUD_NCM_MAX_DATAGRAMS_PER_NTB
+    uint16_t notif_len = sizeof(notify_connected.header) + notify_connected.header.wLength;
+    ncm_epbuf.epnotif = notify_connected;
+    usbd_edpt_xfer(rhport, ncm_interface.ep_notif, (uint8_t *) &ncm_epbuf.epnotif, notif_len);
 
-  uint16_t nth_sequence;          // Sequence number counter for transmitted NTBs
+    ncm_interface.notification_xmit_state = NOTIFICATION_DONE;
+    ncm_interface.notification_xmit_is_running = true;
+  } else {
+    TU_LOG_DRV("  NOTIFICATION_FINISHED\n");
+  }
+} // notification_xmit
 
-  bool transferring;
+//-----------------------------------------------------------------------------
+//
+// everything about packet transmission (driver -> TinyUSB)
+//
 
-} ncm_interface_t;
+/**
+ * Put NTB into the transmitter free list.
+ */
+static void xmit_put_ntb_into_free_list(xmit_ntb_t *free_ntb) {
+  TU_LOG_DRV("xmit_put_ntb_into_free_list() - %p\n", ncm_interface.xmit_tinyusb_ntb);
 
-//--------------------------------------------------------------------+
-// INTERNAL OBJECT & FUNCTION DECLARATION
-//--------------------------------------------------------------------+
+  if (free_ntb == NULL) { // can happen due to ZLPs
+    return;
+  }
 
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static const ntb_parameters_t ntb_parameters = {
-    .wLength                 = sizeof(ntb_parameters_t),
-    .bmNtbFormatsSupported   = 0x01,
-    .dwNtbInMaxSize          = CFG_TUD_NCM_IN_NTB_MAX_SIZE,
-    .wNdbInDivisor           = 4,
-    .wNdbInPayloadRemainder  = 0,
-    .wNdbInAlignment         = CFG_TUD_NCM_ALIGNMENT,
-    .wReserved               = 0,
-    .dwNtbOutMaxSize         = CFG_TUD_NCM_OUT_NTB_MAX_SIZE,
-    .wNdbOutDivisor          = 4,
-    .wNdbOutPayloadRemainder = 0,
-    .wNdbOutAlignment        = CFG_TUD_NCM_ALIGNMENT,
-    .wNtbOutMaxDatagrams     = 0
-};
+  for (int i = 0; i < XMIT_NTB_N; ++i) {
+    if (ncm_interface.xmit_free_ntb[i] == NULL) {
+      ncm_interface.xmit_free_ntb[i] = free_ntb;
+      return;
+    }
+  }
+  TU_LOG_DRV("(EE) xmit_put_ntb_into_free_list - no entry in free list\n");// this should not happen
+} // xmit_put_ntb_into_free_list
 
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static transmit_ntb_t transmit_ntb[2];
+/**
+ * Get an NTB from the free list
+ */
+static xmit_ntb_t *xmit_get_free_ntb(void) {
+  TU_LOG_DRV("xmit_get_free_ntb()\n");
 
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static uint8_t receive_ntb[CFG_TUD_NCM_OUT_NTB_MAX_SIZE];
+  for (int i = 0; i < XMIT_NTB_N; ++i) {
+    if (ncm_interface.xmit_free_ntb[i] != NULL) {
+      xmit_ntb_t *free = ncm_interface.xmit_free_ntb[i];
+      ncm_interface.xmit_free_ntb[i] = NULL;
+      return free;
+    }
+  }
+  return NULL;
+} // xmit_get_free_ntb
 
-static ncm_interface_t ncm_interface;
+/**
+ * Put a filled NTB into the ready list
+ */
+static void xmit_put_ntb_into_ready_list(xmit_ntb_t *ready_ntb) {
+  TU_LOG_DRV("xmit_put_ntb_into_ready_list(%p) %d\n", ready_ntb, ready_ntb->nth.wBlockLength);
 
-/*
- * Set up the NTB state in ncm_interface to be ready to add datagrams.
+  for (int i = 0; i < XMIT_NTB_N; ++i) {
+    if (ncm_interface.xmit_ready_ntb[i] == NULL) {
+      ncm_interface.xmit_ready_ntb[i] = ready_ntb;
+      return;
+    }
+  }
+  TU_LOG_DRV("(EE) xmit_put_ntb_into_ready_list: ready list full\n");// this should not happen
+} // xmit_put_ntb_into_ready_list
+
+/**
+ * Get the next NTB from the ready list (and remove it from the list).
+ * If the ready list is empty, return NULL.
  */
-static void ncm_prepare_for_tx(void) {
-  ncm_interface.datagram_count = 0;
-  // datagrams start after all the headers
-  ncm_interface.next_datagram_offset = sizeof(nth16_t) + sizeof(ndp16_t)
-      + ((CFG_TUD_NCM_MAX_DATAGRAMS_PER_NTB + 1) * sizeof(ndp16_datagram_t));
-}
+static xmit_ntb_t *xmit_get_next_ready_ntb(void) {
+  xmit_ntb_t *r = NULL;
 
-/*
- * If not already transmitting, start sending the current NTB to the host and swap buffers
- * to start filling the other one with datagrams.
+  r = ncm_interface.xmit_ready_ntb[0];
+  memmove(ncm_interface.xmit_ready_ntb + 0, ncm_interface.xmit_ready_ntb + 1, sizeof(ncm_interface.xmit_ready_ntb) - sizeof(ncm_interface.xmit_ready_ntb[0]));
+  ncm_interface.xmit_ready_ntb[XMIT_NTB_N - 1] = NULL;
+
+  TU_LOG_DRV("recv_get_next_ready_ntb: %p\n", r);
+  return r;
+} // xmit_get_next_ready_ntb
+
+/**
+ * Transmit a ZLP if required
+ *
+ * \note
+ *    Insertion of the ZLPs is a little bit different then described in the spec.
+ *    But the below implementation actually works.  Don't know if this is a spec
+ *    or TinyUSB issue.
+ *
+ * \pre
+ *    This must be called from netd_xfer_cb() so that ep_in is ready
+ */
+static bool xmit_insert_required_zlp(uint8_t rhport, uint32_t xferred_bytes) {
+  TU_LOG_DRV("xmit_insert_required_zlp(%d,%ld)\n", rhport, xferred_bytes);
+
+  if (xferred_bytes == 0 || xferred_bytes % CFG_TUD_NET_ENDPOINT_SIZE != 0) {
+    return false;
+  }
+
+  TU_ASSERT(ncm_interface.itf_data_alt == 1, false);
+  TU_ASSERT(!usbd_edpt_busy(rhport, ncm_interface.ep_in), false);
+
+  TU_LOG_DRV("xmit_insert_required_zlp! (%u)\n", (unsigned) xferred_bytes);
+
+  // start transmission of the ZLP
+  usbd_edpt_xfer(rhport, ncm_interface.ep_in, NULL, 0);
+
+  return true;
+} // xmit_insert_required_zlp
+
+/**
+ * Start transmission if it there is a waiting packet and if can be done from interface side.
  */
-static void ncm_start_tx(void) {
-  if (ncm_interface.transferring) {
+static void xmit_start_if_possible(uint8_t rhport) {
+  TU_LOG_DRV("xmit_start_if_possible()\n");
+
+  if (ncm_interface.xmit_tinyusb_ntb != NULL) {
+    TU_LOG_DRV("  !xmit_start_if_possible 1\n");
     return;
   }
+  if (ncm_interface.itf_data_alt != 1) {
+    TU_LOG_DRV("(EE) !xmit_start_if_possible 2\n");
+    return;
+  }
+  if (usbd_edpt_busy(rhport, ncm_interface.ep_in)) {
+    TU_LOG_DRV("  !xmit_start_if_possible 3\n");
+    return;
+  }
+
+  ncm_interface.xmit_tinyusb_ntb = xmit_get_next_ready_ntb();
+  if (ncm_interface.xmit_tinyusb_ntb == NULL) {
+    if (ncm_interface.xmit_glue_ntb == NULL || ncm_interface.xmit_glue_ntb_datagram_ndx == 0) {
+      // -> really nothing is waiting
+      return;
+    }
+    ncm_interface.xmit_tinyusb_ntb = ncm_interface.xmit_glue_ntb;
+    ncm_interface.xmit_glue_ntb = NULL;
+  }
+
+  #if CFG_TUD_NCM_LOG_LEVEL >= 3
+  {
+    uint16_t len = ncm_interface.xmit_tinyusb_ntb->nth.wBlockLength;
+    TU_LOG_BUF(3, ncm_interface.xmit_tinyusb_ntb->data[i], len);
+  }
+  #endif
+
+  if (ncm_interface.xmit_glue_ntb_datagram_ndx != 1) {
+    TU_LOG_DRV(">> %d %d\n", ncm_interface.xmit_tinyusb_ntb->nth.wBlockLength, ncm_interface.xmit_glue_ntb_datagram_ndx);
+  }
+
+  // Kick off an endpoint transfer
+  usbd_edpt_xfer(0, ncm_interface.ep_in, ncm_interface.xmit_tinyusb_ntb->data, ncm_interface.xmit_tinyusb_ntb->nth.wBlockLength);
+} // xmit_start_if_possible
+
+/**
+ * check if a new datagram fits into the current NTB
+ */
+static bool xmit_requested_datagram_fits_into_current_ntb(uint16_t datagram_size) {
+  TU_LOG_DRV("xmit_requested_datagram_fits_into_current_ntb(%d) - %p %p\n", datagram_size, ncm_interface.xmit_tinyusb_ntb, ncm_interface.xmit_glue_ntb);
+
+  if (ncm_interface.xmit_glue_ntb == NULL) {
+    return false;
+  }
+  if (ncm_interface.xmit_glue_ntb_datagram_ndx >= CFG_TUD_NCM_IN_MAX_DATAGRAMS_PER_NTB) {
+    return false;
+  }
+  if (ncm_interface.xmit_glue_ntb->nth.wBlockLength + datagram_size + XMIT_ALIGN_OFFSET(datagram_size) > CFG_TUD_NCM_IN_NTB_MAX_SIZE) {
+    return false;
+  }
+  return true;
+} // xmit_requested_datagram_fits_into_current_ntb
+
+/**
+ * Setup an NTB for the glue logic
+ */
+static bool xmit_setup_next_glue_ntb(void) {
+  TU_LOG_DRV("xmit_setup_next_glue_ntb - %p\n", ncm_interface.xmit_glue_ntb);
+
+  if (ncm_interface.xmit_glue_ntb != NULL) {
+    // put NTB into waiting list (the new datagram did not fit in)
+    xmit_put_ntb_into_ready_list(ncm_interface.xmit_glue_ntb);
+  }
+
+  ncm_interface.xmit_glue_ntb = xmit_get_free_ntb();// get next buffer (if any)
+  if (ncm_interface.xmit_glue_ntb == NULL) {
+    TU_LOG_DRV("  xmit_setup_next_glue_ntb - nothing free\n");// should happen rarely
+    return false;
+  }
+
+  ncm_interface.xmit_glue_ntb_datagram_ndx = 0;
 
-  transmit_ntb_t *ntb = &transmit_ntb[ncm_interface.current_ntb];
-  size_t ntb_length = ncm_interface.next_datagram_offset;
+  xmit_ntb_t *ntb = ncm_interface.xmit_glue_ntb;
 
   // Fill in NTB header
   ntb->nth.dwSignature = NTH16_SIGNATURE;
-  ntb->nth.wHeaderLength = sizeof(nth16_t);
-  ntb->nth.wSequence = ncm_interface.nth_sequence++;
-  ntb->nth.wBlockLength = ntb_length;
-  ntb->nth.wNdpIndex = sizeof(nth16_t);
+  ntb->nth.wHeaderLength = sizeof(ntb->nth);
+  ntb->nth.wSequence = ncm_interface.xmit_sequence++;
+  ntb->nth.wBlockLength = sizeof(ntb->nth) + sizeof(ntb->ndp) + sizeof(ntb->ndp_datagram);
+  ntb->nth.wNdpIndex = sizeof(ntb->nth);
 
   // Fill in NDP16 header and terminator
   ntb->ndp.dwSignature = NDP16_SIGNATURE_NCM0;
-  ntb->ndp.wLength = sizeof(ndp16_t) + (ncm_interface.datagram_count + 1) * sizeof(ndp16_datagram_t);
+  ntb->ndp.wLength = sizeof(ntb->ndp) + sizeof(ntb->ndp_datagram);
   ntb->ndp.wNextNdpIndex = 0;
-  ntb->ndp.datagram[ncm_interface.datagram_count].wDatagramIndex = 0;
-  ntb->ndp.datagram[ncm_interface.datagram_count].wDatagramLength = 0;
 
-  // Kick off an endpoint transfer
-  usbd_edpt_xfer(0, ncm_interface.ep_in, ntb->data, ntb_length);
-  ncm_interface.transferring = true;
+  memset(ntb->ndp_datagram, 0, sizeof(ntb->ndp_datagram));
+  return true;
+} // xmit_setup_next_glue_ntb
 
-  // Swap to the other NTB and clear it out
-  ncm_interface.current_ntb = 1 - ncm_interface.current_ntb;
-  ncm_prepare_for_tx();
-}
+//-----------------------------------------------------------------------------
+//
+// all the recv_*() stuff (TinyUSB -> driver -> glue logic)
+//
 
-static struct ecm_notify_struct ncm_notify_connected =
-{
-    .header = {
-        .bmRequestType_bit = {
-          .recipient = TUSB_REQ_RCPT_INTERFACE,
-          .type      = TUSB_REQ_TYPE_CLASS,
-          .direction = TUSB_DIR_IN
-        },
-        .bRequest      = CDC_NOTIF_NETWORK_CONNECTION,
-        .wValue        = 1 /* Connected */,
-        .wLength       = 0,
-    },
-};
-
-static struct ecm_notify_struct ncm_notify_speed_change =
-{
-    .header = {
-        .bmRequestType_bit = {
-          .recipient = TUSB_REQ_RCPT_INTERFACE,
-          .type      = TUSB_REQ_TYPE_CLASS,
-          .direction = TUSB_DIR_IN
-        },
-        .bRequest = CDC_NOTIF_CONNECTION_SPEED_CHANGE,
-        .wLength = 8,
-    },
-    .downlink = 10000000,
-    .uplink = 10000000,
-};
+/**
+ * Return pointer to an available receive buffer or NULL.
+ * Returned buffer (if any) has the size \a CFG_TUD_NCM_OUT_NTB_MAX_SIZE.
+ */
+static recv_ntb_t *recv_get_free_ntb(void) {
+  TU_LOG_DRV("recv_get_free_ntb()\n");
 
-void tud_network_recv_renew(void)
-{
-  if (!ncm_interface.num_datagrams)
-  {
-    usbd_edpt_xfer(0, ncm_interface.ep_out, receive_ntb, sizeof(receive_ntb));
-    return;
+  for (int i = 0; i < RECV_NTB_N; ++i) {
+    if (ncm_interface.recv_free_ntb[i] != NULL) {
+      recv_ntb_t *free = ncm_interface.recv_free_ntb[i];
+      ncm_interface.recv_free_ntb[i] = NULL;
+      return free;
+    }
   }
+  return NULL;
+} // recv_get_free_ntb
 
-  const ndp16_t *ndp = ncm_interface.ndp;
-  const int i = ncm_interface.current_datagram_index;
-  ncm_interface.current_datagram_index++;
-  ncm_interface.num_datagrams--;
+/**
+ * Get the next NTB from the ready list (and remove it from the list).
+ * If the ready list is empty, return NULL.
+ */
+static recv_ntb_t *recv_get_next_ready_ntb(void) {
+  recv_ntb_t *r = NULL;
 
-  tud_network_recv_cb(receive_ntb + ndp->datagram[i].wDatagramIndex, ndp->datagram[i].wDatagramLength);
-}
+  r = ncm_interface.recv_ready_ntb[0];
+  memmove(ncm_interface.recv_ready_ntb + 0, ncm_interface.recv_ready_ntb + 1, sizeof(ncm_interface.recv_ready_ntb) - sizeof(ncm_interface.recv_ready_ntb[0]));
+  ncm_interface.recv_ready_ntb[RECV_NTB_N - 1] = NULL;
 
-//--------------------------------------------------------------------+
-// USBD Driver API
-//--------------------------------------------------------------------+
+  TU_LOG_DRV("recv_get_next_ready_ntb: %p\n", r);
+  return r;
+} // recv_get_next_ready_ntb
 
-void netd_init(void)
-{
-  tu_memclr(&ncm_interface, sizeof(ncm_interface));
-  ncm_interface.ntb_in_size = CFG_TUD_NCM_IN_NTB_MAX_SIZE;
-  ncm_interface.max_datagrams_per_ntb = CFG_TUD_NCM_MAX_DATAGRAMS_PER_NTB;
-  ncm_prepare_for_tx();
-}
+/**
+ * Put NTB into the receiver free list.
+ */
+static void recv_put_ntb_into_free_list(recv_ntb_t *free_ntb) {
+  TU_LOG_DRV("recv_put_ntb_into_free_list(%p)\n", free_ntb);
 
-void netd_reset(uint8_t rhport)
-{
-  (void) rhport;
+  for (int i = 0; i < RECV_NTB_N; ++i) {
+    if (ncm_interface.recv_free_ntb[i] == NULL) {
+      ncm_interface.recv_free_ntb[i] = free_ntb;
+      return;
+    }
+  }
+  TU_LOG_DRV("(EE) recv_put_ntb_into_free_list - no entry in free list\n");// this should not happen
+} // recv_put_ntb_into_free_list
 
-  netd_init();
-}
+/**
+ * \a ready_ntb holds a validated NTB,
+ * put this buffer into the waiting list.
+ */
+static void recv_put_ntb_into_ready_list(recv_ntb_t *ready_ntb) {
+  TU_LOG_DRV("recv_put_ntb_into_ready_list(%p) %d\n", ready_ntb, ready_ntb->nth.wBlockLength);
+
+  for (int i = 0; i < RECV_NTB_N; ++i) {
+    if (ncm_interface.recv_ready_ntb[i] == NULL) {
+      ncm_interface.recv_ready_ntb[i] = ready_ntb;
+      return;
+    }
+  }
+  TU_LOG_DRV("(EE) recv_put_ntb_into_ready_list: ready list full\n");// this should not happen
+} // recv_put_ntb_into_ready_list
 
-uint16_t netd_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len)
-{
-  // confirm interface hasn't already been allocated
-  TU_ASSERT(0 == ncm_interface.ep_notif, 0);
+/**
+ * If possible, start a new reception TinyUSB -> driver.
+ */
+static void recv_try_to_start_new_reception(uint8_t rhport) {
+  TU_LOG_DRV("recv_try_to_start_new_reception(%d)\n", rhport);
 
-  //------------- Management Interface -------------//
-  ncm_interface.itf_num = itf_desc->bInterfaceNumber;
+  if (ncm_interface.itf_data_alt != 1) {
+    return;
+  }
+  if (ncm_interface.recv_tinyusb_ntb != NULL) {
+    return;
+  }
+  if (usbd_edpt_busy(rhport, ncm_interface.ep_out)) {
+    return;
+  }
 
-  uint16_t drv_len = sizeof(tusb_desc_interface_t);
-  uint8_t const * p_desc = tu_desc_next( itf_desc );
+  ncm_interface.recv_tinyusb_ntb = recv_get_free_ntb();
+  if (ncm_interface.recv_tinyusb_ntb == NULL) {
+    return;
+  }
 
-  // Communication Functional Descriptors
-  while ( TUSB_DESC_CS_INTERFACE == tu_desc_type(p_desc) && drv_len <= max_len )
-  {
-    drv_len += tu_desc_len(p_desc);
-    p_desc   = tu_desc_next(p_desc);
+  // initiate transfer
+  TU_LOG_DRV("  start reception\n");
+  bool r = usbd_edpt_xfer(rhport, ncm_interface.ep_out, ncm_interface.recv_tinyusb_ntb->data, CFG_TUD_NCM_OUT_NTB_MAX_SIZE);
+  if (!r) {
+    recv_put_ntb_into_free_list(ncm_interface.recv_tinyusb_ntb);
+    ncm_interface.recv_tinyusb_ntb = NULL;
   }
+} // recv_try_to_start_new_reception
 
-  // notification endpoint (if any)
-  if ( TUSB_DESC_ENDPOINT == tu_desc_type(p_desc) )
-  {
-    TU_ASSERT( usbd_edpt_open(rhport, (tusb_desc_endpoint_t const *) p_desc), 0 );
+/**
+ * Validate incoming datagram.
+ * \return true if valid
+ *
+ * \note
+ *    \a ndp16->wNextNdpIndex != 0 is not supported
+ */
+static bool recv_validate_datagram(const recv_ntb_t *ntb, uint32_t len) {
+  const nth16_t *nth16 = &(ntb->nth);
 
-    ncm_interface.ep_notif = ((tusb_desc_endpoint_t const *) p_desc)->bEndpointAddress;
+  TU_LOG_DRV("recv_validate_datagram(%p, %d)\n", ntb, (int) len);
 
-    drv_len += tu_desc_len(p_desc);
-    p_desc   = tu_desc_next(p_desc);
+  // check header
+  if (nth16->wHeaderLength != sizeof(nth16_t)) {
+    TU_LOG_DRV("(EE) ill nth16 length: %d\n", nth16->wHeaderLength);
+    return false;
+  }
+  if (nth16->dwSignature != NTH16_SIGNATURE) {
+    TU_LOG_DRV("(EE) ill signature: 0x%08x\n", (unsigned) nth16->dwSignature);
+    return false;
+  }
+  if (len < sizeof(nth16_t) + sizeof(ndp16_t) + 2 * sizeof(ndp16_datagram_t)) {
+    TU_LOG_DRV("(EE) ill min len: %lu\n", len);
+    return false;
+  }
+  if (nth16->wBlockLength > len) {
+    TU_LOG_DRV("(EE) ill block length: %d > %lu\n", nth16->wBlockLength, len);
+    return false;
+  }
+  if (nth16->wBlockLength > CFG_TUD_NCM_OUT_NTB_MAX_SIZE) {
+    TU_LOG_DRV("(EE) ill block length2: %d > %d\n", nth16->wBlockLength, CFG_TUD_NCM_OUT_NTB_MAX_SIZE);
+    return false;
+  }
+  if (nth16->wNdpIndex < sizeof(nth16) || nth16->wNdpIndex > len - (sizeof(ndp16_t) + 2 * sizeof(ndp16_datagram_t))) {
+    TU_LOG_DRV("(EE) ill position of first ndp: %d (%lu)\n", nth16->wNdpIndex, len);
+    return false;
   }
 
-  //------------- Data Interface -------------//
-  // - CDC-NCM data interface has 2 alternate settings
-  //   - 0 : zero endpoints for inactive (default)
-  //   - 1 : IN & OUT endpoints for transfer of NTBs
-  TU_ASSERT(TUSB_DESC_INTERFACE == tu_desc_type(p_desc), 0);
+  // check (first) NDP(16)
+  const ndp16_t *ndp16 = (const ndp16_t *) (ntb->data + nth16->wNdpIndex);
 
-  do
-  {
-    tusb_desc_interface_t const * data_itf_desc = (tusb_desc_interface_t const *) p_desc;
-    TU_ASSERT(TUSB_CLASS_CDC_DATA == data_itf_desc->bInterfaceClass, 0);
+  if (ndp16->wLength < sizeof(ndp16_t) + 2 * sizeof(ndp16_datagram_t)) {
+    TU_LOG_DRV("(EE) ill ndp16 length: %d\n", ndp16->wLength);
+    return false;
+  }
+  if (ndp16->dwSignature != NDP16_SIGNATURE_NCM0 && ndp16->dwSignature != NDP16_SIGNATURE_NCM1) {
+    TU_LOG_DRV("(EE) ill signature: 0x%08x\n", (unsigned) ndp16->dwSignature);
+    return false;
+  }
+  if (ndp16->wNextNdpIndex != 0) {
+    TU_LOG_DRV("(EE) cannot handle wNextNdpIndex!=0 (%d)\n", ndp16->wNextNdpIndex);
+    return false;
+  }
 
-    drv_len += tu_desc_len(p_desc);
-    p_desc   = tu_desc_next(p_desc);
-  } while((TUSB_DESC_INTERFACE == tu_desc_type(p_desc)) && (drv_len <= max_len));
+  const ndp16_datagram_t *ndp16_datagram = (const ndp16_datagram_t *) (ntb->data + nth16->wNdpIndex + sizeof(ndp16_t));
+  int ndx = 0;
+  uint16_t max_ndx = (uint16_t) ((ndp16->wLength - sizeof(ndp16_t)) / sizeof(ndp16_datagram_t));
 
-  // Pair of endpoints
-  TU_ASSERT(TUSB_DESC_ENDPOINT == tu_desc_type(p_desc), 0);
+  if (max_ndx > 2) { // number of datagrams in NTB > 1
+    TU_LOG_DRV("<< %d (%d)\n", max_ndx - 1, ntb->nth.wBlockLength);
+  }
+  if (ndp16_datagram[max_ndx - 1].wDatagramIndex != 0 || ndp16_datagram[max_ndx - 1].wDatagramLength != 0) {
+    TU_LOG_DRV("  max_ndx != 0\n");
+    return false;
+  }
+  while (ndp16_datagram[ndx].wDatagramIndex != 0 && ndp16_datagram[ndx].wDatagramLength != 0) {
+    TU_LOG_DRV("  << %d %d\n", ndp16_datagram[ndx].wDatagramIndex, ndp16_datagram[ndx].wDatagramLength);
+    if (ndp16_datagram[ndx].wDatagramIndex > len) {
+      TU_LOG_DRV("(EE) ill start of datagram[%d]: %d (%lu)\n", ndx, ndp16_datagram[ndx].wDatagramIndex, len);
+      return false;
+    }
+    if (ndp16_datagram[ndx].wDatagramIndex + ndp16_datagram[ndx].wDatagramLength > len) {
+      TU_LOG_DRV("(EE) ill end of datagram[%d]: %d (%lu)\n", ndx, ndp16_datagram[ndx].wDatagramIndex + ndp16_datagram[ndx].wDatagramLength, len);
+      return false;
+    }
+    ++ndx;
+  }
 
-  TU_ASSERT(usbd_open_edpt_pair(rhport, p_desc, 2, TUSB_XFER_BULK, &ncm_interface.ep_out, &ncm_interface.ep_in) );
+  #if CFG_TUD_NCM_LOG_LEVEL >= 3
+  TU_LOG_BUF(3, ntb->data[i], len);
+  #endif
 
-  drv_len += 2*sizeof(tusb_desc_endpoint_t);
+  // -> ntb contains a valid packet structure
+  //    ok... I did not check for garbage within the datagram indices...
+  return true;
+} // recv_validate_datagram
 
-  return drv_len;
-}
+/**
+ * Transfer the next (pending) datagram to the glue logic and return receive buffer if empty.
+ */
+static void recv_transfer_datagram_to_glue_logic(void) {
+  TU_LOG_DRV("recv_transfer_datagram_to_glue_logic()\n");
 
-static void ncm_report(void)
-{
-  uint8_t const rhport = 0;
-  if (ncm_interface.report_state == REPORT_SPEED) {
-    ncm_notify_speed_change.header.wIndex = ncm_interface.itf_num;
-    usbd_edpt_xfer(rhport, ncm_interface.ep_notif, (uint8_t *) &ncm_notify_speed_change, sizeof(ncm_notify_speed_change));
-    ncm_interface.report_state = REPORT_CONNECTED;
-    ncm_interface.report_pending = true;
-  } else if (ncm_interface.report_state == REPORT_CONNECTED) {
-    ncm_notify_connected.header.wIndex = ncm_interface.itf_num;
-    usbd_edpt_xfer(rhport, ncm_interface.ep_notif, (uint8_t *) &ncm_notify_connected, sizeof(ncm_notify_connected));
-    ncm_interface.report_state = REPORT_DONE;
-    ncm_interface.report_pending = true;
+  if (ncm_interface.recv_glue_ntb == NULL) {
+    ncm_interface.recv_glue_ntb = recv_get_next_ready_ntb();
+    TU_LOG_DRV("  new buffer for glue logic: %p\n", ncm_interface.recv_glue_ntb);
+    ncm_interface.recv_glue_ntb_datagram_ndx = 0;
   }
-}
 
-TU_ATTR_WEAK void tud_network_link_state_cb(bool state)
-{
-  (void)state;
-}
+  if (ncm_interface.recv_glue_ntb != NULL) {
+    const ndp16_datagram_t *ndp16_datagram = (ndp16_datagram_t *) (ncm_interface.recv_glue_ntb->data + ncm_interface.recv_glue_ntb->nth.wNdpIndex + sizeof(ndp16_t));
 
-// Handle class control request
-// return false to stall control endpoint (e.g unsupported request)
-bool netd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
-{
-  if ( stage != CONTROL_STAGE_SETUP ) return true;
+    if (ndp16_datagram[ncm_interface.recv_glue_ntb_datagram_ndx].wDatagramIndex == 0) {
+      TU_LOG_DRV("(EE) SOMETHING WENT WRONG 1\n");
+    } else if (ndp16_datagram[ncm_interface.recv_glue_ntb_datagram_ndx].wDatagramLength == 0) {
+      TU_LOG_DRV("(EE) SOMETHING WENT WRONG 2\n");
+    } else {
+      uint16_t datagramIndex = ndp16_datagram[ncm_interface.recv_glue_ntb_datagram_ndx].wDatagramIndex;
+      uint16_t datagramLength = ndp16_datagram[ncm_interface.recv_glue_ntb_datagram_ndx].wDatagramLength;
 
-  switch ( request->bmRequestType_bit.type )
-  {
-    case TUSB_REQ_TYPE_STANDARD:
-      switch ( request->bRequest )
-      {
-        case TUSB_REQ_GET_INTERFACE:
-        {
-          uint8_t const req_itfnum = (uint8_t) request->wIndex;
-          TU_VERIFY(ncm_interface.itf_num + 1 == req_itfnum);
+      TU_LOG_DRV("  recv[%d] - %d %d\n", ncm_interface.recv_glue_ntb_datagram_ndx, datagramIndex, datagramLength);
+      if (tud_network_recv_cb(ncm_interface.recv_glue_ntb->data + datagramIndex, datagramLength)) {
+        // send datagram successfully to glue logic
+        TU_LOG_DRV("    OK\n");
+        datagramIndex = ndp16_datagram[ncm_interface.recv_glue_ntb_datagram_ndx + 1].wDatagramIndex;
+        datagramLength = ndp16_datagram[ncm_interface.recv_glue_ntb_datagram_ndx + 1].wDatagramLength;
 
-          tud_control_xfer(rhport, request, &ncm_interface.itf_data_alt, 1);
+        if (datagramIndex != 0 && datagramLength != 0) {
+          // -> next datagram
+          ++ncm_interface.recv_glue_ntb_datagram_ndx;
+        } else {
+          // end of datagrams reached
+          recv_put_ntb_into_free_list(ncm_interface.recv_glue_ntb);
+          ncm_interface.recv_glue_ntb = NULL;
         }
-          break;
+      }
+    }
+  }
+} // recv_transfer_datagram_to_glue_logic
 
-        case TUSB_REQ_SET_INTERFACE:
-        {
-          uint8_t const req_itfnum = (uint8_t) request->wIndex;
-          uint8_t const req_alt    = (uint8_t) request->wValue;
+//-----------------------------------------------------------------------------
+//
+// all the tud_network_*() stuff (glue logic -> driver)
+//
 
-          // Only valid for Data Interface with Alternate is either 0 or 1
-          TU_VERIFY(ncm_interface.itf_num + 1 == req_itfnum && req_alt < 2);
+/**
+ * Check if the glue logic is allowed to call tud_network_xmit().
+ * This function also fetches a next buffer if required, so that tud_network_xmit() is ready for copy
+ * and transmission operation.
+ */
+bool tud_network_can_xmit(uint16_t size) {
+  TU_LOG_DRV("tud_network_can_xmit(%d)\n", size);
 
-          if (req_alt != ncm_interface.itf_data_alt) {
-            ncm_interface.itf_data_alt = req_alt;
+  TU_ASSERT(size <= CFG_TUD_NCM_IN_NTB_MAX_SIZE - (sizeof(nth16_t) + sizeof(ndp16_t) + 2 * sizeof(ndp16_datagram_t)), false);
 
-            if (ncm_interface.itf_data_alt) {
-              if (!usbd_edpt_busy(rhport, ncm_interface.ep_out)) {
-                tud_network_recv_renew(); // prepare for incoming datagrams
-              }
-              if (!ncm_interface.report_pending) {
-                ncm_report();
-              }
-            }
+  if (xmit_requested_datagram_fits_into_current_ntb(size) || xmit_setup_next_glue_ntb()) {
+    // -> everything is fine
+    return true;
+  }
+  xmit_start_if_possible(ncm_interface.rhport);
+  TU_LOG_DRV("(II) tud_network_can_xmit: request blocked\n");// could happen if all xmit buffers are full (but should happen rarely)
+  return false;
+} // tud_network_can_xmit
 
-            tud_network_link_state_cb(ncm_interface.itf_data_alt);
-          }
+/**
+ * Put a datagram into a waiting NTB.
+ * If currently no transmission is started, then initiate transmission.
+ */
+void tud_network_xmit(void *ref, uint16_t arg) {
+  TU_LOG_DRV("tud_network_xmit(%p, %d)\n", ref, arg);
 
-          tud_control_status(rhport, request);
-        }
-          break;
+  if (ncm_interface.xmit_glue_ntb == NULL) {
+    TU_LOG_DRV("(EE) tud_network_xmit: no buffer\n");// must not happen (really)
+    return;
+  }
 
-          // unsupported request
-        default: return false;
-      }
-      break;
+  xmit_ntb_t *ntb = ncm_interface.xmit_glue_ntb;
 
-    case TUSB_REQ_TYPE_CLASS:
-      TU_VERIFY (ncm_interface.itf_num == request->wIndex);
+  // copy new datagram to the end of the current NTB
+  uint16_t size = tud_network_xmit_cb(ntb->data + ntb->nth.wBlockLength, ref, arg);
 
-      if (NCM_GET_NTB_PARAMETERS == request->bRequest)
-      {
-        tud_control_xfer(rhport, request, (void*)&ntb_parameters, sizeof(ntb_parameters));
-      }
+  // correct NTB internals
+  ntb->ndp_datagram[ncm_interface.xmit_glue_ntb_datagram_ndx].wDatagramIndex = ntb->nth.wBlockLength;
+  ntb->ndp_datagram[ncm_interface.xmit_glue_ntb_datagram_ndx].wDatagramLength = size;
+  ncm_interface.xmit_glue_ntb_datagram_ndx += 1;
 
-      break;
+  ntb->nth.wBlockLength += (uint16_t) (size + XMIT_ALIGN_OFFSET(size));
 
-      // unsupported request
-    default: return false;
+  if (ntb->nth.wBlockLength > CFG_TUD_NCM_IN_NTB_MAX_SIZE) {
+    TU_LOG_DRV("(EE) tud_network_xmit: buffer overflow\n"); // must not happen (really)
+    return;
   }
 
-  return true;
-}
+  xmit_start_if_possible(ncm_interface.rhport);
+} // tud_network_xmit
+
+/**
+ * Keep the receive logic busy and transfer pending packets to the glue logic.
+ * Avoid recursive calls due to wrong expectations of the net glue logic,
+ * see https://github.com/hathach/tinyusb/issues/2711
+ */
+void tud_network_recv_renew(void) {
+  TU_LOG_DRV("tud_network_recv_renew()\n");
 
-static void handle_incoming_datagram(uint32_t len)
-{
-  uint32_t size = len;
+  ncm_interface.tud_network_recv_renew_process_again = true;
 
-  if (len == 0) {
+  if (ncm_interface.tud_network_recv_renew_active) {
+    TU_LOG_DRV("Re-entrant into tud_network_recv_renew, will process later\n");
     return;
   }
 
-  TU_ASSERT(size >= sizeof(nth16_t), );
+  while (ncm_interface.tud_network_recv_renew_process_again) {
+    ncm_interface.tud_network_recv_renew_process_again = false;
 
-  const nth16_t *hdr = (const nth16_t *)receive_ntb;
-  TU_ASSERT(hdr->dwSignature == NTH16_SIGNATURE, );
-  TU_ASSERT(hdr->wNdpIndex >= sizeof(nth16_t) && (hdr->wNdpIndex + sizeof(ndp16_t)) <= len, );
+    // If the current function is called within recv_transfer_datagram_to_glue_logic,
+    // tud_network_recv_renew_process_again will become true, and the loop will run again
+    // Otherwise the loop will not run again
+    ncm_interface.tud_network_recv_renew_active = true;
+    recv_transfer_datagram_to_glue_logic();
+    ncm_interface.tud_network_recv_renew_active = false;
+  }
+  recv_try_to_start_new_reception(ncm_interface.rhport);
+} // tud_network_recv_renew
 
-  const ndp16_t *ndp = (const ndp16_t *)(receive_ntb + hdr->wNdpIndex);
-  TU_ASSERT(ndp->dwSignature == NDP16_SIGNATURE_NCM0 || ndp->dwSignature == NDP16_SIGNATURE_NCM1, );
-  TU_ASSERT(hdr->wNdpIndex + ndp->wLength <= len, );
+/**
+ * Same as tud_network_recv_renew() but knows \a rhport
+ */
+static void tud_network_recv_renew_r(uint8_t rhport) {
+  TU_LOG_DRV("tud_network_recv_renew_r(%d)\n", rhport);
 
-  int num_datagrams = (ndp->wLength - 12) / 4;
-  ncm_interface.current_datagram_index = 0;
-  ncm_interface.num_datagrams = 0;
-  ncm_interface.ndp = ndp;
-  for (int i = 0; i < num_datagrams && ndp->datagram[i].wDatagramIndex && ndp->datagram[i].wDatagramLength; i++)
-  {
-    ncm_interface.num_datagrams++;
-  }
-  
+  ncm_interface.rhport = rhport;
   tud_network_recv_renew();
+} // tud_network_recv_renew
+
+//-----------------------------------------------------------------------------
+//
+// all the netd_*() stuff (interface TinyUSB -> driver)
+//
+/**
+ * Initialize the driver data structures.
+ * Might be called several times.
+ */
+void netd_init(void) {
+  TU_LOG_DRV("netd_init()\n");
+
+  memset(&ncm_interface, 0, sizeof(ncm_interface));
+
+  for (int i = 0; i < XMIT_NTB_N; ++i) {
+    ncm_interface.xmit_free_ntb[i] = &ncm_epbuf.xmit[i].ntb;
+  }
+  for (int i = 0; i < RECV_NTB_N; ++i) {
+    ncm_interface.recv_free_ntb[i] = &ncm_epbuf.recv[i].ntb;
+  }
+} // netd_init
+
+/**
+ * Deinit driver
+ */
+bool netd_deinit(void) {
+  return true;
 }
 
-bool netd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
-{
+/**
+ * Resets the port.
+ * In this driver this is the same as netd_init()
+ */
+void netd_reset(uint8_t rhport) {
   (void) rhport;
-  (void) result;
 
-  /* new datagram receive_ntb */
-  if (ep_addr == ncm_interface.ep_out )
-  {
-    handle_incoming_datagram(xferred_bytes);
-  }
+  netd_init();
+} // netd_reset
 
-  /* data transmission finished */
-  if (ep_addr == ncm_interface.ep_in )
-  {
-    if (ncm_interface.transferring) {
-      ncm_interface.transferring = false;
-    }
+/**
+ * Open the USB interface.
+ * - parse the USB descriptor \a TUD_CDC_NCM_DESCRIPTOR for itfnum and endpoints
+ * - a specific order of elements in the descriptor is tested.
+ *
+ * \note
+ *   Actually all of the information could be read directly from \a itf_desc, because the
+ *   structure and the values are well known.  But we do it this way.
+ *
+ * \post
+ * - \a itf_num set
+ * - \a ep_notif, \a ep_in and \a ep_out are set
+ * - USB interface is open
+ */
+uint16_t netd_open(uint8_t rhport, tusb_desc_interface_t const *itf_desc, uint16_t max_len) {
+  TU_ASSERT(ncm_interface.ep_notif == 0, 0);// assure that the interface is only opened once
 
-    // If there are datagrams queued up that we tried to send while this NTB was being emitted, send them now
-    if (ncm_interface.datagram_count && ncm_interface.itf_data_alt == 1) {
-      ncm_start_tx();
-    }
-  }
+  ncm_interface.itf_num = itf_desc->bInterfaceNumber;// management interface
 
-  if (ep_addr == ncm_interface.ep_notif )
-  {
-    ncm_interface.report_pending = false;
-    ncm_report();
+  // skip the two first entries and the following TUSB_DESC_CS_INTERFACE entries
+  uint16_t drv_len = sizeof(tusb_desc_interface_t);
+  uint8_t const *p_desc = tu_desc_next(itf_desc);
+  while (tu_desc_type(p_desc) == TUSB_DESC_CS_INTERFACE && drv_len <= max_len) {
+    drv_len += tu_desc_len(p_desc);
+    p_desc = tu_desc_next(p_desc);
   }
 
-  return true;
-}
+  // get notification endpoint
+  TU_ASSERT(tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT, 0);
+  TU_ASSERT(usbd_edpt_open(rhport, (tusb_desc_endpoint_t const *) p_desc), 0);
+  ncm_interface.ep_notif = ((tusb_desc_endpoint_t const *) p_desc)->bEndpointAddress;
+  drv_len += tu_desc_len(p_desc);
+  p_desc = tu_desc_next(p_desc);
 
-// poll network driver for its ability to accept another packet to transmit
-bool tud_network_can_xmit(uint16_t size)
-{
-  TU_VERIFY(ncm_interface.itf_data_alt == 1);
+  // skip the following TUSB_DESC_INTERFACE entries (which must be TUSB_CLASS_CDC_DATA)
+  while (tu_desc_type(p_desc) == TUSB_DESC_INTERFACE && drv_len <= max_len) {
+    tusb_desc_interface_t const *data_itf_desc = (tusb_desc_interface_t const *) p_desc;
+    TU_ASSERT(data_itf_desc->bInterfaceClass == TUSB_CLASS_CDC_DATA, 0);
 
-  if (ncm_interface.datagram_count >= ncm_interface.max_datagrams_per_ntb) {
-    TU_LOG2("NTB full [by count]\r\n");
-    return false;
+    drv_len += tu_desc_len(p_desc);
+    p_desc = tu_desc_next(p_desc);
   }
 
-  size_t next_datagram_offset = ncm_interface.next_datagram_offset;
-  if (next_datagram_offset + size > ncm_interface.ntb_in_size) {
-    TU_LOG2("ntb full [by size]\r\n");
-    return false;
+  // a TUSB_DESC_ENDPOINT (actually two) must follow, open these endpoints
+  TU_ASSERT(tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT, 0);
+  TU_ASSERT(usbd_open_edpt_pair(rhport, p_desc, 2, TUSB_XFER_BULK, &ncm_interface.ep_out, &ncm_interface.ep_in));
+  drv_len += 2 * sizeof(tusb_desc_endpoint_t);
+
+  return drv_len;
+} // netd_open
+
+/**
+ * Handle TinyUSB requests to process transfer events.
+ */
+bool netd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) {
+  (void) result;
+
+  if (ep_addr == ncm_interface.ep_out) {
+    // new NTB received
+    // - make the NTB valid
+    // - if ready transfer datagrams to the glue logic for further processing
+    // - if there is a free receive buffer, initiate reception
+    if (!recv_validate_datagram(ncm_interface.recv_tinyusb_ntb, xferred_bytes)) {
+      // verification failed: ignore NTB and return it to free
+      TU_LOG_DRV("Invalid datatagram. Ignoring NTB\n");
+      recv_put_ntb_into_free_list(ncm_interface.recv_tinyusb_ntb);
+    } else {
+      // packet ok -> put it into ready list
+      recv_put_ntb_into_ready_list(ncm_interface.recv_tinyusb_ntb);
+    }
+    ncm_interface.recv_tinyusb_ntb = NULL;
+    tud_network_recv_renew_r(rhport);
+  } else if (ep_addr == ncm_interface.ep_in) {
+    // transmission of an NTB finished
+    // - free the transmitted NTB buffer
+    // - insert ZLPs when necessary
+    // - if there is another transmit NTB waiting, try to start transmission
+    xmit_put_ntb_into_free_list(ncm_interface.xmit_tinyusb_ntb);
+    ncm_interface.xmit_tinyusb_ntb = NULL;
+    if (!xmit_insert_required_zlp(rhport, xferred_bytes)) {
+      xmit_start_if_possible(rhport);
+    }
+  } else if (ep_addr == ncm_interface.ep_notif) {
+    // next transfer on notification channel
+    notification_xmit(rhport, true);
   }
 
   return true;
-}
+} // netd_xfer_cb
+
+/**
+ * Respond to TinyUSB control requests.
+ * At startup transmission of notification packets are done here.
+ */
+bool netd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const *request) {
+  if (stage != CONTROL_STAGE_SETUP) {
+    return true;
+  }
+
+  switch (request->bmRequestType_bit.type) {
+    case TUSB_REQ_TYPE_STANDARD:
 
-void tud_network_xmit(void *ref, uint16_t arg)
-{
-  transmit_ntb_t *ntb = &transmit_ntb[ncm_interface.current_ntb];
-  size_t next_datagram_offset = ncm_interface.next_datagram_offset;
+      switch (request->bRequest) {
+        case TUSB_REQ_GET_INTERFACE: {
+          TU_VERIFY(ncm_interface.itf_num + 1 == request->wIndex, false);
 
-  uint16_t size = tud_network_xmit_cb(ntb->data + next_datagram_offset, ref, arg);
+          tud_control_xfer(rhport, request, &ncm_interface.itf_data_alt, 1);
+        } break;
 
-  ntb->ndp.datagram[ncm_interface.datagram_count].wDatagramIndex = ncm_interface.next_datagram_offset;
-  ntb->ndp.datagram[ncm_interface.datagram_count].wDatagramLength = size;
+        case TUSB_REQ_SET_INTERFACE: {
+          TU_VERIFY(ncm_interface.itf_num + 1 == request->wIndex && request->wValue < 2, false);
 
-  ncm_interface.datagram_count++;
-  next_datagram_offset += size;
+          ncm_interface.itf_data_alt = (uint8_t) request->wValue;
 
-  // round up so the next datagram is aligned correctly
-  next_datagram_offset += (CFG_TUD_NCM_ALIGNMENT - 1);
-  next_datagram_offset -= (next_datagram_offset % CFG_TUD_NCM_ALIGNMENT);
+          if (ncm_interface.itf_data_alt == 1) {
+            tud_network_recv_renew_r(rhport);
+            notification_xmit(rhport, false);
+          }
+          tud_control_status(rhport, request);
+        } break;
 
-  ncm_interface.next_datagram_offset = next_datagram_offset;
+        // unsupported request
+        default:
+          return false;
+      }
+      break;
 
-  ncm_start_tx();
-}
+    case TUSB_REQ_TYPE_CLASS:
+      TU_VERIFY(ncm_interface.itf_num == request->wIndex, false);
+      switch (request->bRequest) {
+        case NCM_GET_NTB_PARAMETERS: {
+          // transfer NTB parameters to host.
+          tud_control_xfer(rhport, request, (void *) (uintptr_t) &ntb_parameters, sizeof(ntb_parameters));
+        } break;
 
-#endif
+          // unsupported request
+        default:
+          return false;
+      }
+      break;
+      // unsupported request
+    default:
+      return false;
+  }
+
+  return true;
+} // netd_control_xfer_cb
+
+#endif // ( CFG_TUD_ENABLED && CFG_TUD_NCM )
diff --git a/src/class/net/net_device.h b/src/class/net/net_device.h
index 6e294465b..4c9a92f2d 100644
--- a/src/class/net/net_device.h
+++ b/src/class/net/net_device.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2020 Peter Lawrence
@@ -28,14 +28,13 @@
 #ifndef _TUSB_NET_DEVICE_H_
 #define _TUSB_NET_DEVICE_H_
 
+#include <stdint.h>
 #include "class/cdc/cdc.h"
 
 #if CFG_TUD_ECM_RNDIS && CFG_TUD_NCM
 #error "Cannot enable both ECM_RNDIS and NCM network drivers"
 #endif
 
-#include "ncm.h"
-
 /* declared here, NOT in usb_descriptors.c, so that the driver can intelligently ZLP as needed */
 #define CFG_TUD_NET_ENDPOINT_SIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
 
@@ -44,21 +43,13 @@
 #define CFG_TUD_NET_MTU           1514
 #endif
 
-#ifndef CFG_TUD_NCM_IN_NTB_MAX_SIZE
-#define CFG_TUD_NCM_IN_NTB_MAX_SIZE 3200
-#endif
-
-#ifndef CFG_TUD_NCM_OUT_NTB_MAX_SIZE
-#define CFG_TUD_NCM_OUT_NTB_MAX_SIZE 3200
-#endif
 
-#ifndef CFG_TUD_NCM_MAX_DATAGRAMS_PER_NTB
-#define CFG_TUD_NCM_MAX_DATAGRAMS_PER_NTB 8
-#endif
+// Table 4.3 Data Class Interface Protocol Codes
+typedef enum
+{
+  NCM_DATA_PROTOCOL_NETWORK_TRANSFER_BLOCK = 0x01
+} ncm_data_interface_protocol_code_t;
 
-#ifndef CFG_TUD_NCM_ALIGNMENT
-#define CFG_TUD_NCM_ALIGNMENT 4
-#endif
 
 #ifdef __cplusplus
  extern "C" {
@@ -94,17 +85,13 @@ void tud_network_init_cb(void);
 
 // client must provide this: 48-bit MAC address
 // TODO removed later since it is not part of tinyusb stack
-extern const uint8_t tud_network_mac_address[6];
-
-//------------- NCM -------------//
-
-// callback to client providing optional indication of internal state of network driver
-void tud_network_link_state_cb(bool state);
+extern uint8_t tud_network_mac_address[6];
 
 //--------------------------------------------------------------------+
 // INTERNAL USBD-CLASS DRIVER API
 //--------------------------------------------------------------------+
 void     netd_init            (void);
+bool     netd_deinit          (void);
 void     netd_reset           (uint8_t rhport);
 uint16_t netd_open            (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 bool     netd_control_xfer_cb (uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
diff --git a/src/class/usbtmc/usbtmc.h b/src/class/usbtmc/usbtmc.h
index e7016ae24..327de087c 100644
--- a/src/class/usbtmc/usbtmc.h
+++ b/src/class/usbtmc/usbtmc.h
@@ -158,7 +158,7 @@ enum {
   USBTMC_BULK_IN_ERR_DATA_TOO_SHORT = 4u,
   USBTMC_BULK_IN_ERR_DATA_TOO_LONG = 5u,
 };
-// bult-in halt errors
+// built-in halt errors
 enum {
   USBTMC_BULK_IN_ERR = 1u, ///< receives a USBTMC command message that expects a response while a
                            /// Bulk-IN transfer is in progress
@@ -184,6 +184,23 @@ typedef enum {
 } usmtmc_request_type_enum;
 
 typedef enum {
+  // The last and first valid bNotify1 for use by the USBTMC class specification.
+  USBTMC_bNOTIFY1_USBTMC_FIRST          = 0x00,
+  USBTMC_bNOTIFY1_USBTMC_LAST           = 0x3F,
+
+  // The last and first valid bNotify1 for use by vendors.
+  USBTMC_bNOTIFY1_VENDOR_SPECIFIC_FIRST = 0x40,
+  USBTMC_bNOTIFY1_VENDOR_SPECIFIC_LAST  = 0x7F,
+
+  // The last and first valid bNotify1 for use by USBTMC subclass specifications.
+  USBTMC_bNOTIFY1_SUBCLASS_FIRST        = 0x80,
+  USBTMC_bNOTIFY1_SUBCLASS_LAST         = 0xFF,
+
+  // From the USB488 Subclass Specification, Section 3.4.
+  USB488_bNOTIFY1_SRQ                   = 0x81,
+} usbtmc_int_in_payload_format;
+
+typedef enum {
   USBTMC_STATUS_SUCCESS = 0x01,
   USBTMC_STATUS_PENDING = 0x02,
   USBTMC_STATUS_FAILED = 0x80,
@@ -262,14 +279,14 @@ typedef struct TU_ATTR_PACKED
 
   struct TU_ATTR_PACKED
   {
-    unsigned int listenOnly :1;
-    unsigned int talkOnly :1;
-    unsigned int supportsIndicatorPulse :1;
+    uint8_t listenOnly :1;
+    uint8_t talkOnly :1;
+    uint8_t supportsIndicatorPulse :1;
   } bmIntfcCapabilities;
 
   struct TU_ATTR_PACKED
   {
-    unsigned int canEndBulkInOnTermChar :1;
+    uint8_t canEndBulkInOnTermChar :1;
   } bmDevCapabilities;
 
   uint8_t _reserved2[6];
@@ -277,17 +294,17 @@ typedef struct TU_ATTR_PACKED
 
   struct TU_ATTR_PACKED
   {
-    unsigned int is488_2 :1;
-    unsigned int supportsREN_GTL_LLO :1;
-    unsigned int supportsTrigger :1;
+    uint8_t supportsTrigger :1;
+    uint8_t supportsREN_GTL_LLO :1;
+    uint8_t is488_2 :1;
   } bmIntfcCapabilities488;
 
   struct TU_ATTR_PACKED
   {
-    unsigned int SCPI :1;
-    unsigned int SR1 :1;
-    unsigned int RL1 :1;
-    unsigned int DT1 :1;
+    uint8_t DT1 :1;
+    uint8_t RL1 :1;
+    uint8_t SR1 :1;
+    uint8_t SCPI :1;
   } bmDevCapabilities488;
   uint8_t _reserved3[8];
 } usbtmc_response_capabilities_488_t;
@@ -305,6 +322,14 @@ TU_VERIFY_STATIC(sizeof(usbtmc_read_stb_rsp_488_t) == 3u, "struct wrong length")
 
 typedef struct TU_ATTR_PACKED
 {
+  uint8_t bNotify1; // Must be USB488_bNOTIFY1_SRQ
+  uint8_t StatusByte;
+} usbtmc_srq_interrupt_488_t;
+
+TU_VERIFY_STATIC(sizeof(usbtmc_srq_interrupt_488_t) == 2u, "struct wrong length");
+
+typedef struct TU_ATTR_PACKED
+{
   struct TU_ATTR_PACKED
   {
       unsigned int bTag : 7;
@@ -316,4 +341,3 @@ typedef struct TU_ATTR_PACKED
 TU_VERIFY_STATIC(sizeof(usbtmc_read_stb_interrupt_488_t) == 2u, "struct wrong length");
 
 #endif
-
diff --git a/src/class/usbtmc/usbtmc_device.c b/src/class/usbtmc/usbtmc_device.c
index af4a92732..abde9679f 100644
--- a/src/class/usbtmc/usbtmc_device.c
+++ b/src/class/usbtmc/usbtmc_device.c
@@ -78,14 +78,19 @@
 
 #ifdef xDEBUG
 #include "uart_util.h"
-static char logMsg[150];
+tu_static char logMsg[150];
 #endif
 
 // Buffer size must be an exact multiple of the max packet size for both
 // bulk  (up to 64 bytes for FS, 512 bytes for HS). In addation, this driver
-// imposes a minimum buffer size of 32 bytes. 
+// imposes a minimum buffer size of 32 bytes.
 #define USBTMCD_BUFFER_SIZE (TUD_OPT_HIGH_SPEED ? 512 : 64)
 
+// Interrupt endpoint buffer size, default to 2 bytes as USB488 specification.
+#ifndef CFG_TUD_USBTMC_INT_EP_SIZE
+#define CFG_TUD_USBTMC_INT_EP_SIZE 2
+#endif
+
 /*
  * The state machine does not allow simultaneous reading and writing. This is
  * consistent with USBTMC.
@@ -124,14 +129,8 @@ typedef struct
   uint8_t ep_bulk_in;
   uint8_t ep_bulk_out;
   uint8_t ep_int_in;
-  // IN buffer is only used for first packet, not the remainder
-  // in order to deal with prepending header
-  CFG_TUSB_MEM_ALIGN uint8_t ep_bulk_in_buf[USBTMCD_BUFFER_SIZE];
   uint32_t ep_bulk_in_wMaxPacketSize;
-  // OUT buffer receives one packet at a time
-  CFG_TUSB_MEM_ALIGN uint8_t ep_bulk_out_buf[USBTMCD_BUFFER_SIZE];
   uint32_t ep_bulk_out_wMaxPacketSize;
-
   uint32_t transfer_size_remaining; // also used for requested length for bulk IN.
   uint32_t transfer_size_sent;      // To keep track of data bytes that have been queued in FIFO (not header bytes)
 
@@ -143,28 +142,45 @@ typedef struct
   usbtmc_capabilities_specific_t const * capabilities;
 } usbtmc_interface_state_t;
 
-CFG_TUSB_MEM_SECTION static usbtmc_interface_state_t usbtmc_state =
-{
-    .itf_id = 0xFF,
+typedef struct {
+  // IN buffer is only used for first packet, not the remainder in order to deal with prepending header
+  TUD_EPBUF_DEF(epin, USBTMCD_BUFFER_SIZE);
+
+  // OUT buffer receives one packet at a time
+  TUD_EPBUF_DEF(epout, USBTMCD_BUFFER_SIZE);
+
+  // Buffer int msg
+  TUD_EPBUF_DEF(epnotif, CFG_TUD_USBTMC_INT_EP_SIZE);
+} usbtmc_epbuf_t;
+
+static usbtmc_interface_state_t usbtmc_state = {
+  .itf_id = 0xFF,
 };
 
+CFG_TUD_MEM_SECTION static usbtmc_epbuf_t usbtmc_epbuf;
+
 // We need all headers to fit in a single packet in this implementation, 32 bytes will fit all standard USBTMC headers
 TU_VERIFY_STATIC(USBTMCD_BUFFER_SIZE >= 32u,"USBTMC dev buffer size too small");
 
 static bool handle_devMsgOutStart(uint8_t rhport, void *data, size_t len);
 static bool handle_devMsgOut(uint8_t rhport, void *data, size_t len, size_t packetLen);
 
-static uint8_t termChar;
-static uint8_t termCharRequested = false;
+#ifndef NDEBUG
+tu_static uint8_t termChar;
+#endif
 
-osal_mutex_def_t usbtmcLockBuffer;
-static osal_mutex_t usbtmcLock;
+tu_static uint8_t termCharRequested = false;
+
+#if OSAL_MUTEX_REQUIRED
+static OSAL_MUTEX_DEF(usbtmcLockBuffer);
+#endif
+osal_mutex_t usbtmcLock;
 
 // Our own private lock, mostly for the state variable.
-#define criticalEnter() do {osal_mutex_lock(usbtmcLock,OSAL_TIMEOUT_WAIT_FOREVER); } while (0)
-#define criticalLeave() do {osal_mutex_unlock(usbtmcLock); } while (0)
+#define criticalEnter() do { (void) osal_mutex_lock(usbtmcLock,OSAL_TIMEOUT_WAIT_FOREVER); } while (0)
+#define criticalLeave() do { (void) osal_mutex_unlock(usbtmcLock); } while (0)
 
-bool atomicChangeState(usbtmcd_state_enum expectedState, usbtmcd_state_enum newState)
+static bool atomicChangeState(usbtmcd_state_enum expectedState, usbtmcd_state_enum newState)
 {
   bool ret = true;
   criticalEnter();
@@ -193,7 +209,7 @@ bool tud_usbtmc_transmit_dev_msg_data(
     bool endOfMessage,
     bool usingTermChar)
 {
-  const unsigned int txBufLen = sizeof(usbtmc_state.ep_bulk_in_buf);
+  const unsigned int txBufLen = USBTMCD_BUFFER_SIZE;
 
 #ifndef NDEBUG
   TU_ASSERT(len > 0u);
@@ -208,7 +224,7 @@ bool tud_usbtmc_transmit_dev_msg_data(
 #endif
 
   TU_VERIFY(usbtmc_state.state == STATE_TX_REQUESTED);
-  usbtmc_msg_dev_dep_msg_in_header_t *hdr = (usbtmc_msg_dev_dep_msg_in_header_t*)usbtmc_state.ep_bulk_in_buf;
+  usbtmc_msg_dev_dep_msg_in_header_t *hdr = (usbtmc_msg_dev_dep_msg_in_header_t*)usbtmc_epbuf.epin;
   tu_varclr(hdr);
   hdr->header.MsgID = USBTMC_MSGID_DEV_DEP_MSG_IN;
   hdr->header.bTag = usbtmc_state.lastBulkInTag;
@@ -223,7 +239,7 @@ bool tud_usbtmc_transmit_dev_msg_data(
                             len : (txBufLen - headerLen);
   const size_t packetLen = headerLen + dataLen;
 
-  memcpy((uint8_t*)(usbtmc_state.ep_bulk_in_buf) + headerLen, data, dataLen);
+  memcpy((uint8_t*)(usbtmc_epbuf.epin) + headerLen, data, dataLen);
   usbtmc_state.transfer_size_remaining = len - dataLen;
   usbtmc_state.transfer_size_sent = dataLen;
   usbtmc_state.devInBuffer = (uint8_t const*) data + (dataLen);
@@ -231,7 +247,20 @@ bool tud_usbtmc_transmit_dev_msg_data(
   bool stateChanged =
       atomicChangeState(STATE_TX_REQUESTED, (packetLen >= txBufLen) ? STATE_TX_INITIATED : STATE_TX_SHORTED);
   TU_VERIFY(stateChanged);
-  TU_VERIFY(usbd_edpt_xfer(usbtmc_state.rhport, usbtmc_state.ep_bulk_in, usbtmc_state.ep_bulk_in_buf, (uint16_t)packetLen));
+  TU_VERIFY(usbd_edpt_xfer(usbtmc_state.rhport, usbtmc_state.ep_bulk_in, usbtmc_epbuf.epin, (uint16_t)packetLen));
+  return true;
+}
+
+bool tud_usbtmc_transmit_notification_data(const void * data, size_t len)
+{
+#ifndef NDEBUG
+  TU_ASSERT(len > 0);
+  TU_ASSERT(usbtmc_state.ep_int_in != 0);
+#endif
+  TU_VERIFY(usbd_edpt_busy(usbtmc_state.rhport, usbtmc_state.ep_int_in));
+
+  TU_VERIFY(tu_memcpy_s(usbtmc_epbuf.epnotif, CFG_TUD_USBTMC_INT_EP_SIZE, data, len) == 0);
+  TU_VERIFY(usbd_edpt_xfer(usbtmc_state.rhport, usbtmc_state.ep_int_in, usbtmc_epbuf.epnotif, (uint16_t)len));
   return true;
 }
 
@@ -255,6 +284,13 @@ void usbtmcd_init_cb(void)
   usbtmcLock = osal_mutex_create(&usbtmcLockBuffer);
 }
 
+bool usbtmcd_deinit(void) {
+  #if OSAL_MUTEX_REQUIRED
+  osal_mutex_delete(usbtmcLock);
+  #endif
+  return true;
+}
+
 uint16_t usbtmcd_open_cb(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len)
 {
   (void)rhport;
@@ -348,7 +384,7 @@ uint16_t usbtmcd_open_cb(uint8_t rhport, tusb_desc_interface_t const * itf_desc,
 // processing a command (such as a clear). Returns true if it was
 // in the NAK state and successfully transitioned to the ACK wait
 // state.
-bool tud_usbtmc_start_bus_read()
+bool tud_usbtmc_start_bus_read(void)
 {
   usbtmcd_state_enum oldState = usbtmc_state.state;
   switch(oldState)
@@ -362,9 +398,9 @@ bool tud_usbtmc_start_bus_read()
   case STATE_RCV:
     break;
   default:
-    TU_VERIFY(false);
+    return false;
   }
-  TU_VERIFY(usbd_edpt_xfer(usbtmc_state.rhport, usbtmc_state.ep_bulk_out, usbtmc_state.ep_bulk_out_buf, 64));
+  TU_VERIFY(usbd_edpt_xfer(usbtmc_state.rhport, usbtmc_state.ep_bulk_out, usbtmc_epbuf.epout, (uint16_t)usbtmc_state.ep_bulk_out_wMaxPacketSize));
   return true;
 }
 
@@ -440,7 +476,10 @@ static bool handle_devMsgIn(void *data, size_t len)
   usbtmc_state.transfer_size_sent = 0u;
 
   termCharRequested = msg->bmTransferAttributes.TermCharEnabled;
+
+#ifndef NDEBUG
   termChar = msg->TermChar;
+#endif
 
   if(termCharRequested)
     TU_VERIFY(usbtmc_state.capabilities->bmDevCapabilities.canEndBulkInOnTermChar);
@@ -464,53 +503,53 @@ bool usbtmcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint
     switch(usbtmc_state.state)
     {
     case STATE_IDLE:
-      TU_VERIFY(xferred_bytes >= sizeof(usbtmc_msg_generic_t));
-      msg = (usbtmc_msg_generic_t*)(usbtmc_state.ep_bulk_out_buf);
-      uint8_t invInvTag = (uint8_t)~(msg->header.bTagInverse);
-      TU_VERIFY(msg->header.bTag == invInvTag);
-      TU_VERIFY(msg->header.bTag != 0x00);
+      {
+        TU_VERIFY(xferred_bytes >= sizeof(usbtmc_msg_generic_t));
+        msg = (usbtmc_msg_generic_t*)(usbtmc_epbuf.epout);
+        uint8_t invInvTag = (uint8_t)~(msg->header.bTagInverse);
+        TU_VERIFY(msg->header.bTag == invInvTag);
+        TU_VERIFY(msg->header.bTag != 0x00);
 
-      switch(msg->header.MsgID) {
-      case USBTMC_MSGID_DEV_DEP_MSG_OUT:
-        if(!handle_devMsgOutStart(rhport, msg, xferred_bytes))
-        {
-          usbd_edpt_stall(rhport, usbtmc_state.ep_bulk_out);
-          TU_VERIFY(false);
-        }
-        break;
+        switch(msg->header.MsgID) {
+        case USBTMC_MSGID_DEV_DEP_MSG_OUT:
+          if(!handle_devMsgOutStart(rhport, msg, xferred_bytes))
+          {
+            usbd_edpt_stall(rhport, usbtmc_state.ep_bulk_out);
+            return false;
+          }
+          break;
 
-      case USBTMC_MSGID_DEV_DEP_MSG_IN:
-        TU_VERIFY(handle_devMsgIn(msg, xferred_bytes));
-        break;
+        case USBTMC_MSGID_DEV_DEP_MSG_IN:
+          TU_VERIFY(handle_devMsgIn(msg, xferred_bytes));
+          break;
 
 #if (CFG_TUD_USBTMC_ENABLE_488)
-      case USBTMC_MSGID_USB488_TRIGGER:
-        // Spec says we halt the EP if we didn't declare we support it.
-        TU_VERIFY(usbtmc_state.capabilities->bmIntfcCapabilities488.supportsTrigger);
-        TU_VERIFY(tud_usbtmc_msg_trigger_cb(msg));
+        case USBTMC_MSGID_USB488_TRIGGER:
+          // Spec says we halt the EP if we didn't declare we support it.
+          TU_VERIFY(usbtmc_state.capabilities->bmIntfcCapabilities488.supportsTrigger);
+          TU_VERIFY(tud_usbtmc_msg_trigger_cb(msg));
 
-        break;
+          break;
 #endif
-      case USBTMC_MSGID_VENDOR_SPECIFIC_MSG_OUT:
-      case USBTMC_MSGID_VENDOR_SPECIFIC_IN:
-      default:
-        usbd_edpt_stall(rhport, usbtmc_state.ep_bulk_out);
-        TU_VERIFY(false);
-        return false;
+        case USBTMC_MSGID_VENDOR_SPECIFIC_MSG_OUT:
+        case USBTMC_MSGID_VENDOR_SPECIFIC_IN:
+        default:
+          usbd_edpt_stall(rhport, usbtmc_state.ep_bulk_out);
+          return false;
+        }
+        return true;
       }
-      return true;
-
     case STATE_RCV:
-      if(!handle_devMsgOut(rhport, usbtmc_state.ep_bulk_out_buf, xferred_bytes, xferred_bytes))
+      if(!handle_devMsgOut(rhport, usbtmc_epbuf.epout, xferred_bytes, xferred_bytes))
       {
         usbd_edpt_stall(rhport, usbtmc_state.ep_bulk_out);
-        TU_VERIFY(false);
+        return false;
       }
       return true;
 
     case STATE_ABORTING_BULK_OUT:
-      TU_VERIFY(false);
-      return false; // Should be stalled by now, shouldn't have received a packet.
+      // Should be stalled by now, shouldn't have received a packet.
+      return false;
 
     case STATE_TX_REQUESTED:
     case STATE_TX_INITIATED:
@@ -518,7 +557,7 @@ bool usbtmcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint
     case STATE_ABORTING_BULK_IN_SHORTED:
     case STATE_ABORTING_BULK_IN_ABORTED:
     default:
-      TU_VERIFY(false);
+      return false;
     }
   }
   else if(ep_addr == usbtmc_state.ep_bulk_in)
@@ -530,23 +569,23 @@ bool usbtmcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint
       break;
 
     case STATE_TX_INITIATED:
-      if(usbtmc_state.transfer_size_remaining >= sizeof(usbtmc_state.ep_bulk_in_buf))
+      if(usbtmc_state.transfer_size_remaining >= USBTMCD_BUFFER_SIZE)
       {
-        // FIXME! This removes const below!
-        TU_VERIFY( usbd_edpt_xfer(rhport, usbtmc_state.ep_bulk_in,
-            (void*)(uintptr_t) usbtmc_state.devInBuffer, sizeof(usbtmc_state.ep_bulk_in_buf)));
-        usbtmc_state.devInBuffer += sizeof(usbtmc_state.ep_bulk_in_buf);
-        usbtmc_state.transfer_size_remaining -= sizeof(usbtmc_state.ep_bulk_in_buf);
-        usbtmc_state.transfer_size_sent += sizeof(usbtmc_state.ep_bulk_in_buf);
+        // Copy buffer to ensure alignment correctness
+        memcpy(usbtmc_epbuf.epin, usbtmc_state.devInBuffer, USBTMCD_BUFFER_SIZE);
+        TU_VERIFY(usbd_edpt_xfer(rhport, usbtmc_state.ep_bulk_in, usbtmc_epbuf.epin, USBTMCD_BUFFER_SIZE));
+        usbtmc_state.devInBuffer += USBTMCD_BUFFER_SIZE;
+        usbtmc_state.transfer_size_remaining -= USBTMCD_BUFFER_SIZE;
+        usbtmc_state.transfer_size_sent += USBTMCD_BUFFER_SIZE;
       }
       else // last packet
       {
         size_t packetLen = usbtmc_state.transfer_size_remaining;
-        memcpy(usbtmc_state.ep_bulk_in_buf, usbtmc_state.devInBuffer, usbtmc_state.transfer_size_remaining);
+        memcpy(usbtmc_epbuf.epin, usbtmc_state.devInBuffer, usbtmc_state.transfer_size_remaining);
           usbtmc_state.transfer_size_sent += sizeof(usbtmc_state.transfer_size_remaining);
         usbtmc_state.transfer_size_remaining = 0;
         usbtmc_state.devInBuffer = NULL;
-        TU_VERIFY( usbd_edpt_xfer(rhport, usbtmc_state.ep_bulk_in, usbtmc_state.ep_bulk_in_buf, (uint16_t)packetLen) );
+        TU_VERIFY( usbd_edpt_xfer(rhport, usbtmc_state.ep_bulk_in, usbtmc_epbuf.epin, (uint16_t)packetLen) );
         if(((packetLen % usbtmc_state.ep_bulk_in_wMaxPacketSize) != 0) || (packetLen == 0 ))
         {
           usbtmc_state.state = STATE_TX_SHORTED;
@@ -556,7 +595,7 @@ bool usbtmcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint
 
     case STATE_ABORTING_BULK_IN:
       // need to send short packet  (ZLP?)
-      TU_VERIFY( usbd_edpt_xfer(rhport, usbtmc_state.ep_bulk_in, usbtmc_state.ep_bulk_in_buf,(uint16_t)0u));
+      TU_VERIFY( usbd_edpt_xfer(rhport, usbtmc_state.ep_bulk_in, usbtmc_epbuf.epin,(uint16_t)0u));
       usbtmc_state.state = STATE_ABORTING_BULK_IN_SHORTED;
       return true;
 
@@ -567,11 +606,12 @@ bool usbtmcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint
 
     default:
       TU_ASSERT(false);
-      return false;
     }
   }
   else if (ep_addr == usbtmc_state.ep_int_in) {
-    // Good?
+    if (tud_usbtmc_notification_complete_cb) {
+      TU_VERIFY(tud_usbtmc_notification_complete_cb());
+    }
     return true;
   }
   return false;
@@ -598,8 +638,8 @@ bool usbtmcd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request
     uint32_t ep_addr = (request->wIndex);
 
     // At this point, a transfer MAY be in progress. Based on USB spec, when clearing bulk EP HALT,
-    // the EP transfer buffer needs to be cleared and DTOG needs to be reset, even if 
-    // the EP is not halted. The only USBD API interface to do this is to stall and then unstall the EP.
+    // the EP transfer buffer needs to be cleared and DTOG needs to be reset, even if
+    // the EP is not halted. The only USBD API interface to do this is to stall and then un-stall the EP.
     if(ep_addr == usbtmc_state.ep_bulk_out)
     {
       criticalEnter();
@@ -707,7 +747,7 @@ bool usbtmcd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request
       if(usbtmc_state.transfer_size_sent  == 0)
       {
         // Send short packet, nothing is in the buffer yet
-        TU_VERIFY( usbd_edpt_xfer(rhport, usbtmc_state.ep_bulk_in, usbtmc_state.ep_bulk_in_buf,(uint16_t)0u));
+        TU_VERIFY( usbd_edpt_xfer(rhport, usbtmc_state.ep_bulk_in, usbtmc_epbuf.epin,(uint16_t)0u));
         usbtmc_state.state = STATE_ABORTING_BULK_IN_SHORTED;
       }
       TU_VERIFY(tud_usbtmc_initiate_abort_bulk_in_cb(&(rsp.USBTMC_status)));
@@ -871,16 +911,13 @@ bool usbtmcd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request
   case USB488_bREQUEST_LOCAL_LOCKOUT:
     {
       TU_VERIFY(request->bmRequestType == 0xA1); // in,class,interface
-      TU_VERIFY(false);
       return false;
     }
 #endif
 
   default:
-    TU_VERIFY(false);
     return false;
   }
-  TU_VERIFY(false);
 }
 
 #endif /* CFG_TUD_TSMC */
diff --git a/src/class/usbtmc/usbtmc_device.h b/src/class/usbtmc/usbtmc_device.h
index 144b3315d..b85ef12b5 100644
--- a/src/class/usbtmc/usbtmc_device.h
+++ b/src/class/usbtmc/usbtmc_device.h
@@ -36,7 +36,7 @@
 #endif
 
 /***********************************************
- *  Functions to be implemeted by the class implementation
+ *  Functions to be implemented by the class implementation
  */
 
 // In order to proceed, app must call call tud_usbtmc_start_bus_read(rhport) during or soon after:
@@ -73,6 +73,10 @@ bool tud_usbtmc_check_abort_bulk_in_cb(usbtmc_check_abort_bulk_rsp_t *rsp);
 bool tud_usbtmc_check_abort_bulk_out_cb(usbtmc_check_abort_bulk_rsp_t *rsp);
 bool tud_usbtmc_check_clear_cb(usbtmc_get_clear_status_rsp_t *rsp);
 
+// The interrupt-IN endpoint buffer was transmitted to the host. Use
+// tud_usbtmc_transmit_notification_data to send another notification.
+TU_ATTR_WEAK bool tud_usbtmc_notification_complete_cb(void);
+
 // Indicator pulse should be 0.5 to 1.0 seconds long
 TU_ATTR_WEAK bool tud_usbtmc_indicator_pulse_cb(tusb_control_request_t const * msg, uint8_t *tmcResult);
 
@@ -82,31 +86,33 @@ TU_ATTR_WEAK bool tud_usbtmc_msg_trigger_cb(usbtmc_msg_generic_t* msg);
 //TU_ATTR_WEAK bool tud_usbtmc_app_go_to_local_cb();
 #endif
 
-/*******************************************
- * Called from app
- *
- * We keep a reference to the buffer, so it MUST not change until the app is
- * notified that the transfer is complete.
- ******************************************/
-
+// Called from app
+//
+// We keep a reference to the buffer, so it MUST not change until the app is
+// notified that the transfer is complete.
 bool tud_usbtmc_transmit_dev_msg_data(
     const void * data, size_t len,
     bool endOfMessage, bool usingTermChar);
 
+// Buffers a notification to be sent to the host. The data starts
+// with the bNotify1 field, see the USBTMC Specification, Table 13.
+//
+// If the previous notification data has not yet been sent, this
+// returns false.
+//
+// Requires an interrupt endpoint in the interface.
+bool tud_usbtmc_transmit_notification_data(const void * data, size_t len);
+
 bool tud_usbtmc_start_bus_read(void);
 
 
 /* "callbacks" from USB device core */
 
+void     usbtmcd_init_cb(void);
+bool     usbtmcd_deinit(void);
 uint16_t usbtmcd_open_cb(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 void     usbtmcd_reset_cb(uint8_t rhport);
 bool     usbtmcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes);
 bool     usbtmcd_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
-void     usbtmcd_init_cb(void);
-
-/************************************************************
- * USBTMC Descriptor Templates
- *************************************************************/
-
 
 #endif /* CLASS_USBTMC_USBTMC_DEVICE_H_ */
diff --git a/src/class/vendor/vendor_device.c b/src/class/vendor/vendor_device.c
index 3b81a108f..6f9b4853f 100644
--- a/src/class/vendor/vendor_device.c
+++ b/src/class/vendor/vendor_device.c
@@ -36,172 +36,173 @@
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
-typedef struct
-{
+typedef struct {
   uint8_t itf_num;
-  uint8_t ep_in;
-  uint8_t ep_out;
 
   /*------------- From this point, data is not cleared by bus reset -------------*/
-  tu_fifo_t rx_ff;
-  tu_fifo_t tx_ff;
+  struct {
+    tu_edpt_stream_t stream;
+    #if CFG_TUD_VENDOR_TX_BUFSIZE > 0
+    uint8_t ff_buf[CFG_TUD_VENDOR_TX_BUFSIZE];
+    #endif
+  } tx;
 
-  uint8_t rx_ff_buf[CFG_TUD_VENDOR_RX_BUFSIZE];
-  uint8_t tx_ff_buf[CFG_TUD_VENDOR_TX_BUFSIZE];
+  struct {
+    tu_edpt_stream_t stream;
+    #if CFG_TUD_VENDOR_RX_BUFSIZE > 0
+    uint8_t ff_buf[CFG_TUD_VENDOR_RX_BUFSIZE];
+    #endif
+  } rx;
 
-#if CFG_FIFO_MUTEX
-  osal_mutex_def_t rx_ff_mutex;
-  osal_mutex_def_t tx_ff_mutex;
-#endif
-
-  // Endpoint Transfer buffer
-  CFG_TUSB_MEM_ALIGN uint8_t epout_buf[CFG_TUD_VENDOR_EPSIZE];
-  CFG_TUSB_MEM_ALIGN uint8_t epin_buf[CFG_TUD_VENDOR_EPSIZE];
 } vendord_interface_t;
 
-CFG_TUSB_MEM_SECTION static vendord_interface_t _vendord_itf[CFG_TUD_VENDOR];
+#define ITF_MEM_RESET_SIZE   (offsetof(vendord_interface_t, itf_num) + sizeof(((vendord_interface_t *)0)->itf_num))
 
-#define ITF_MEM_RESET_SIZE   offsetof(vendord_interface_t, rx_ff)
+static vendord_interface_t _vendord_itf[CFG_TUD_VENDOR];
 
+typedef struct {
+  TUD_EPBUF_DEF(epout, CFG_TUD_VENDOR_EPSIZE);
+  TUD_EPBUF_DEF(epin, CFG_TUD_VENDOR_EPSIZE);
+} vendord_epbuf_t;
 
-bool tud_vendor_n_mounted (uint8_t itf)
-{
-  return _vendord_itf[itf].ep_in && _vendord_itf[itf].ep_out;
-}
+CFG_TUD_MEM_SECTION static vendord_epbuf_t _vendord_epbuf[CFG_TUD_VENDOR];
 
-uint32_t tud_vendor_n_available (uint8_t itf)
-{
-  return tu_fifo_count(&_vendord_itf[itf].rx_ff);
-}
+//--------------------------------------------------------------------
+// Application API
+//--------------------------------------------------------------------
 
-bool tud_vendor_n_peek(uint8_t itf, uint8_t* u8)
-{
-  return tu_fifo_peek(&_vendord_itf[itf].rx_ff, u8);
+bool tud_vendor_n_mounted(uint8_t itf) {
+  TU_VERIFY(itf < CFG_TUD_VENDOR);
+  vendord_interface_t* p_itf = &_vendord_itf[itf];
+  return p_itf->rx.stream.ep_addr || p_itf->tx.stream.ep_addr;
 }
 
 //--------------------------------------------------------------------+
 // Read API
 //--------------------------------------------------------------------+
-static void _prep_out_transaction (vendord_interface_t* p_itf)
-{
-  uint8_t const rhport = 0;
+uint32_t tud_vendor_n_available(uint8_t itf) {
+  TU_VERIFY(itf < CFG_TUD_VENDOR, 0);
+  vendord_interface_t* p_itf = &_vendord_itf[itf];
 
-  // skip if previous transfer not complete
-  if ( usbd_edpt_busy(rhport, p_itf->ep_out) ) return;
+  return tu_edpt_stream_read_available(&p_itf->rx.stream);
+}
 
-  // Prepare for incoming data but only allow what we can store in the ring buffer.
-  uint16_t max_read = tu_fifo_remaining(&p_itf->rx_ff);
-  if ( max_read >= CFG_TUD_VENDOR_EPSIZE )
-  {
-    usbd_edpt_xfer(rhport, p_itf->ep_out, p_itf->epout_buf, CFG_TUD_VENDOR_EPSIZE);
-  }
+bool tud_vendor_n_peek(uint8_t itf, uint8_t* u8) {
+  TU_VERIFY(itf < CFG_TUD_VENDOR);
+  vendord_interface_t* p_itf = &_vendord_itf[itf];
+
+  return tu_edpt_stream_peek(&p_itf->rx.stream, u8);
 }
 
-uint32_t tud_vendor_n_read (uint8_t itf, void* buffer, uint32_t bufsize)
-{
+uint32_t tud_vendor_n_read (uint8_t itf, void* buffer, uint32_t bufsize) {
+  TU_VERIFY(itf < CFG_TUD_VENDOR, 0);
   vendord_interface_t* p_itf = &_vendord_itf[itf];
-  uint32_t num_read = tu_fifo_read_n(&p_itf->rx_ff, buffer, (uint16_t) bufsize);
-  _prep_out_transaction(p_itf);
-  return num_read;
+  const uint8_t rhport = 0;
+
+  return tu_edpt_stream_read(rhport, &p_itf->rx.stream, buffer, bufsize);
 }
 
-void tud_vendor_n_read_flush (uint8_t itf)
-{
+void tud_vendor_n_read_flush (uint8_t itf) {
+  TU_VERIFY(itf < CFG_TUD_VENDOR, );
   vendord_interface_t* p_itf = &_vendord_itf[itf];
-  tu_fifo_clear(&p_itf->rx_ff);
-  _prep_out_transaction(p_itf);
+  const uint8_t rhport = 0;
+
+  tu_edpt_stream_clear(&p_itf->rx.stream);
+  tu_edpt_stream_read_xfer(rhport, &p_itf->rx.stream);
 }
 
 //--------------------------------------------------------------------+
 // Write API
 //--------------------------------------------------------------------+
-static uint16_t maybe_transmit(vendord_interface_t* p_itf)
-{
-  uint8_t const rhport = 0;
-
-  // skip if previous transfer not complete
-  TU_VERIFY( !usbd_edpt_busy(rhport, p_itf->ep_in) );
+uint32_t tud_vendor_n_write (uint8_t itf, const void* buffer, uint32_t bufsize) {
+  TU_VERIFY(itf < CFG_TUD_VENDOR, 0);
+  vendord_interface_t* p_itf = &_vendord_itf[itf];
+  const uint8_t rhport = 0;
 
-  uint16_t count = tu_fifo_read_n(&p_itf->tx_ff, p_itf->epin_buf, CFG_TUD_VENDOR_EPSIZE);
-  if (count > 0)
-  {
-    TU_ASSERT( usbd_edpt_xfer(rhport, p_itf->ep_in, p_itf->epin_buf, count) );
-  }
-  return count;
+  return tu_edpt_stream_write(rhport, &p_itf->tx.stream, buffer, (uint16_t) bufsize);
 }
 
-uint32_t tud_vendor_n_write (uint8_t itf, void const* buffer, uint32_t bufsize)
-{
+uint32_t tud_vendor_n_write_flush (uint8_t itf) {
+  TU_VERIFY(itf < CFG_TUD_VENDOR, 0);
   vendord_interface_t* p_itf = &_vendord_itf[itf];
-  uint16_t ret = tu_fifo_write_n(&p_itf->tx_ff, buffer, (uint16_t) bufsize);
-  if (tu_fifo_count(&p_itf->tx_ff) >= CFG_TUD_VENDOR_EPSIZE) {
-    maybe_transmit(p_itf);
-  }
-  return ret;
+  const uint8_t rhport = 0;
+
+  return tu_edpt_stream_write_xfer(rhport, &p_itf->tx.stream);
 }
 
-uint32_t tud_vendor_n_flush (uint8_t itf)
-{
+uint32_t tud_vendor_n_write_available (uint8_t itf) {
+  TU_VERIFY(itf < CFG_TUD_VENDOR, 0);
   vendord_interface_t* p_itf = &_vendord_itf[itf];
-  uint32_t ret = maybe_transmit(p_itf);
-
-  return ret;
-}
+  const uint8_t rhport = 0;
 
-uint32_t tud_vendor_n_write_available (uint8_t itf)
-{
-  return tu_fifo_remaining(&_vendord_itf[itf].tx_ff);
+  return tu_edpt_stream_write_available(rhport, &p_itf->tx.stream);
 }
 
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void vendord_init(void)
-{
+void vendord_init(void) {
   tu_memclr(_vendord_itf, sizeof(_vendord_itf));
 
-  for(uint8_t i=0; i<CFG_TUD_VENDOR; i++)
-  {
+  for(uint8_t i=0; i<CFG_TUD_VENDOR; i++) {
     vendord_interface_t* p_itf = &_vendord_itf[i];
+    vendord_epbuf_t* p_epbuf = &_vendord_epbuf[i];
 
-    // config fifo
-    tu_fifo_config(&p_itf->rx_ff, p_itf->rx_ff_buf, CFG_TUD_VENDOR_RX_BUFSIZE, 1, false);
-    tu_fifo_config(&p_itf->tx_ff, p_itf->tx_ff_buf, CFG_TUD_VENDOR_TX_BUFSIZE, 1, false);
+    uint8_t* rx_ff_buf =
+                        #if CFG_TUD_VENDOR_RX_BUFSIZE > 0
+                          p_itf->rx.ff_buf;
+                        #else
+                          NULL;
+                        #endif
 
-#if CFG_FIFO_MUTEX
-    tu_fifo_config_mutex(&p_itf->rx_ff, NULL, osal_mutex_create(&p_itf->rx_ff_mutex));
-    tu_fifo_config_mutex(&p_itf->tx_ff, osal_mutex_create(&p_itf->tx_ff_mutex), NULL);
-#endif
+    tu_edpt_stream_init(&p_itf->rx.stream, false, false, false,
+                        rx_ff_buf, CFG_TUD_VENDOR_RX_BUFSIZE,
+                        p_epbuf->epout, CFG_TUD_VENDOR_EPSIZE);
+
+    uint8_t* tx_ff_buf =
+                        #if CFG_TUD_VENDOR_TX_BUFSIZE > 0
+                          p_itf->tx.ff_buf;
+                        #else
+                          NULL;
+                        #endif
+
+    tu_edpt_stream_init(&p_itf->tx.stream, false, true, false,
+                        tx_ff_buf, CFG_TUD_VENDOR_TX_BUFSIZE,
+                        p_epbuf->epin, CFG_TUD_VENDOR_EPSIZE);
+  }
+}
+
+bool vendord_deinit(void) {
+  for(uint8_t i=0; i<CFG_TUD_VENDOR; i++) {
+    vendord_interface_t* p_itf = &_vendord_itf[i];
+    tu_edpt_stream_deinit(&p_itf->rx.stream);
+    tu_edpt_stream_deinit(&p_itf->tx.stream);
   }
+  return true;
 }
 
-void vendord_reset(uint8_t rhport)
-{
+void vendord_reset(uint8_t rhport) {
   (void) rhport;
 
-  for(uint8_t i=0; i<CFG_TUD_VENDOR; i++)
-  {
+  for(uint8_t i=0; i<CFG_TUD_VENDOR; i++) {
     vendord_interface_t* p_itf = &_vendord_itf[i];
-
     tu_memclr(p_itf, ITF_MEM_RESET_SIZE);
-    tu_fifo_clear(&p_itf->rx_ff);
-    tu_fifo_clear(&p_itf->tx_ff);
+    tu_edpt_stream_clear(&p_itf->rx.stream);
+    tu_edpt_stream_clear(&p_itf->tx.stream);
+    tu_edpt_stream_close(&p_itf->rx.stream);
+    tu_edpt_stream_close(&p_itf->tx.stream);
   }
 }
 
-uint16_t vendord_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, uint16_t max_len)
-{
+uint16_t vendord_open(uint8_t rhport, const tusb_desc_interface_t* desc_itf, uint16_t max_len) {
   TU_VERIFY(TUSB_CLASS_VENDOR_SPECIFIC == desc_itf->bInterfaceClass, 0);
-
-  uint8_t const * p_desc = tu_desc_next(desc_itf);
-  uint8_t const * desc_end = p_desc + max_len;
+  const uint8_t* p_desc = tu_desc_next(desc_itf);
+  const uint8_t* desc_end = p_desc + max_len;
 
   // Find available interface
   vendord_interface_t* p_vendor = NULL;
-  for(uint8_t i=0; i<CFG_TUD_VENDOR; i++)
-  {
-    if ( _vendord_itf[i].ep_in == 0 && _vendord_itf[i].ep_out == 0 )
-    {
+  for(uint8_t i=0; i<CFG_TUD_VENDOR; i++) {
+    if (!tud_vendor_n_mounted(i)) {
       p_vendor = &_vendord_itf[i];
       break;
     }
@@ -209,61 +210,72 @@ uint16_t vendord_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, ui
   TU_VERIFY(p_vendor, 0);
 
   p_vendor->itf_num = desc_itf->bInterfaceNumber;
-  if (desc_itf->bNumEndpoints)
-  {
+  uint8_t found_ep = 0;
+  while (found_ep < desc_itf->bNumEndpoints) {
     // skip non-endpoint descriptors
-    while ( (TUSB_DESC_ENDPOINT != tu_desc_type(p_desc)) && (p_desc < desc_end) )
-    {
+    while ( (TUSB_DESC_ENDPOINT != tu_desc_type(p_desc)) && (p_desc < desc_end) ) {
       p_desc = tu_desc_next(p_desc);
     }
+    if (p_desc >= desc_end) {
+      break;
+    }
 
-    // Open endpoint pair with usbd helper
-    TU_ASSERT(usbd_open_edpt_pair(rhport, p_desc, desc_itf->bNumEndpoints, TUSB_XFER_BULK, &p_vendor->ep_out, &p_vendor->ep_in), 0);
-
-    p_desc += desc_itf->bNumEndpoints*sizeof(tusb_desc_endpoint_t);
+    const tusb_desc_endpoint_t* desc_ep = (const tusb_desc_endpoint_t*) p_desc;
+    TU_ASSERT(usbd_edpt_open(rhport, desc_ep));
+    found_ep++;
 
-    // Prepare for incoming data
-    if ( p_vendor->ep_out )
-    {
-      TU_ASSERT(usbd_edpt_xfer(rhport, p_vendor->ep_out, p_vendor->epout_buf, sizeof(p_vendor->epout_buf)), 0);
+    if (tu_edpt_dir(desc_ep->bEndpointAddress) == TUSB_DIR_IN) {
+      tu_edpt_stream_open(&p_vendor->tx.stream, desc_ep);
+      tud_vendor_n_write_flush((uint8_t)(p_vendor - _vendord_itf));
+    } else {
+      tu_edpt_stream_open(&p_vendor->rx.stream, desc_ep);
+      TU_ASSERT(tu_edpt_stream_read_xfer(rhport, &p_vendor->rx.stream) > 0, 0); // prepare for incoming data
     }
 
-    if ( p_vendor->ep_in ) maybe_transmit(p_vendor);
+    p_desc = tu_desc_next(p_desc);
   }
 
   return (uint16_t) ((uintptr_t) p_desc - (uintptr_t) desc_itf);
 }
 
-bool vendord_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
-{
-  (void) rhport;
+bool vendord_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) {
   (void) result;
 
-  uint8_t itf = 0;
-  vendord_interface_t* p_itf = _vendord_itf;
-
-  for ( ; ; itf++, p_itf++)
-  {
-    if (itf >= TU_ARRAY_SIZE(_vendord_itf)) return false;
+  uint8_t itf;
+  vendord_interface_t* p_vendor;
 
-    if ( ( ep_addr == p_itf->ep_out ) || ( ep_addr == p_itf->ep_in ) ) break;
+  for (itf = 0; itf < CFG_TUD_VENDOR; itf++) {
+    p_vendor = &_vendord_itf[itf];
+    if ((ep_addr == p_vendor->rx.stream.ep_addr) || (ep_addr == p_vendor->tx.stream.ep_addr)) {
+      break;
+    }
   }
+  TU_VERIFY(itf < CFG_TUD_VENDOR);
+  vendord_epbuf_t* p_epbuf = &_vendord_epbuf[itf];
 
-  if ( ep_addr == p_itf->ep_out )
-  {
-    // Receive new data
-    tu_fifo_write_n(&p_itf->rx_ff, p_itf->epout_buf, (uint16_t) xferred_bytes);
+  if ( ep_addr == p_vendor->rx.stream.ep_addr ) {
+    // Received new data: put into stream's fifo
+    tu_edpt_stream_read_xfer_complete(&p_vendor->rx.stream, xferred_bytes);
 
     // Invoked callback if any
-    if (tud_vendor_rx_cb) tud_vendor_rx_cb(itf);
+    if (tud_vendor_rx_cb) {
+      tud_vendor_rx_cb(itf, p_epbuf->epout, (uint16_t) xferred_bytes);
+    }
 
-    _prep_out_transaction(p_itf);
-  }
-  else if ( ep_addr == p_itf->ep_in )
-  {
-    if (tud_vendor_tx_cb) tud_vendor_tx_cb(itf, (uint16_t) xferred_bytes);
-    // Send complete, try to send more if possible
-    maybe_transmit(p_itf);
+    tu_edpt_stream_read_xfer(rhport, &p_vendor->rx.stream);
+  } else if ( ep_addr == p_vendor->tx.stream.ep_addr ) {
+    // Send complete
+    if (tud_vendor_tx_cb) {
+      tud_vendor_tx_cb(itf, (uint16_t) xferred_bytes);
+    }
+
+    #if CFG_TUD_VENDOR_TX_BUFSIZE > 0
+    // try to send more if possible
+    if ( 0 == tu_edpt_stream_write_xfer(rhport, &p_vendor->tx.stream) ) {
+      // If there is no data left, a ZLP should be sent if xferred_bytes is multiple of EP Packet size and not zero
+      tu_edpt_stream_write_zlp_if_needed(rhport, &p_vendor->tx.stream, xferred_bytes);
+    }
+    #endif
   }
 
   return true;
diff --git a/src/class/vendor/vendor_device.h b/src/class/vendor/vendor_device.h
index 4a873e5fc..149ae2d56 100644
--- a/src/class/vendor/vendor_device.h
+++ b/src/class/vendor/vendor_device.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
@@ -33,107 +33,106 @@
 #define CFG_TUD_VENDOR_EPSIZE     64
 #endif
 
+// RX FIFO can be disabled by setting this value to 0
+#ifndef CFG_TUD_VENDOR_RX_BUFSIZE
+#define CFG_TUD_VENDOR_RX_BUFSIZE    64
+#endif
+
+// TX FIFO can be disabled by setting this value to 0
+#ifndef CFG_TUD_VENDOR_TX_BUFSIZE
+#define CFG_TUD_VENDOR_TX_BUFSIZE    64
+#endif
+
 #ifdef __cplusplus
  extern "C" {
 #endif
 
 //--------------------------------------------------------------------+
-// Application API (Multiple Interfaces)
+// Application API (Multiple Interfaces) i.e CFG_TUD_VENDOR > 1
 //--------------------------------------------------------------------+
 bool     tud_vendor_n_mounted         (uint8_t itf);
-
 uint32_t tud_vendor_n_available       (uint8_t itf);
 uint32_t tud_vendor_n_read            (uint8_t itf, void* buffer, uint32_t bufsize);
 bool     tud_vendor_n_peek            (uint8_t itf, uint8_t* ui8);
 void     tud_vendor_n_read_flush      (uint8_t itf);
 
 uint32_t tud_vendor_n_write           (uint8_t itf, void const* buffer, uint32_t bufsize);
+uint32_t tud_vendor_n_write_flush     (uint8_t itf);
 uint32_t tud_vendor_n_write_available (uint8_t itf);
 
-static inline
-uint32_t tud_vendor_n_write_str       (uint8_t itf, char const* str);
-uint32_t tud_vendor_n_flush           (uint8_t itf);
-
-//--------------------------------------------------------------------+
-// Application API (Single Port)
-//--------------------------------------------------------------------+
-static inline bool     tud_vendor_mounted         (void);
-static inline uint32_t tud_vendor_available       (void);
-static inline uint32_t tud_vendor_read            (void* buffer, uint32_t bufsize);
-static inline bool     tud_vendor_peek            (uint8_t* ui8);
-static inline void     tud_vendor_read_flush      (void);
-static inline uint32_t tud_vendor_write           (void const* buffer, uint32_t bufsize);
-static inline uint32_t tud_vendor_write_str       (char const* str);
-static inline uint32_t tud_vendor_write_available (void);
-static inline uint32_t tud_vendor_flush           (void);
-
-//--------------------------------------------------------------------+
-// Application Callback API (weak is optional)
-//--------------------------------------------------------------------+
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_vendor_n_write_str (uint8_t itf, char const* str);
 
-// Invoked when received new data
-TU_ATTR_WEAK void tud_vendor_rx_cb(uint8_t itf);
-// Invoked when last rx transfer finished
-TU_ATTR_WEAK void tud_vendor_tx_cb(uint8_t itf, uint32_t sent_bytes);
+// backward compatible
+#define tud_vendor_n_flush(itf) tud_vendor_n_write_flush(itf)
 
 //--------------------------------------------------------------------+
-// Inline Functions
+// Application API (Single Port) i.e CFG_TUD_VENDOR = 1
 //--------------------------------------------------------------------+
 
-static inline uint32_t tud_vendor_n_write_str (uint8_t itf, char const* str)
-{
-  return tud_vendor_n_write(itf, str, strlen(str));
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_vendor_n_write_str(uint8_t itf, char const* str) {
+ return tud_vendor_n_write(itf, str, strlen(str));
 }
 
-static inline bool tud_vendor_mounted (void)
-{
-  return tud_vendor_n_mounted(0);
+TU_ATTR_ALWAYS_INLINE static inline bool tud_vendor_mounted(void) {
+ return tud_vendor_n_mounted(0);
 }
 
-static inline uint32_t tud_vendor_available (void)
-{
-  return tud_vendor_n_available(0);
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_vendor_available(void) {
+ return tud_vendor_n_available(0);
 }
 
-static inline uint32_t tud_vendor_read (void* buffer, uint32_t bufsize)
-{
-  return tud_vendor_n_read(0, buffer, bufsize);
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_vendor_read(void* buffer, uint32_t bufsize) {
+ return tud_vendor_n_read(0, buffer, bufsize);
 }
 
-static inline bool tud_vendor_peek (uint8_t* ui8)
-{
-  return tud_vendor_n_peek(0, ui8);
+TU_ATTR_ALWAYS_INLINE static inline bool tud_vendor_peek(uint8_t* ui8) {
+ return tud_vendor_n_peek(0, ui8);
 }
 
-static inline void tud_vendor_read_flush(void)
-{
-    tud_vendor_n_read_flush(0);
+TU_ATTR_ALWAYS_INLINE static inline void tud_vendor_read_flush(void) {
+ tud_vendor_n_read_flush(0);
 }
 
-static inline uint32_t tud_vendor_write (void const* buffer, uint32_t bufsize)
-{
-  return tud_vendor_n_write(0, buffer, bufsize);
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_vendor_write(void const* buffer, uint32_t bufsize) {
+ return tud_vendor_n_write(0, buffer, bufsize);
 }
 
-static inline uint32_t tud_vendor_write_str (char const* str)
-{
-  return tud_vendor_n_write_str(0, str);
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_vendor_write_str(char const* str) {
+ return tud_vendor_n_write_str(0, str);
 }
 
-static inline uint32_t tud_vendor_write_available (void)
-{
-  return tud_vendor_n_write_available(0);
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_vendor_write_flush(void) {
+ return tud_vendor_n_write_flush(0);
 }
 
-static inline uint32_t tud_vendor_flush (void)
-{
-  return tud_vendor_n_flush(0);
+#if CFG_TUD_VENDOR_TX_BUFSIZE > 0
+TU_ATTR_ALWAYS_INLINE static inline uint32_t tud_vendor_write_available(void) {
+ return tud_vendor_n_write_available(0);
 }
+#endif
+
+// backward compatible
+#define tud_vendor_flush() tud_vendor_write_flush()
+
+//--------------------------------------------------------------------+
+// Application Callback API (weak is optional)
+//--------------------------------------------------------------------+
+
+// Invoked when received new data
+TU_ATTR_WEAK void tud_vendor_rx_cb(uint8_t itf, uint8_t const* buffer, uint16_t bufsize);
+// Invoked when last rx transfer finished
+TU_ATTR_WEAK void tud_vendor_tx_cb(uint8_t itf, uint32_t sent_bytes);
+
+//--------------------------------------------------------------------+
+// Inline Functions
+//--------------------------------------------------------------------+
+
 
 //--------------------------------------------------------------------+
 // Internal Class Driver API
 //--------------------------------------------------------------------+
 void     vendord_init(void);
+bool     vendord_deinit(void);
 void     vendord_reset(uint8_t rhport);
 uint16_t vendord_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 bool     vendord_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t event, uint32_t xferred_bytes);
diff --git a/src/class/vendor/vendor_host.c b/src/class/vendor/vendor_host.c
index dbea1228d..e66c5007f 100644
--- a/src/class/vendor/vendor_host.c
+++ b/src/class/vendor/vendor_host.c
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
diff --git a/src/class/vendor/vendor_host.h b/src/class/vendor/vendor_host.h
index 65223fbca..acfebe7a4 100644
--- a/src/class/vendor/vendor_host.h
+++ b/src/class/vendor/vendor_host.h
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2019 Ha Thach (tinyusb.org)
diff --git a/src/class/video/video.h b/src/class/video/video.h
index e8227ea60..f348e187b 100644
--- a/src/class/video/video.h
+++ b/src/class/video/video.h
@@ -29,6 +29,10 @@
 
 #include "common/tusb_common.h"
 
+enum {
+  VIDEO_BCD_1_50 = 0x0150,
+};
+
 // Table 3-19 Color Matching Descriptor
 typedef enum {
   VIDEO_COLOR_PRIMARIES_UNDEFINED = 0x00,
@@ -156,22 +160,23 @@ typedef enum {
 /* A.9.1 VideoControl Interface Control Selectors */
 typedef enum {
   VIDEO_VC_CTL_UNDEFINED = 0x00,
-  VIDEO_VC_CTL_VIDEO_POWER_MODE,
-  VIDEO_VC_CTL_REQUEST_ERROR_CODE,
+  VIDEO_VC_CTL_VIDEO_POWER_MODE,   // 0x01
+  VIDEO_VC_CTL_REQUEST_ERROR_CODE, // 0x02
 } video_interface_control_selector_t;
 
 /* A.9.8 VideoStreaming Interface Control Selectors */
 typedef enum {
   VIDEO_VS_CTL_UNDEFINED = 0x00,
-  VIDEO_VS_CTL_PROBE,
-  VIDEO_VS_CTL_COMMIT,
-  VIDEO_VS_CTL_STILL_PROBE,
-  VIDEO_VS_CTL_STILL_COMMIT,
-  VIDEO_VS_CTL_STILL_IMAGE_TRIGGER,
-  VIDEO_VS_CTL_STREAM_ERROR_CODE,
-  VIDEO_VS_CTL_GENERATE_KEY_FRAME,
-  VIDEO_VS_CTL_UPDATE_FRAME_SEGMENT,
-  VIDEO_VS_CTL_SYNCH_DELAY_CONTROL,
+  VIDEO_VS_CTL_PROBE,                // 0x01
+  VIDEO_VS_CTL_COMMIT,               // 0x02
+  VIDEO_VS_CTL_STILL_PROBE,          // 0x03
+  VIDEO_VS_CTL_STILL_COMMIT,         // 0x04
+  VIDEO_VS_CTL_STILL_IMAGE_TRIGGER,  // 0x05
+  VIDEO_VS_CTL_STREAM_ERROR_CODE,    // 0x06
+  VIDEO_VS_CTL_GENERATE_KEY_FRAME,   // 0x07
+  VIDEO_VS_CTL_UPDATE_FRAME_SEGMENT, // 0x08
+  VIDEO_VS_CTL_SYNCH_DELAY_CONTROL,  // 0x09
+
 } video_interface_streaming_selector_t;
 
 /* B. Terminal Types */
@@ -198,55 +203,98 @@ typedef enum {
 } video_terminal_type_t;
 
 //--------------------------------------------------------------------+
-// Descriptors
+// Video Control (VC) Descriptors
 //--------------------------------------------------------------------+
 
 /* 2.3.4.2 */
+#define tusb_desc_video_control_header_nitf_t(_nitf) \
+  struct TU_ATTR_PACKED { \
+    uint8_t  bLength; \
+    uint8_t  bDescriptorType; \
+    uint8_t  bDescriptorSubType; \
+    uint16_t bcdUVC; \
+    uint16_t wTotalLength; \
+    uint32_t dwClockFrequency; /* deprecated */ \
+    uint8_t  bInCollection; \
+    uint8_t  baInterfaceNr[_nitf]; \
+  }
+
+typedef tusb_desc_video_control_header_nitf_t()  tusb_desc_video_control_header_t;
+typedef tusb_desc_video_control_header_nitf_t(1) tusb_desc_video_control_header_1itf_t;
+typedef tusb_desc_video_control_header_nitf_t(2) tusb_desc_video_control_header_2itf_t;
+typedef tusb_desc_video_control_header_nitf_t(3) tusb_desc_video_control_header_3itf_t;
+typedef tusb_desc_video_control_header_nitf_t(4) tusb_desc_video_control_header_4itf_t;
+
 typedef struct TU_ATTR_PACKED {
   uint8_t  bLength;
   uint8_t  bDescriptorType;
   uint8_t  bDescriptorSubType;
-  uint16_t bcdUVC;
-  uint16_t wTotalLength;
-  uint32_t dwClockFrequency;
-  uint8_t  bInCollection;
-  uint8_t  baInterfaceNr[];
-} tusb_desc_cs_video_ctl_itf_hdr_t;
+  uint8_t  bTerminalID;
+  uint16_t wTerminalType;
+  uint8_t  bAssocTerminal;
+  uint8_t  iTerminal;
+} tusb_desc_video_control_input_terminal_t;
+
+TU_VERIFY_STATIC(sizeof(tusb_desc_video_control_input_terminal_t) == 8, "size is not correct");
 
-/* 2.4.3.3 */
 typedef struct TU_ATTR_PACKED {
-  uint8_t bHeaderLength;
-  union {
-    uint8_t bmHeaderInfo;
-    struct {
-      uint8_t FrameID:              1;
-      uint8_t EndOfFrame:           1;
-      uint8_t PresentationTime:     1;
-      uint8_t SourceClockReference: 1;
-      uint8_t PayloadSpecific:      1;
-      uint8_t StillImage:           1;
-      uint8_t Error:                1;
-      uint8_t EndOfHeader:          1;
-    };
-  };
-} tusb_video_payload_header_t;
+  uint8_t  bLength;
+  uint8_t  bDescriptorType;
+  uint8_t  bDescriptorSubType;
+  uint8_t  bTerminalID;
+  uint16_t wTerminalType;
+  uint8_t  bAssocTerminal;
+  uint8_t  bSourceID;
+  uint8_t  iTerminal;
+} tusb_desc_video_control_output_terminal_t;
+
+TU_VERIFY_STATIC(sizeof(tusb_desc_video_control_output_terminal_t) == 9, "size is not correct");
 
-/* 3.9.2.1 */
 typedef struct TU_ATTR_PACKED {
   uint8_t  bLength;
   uint8_t  bDescriptorType;
   uint8_t  bDescriptorSubType;
-  uint8_t  bNumFormats;
-  uint16_t wTotalLength;
-  uint8_t  bEndpointAddress;
-  uint8_t  bmInfo;
-  uint8_t  bTerminalLink;
-  uint8_t  bStillCaptureMethod;
-  uint8_t  bTriggerSupport;
-  uint8_t  bTriggerUsage;
+  uint8_t  bTerminalID;
+  uint16_t wTerminalType;
+  uint8_t  bAssocTerminal;
+  uint8_t  iTerminal;
+
+  uint16_t wObjectiveFocalLengthMin;
+  uint16_t wObjectiveFocalLengthMax;
+  uint16_t wOcularFocalLength;
   uint8_t  bControlSize;
-  uint8_t  bmaControls[];
-} tusb_desc_cs_video_stm_itf_in_hdr_t;
+  uint8_t  bmControls[3];
+} tusb_desc_video_control_camera_terminal_t;
+
+TU_VERIFY_STATIC(sizeof(tusb_desc_video_control_camera_terminal_t) == 18, "size is not correct");
+
+//--------------------------------------------------------------------+
+// Video Streaming (VS) Descriptors
+//--------------------------------------------------------------------+
+
+/* 3.9.2.1 */
+#define tusb_desc_video_streaming_input_header_nbyte_t(_nb) \
+  struct TU_ATTR_PACKED { \
+    uint8_t  bLength; \
+    uint8_t  bDescriptorType; \
+    uint8_t  bDescriptorSubType; \
+    uint8_t  bNumFormats; /* Number of video payload Format descriptors for this interface */ \
+    uint16_t wTotalLength; \
+    uint8_t  bEndpointAddress; \
+    uint8_t  bmInfo; /* Bit 0: dynamic format change supported */ \
+    uint8_t  bTerminalLink; \
+    uint8_t  bStillCaptureMethod; \
+    uint8_t  bTriggerSupport; /* Hardware trigger supported */ \
+    uint8_t  bTriggerUsage; \
+    uint8_t  bControlSize; /* sizeof of each control item */ \
+    uint8_t  bmaControls[_nb]; \
+  }
+
+typedef tusb_desc_video_streaming_input_header_nbyte_t() tusb_desc_video_streaming_input_header_t;
+typedef tusb_desc_video_streaming_input_header_nbyte_t(1) tusb_desc_video_streaming_input_header_1byte_t;
+typedef tusb_desc_video_streaming_input_header_nbyte_t(2) tusb_desc_video_streaming_input_header_2byte_t;
+typedef tusb_desc_video_streaming_input_header_nbyte_t(3) tusb_desc_video_streaming_input_header_3byte_t;
+typedef tusb_desc_video_streaming_input_header_nbyte_t(4) tusb_desc_video_streaming_input_header_4byte_t;
 
 /* 3.9.2.2 */
 typedef struct TU_ATTR_PACKED {
@@ -259,7 +307,7 @@ typedef struct TU_ATTR_PACKED {
   uint8_t  bTerminalLink;
   uint8_t  bControlSize;
   uint8_t  bmaControls[];
-} tusb_desc_cs_video_stm_itf_out_hdr_t;
+} tusb_desc_video_streaming_output_header_t;
 
 typedef struct TU_ATTR_PACKED {
   uint8_t  bLength;
@@ -285,14 +333,33 @@ typedef struct TU_ATTR_PACKED {
       uint8_t  bmaControls[];
     } output;
   };
-} tusb_desc_cs_video_stm_itf_hdr_t;
+} tusb_desc_video_streaming_inout_header_t;
+
+// 3.9.2.6 Color Matching Descriptor
+typedef struct TU_ATTR_PACKED {
+  uint8_t  bLength;
+  uint8_t  bDescriptorType;
+  uint8_t  bDescriptorSubType;
+  uint8_t  bColorPrimaries;
+  uint8_t  bTransferCharacteristics;
+  uint8_t  bMatrixCoefficients;
+} tusb_desc_video_streaming_color_matching_t;
+
+TU_VERIFY_STATIC(sizeof(tusb_desc_video_streaming_color_matching_t) == 6, "size is not correct");
+
+//--------------------------------------------------------------------+
+// Format and Frame Descriptor
+// Note: bFormatIndex & bFrameIndex are 1-based index
+//--------------------------------------------------------------------+
 
+//------------- Uncompressed -------------//
+// Uncompressed payload specs: 3.1.1 format descriptor
 typedef struct TU_ATTR_PACKED {
   uint8_t bLength;
   uint8_t bDescriptorType;
   uint8_t bDescriptorSubType;
   uint8_t bFormatIndex;
-  uint8_t bNumFrameDescriptors;
+  uint8_t bNumFrameDescriptors; // Number of frame descriptors for this format
   uint8_t guidFormat[16];
   uint8_t bBitsPerPixel;
   uint8_t bDefaultFrameIndex;
@@ -300,22 +367,69 @@ typedef struct TU_ATTR_PACKED {
   uint8_t bAspectRatioY;
   uint8_t bmInterlaceFlags;
   uint8_t bCopyProtect;
-} tusb_desc_cs_video_fmt_uncompressed_t;
+} tusb_desc_video_format_uncompressed_t;
+
+TU_VERIFY_STATIC(sizeof(tusb_desc_video_format_uncompressed_t) == 27, "size is not correct");
+
+// Uncompressed payload specs: 3.1.2 frame descriptor
+#define tusb_desc_video_frame_uncompressed_nint_t(_nint) \
+  struct TU_ATTR_PACKED { \
+    uint8_t  bLength; \
+    uint8_t  bDescriptorType; \
+    uint8_t  bDescriptorSubType; \
+    uint8_t  bFrameIndex; \
+    uint8_t  bmCapabilities; \
+    uint16_t wWidth; \
+    uint16_t wHeight; \
+    uint32_t dwMinBitRate; \
+    uint32_t dwMaxBitRate; \
+    uint32_t dwMaxVideoFrameBufferSize; /* deprecated in 1.5 */ \
+    uint32_t dwDefaultFrameInterval; /* 100ns unit */\
+    uint8_t  bFrameIntervalType; \
+    uint32_t dwFrameInterval[_nint]; \
+  }
+
+typedef tusb_desc_video_frame_uncompressed_nint_t() tusb_desc_video_frame_uncompressed_t;
+typedef tusb_desc_video_frame_uncompressed_nint_t(1) tusb_desc_video_frame_uncompressed_1int_t;
+typedef tusb_desc_video_frame_uncompressed_nint_t(2) tusb_desc_video_frame_uncompressed_2int_t;
+typedef tusb_desc_video_frame_uncompressed_nint_t(3) tusb_desc_video_frame_uncompressed_3int_t;
+typedef tusb_desc_video_frame_uncompressed_nint_t(4) tusb_desc_video_frame_uncompressed_4int_t;
 
+// continuous = 3 intervals: min, max, step
+typedef tusb_desc_video_frame_uncompressed_3int_t tusb_desc_video_frame_uncompressed_continuous_t;
+
+TU_VERIFY_STATIC(sizeof(tusb_desc_video_frame_uncompressed_continuous_t) == 38, "size is not correct");
+
+//------------- MJPEG -------------//
+// MJPEG payload specs: 3.1.1 format descriptor
 typedef struct TU_ATTR_PACKED {
   uint8_t bLength;
   uint8_t bDescriptorType;
   uint8_t bDescriptorSubType;
   uint8_t bFormatIndex;
   uint8_t bNumFrameDescriptors;
-  uint8_t bmFlags;
+  uint8_t bmFlags; // Bit 0: fixed size samples (1 = yes)
   uint8_t bDefaultFrameIndex;
   uint8_t bAspectRatioX;
   uint8_t bAspectRatioY;
   uint8_t bmInterlaceFlags;
   uint8_t bCopyProtect;
-} tusb_desc_cs_video_fmt_mjpeg_t;
+} tusb_desc_video_format_mjpeg_t;
+
+TU_VERIFY_STATIC(sizeof(tusb_desc_video_format_mjpeg_t) == 11, "size is not correct");
 
+// MJPEG payload specs: 3.1.2 frame descriptor (same as uncompressed)
+typedef tusb_desc_video_frame_uncompressed_t tusb_desc_video_frame_mjpeg_t;
+typedef tusb_desc_video_frame_uncompressed_1int_t tusb_desc_video_frame_mjpeg_1int_t;
+typedef tusb_desc_video_frame_uncompressed_2int_t tusb_desc_video_frame_mjpeg_2int_t;
+typedef tusb_desc_video_frame_uncompressed_3int_t tusb_desc_video_frame_mjpeg_3int_t;
+typedef tusb_desc_video_frame_uncompressed_4int_t tusb_desc_video_frame_mjpeg_4int_t;
+
+// continuous = 3 intervals: min, max, step
+typedef tusb_desc_video_frame_mjpeg_3int_t tusb_desc_video_frame_mjpeg_continuous_t;
+
+//------------- DV -------------//
+// DV payload specs: 3.1.1
 typedef struct TU_ATTR_PACKED {
   uint8_t  bLength;
   uint8_t  bDescriptorType;
@@ -323,8 +437,9 @@ typedef struct TU_ATTR_PACKED {
   uint8_t  bFormatIndex;
   uint32_t dwMaxVideoFrameBufferSize; /* deprecated */
   uint8_t  bFormatType;
-} tusb_desc_cs_video_fmt_dv_t;
+} tusb_desc_video_format_dv_t;
 
+// Frame Based payload specs: 3.1.1
 typedef struct TU_ATTR_PACKED {
   uint8_t bLength;
   uint8_t bDescriptorType;
@@ -339,25 +454,7 @@ typedef struct TU_ATTR_PACKED {
   uint8_t bmInterlaceFlags;
   uint8_t bCopyProtect;
   uint8_t bVaribaleSize;
-} tusb_desc_cs_video_fmt_frame_based_t;
-
-typedef struct TU_ATTR_PACKED {
-  uint8_t  bLength;
-  uint8_t  bDescriptorType;
-  uint8_t  bDescriptorSubType;
-  uint8_t  bFrameIndex;
-  uint8_t  bmCapabilities;
-  uint16_t wWidth;
-  uint16_t wHeight;
-  uint32_t dwMinBitRate;
-  uint32_t dwMaxBitRate;
-  uint32_t dwMaxVideoFrameBufferSize; /* deprecated */
-  uint32_t dwDefaultFrameInterval;
-  uint8_t  bFrameIntervalType;
-  uint32_t dwFrameInterval[];
-} tusb_desc_cs_video_frm_uncompressed_t;
-
-typedef tusb_desc_cs_video_frm_uncompressed_t tusb_desc_cs_video_frm_mjpeg_t;
+} tusb_desc_video_format_framebased_t;
 
 typedef struct TU_ATTR_PACKED {
   uint8_t  bLength;
@@ -373,12 +470,30 @@ typedef struct TU_ATTR_PACKED {
   uint8_t  bFrameIntervalType;
   uint32_t dwBytesPerLine;
   uint32_t dwFrameInterval[];
-} tusb_desc_cs_video_frm_frame_based_t;
+} tusb_desc_video_frame_framebased_t;
 
 //--------------------------------------------------------------------+
 // Requests
 //--------------------------------------------------------------------+
 
+/* 2.4.3.3 */
+typedef struct TU_ATTR_PACKED {
+  uint8_t bHeaderLength;
+  union {
+    uint8_t bmHeaderInfo;
+    struct {
+      uint8_t FrameID:              1;
+      uint8_t EndOfFrame:           1;
+      uint8_t PresentationTime:     1;
+      uint8_t SourceClockReference: 1;
+      uint8_t PayloadSpecific:      1;
+      uint8_t StillImage:           1;
+      uint8_t Error:                1;
+      uint8_t EndOfHeader:          1;
+    };
+  };
+} tusb_video_payload_header_t;
+
 /* 4.3.1.1 */
 typedef struct TU_ATTR_PACKED {
   union {
@@ -425,32 +540,36 @@ typedef struct TU_ATTR_PACKED {
 
 TU_VERIFY_STATIC( sizeof(video_probe_and_commit_control_t) == 48, "size is not correct");
 
-#define TUD_VIDEO_DESC_IAD_LEN                    8
-#define TUD_VIDEO_DESC_STD_VC_LEN                 9
-#define TUD_VIDEO_DESC_CS_VC_LEN                  12
-#define TUD_VIDEO_DESC_INPUT_TERM_LEN             8
-#define TUD_VIDEO_DESC_OUTPUT_TERM_LEN            9
-#define TUD_VIDEO_DESC_CAMERA_TERM_LEN            18
-#define TUD_VIDEO_DESC_STD_VS_LEN                 9
-#define TUD_VIDEO_DESC_CS_VS_IN_LEN               13
-#define TUD_VIDEO_DESC_CS_VS_OUT_LEN              9
-#define TUD_VIDEO_DESC_CS_VS_FMT_UNCOMPR_LEN      27
-#define TUD_VIDEO_DESC_CS_VS_FMT_MJPEG_LEN        11
-#define TUD_VIDEO_DESC_CS_VS_FRM_UNCOMPR_CONT_LEN 38
-#define TUD_VIDEO_DESC_CS_VS_FRM_UNCOMPR_DISC_LEN 26
-#define TUD_VIDEO_DESC_CS_VS_FRM_MJPEG_CONT_LEN   38
-#define TUD_VIDEO_DESC_CS_VS_FRM_MJPEG_DISC_LEN   26
-#define TUD_VIDEO_DESC_CS_VS_COLOR_MATCHING_LEN   6
+#define TUD_VIDEO_DESC_IAD_LEN                          8
+#define TUD_VIDEO_DESC_STD_VC_LEN                       9
+#define TUD_VIDEO_DESC_CS_VC_LEN                        12
+#define TUD_VIDEO_DESC_INPUT_TERM_LEN                   8
+#define TUD_VIDEO_DESC_OUTPUT_TERM_LEN                  9
+#define TUD_VIDEO_DESC_CAMERA_TERM_LEN                  18
+#define TUD_VIDEO_DESC_STD_VS_LEN                       9
+#define TUD_VIDEO_DESC_CS_VS_IN_LEN                     13
+#define TUD_VIDEO_DESC_CS_VS_OUT_LEN                    9
+#define TUD_VIDEO_DESC_CS_VS_FMT_UNCOMPR_LEN            27
+#define TUD_VIDEO_DESC_CS_VS_FMT_MJPEG_LEN              11
+#define TUD_VIDEO_DESC_CS_VS_FMT_FRAME_BASED_LEN        28
+#define TUD_VIDEO_DESC_CS_VS_FRM_UNCOMPR_CONT_LEN       38
+#define TUD_VIDEO_DESC_CS_VS_FRM_UNCOMPR_DISC_LEN       26
+#define TUD_VIDEO_DESC_CS_VS_FRM_MJPEG_CONT_LEN         38
+#define TUD_VIDEO_DESC_CS_VS_FRM_MJPEG_DISC_LEN         26
+#define TUD_VIDEO_DESC_CS_VS_FRM_FRAME_BASED_CONT_LEN   38
+#define TUD_VIDEO_DESC_CS_VS_FRM_FRAME_BASED_DISC_LEN   26
+#define TUD_VIDEO_DESC_CS_VS_COLOR_MATCHING_LEN         6
 
 /* 2.2 compression formats */
 #define TUD_VIDEO_GUID_YUY2   0x59,0x55,0x59,0x32,0x00,0x00,0x10,0x00,0x80,0x00,0x00,0xAA,0x00,0x38,0x9B,0x71
 #define TUD_VIDEO_GUID_NV12   0x4E,0x56,0x31,0x32,0x00,0x00,0x10,0x00,0x80,0x00,0x00,0xAA,0x00,0x38,0x9B,0x71
 #define TUD_VIDEO_GUID_M420   0x4D,0x34,0x32,0x30,0x00,0x00,0x10,0x00,0x80,0x00,0x00,0xAA,0x00,0x38,0x9B,0x71
 #define TUD_VIDEO_GUID_I420   0x49,0x34,0x32,0x30,0x00,0x00,0x10,0x00,0x80,0x00,0x00,0xAA,0x00,0x38,0x9B,0x71
+#define TUD_VIDEO_GUID_H264   0x48,0x32,0x36,0x34,0x00,0x00,0x10,0x00,0x80,0x00,0x00,0xAA,0x00,0x38,0x9B,0x71
 
-#define TUD_VIDEO_DESC_IAD(_firstitfs, _nitfs, _stridx) \
+#define TUD_VIDEO_DESC_IAD(_firstitf, _nitfs, _stridx) \
   TUD_VIDEO_DESC_IAD_LEN, TUSB_DESC_INTERFACE_ASSOCIATION, \
-  _firstitfs, _nitfs, TUSB_CLASS_VIDEO, VIDEO_SUBCLASS_INTERFACE_COLLECTION, \
+  _firstitf, _nitfs, TUSB_CLASS_VIDEO, VIDEO_SUBCLASS_INTERFACE_COLLECTION, \
   VIDEO_ITF_PROTOCOL_UNDEFINED, _stridx
 
 #define TUD_VIDEO_DESC_STD_VC(_itfnum, _nEPs, _stridx) \
@@ -537,10 +656,29 @@ TU_VERIFY_STATIC( sizeof(video_probe_and_commit_control_t) == 48, "size is not c
 /* Motion-JPEG 3.1.1 Table 3-2 and 3-4 */
 #define TUD_VIDEO_DESC_CS_VS_FRM_MJPEG_DISC(_frmidx, _cap, _width, _height, _minbr, _maxbr, _maxfrmbufsz, _frminterval, ...) \
   TUD_VIDEO_DESC_CS_VS_FRM_MJPEG_DISC_LEN + (TU_ARGS_NUM(__VA_ARGS__)) * 4, \
-  TUSB_DESC_CS_INTERFACE, VIDEO_CS_VS_INTERFACE_FRAME_MJPEG, \
+  TUSB_DESC_CS_INTERFACE, VIDEO_CS_ITF_VS_FRAME_MJPEG, \
   _frmidx, _cap, U16_TO_U8S_LE(_width), U16_TO_U8S_LE(_height), U32_TO_U8S_LE(_minbr), U32_TO_U8S_LE(_maxbr), \
   U32_TO_U8S_LE(_maxfrmbufsz), U32_TO_U8S_LE(_frminterval), (TU_ARGS_NUM(__VA_ARGS__)), __VA_ARGS__
 
+/* Motion-Frame-Based 3.1.1 Table 3-1 */
+#define TUD_VIDEO_DESC_CS_VS_FMT_FRAME_BASED(_fmtidx, _numfrmdesc, _guid, _bitsperpix, _frmidx, _asrx, _asry, _interlace, _cp, _variablesize) \
+  TUD_VIDEO_DESC_CS_VS_FMT_FRAME_BASED_LEN, TUSB_DESC_CS_INTERFACE, VIDEO_CS_ITF_VS_FORMAT_FRAME_BASED, \
+  _fmtidx, _numfrmdesc, TUD_VIDEO_GUID(_guid), _bitsperpix, _frmidx, _asrx,  _asry, _interlace, _cp, _variablesize
+
+/* Motion-Frame-Based 3.1.1 Table 3-2 and 3-3 */
+#define TUD_VIDEO_DESC_CS_VS_FRM_FRAME_BASED_CONT(_frmidx, _cap, _width, _height, _minbr, _maxbr, _frminterval, _bytesperline, _minfrminterval, _maxfrminterval, _frmintervalstep) \
+  TUD_VIDEO_DESC_CS_VS_FRM_FRAME_BASED_CONT_LEN, TUSB_DESC_CS_INTERFACE, VIDEO_CS_ITF_VS_FRAME_FRAME_BASED, \
+  _frmidx, _cap, U16_TO_U8S_LE(_width), U16_TO_U8S_LE(_height), U32_TO_U8S_LE(_minbr), U32_TO_U8S_LE(_maxbr), \
+  U32_TO_U8S_LE(_frminterval), 0, U32_TO_U8S_LE(_bytesperline), \
+  U32_TO_U8S_LE(_minfrminterval), U32_TO_U8S_LE(_maxfrminterval), U32_TO_U8S_LE(_frmintervalstep)
+
+/* Motion-Frame-Based 3.1.1 Table 3-2 and 3-4 */
+#define TUD_VIDEO_DESC_CS_VS_FRM_FRAME_BASED_DISC(_frmidx, _cap, _width, _height, _minbr, _maxbr, _frminterval, _bytesperline, ...) \
+  TUD_VIDEO_DESC_CS_VS_FRM_FRAME_BASED_DISC_LEN + (TU_ARGS_NUM(__VA_ARGS__)) * 4, \
+  TUSB_DESC_CS_INTERFACE, VIDEO_CS_ITF_VS_FRAME_FRAME_BASED, \
+  _frmidx, _cap, U16_TO_U8S_LE(_width), U16_TO_U8S_LE(_height), U32_TO_U8S_LE(_minbr), U32_TO_U8S_LE(_maxbr), \
+U32_TO_U8S_LE(_frminterval), U32_TO_U8S_LE(_bytesperline), (TU_ARGS_NUM(__VA_ARGS__)), __VA_ARGS__
+
 /* 3.9.2.6 */
 #define TUD_VIDEO_DESC_CS_VS_COLOR_MATCHING(_color, _trns, _mat) \
   TUD_VIDEO_DESC_CS_VS_COLOR_MATCHING_LEN, \
@@ -549,7 +687,7 @@ TU_VERIFY_STATIC( sizeof(video_probe_and_commit_control_t) == 48, "size is not c
 
 /* 3.10.1.1 */
 #define TUD_VIDEO_DESC_EP_ISO(_ep, _epsize, _ep_interval) \
-  7, TUSB_DESC_ENDPOINT, _ep, TUSB_XFER_ISOCHRONOUS | TUSB_ISO_EP_ATT_ASYNCHRONOUS,\
+  7, TUSB_DESC_ENDPOINT, _ep, (uint8_t) (TUSB_XFER_ISOCHRONOUS | TUSB_ISO_EP_ATT_ASYNCHRONOUS),\
   U16_TO_U8S_LE(_epsize), _ep_interval
 
 /* 3.10.1.2 */
diff --git a/src/class/video/video_device.c b/src/class/video/video_device.c
index a6d2724c1..3124d5596 100644
--- a/src/class/video/video_device.c
+++ b/src/class/video/video_device.c
@@ -1,4 +1,4 @@
-/* 
+/*
  * The MIT License (MIT)
  *
  * Copyright (c) 2021 Koji KITAYAMA
@@ -34,22 +34,33 @@
 
 #include "video_device.h"
 
+// Level where CFG_TUSB_DEBUG must be at least for this driver is logged
+#ifndef CFG_TUD_VIDEO_LOG_LEVEL
+  #define CFG_TUD_VIDEO_LOG_LEVEL   CFG_TUD_LOG_LEVEL
+#endif
+
+#define TU_LOG_DRV(...)   TU_LOG(CFG_TUD_VIDEO_LOG_LEVEL, __VA_ARGS__)
+
 //--------------------------------------------------------------------+
 // MACRO CONSTANT TYPEDEF
 //--------------------------------------------------------------------+
+#define VS_STATE_PROBING      0     /* Configuration in progress */
+#define VS_STATE_COMMITTED    1     /* Ready for streaming or Streaming via bulk endpoint */
+#define VS_STATE_STREAMING    2     /* Streaming via isochronous endpoint */
+
 typedef struct {
   tusb_desc_interface_t            std;
-  tusb_desc_cs_video_ctl_itf_hdr_t ctl;
+  tusb_desc_video_control_header_t ctl;
 } tusb_desc_vc_itf_t;
 
 typedef struct {
   tusb_desc_interface_t            std;
-  tusb_desc_cs_video_stm_itf_hdr_t stm;
+  tusb_desc_video_streaming_inout_header_t stm;
 } tusb_desc_vs_itf_t;
 
 typedef union {
-  tusb_desc_cs_video_ctl_itf_hdr_t ctl;
-  tusb_desc_cs_video_stm_itf_hdr_t stm;
+  tusb_desc_video_control_header_t ctl;
+  tusb_desc_video_streaming_inout_header_t stm;
 } tusb_desc_video_itf_hdr_t;
 
 typedef struct TU_ATTR_PACKED {
@@ -67,9 +78,9 @@ typedef union {
     uint8_t bFormatIndex;
     uint8_t bNumFrameDescriptors;
   };
-  tusb_desc_cs_video_fmt_uncompressed_t uncompressed;
-  tusb_desc_cs_video_fmt_mjpeg_t        mjpeg;
-  tusb_desc_cs_video_fmt_frame_based_t  frame_based;
+  tusb_desc_video_format_uncompressed_t uncompressed;
+  tusb_desc_video_format_mjpeg_t        mjpeg;
+  tusb_desc_video_format_framebased_t  frame_based;
 } tusb_desc_cs_video_fmt_t;
 
 typedef union {
@@ -82,9 +93,9 @@ typedef union {
     uint16_t wWidth;
     uint16_t wHeight;
   };
-  tusb_desc_cs_video_frm_uncompressed_t uncompressed;
-  tusb_desc_cs_video_frm_mjpeg_t        mjpeg;
-  tusb_desc_cs_video_frm_frame_based_t  frame_based;
+  tusb_desc_video_frame_uncompressed_t uncompressed;
+  tusb_desc_video_frame_mjpeg_t        mjpeg;
+  tusb_desc_video_frame_framebased_t  frame_based;
 } tusb_desc_cs_video_frm_t;
 
 /* video streaming interface */
@@ -102,42 +113,95 @@ typedef struct TU_ATTR_PACKED {
   uint32_t offset;   /* offset for the next payload transfer */
   uint32_t max_payload_transfer_size;
   uint8_t  error_code;/* error code */
-  /*------------- From this point, data is not cleared by bus reset -------------*/
-  CFG_TUSB_MEM_ALIGN uint8_t ep_buf[CFG_TUD_VIDEO_STREAMING_EP_BUFSIZE]; /* EP transfer buffer for streaming */
+  uint8_t  state;    /* 0:probing 1:committed 2:streaming */
+
+  video_probe_and_commit_control_t probe_commit_payload; /* Probe and Commit control */
 } videod_streaming_interface_t;
 
+typedef struct {
+  TUD_EPBUF_DEF(buf, CFG_TUD_VIDEO_STREAMING_EP_BUFSIZE);
+} videod_streaming_epbuf_t;
+
 /* video control interface */
 typedef struct TU_ATTR_PACKED {
-  void const *beg;  /* The head of the first video control interface descriptor */
-  uint16_t    len;  /* Byte length of the descriptors */
-  uint16_t    cur;  /* offset for current video control interface */
-  uint8_t     stm[CFG_TUD_VIDEO_STREAMING]; /* Indices of streaming interface */
-  uint8_t error_code;  /* error code */
-  uint8_t power_mode;
-
-  /*------------- From this point, data is not cleared by bus reset -------------*/
-  // CFG_TUSB_MEM_ALIGN uint8_t ctl_buf[64]; /* EP transfer buffer for interrupt transfer */
-
+  const uint8_t*beg;                     /* The head of the first video control interface descriptor */
+  uint16_t len;                          /* Byte length of the descriptors */
+  uint16_t cur;                          /* offset for current video control interface */
+  uint8_t  stm[CFG_TUD_VIDEO_STREAMING]; /* Indices of streaming interface */
+  uint8_t  error_code;                   /* error code */
+  uint8_t  power_mode;
 } videod_interface_t;
 
-#define ITF_STM_MEM_RESET_SIZE   offsetof(videod_streaming_interface_t, ep_buf)
-
 //--------------------------------------------------------------------+
 // INTERNAL OBJECT & FUNCTION DECLARATION
 //--------------------------------------------------------------------+
-CFG_TUSB_MEM_SECTION static videod_interface_t _videod_itf[CFG_TUD_VIDEO];
-CFG_TUSB_MEM_SECTION static videod_streaming_interface_t _videod_streaming_itf[CFG_TUD_VIDEO_STREAMING];
+static videod_interface_t _videod_itf[CFG_TUD_VIDEO];
+
+static videod_streaming_interface_t _videod_streaming_itf[CFG_TUD_VIDEO_STREAMING];
+CFG_TUD_MEM_SECTION static videod_streaming_epbuf_t _videod_streaming_epbuf[CFG_TUD_VIDEO_STREAMING];
 
 static uint8_t const _cap_get     = 0x1u; /* support for GET */
 static uint8_t const _cap_get_set = 0x3u; /* support for GET and SET */
 
+//--------------------------------------------------------------------+
+// Debug
+//--------------------------------------------------------------------+
+#if CFG_TUSB_DEBUG >= CFG_TUD_VIDEO_LOG_LEVEL
+
+static tu_lookup_entry_t const tu_lookup_video_request[] = {
+    {.key = VIDEO_REQUEST_UNDEFINED, .data = "Undefined"},
+    {.key = VIDEO_REQUEST_SET_CUR, .data = "SetCur"},
+    {.key = VIDEO_REQUEST_SET_CUR_ALL, .data = "SetCurAll"},
+    {.key = VIDEO_REQUEST_GET_CUR, .data = "GetCur"},
+    {.key = VIDEO_REQUEST_GET_MIN, .data = "GetMin"},
+    {.key = VIDEO_REQUEST_GET_MAX, .data = "GetMax"},
+    {.key = VIDEO_REQUEST_GET_RES, .data = "GetRes"},
+    {.key = VIDEO_REQUEST_GET_LEN, .data = "GetLen"},
+    {.key = VIDEO_REQUEST_GET_INFO, .data = "GetInfo"},
+    {.key = VIDEO_REQUEST_GET_DEF, .data = "GetDef"},
+    {.key = VIDEO_REQUEST_GET_CUR_ALL, .data = "GetCurAll"},
+    {.key = VIDEO_REQUEST_GET_MIN_ALL, .data = "GetMinAll"},
+    {.key = VIDEO_REQUEST_GET_MAX_ALL, .data = "GetMaxAll"},
+    {.key = VIDEO_REQUEST_GET_RES_ALL, .data = "GetResAll"},
+    {.key = VIDEO_REQUEST_GET_DEF_ALL, .data = "GetDefAll"},
+};
+
+static tu_lookup_table_t const tu_table_video_request = {
+    .count = TU_ARRAY_SIZE(tu_lookup_video_request),
+    .items = tu_lookup_video_request
+};
+
+static char const* const tu_str_video_vc_control_selector[] = {
+    "Undefined",
+    "Video Power Mode",
+    "Request Error Code",
+};
+
+static char const* const tu_str_video_vs_control_selector[] = {
+    "Undefined",
+    "Probe",
+    "Commit",
+    "Still Probe",
+    "Still Commit",
+    "Still Image Trigger",
+    "Stream Error Code",
+    "Generate Key Frame",
+    "Update Frame Segment",
+    "Sync Delay",
+};
+
+#endif
+
+//--------------------------------------------------------------------+
+//
+//--------------------------------------------------------------------+
+
 /** Get interface number from the interface descriptor
  *
  * @param[in] desc    interface descriptor
  *
  * @return bInterfaceNumber */
-static inline uint8_t _desc_itfnum(void const *desc)
-{
+static inline uint8_t _desc_itfnum(void const *desc) {
   return ((uint8_t const*)desc)[2];
 }
 
@@ -146,8 +210,7 @@ static inline uint8_t _desc_itfnum(void const *desc)
  * @param[in] desc    endpoint descriptor
  *
  * @return bEndpointAddress */
-static inline uint8_t _desc_ep_addr(void const *desc)
-{
+static inline uint8_t _desc_ep_addr(void const *desc) {
   return ((uint8_t const*)desc)[2];
 }
 
@@ -157,8 +220,7 @@ static inline uint8_t _desc_ep_addr(void const *desc)
  * @param[in] stm_idx    index number of streaming interface
  *
  * @return instance */
-static videod_streaming_interface_t* _get_instance_streaming(uint_fast8_t ctl_idx, uint_fast8_t stm_idx)
-{
+static videod_streaming_interface_t* _get_instance_streaming(uint_fast8_t ctl_idx, uint_fast8_t stm_idx) {
   videod_interface_t *ctl = &_videod_itf[ctl_idx];
   if (!ctl->beg) return NULL;
   videod_streaming_interface_t *stm = &_videod_streaming_itf[ctl->stm[stm_idx]];
@@ -166,15 +228,13 @@ static videod_streaming_interface_t* _get_instance_streaming(uint_fast8_t ctl_id
   return stm;
 }
 
-static tusb_desc_vc_itf_t const* _get_desc_vc(videod_interface_t const *self)
-{
+static tusb_desc_vc_itf_t const* _get_desc_vc(videod_interface_t const *self) {
   return (tusb_desc_vc_itf_t const *)(self->beg + self->cur);
 }
 
-static tusb_desc_vs_itf_t const* _get_desc_vs(videod_streaming_interface_t const *self)
-{
+static tusb_desc_vs_itf_t const* _get_desc_vs(videod_streaming_interface_t const *self) {
   if (!self->desc.cur) return NULL;
-  void const *desc = _videod_itf[self->index_vc].beg;
+  uint8_t const *desc = _videod_itf[self->index_vc].beg;
   return (tusb_desc_vs_itf_t const*)(desc + self->desc.cur);
 }
 
@@ -186,8 +246,7 @@ static tusb_desc_vs_itf_t const* _get_desc_vs(videod_streaming_interface_t const
  *
  * @return The pointer for interface descriptor.
  * @retval end   did not found interface descriptor */
-static void const* _find_desc(void const *beg, void const *end, uint_fast8_t desc_type)
-{
+static void const* _find_desc(void const *beg, void const *end, uint_fast8_t desc_type) {
   void const *cur = beg;
   while ((cur < end) && (desc_type != tu_desc_type(cur))) {
     cur = tu_desc_next(cur);
@@ -195,6 +254,24 @@ static void const* _find_desc(void const *beg, void const *end, uint_fast8_t des
   return cur;
 }
 
+/** Find the first descriptor of two given types
+ *
+ * @param[in] beg        The head of descriptor byte array.
+ * @param[in] end        The tail of descriptor byte array.
+ * @param[in] desc_type_0 The first target descriptor type.
+ * @param[in] desc_type_1 The second target descriptor type.
+ *
+ * @return The pointer for interface descriptor.
+ * @retval end   did not found interface descriptor */
+static void const* _find_desc_2_type(void const *beg, void const *end, uint_fast8_t desc_type_0, uint_fast8_t desc_type_1)
+{
+  void const *cur = beg;
+  while ((cur < end) && (desc_type_0 != tu_desc_type(cur)) && (desc_type_1 != tu_desc_type(cur))) {
+    cur = tu_desc_next(cur);
+  }
+  return cur;
+}
+
 /** Find the first descriptor specified by the arguments
  *
  * @param[in] beg        The head of descriptor byte array.
@@ -208,8 +285,7 @@ static void const* _find_desc(void const *beg, void const *end, uint_fast8_t des
 static void const* _find_desc_3(void const *beg, void const *end,
                                 uint_fast8_t desc_type,
                                 uint_fast8_t element_0,
-                                uint_fast8_t element_1)
-{
+                                uint_fast8_t element_1) {
   for (void const *cur = beg; cur < end; cur = _find_desc(cur, end, desc_type)) {
     uint8_t const *p = (uint8_t const *)cur;
     if ((p[2] == element_0) && (p[3] == element_1)) {
@@ -221,19 +297,22 @@ static void const* _find_desc_3(void const *beg, void const *end,
 }
 
 /** Return the next interface descriptor which has another interface number.
+ *  If there are multiple VC interfaces, there will be an IAD descriptor before
+ *  the next interface descriptor. Check both the IAD descriptor and the interface
+ *  descriptor.
+ *  3.1 Descriptor Layout Overview
  *
  * @param[in] beg     The head of descriptor byte array.
  * @param[in] end     The tail of descriptor byte array.
  *
  * @return The pointer for interface descriptor.
  * @retval end   did not found interface descriptor */
-static void const* _next_desc_itf(void const *beg, void const *end)
-{
+static void const* _next_desc_itf(void const *beg, void const *end) {
   void const *cur = beg;
   uint_fast8_t itfnum = ((tusb_desc_interface_t const*)cur)->bInterfaceNumber;
   while ((cur < end) &&
          (itfnum == ((tusb_desc_interface_t const*)cur)->bInterfaceNumber)) {
-    cur = _find_desc(tu_desc_next(cur), end, TUSB_DESC_INTERFACE);
+    cur = _find_desc_2_type(tu_desc_next(cur), end, TUSB_DESC_INTERFACE, TUSB_DESC_INTERFACE_ASSOCIATION);
   }
   return cur;
 }
@@ -247,9 +326,9 @@ static void const* _next_desc_itf(void const *beg, void const *end)
  *
  * @return The pointer for interface descriptor.
  * @retval end   did not found interface descriptor */
-static inline void const* _find_desc_itf(void const *beg, void const *end, uint_fast8_t itfnum, uint_fast8_t altnum)
+static inline uint8_t const* _find_desc_itf(void const *beg, void const *end, uint_fast8_t itfnum, uint_fast8_t altnum)
 {
-  return _find_desc_3(beg, end, TUSB_DESC_INTERFACE, itfnum, altnum);
+  return (uint8_t const*) _find_desc_3(beg, end, TUSB_DESC_INTERFACE, itfnum, altnum);
 }
 
 /** Find the first endpoint descriptor belonging to the current interface descriptor.
@@ -275,7 +354,7 @@ static void const* _find_desc_ep(void const *beg, void const *end)
 static inline void const* _end_of_control_descriptor(void const *desc)
 {
   tusb_desc_vc_itf_t const *vc = (tusb_desc_vc_itf_t const *)desc;
-  return desc + vc->std.bLength + vc->ctl.wTotalLength;
+  return ((uint8_t const*) desc) + vc->std.bLength + tu_le16toh(vc->ctl.wTotalLength);
 }
 
 /** Find the first entity descriptor with the entity ID
@@ -305,7 +384,7 @@ static void const* _find_desc_entity(void const *desc, uint_fast8_t entityid)
 static inline void const* _end_of_streaming_descriptor(void const *desc)
 {
   tusb_desc_vs_itf_t const *vs = (tusb_desc_vs_itf_t const *)desc;
-  return desc + vs->std.bLength + vs->stm.wTotalLength;
+  return ((uint8_t const*) desc) + vs->std.bLength + tu_le16toh(vs->stm.wTotalLength);
 }
 
 /** Find the first format descriptor with the specified format number. */
@@ -317,7 +396,7 @@ static inline void const *_find_desc_format(void const *beg, void const *end, ui
     if ((fmt == VIDEO_CS_ITF_VS_FORMAT_UNCOMPRESSED ||
          fmt == VIDEO_CS_ITF_VS_FORMAT_MJPEG ||
          fmt == VIDEO_CS_ITF_VS_FORMAT_DV ||
-         fmt == VIDEO_CS_ITF_VS_FRAME_FRAME_BASED) &&
+         fmt == VIDEO_CS_ITF_VS_FORMAT_FRAME_BASED) &&
         fmtnum == p[3]) {
       return cur;
     }
@@ -379,8 +458,13 @@ static bool _update_streaming_parameters(videod_streaming_interface_t const *stm
     case VIDEO_CS_ITF_VS_FORMAT_UNCOMPRESSED:
       param->wCompQuality = 1; /* 1 to 10000 */
       break;
-  case VIDEO_CS_ITF_VS_FORMAT_MJPEG:
+
+    case VIDEO_CS_ITF_VS_FORMAT_MJPEG:
+      break;
+
+    case VIDEO_CS_ITF_VS_FORMAT_FRAME_BASED:
       break;
+
     default: return false;
   }
 
@@ -401,9 +485,15 @@ static bool _update_streaming_parameters(videod_streaming_interface_t const *stm
       case VIDEO_CS_ITF_VS_FORMAT_UNCOMPRESSED:
         frame_size = (uint_fast32_t)frm->wWidth * frm->wHeight * fmt->uncompressed.bBitsPerPixel / 8;
         break;
+
       case VIDEO_CS_ITF_VS_FORMAT_MJPEG:
         frame_size = (uint_fast32_t)frm->wWidth * frm->wHeight * 16 / 8; /* YUV422 */
         break;
+
+      case VIDEO_CS_ITF_VS_FORMAT_FRAME_BASED:
+        frame_size = (uint_fast32_t)frm->wWidth * frm->wHeight * 16 / 8; /* YUV422 */
+        break;
+
       default: break;
     }
     param->dwMaxVideoFrameSize = frame_size;
@@ -422,8 +512,9 @@ static bool _update_streaming_parameters(videod_streaming_interface_t const *stm
   uint_fast32_t interval_ms = interval / 10000;
   TU_ASSERT(interval_ms);
   uint_fast32_t payload_size = (frame_size + interval_ms - 1) / interval_ms + 2;
-  if (CFG_TUD_VIDEO_STREAMING_EP_BUFSIZE < payload_size)
+  if (CFG_TUD_VIDEO_STREAMING_EP_BUFSIZE < payload_size) {
     payload_size = CFG_TUD_VIDEO_STREAMING_EP_BUFSIZE;
+  }
   param->dwMaxPayloadTransferSize = payload_size;
   return true;
 }
@@ -443,10 +534,12 @@ static bool _negotiate_streaming_parameters(videod_streaming_interface_t const *
         if (_get_desc_vs(stm))
           param->bFormatIndex = _get_desc_vs(stm)->stm.bNumFormats;
         break;
+
       case VIDEO_REQUEST_GET_MIN:
       case VIDEO_REQUEST_GET_DEF:
         param->bFormatIndex = 1;
         break;
+
       default: return false;
     }
     /* Set the parameters determined by the format  */
@@ -475,18 +568,26 @@ static bool _negotiate_streaming_parameters(videod_streaming_interface_t const *
       case VIDEO_REQUEST_GET_MAX:
         frmnum = fmt->bNumFrameDescriptors;
         break;
+
       case VIDEO_REQUEST_GET_MIN:
         frmnum = 1;
         break;
+
       case VIDEO_REQUEST_GET_DEF:
         switch (fmt->bDescriptorSubType) {
-        case VIDEO_CS_ITF_VS_FORMAT_UNCOMPRESSED:
-          frmnum = fmt->uncompressed.bDefaultFrameIndex;
-          break;
-        case VIDEO_CS_ITF_VS_FORMAT_MJPEG:
-          frmnum = fmt->mjpeg.bDefaultFrameIndex;
-          break;
-        default: return false;
+          case VIDEO_CS_ITF_VS_FORMAT_UNCOMPRESSED:
+            frmnum = fmt->uncompressed.bDefaultFrameIndex;
+            break;
+
+          case VIDEO_CS_ITF_VS_FORMAT_MJPEG:
+            frmnum = fmt->mjpeg.bDefaultFrameIndex;
+            break;
+
+          case VIDEO_CS_ITF_VS_FORMAT_FRAME_BASED:
+            frmnum = fmt->frame_based.bDefaultFrameIndex;
+            break;
+
+          default: return false;
         }
         break;
       default: return false;
@@ -499,9 +600,15 @@ static bool _negotiate_streaming_parameters(videod_streaming_interface_t const *
       case VIDEO_CS_ITF_VS_FORMAT_UNCOMPRESSED:
         frame_size = (uint_fast32_t)frm->wWidth * frm->wHeight * fmt->uncompressed.bBitsPerPixel / 8;
         break;
+
       case VIDEO_CS_ITF_VS_FORMAT_MJPEG:
         frame_size = (uint_fast32_t)frm->wWidth * frm->wHeight * 16 / 8; /* YUV422 */
         break;
+
+      case VIDEO_CS_ITF_VS_FORMAT_FRAME_BASED:
+        frame_size = (uint_fast32_t)frm->wWidth * frm->wHeight * 16 / 8; /* YUV422 */
+        break;
+
       default: return false;
     }
     param->dwMaxVideoFrameSize = frame_size;
@@ -517,41 +624,43 @@ static bool _negotiate_streaming_parameters(videod_streaming_interface_t const *
 
     uint_fast32_t interval, interval_ms;
     switch (request) {
-      case VIDEO_REQUEST_GET_MAX:
-        {
-          uint_fast32_t min_interval, max_interval;
-          uint_fast8_t num_intervals = frm->uncompressed.bFrameIntervalType;
-          max_interval = num_intervals ? frm->uncompressed.dwFrameInterval[num_intervals - 1]: frm->uncompressed.dwFrameInterval[1];
-          min_interval = frm->uncompressed.dwFrameInterval[0];
-          interval = max_interval;
-          interval_ms = min_interval / 10000;
-        }
+      case VIDEO_REQUEST_GET_MAX: {
+        uint_fast32_t min_interval, max_interval;
+        uint_fast8_t num_intervals = frm->uncompressed.bFrameIntervalType;
+        max_interval = num_intervals ? frm->uncompressed.dwFrameInterval[num_intervals - 1]: frm->uncompressed.dwFrameInterval[1];
+        min_interval = frm->uncompressed.dwFrameInterval[0];
+        interval = max_interval;
+        interval_ms = min_interval / 10000;
         break;
-      case VIDEO_REQUEST_GET_MIN:
-        {
-          uint_fast32_t min_interval, max_interval;
-          uint_fast8_t num_intervals = frm->uncompressed.bFrameIntervalType;
-          max_interval = num_intervals ? frm->uncompressed.dwFrameInterval[num_intervals - 1]: frm->uncompressed.dwFrameInterval[1];
-          min_interval = frm->uncompressed.dwFrameInterval[0];
-          interval = min_interval;
-          interval_ms = max_interval / 10000;
-        }
+      }
+
+      case VIDEO_REQUEST_GET_MIN: {
+        uint_fast32_t min_interval, max_interval;
+        uint_fast8_t num_intervals = frm->uncompressed.bFrameIntervalType;
+        max_interval = num_intervals ? frm->uncompressed.dwFrameInterval[num_intervals - 1]: frm->uncompressed.dwFrameInterval[1];
+        min_interval = frm->uncompressed.dwFrameInterval[0];
+        interval = min_interval;
+        interval_ms = max_interval / 10000;
         break;
+      }
+
       case VIDEO_REQUEST_GET_DEF:
         interval = frm->uncompressed.dwDefaultFrameInterval;
         interval_ms = interval / 10000;
         break;
-      case VIDEO_REQUEST_GET_RES:
-        {
-          uint_fast8_t num_intervals = frm->uncompressed.bFrameIntervalType;
-          if (num_intervals) {
-            interval = 0;
-          } else {
-            interval = frm->uncompressed.dwFrameInterval[2];
-            interval_ms = interval / 10000;
-          }
+
+      case VIDEO_REQUEST_GET_RES: {
+        uint_fast8_t num_intervals = frm->uncompressed.bFrameIntervalType;
+        if (num_intervals) {
+          interval = 0;
+          interval_ms = 0;
+        } else {
+          interval = frm->uncompressed.dwFrameInterval[2];
+          interval_ms = interval / 10000;
         }
         break;
+      }
+
       default: return false;
     }
     param->dwFrameInterval = interval;
@@ -565,8 +674,9 @@ static bool _negotiate_streaming_parameters(videod_streaming_interface_t const *
       } else {
         payload_size = (frame_size + interval_ms - 1) / interval_ms + 2;
       }
-      if (CFG_TUD_VIDEO_STREAMING_EP_BUFSIZE < payload_size)
+      if (CFG_TUD_VIDEO_STREAMING_EP_BUFSIZE < payload_size) {
         payload_size = CFG_TUD_VIDEO_STREAMING_EP_BUFSIZE;
+      }
       param->dwMaxPayloadTransferSize = payload_size;
     }
     return true;
@@ -581,8 +691,10 @@ static bool _negotiate_streaming_parameters(videod_streaming_interface_t const *
 static bool _close_vc_itf(uint8_t rhport, videod_interface_t *self)
 {
   tusb_desc_vc_itf_t const *vc = _get_desc_vc(self);
+
   /* The next descriptor after the class-specific VC interface header descriptor. */
-  void const *cur = (void const*)vc + vc->std.bLength + vc->ctl.bLength;
+  void const *cur = (uint8_t const*)vc + vc->std.bLength + vc->ctl.bLength;
+
   /* The end of the video control interface descriptor. */
   void const *end = _end_of_control_descriptor(vc);
   if (vc->std.bNumEndpoints) {
@@ -602,24 +714,26 @@ static bool _close_vc_itf(uint8_t rhport, videod_interface_t *self)
  * @param[in]     altnum   The target alternate setting number. */
 static bool _open_vc_itf(uint8_t rhport, videod_interface_t *self, uint_fast8_t altnum)
 {
-  TU_LOG2("    open VC %d\n", altnum);
-  void const *beg = self->beg;
-  void const *end = beg + self->len;
+  TU_LOG_DRV("    open VC %d\r\n", altnum);
+  uint8_t const *beg = self->beg;
+  uint8_t const *end = beg + self->len;
+
   /* The first descriptor is a video control interface descriptor. */
-  void const *cur = _find_desc_itf(beg, end, _desc_itfnum(beg), altnum);
-  TU_LOG2("    cur %d\n", cur - beg);
+  uint8_t const *cur = _find_desc_itf(beg, end, _desc_itfnum(beg), altnum);
+  TU_LOG_DRV("    cur %" PRId32 "\r\n", (int32_t) (cur - beg));
   TU_VERIFY(cur < end);
 
   tusb_desc_vc_itf_t const *vc = (tusb_desc_vc_itf_t const *)cur;
-  TU_LOG2("    bInCollection %d\n", vc->ctl.bInCollection);
+  TU_LOG_DRV("    bInCollection %d\r\n", vc->ctl.bInCollection);
   /* Support for up to 2 streaming interfaces only. */
   TU_ASSERT(vc->ctl.bInCollection <= CFG_TUD_VIDEO_STREAMING);
 
   /* Update to point the end of the video control interface descriptor. */
-  end  = _end_of_control_descriptor(cur);
+  end = _end_of_control_descriptor(cur);
+
   /* Advance to the next descriptor after the class-specific VC interface header descriptor. */
   cur += vc->std.bLength + vc->ctl.bLength;
-  TU_LOG2("    bNumEndpoints %d\n", vc->std.bNumEndpoints);
+  TU_LOG_DRV("    bNumEndpoints %d\r\n", vc->std.bNumEndpoints);
   /* Open the notification endpoint if it exist. */
   if (vc->std.bNumEndpoints) {
     /* Support for 1 endpoint only. */
@@ -631,10 +745,19 @@ static bool _open_vc_itf(uint8_t rhport, videod_interface_t *self, uint_fast8_t
     /* Open the notification endpoint */
     TU_ASSERT(usbd_edpt_open(rhport, notif));
   }
-  self->cur = (uint16_t) ((void const*)vc - beg);
+  self->cur = (uint16_t) ((uint8_t const*)vc - beg);
   return true;
 }
 
+static bool _init_vs_configuration(videod_streaming_interface_t *stm) {
+  /* initialize streaming settings */
+  stm->state = VS_STATE_PROBING;
+  stm->max_payload_transfer_size = 0;
+  video_probe_and_commit_control_t *param = &stm->probe_commit_payload;
+  tu_memclr(param, sizeof(*param));
+  return _update_streaming_parameters(stm, param);
+}
+
 /** Set the alternate setting to own video streaming interface.
  *
  * @param[in,out] stm      Streaming interface context.
@@ -642,85 +765,84 @@ static bool _open_vc_itf(uint8_t rhport, videod_interface_t *self, uint_fast8_t
 static bool _open_vs_itf(uint8_t rhport, videod_streaming_interface_t *stm, uint_fast8_t altnum)
 {
   uint_fast8_t i;
-  TU_LOG2("    reopen VS %d\n", altnum);
-  void const *desc = _videod_itf[stm->index_vc].beg;
+  TU_LOG_DRV("    reopen VS %d\r\n", altnum);
+  uint8_t const *desc = _videod_itf[stm->index_vc].beg;
 
+#ifndef TUP_DCD_EDPT_ISO_ALLOC
   /* Close endpoints of previous settings. */
   for (i = 0; i < TU_ARRAY_SIZE(stm->desc.ep); ++i) {
     uint_fast16_t ofs_ep = stm->desc.ep[i];
     if (!ofs_ep) break;
-    uint8_t  ep_adr = _desc_ep_addr(desc + ofs_ep);
-    usbd_edpt_close(rhport, ep_adr);
-    stm->desc.ep[i] = 0;
-    TU_LOG2("    close EP%02x\n", ep_adr);
+    tusb_desc_endpoint_t const *ep = (tusb_desc_endpoint_t const*)(desc + ofs_ep);
+    /* Only ISO endpoints needs to be closed */
+    if(ep->bmAttributes.xfer == TUSB_XFER_ISOCHRONOUS) {
+      stm->desc.ep[i] = 0;
+      usbd_edpt_close(rhport, ep->bEndpointAddress);
+      TU_LOG_DRV("    close EP%02x\r\n", ep->bEndpointAddress);
+    }
   }
+#endif
+
   /* clear transfer management information */
   stm->buffer  = NULL;
   stm->bufsize = 0;
   stm->offset  = 0;
 
   /* Find a alternate interface */
-  void const *beg = desc + stm->desc.beg;
-  void const *end = desc + stm->desc.end;
-  void const *cur = _find_desc_itf(beg, end, _desc_itfnum(beg), altnum);
+  uint8_t const *beg = desc + stm->desc.beg;
+  uint8_t const *end = desc + stm->desc.end;
+  uint8_t const *cur = _find_desc_itf(beg, end, _desc_itfnum(beg), altnum);
   TU_VERIFY(cur < end);
+
   uint_fast8_t numeps = ((tusb_desc_interface_t const *)cur)->bNumEndpoints;
   TU_ASSERT(numeps <= TU_ARRAY_SIZE(stm->desc.ep));
-  stm->desc.cur = (uint16_t) (cur - desc); /* Save the offset of the new settings */
-  if (!altnum) {
-    /* initialize streaming settings */
-    stm->max_payload_transfer_size = 0;
-    video_probe_and_commit_control_t *param =
-      (video_probe_and_commit_control_t *)&stm->ep_buf;
-    tu_memclr(param, sizeof(*param));
-    TU_LOG2("    done 0\n");
-    return _update_streaming_parameters(stm, param);
+  stm->desc.cur = (uint16_t)(cur - desc); /* Save the offset of the new settings */
+  if (!altnum && (VS_STATE_COMMITTED != stm->state)) {
+    TU_VERIFY(_init_vs_configuration(stm));
   }
-  /* Open endpoints of the new settings. */
+  /* Open bulk or isochronous endpoints of the new settings. */
   for (i = 0, cur = tu_desc_next(cur); i < numeps; ++i, cur = tu_desc_next(cur)) {
     cur = _find_desc_ep(cur, end);
     TU_ASSERT(cur < end);
     tusb_desc_endpoint_t const *ep = (tusb_desc_endpoint_t const*)cur;
-    if (!stm->max_payload_transfer_size) {
-      video_probe_and_commit_control_t const *param = (video_probe_and_commit_control_t const*)&stm->ep_buf;
-      uint_fast32_t max_size = param->dwMaxPayloadTransferSize;
-      if ((TUSB_XFER_ISOCHRONOUS == ep->bmAttributes.xfer) &&
-          (tu_edpt_packet_size(ep) < max_size))
-      {
-        /* FS must be less than or equal to max packet size */
-        return false;
-      }
-      /* Set the negotiated value */
-      stm->max_payload_transfer_size = max_size;
+    uint_fast32_t max_size = stm->max_payload_transfer_size;
+    if (altnum && (TUSB_XFER_ISOCHRONOUS == ep->bmAttributes.xfer)) {
+      /* FS must be less than or equal to max packet size */
+      TU_VERIFY (tu_edpt_packet_size(ep) >= max_size);
+#ifdef TUP_DCD_EDPT_ISO_ALLOC
+      usbd_edpt_iso_activate(rhport, ep);
+#else
+      TU_ASSERT(usbd_edpt_open(rhport, ep));
+#endif
+    } else {
+      TU_VERIFY(TUSB_XFER_BULK == ep->bmAttributes.xfer);
+      TU_ASSERT(usbd_edpt_open(rhport, ep));
     }
-    TU_ASSERT(usbd_edpt_open(rhport, ep));
     stm->desc.ep[i] = (uint16_t) (cur - desc);
-    TU_LOG2("    open EP%02x\n", _desc_ep_addr(cur));
+    TU_LOG_DRV("    open EP%02x\r\n", _desc_ep_addr(cur));
   }
-  /* initialize payload header */
-  tusb_video_payload_header_t *hdr = (tusb_video_payload_header_t*)stm->ep_buf;
-  hdr->bHeaderLength = sizeof(*hdr);
-  hdr->bmHeaderInfo  = 0;
-
-  TU_LOG2("    done\n");
+  if (altnum) {
+    stm->state = VS_STATE_STREAMING;
+  }
+  TU_LOG_DRV("    done\r\n");
   return true;
 }
 
 /** Prepare the next packet payload. */
-static uint_fast16_t _prepare_in_payload(videod_streaming_interface_t *stm)
-{
+static uint_fast16_t _prepare_in_payload(videod_streaming_interface_t *stm, uint8_t* ep_buf) {
   uint_fast16_t remaining = stm->bufsize - stm->offset;
-  uint_fast16_t hdr_len   = stm->ep_buf[0];
+  uint_fast16_t hdr_len   = ep_buf[0];
   uint_fast16_t pkt_len   = stm->max_payload_transfer_size;
   if (hdr_len + remaining < pkt_len) {
     pkt_len = hdr_len + remaining;
   }
+  TU_ASSERT(pkt_len >= hdr_len);
   uint_fast16_t data_len = pkt_len - hdr_len;
-  memcpy(&stm->ep_buf[hdr_len], stm->buffer + stm->offset, data_len);
+  memcpy(&ep_buf[hdr_len], stm->buffer + stm->offset, data_len);
   stm->offset += data_len;
   remaining -= data_len;
   if (!remaining) {
-    tusb_video_payload_header_t *hdr = (tusb_video_payload_header_t*)stm->ep_buf;
+    tusb_video_payload_header_t *hdr = (tusb_video_payload_header_t*) ep_buf;
     hdr->EndOfFrame = 1;
   }
   return hdr_len + data_len;
@@ -731,6 +853,7 @@ static int handle_video_ctl_std_req(uint8_t rhport, uint8_t stage,
                                     tusb_control_request_t const *request,
                                     uint_fast8_t ctl_idx)
 {
+  TU_LOG_DRV("\r\n");
   switch (request->bRequest) {
     case TUSB_REQ_GET_INTERFACE:
       if (stage == CONTROL_STAGE_SETUP)
@@ -768,7 +891,10 @@ static int handle_video_ctl_cs_req(uint8_t rhport, uint8_t stage,
   videod_interface_t *self = &_videod_itf[ctl_idx];
 
   /* 4.2.1 Interface Control Request */
-  switch (TU_U16_HIGH(request->wValue)) {
+  uint8_t const ctrl_sel = TU_U16_HIGH(request->wValue);
+  TU_LOG_DRV("%s_Control(%s)\r\n",  tu_str_video_vc_control_selector[ctrl_sel], tu_lookup_find(&tu_table_video_request, request->bRequest));
+
+  switch (ctrl_sel) {
     case VIDEO_VC_CTL_VIDEO_POWER_MODE:
       switch (request->bRequest) {
         case VIDEO_REQUEST_SET_CUR:
@@ -832,19 +958,19 @@ static int handle_video_ctl_req(uint8_t rhport, uint8_t stage,
                                 tusb_control_request_t const *request,
                                 uint_fast8_t ctl_idx)
 {
-  uint_fast8_t entity_id;
   switch (request->bmRequestType_bit.type) {
     case TUSB_REQ_TYPE_STANDARD:
       return handle_video_ctl_std_req(rhport, stage, request, ctl_idx);
 
-    case TUSB_REQ_TYPE_CLASS:
-      entity_id = TU_U16_HIGH(request->wIndex);
+    case TUSB_REQ_TYPE_CLASS: {
+      uint_fast8_t entity_id = TU_U16_HIGH(request->wIndex);
       if (!entity_id) {
         return handle_video_ctl_cs_req(rhport, stage, request, ctl_idx);
       } else {
         TU_VERIFY(_find_desc_entity(_get_desc_vc(&_videod_itf[ctl_idx]), entity_id), VIDEO_ERROR_INVALID_REQUEST);
         return VIDEO_ERROR_NONE;
       }
+    }
 
     default:
       return VIDEO_ERROR_INVALID_REQUEST;
@@ -855,6 +981,7 @@ static int handle_video_stm_std_req(uint8_t rhport, uint8_t stage,
                                     tusb_control_request_t const *request,
                                     uint_fast8_t stm_idx)
 {
+  TU_LOG_DRV("\r\n");
   videod_streaming_interface_t *self = &_videod_streaming_itf[stm_idx];
   switch (request->bRequest) {
     case TUSB_REQ_GET_INTERFACE:
@@ -870,8 +997,7 @@ static int handle_video_stm_std_req(uint8_t rhport, uint8_t stage,
       return VIDEO_ERROR_NONE;
 
     case TUSB_REQ_SET_INTERFACE:
-      if (stage == CONTROL_STAGE_SETUP)
-      {
+      if (stage == CONTROL_STAGE_SETUP) {
         TU_VERIFY(_open_vs_itf(rhport, self, request->wValue), VIDEO_ERROR_UNKNOWN);
         tud_control_status(rhport, request);
       }
@@ -885,26 +1011,27 @@ static int handle_video_stm_std_req(uint8_t rhport, uint8_t stage,
 
 static int handle_video_stm_cs_req(uint8_t rhport, uint8_t stage,
                                    tusb_control_request_t const *request,
-                                   uint_fast8_t stm_idx)
-{
+                                   uint_fast8_t stm_idx) {
   (void)rhport;
-  videod_streaming_interface_t *self = &_videod_streaming_itf[stm_idx];
+  videod_streaming_interface_t *stm = &_videod_streaming_itf[stm_idx];
+  videod_streaming_epbuf_t *stm_epbuf = &_videod_streaming_epbuf[stm_idx];
+
+  uint8_t const ctrl_sel = TU_U16_HIGH(request->wValue);
+  TU_LOG_DRV("%s_Control(%s)\r\n", tu_str_video_vs_control_selector[ctrl_sel], tu_lookup_find(&tu_table_video_request, request->bRequest));
 
   /* 4.2.1 Interface Control Request */
-  switch (TU_U16_HIGH(request->wValue)) {
+  switch (ctrl_sel) {
     case VIDEO_VS_CTL_STREAM_ERROR_CODE:
       switch (request->bRequest) {
         case VIDEO_REQUEST_GET_CUR:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             /* TODO */
-            TU_VERIFY(tud_control_xfer(rhport, request, &self->error_code, sizeof(uint8_t)), VIDEO_ERROR_UNKNOWN);
+            TU_VERIFY(tud_control_xfer(rhport, request, &stm->error_code, sizeof(uint8_t)), VIDEO_ERROR_UNKNOWN);
           }
           return VIDEO_ERROR_NONE;
 
         case VIDEO_REQUEST_GET_INFO:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)(uintptr_t) &_cap_get, sizeof(_cap_get)), VIDEO_ERROR_UNKNOWN);
           }
           return VIDEO_ERROR_NONE;
@@ -914,23 +1041,25 @@ static int handle_video_stm_cs_req(uint8_t rhport, uint8_t stage,
       break;
 
     case VIDEO_VS_CTL_PROBE:
+      if (stm->state != VS_STATE_PROBING) {
+        stm->state = VS_STATE_PROBING;
+      }
+
       switch (request->bRequest) {
         case VIDEO_REQUEST_SET_CUR:
           if (stage == CONTROL_STAGE_SETUP) {
-            TU_VERIFY(sizeof(video_probe_and_commit_control_t) == request->wLength, VIDEO_ERROR_UNKNOWN);
-            TU_VERIFY(tud_control_xfer(rhport, request, self->ep_buf, sizeof(video_probe_and_commit_control_t)),
+            TU_VERIFY(tud_control_xfer(rhport, request, &stm->probe_commit_payload, sizeof(video_probe_and_commit_control_t)),
                       VIDEO_ERROR_UNKNOWN);
           } else if (stage == CONTROL_STAGE_DATA) {
-            TU_VERIFY(_update_streaming_parameters(self, (video_probe_and_commit_control_t*)self->ep_buf),
+            TU_VERIFY(_update_streaming_parameters(stm, &stm->probe_commit_payload),
                       VIDEO_ERROR_INVALID_VALUE_WITHIN_RANGE);
           }
           return VIDEO_ERROR_NONE;
 
         case VIDEO_REQUEST_GET_CUR:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             TU_VERIFY(request->wLength, VIDEO_ERROR_UNKNOWN);
-            TU_VERIFY(tud_control_xfer(rhport, request, self->ep_buf, sizeof(video_probe_and_commit_control_t)), VIDEO_ERROR_UNKNOWN);
+            TU_VERIFY(tud_control_xfer(rhport, request, &stm->probe_commit_payload, sizeof(video_probe_and_commit_control_t)), VIDEO_ERROR_UNKNOWN);
           }
           return VIDEO_ERROR_NONE;
 
@@ -938,19 +1067,16 @@ static int handle_video_stm_cs_req(uint8_t rhport, uint8_t stage,
         case VIDEO_REQUEST_GET_MAX:
         case VIDEO_REQUEST_GET_RES:
         case VIDEO_REQUEST_GET_DEF:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             TU_VERIFY(request->wLength, VIDEO_ERROR_UNKNOWN);
-            video_probe_and_commit_control_t tmp;
-            tmp = *(video_probe_and_commit_control_t*)&self->ep_buf;
-            TU_VERIFY(_negotiate_streaming_parameters(self, request->bRequest, &tmp), VIDEO_ERROR_INVALID_VALUE_WITHIN_RANGE);
+            video_probe_and_commit_control_t tmp = stm->probe_commit_payload;
+            TU_VERIFY(_negotiate_streaming_parameters(stm, request->bRequest, &tmp), VIDEO_ERROR_INVALID_VALUE_WITHIN_RANGE);
             TU_VERIFY(tud_control_xfer(rhport, request, &tmp, sizeof(tmp)), VIDEO_ERROR_UNKNOWN);
           }
           return VIDEO_ERROR_NONE;
 
         case VIDEO_REQUEST_GET_LEN:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             TU_VERIFY(2 == request->wLength, VIDEO_ERROR_UNKNOWN);
             uint16_t len = sizeof(video_probe_and_commit_control_t);
             TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)&len, sizeof(len)), VIDEO_ERROR_UNKNOWN);
@@ -958,8 +1084,7 @@ static int handle_video_stm_cs_req(uint8_t rhport, uint8_t stage,
           return VIDEO_ERROR_NONE;
 
         case VIDEO_REQUEST_GET_INFO:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             TU_VERIFY(1 == request->wLength, VIDEO_ERROR_UNKNOWN);
             TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)(uintptr_t)&_cap_get_set, sizeof(_cap_get_set)), VIDEO_ERROR_UNKNOWN);
           }
@@ -973,27 +1098,38 @@ static int handle_video_stm_cs_req(uint8_t rhport, uint8_t stage,
       switch (request->bRequest) {
         case VIDEO_REQUEST_SET_CUR:
           if (stage == CONTROL_STAGE_SETUP) {
-            TU_VERIFY(sizeof(video_probe_and_commit_control_t) == request->wLength, VIDEO_ERROR_UNKNOWN);
-            TU_VERIFY(tud_control_xfer(rhport, request, self->ep_buf, sizeof(video_probe_and_commit_control_t)), VIDEO_ERROR_UNKNOWN);
+            TU_VERIFY(tud_control_xfer(rhport, request, &stm->probe_commit_payload, sizeof(video_probe_and_commit_control_t)), VIDEO_ERROR_UNKNOWN);
           } else if (stage == CONTROL_STAGE_DATA) {
-            TU_VERIFY(_update_streaming_parameters(self, (video_probe_and_commit_control_t*)self->ep_buf), VIDEO_ERROR_INVALID_VALUE_WITHIN_RANGE);
+            video_probe_and_commit_control_t *param = &stm->probe_commit_payload;
+            TU_VERIFY(_update_streaming_parameters(stm, param), VIDEO_ERROR_INVALID_VALUE_WITHIN_RANGE);
+            /* Set the negotiated value */
+            stm->max_payload_transfer_size = param->dwMaxPayloadTransferSize;
+            int ret = VIDEO_ERROR_NONE;
             if (tud_video_commit_cb) {
-              return tud_video_commit_cb(self->index_vc, self->index_vs, (video_probe_and_commit_control_t*)self->ep_buf);
+              ret = tud_video_commit_cb(stm->index_vc, stm->index_vs, param);
+            }
+            if (VIDEO_ERROR_NONE == ret) {
+              stm->state   = VS_STATE_COMMITTED;
+              stm->buffer  = NULL;
+              stm->bufsize = 0;
+              stm->offset  = 0;
+              /* initialize payload header */
+              tusb_video_payload_header_t *hdr = (tusb_video_payload_header_t*)stm_epbuf->buf;
+              hdr->bHeaderLength = sizeof(*hdr);
+              hdr->bmHeaderInfo  = 0;
             }
           }
           return VIDEO_ERROR_NONE;
 
         case VIDEO_REQUEST_GET_CUR:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             TU_VERIFY(request->wLength, VIDEO_ERROR_UNKNOWN);
-            TU_VERIFY(tud_control_xfer(rhport, request, self->ep_buf, sizeof(video_probe_and_commit_control_t)), VIDEO_ERROR_UNKNOWN);
+            TU_VERIFY(tud_control_xfer(rhport, request, &stm->probe_commit_payload, sizeof(video_probe_and_commit_control_t)), VIDEO_ERROR_UNKNOWN);
           }
           return VIDEO_ERROR_NONE;
 
         case VIDEO_REQUEST_GET_LEN:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             TU_VERIFY(2 == request->wLength, VIDEO_ERROR_UNKNOWN);
             uint16_t len = sizeof(video_probe_and_commit_control_t);
             TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)&len, sizeof(len)), VIDEO_ERROR_UNKNOWN);
@@ -1001,8 +1137,7 @@ static int handle_video_stm_cs_req(uint8_t rhport, uint8_t stage,
           return VIDEO_ERROR_NONE;
 
         case VIDEO_REQUEST_GET_INFO:
-          if (stage == CONTROL_STAGE_SETUP)
-          {
+          if (stage == CONTROL_STAGE_SETUP) {
             TU_VERIFY(1 == request->wLength, VIDEO_ERROR_UNKNOWN);
             TU_VERIFY(tud_control_xfer(rhport, request, (uint8_t*)(uintptr_t) &_cap_get_set, sizeof(_cap_get_set)), VIDEO_ERROR_UNKNOWN);
           }
@@ -1043,7 +1178,6 @@ static int handle_video_stm_req(uint8_t rhport, uint8_t stage,
 
     default: return VIDEO_ERROR_INVALID_REQUEST;
   }
-  return VIDEO_ERROR_UNKNOWN;
 }
 
 //--------------------------------------------------------------------+
@@ -1064,19 +1198,33 @@ bool tud_video_n_streaming(uint_fast8_t ctl_idx, uint_fast8_t stm_idx)
   TU_ASSERT(stm_idx < CFG_TUD_VIDEO_STREAMING);
   videod_streaming_interface_t *stm = _get_instance_streaming(ctl_idx, stm_idx);
   if (!stm || !stm->desc.ep[0]) return false;
+  if (stm->state == VS_STATE_PROBING) return false;
+
+#ifdef TUP_DCD_EDPT_ISO_ALLOC
+  uint8_t const *desc = _videod_itf[stm->index_vc].beg;
+  uint_fast16_t ofs_ep = stm->desc.ep[0];
+  tusb_desc_endpoint_t const *ep = (tusb_desc_endpoint_t const*)(desc + ofs_ep);
+  if (ep->bmAttributes.xfer == TUSB_XFER_ISOCHRONOUS) {
+    if (stm->state == VS_STATE_COMMITTED) return false;
+  }
+#endif
+
   return true;
 }
 
-bool tud_video_n_frame_xfer(uint_fast8_t ctl_idx, uint_fast8_t stm_idx, void *buffer, size_t bufsize)
-{
+bool tud_video_n_frame_xfer(uint_fast8_t ctl_idx, uint_fast8_t stm_idx, void *buffer, size_t bufsize) {
   TU_ASSERT(ctl_idx < CFG_TUD_VIDEO);
   TU_ASSERT(stm_idx < CFG_TUD_VIDEO_STREAMING);
+
   if (!buffer || !bufsize) return false;
   videod_streaming_interface_t *stm = _get_instance_streaming(ctl_idx, stm_idx);
+  videod_streaming_epbuf_t *stm_epbuf = &_videod_streaming_epbuf[ctl_idx];
+
   if (!stm || !stm->desc.ep[0] || stm->buffer) return false;
+  if (stm->state == VS_STATE_PROBING) return false;
 
   /* Find EP address */
-  void const *desc = _videod_itf[stm->index_vc].beg;
+  uint8_t const *desc = _videod_itf[stm->index_vc].beg;
   uint8_t ep_addr = 0;
   for (uint_fast8_t i = 0; i < CFG_TUD_VIDEO_STREAMING; ++i) {
     uint_fast16_t ofs_ep = stm->desc.ep[i];
@@ -1088,34 +1236,36 @@ bool tud_video_n_frame_xfer(uint_fast8_t ctl_idx, uint_fast8_t stm_idx, void *bu
 
   TU_VERIFY( usbd_edpt_claim(0, ep_addr) );
   /* update the packet header */
-  tusb_video_payload_header_t *hdr = (tusb_video_payload_header_t*)stm->ep_buf;
+  tusb_video_payload_header_t *hdr = (tusb_video_payload_header_t*)stm_epbuf->buf;
   hdr->FrameID   ^= 1;
   hdr->EndOfFrame = 0;
   /* update the packet data */
   stm->buffer     = (uint8_t*)buffer;
   stm->bufsize    = bufsize;
-  uint_fast16_t pkt_len = _prepare_in_payload(stm);
-  TU_ASSERT( usbd_edpt_xfer(0, ep_addr, stm->ep_buf, (uint16_t) pkt_len), 0);
+  uint_fast16_t pkt_len = _prepare_in_payload(stm, stm_epbuf->buf);
+  TU_ASSERT( usbd_edpt_xfer(0, ep_addr, stm_epbuf->buf, (uint16_t) pkt_len), 0);
   return true;
 }
 
 //--------------------------------------------------------------------+
 // USBD Driver API
 //--------------------------------------------------------------------+
-void videod_init(void)
-{
+void videod_init(void) {
   for (uint_fast8_t i = 0; i < CFG_TUD_VIDEO; ++i) {
     videod_interface_t* ctl = &_videod_itf[i];
     tu_memclr(ctl, sizeof(*ctl));
   }
   for (uint_fast8_t i = 0; i < CFG_TUD_VIDEO_STREAMING; ++i) {
     videod_streaming_interface_t *stm = &_videod_streaming_itf[i];
-    tu_memclr(stm, ITF_STM_MEM_RESET_SIZE);
+    tu_memclr(stm, sizeof(videod_streaming_interface_t));
   }
 }
 
-void videod_reset(uint8_t rhport)
-{
+bool videod_deinit(void) {
+  return true;
+}
+
+void videod_reset(uint8_t rhport) {
   (void) rhport;
   for (uint_fast8_t i = 0; i < CFG_TUD_VIDEO; ++i) {
     videod_interface_t* ctl = &_videod_itf[i];
@@ -1123,12 +1273,11 @@ void videod_reset(uint8_t rhport)
   }
   for (uint_fast8_t i = 0; i < CFG_TUD_VIDEO_STREAMING; ++i) {
     videod_streaming_interface_t *stm = &_videod_streaming_itf[i];
-    tu_memclr(stm, ITF_STM_MEM_RESET_SIZE);
+    tu_memclr(stm, sizeof(videod_streaming_interface_t));
   }
 }
 
-uint16_t videod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len)
-{
+uint16_t videod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len) {
   TU_VERIFY((TUSB_CLASS_VIDEO       == itf_desc->bInterfaceClass) &&
             (VIDEO_SUBCLASS_CONTROL == itf_desc->bInterfaceSubClass) &&
             (VIDEO_ITF_PROTOCOL_15  == itf_desc->bInterfaceProtocol), 0);
@@ -1143,13 +1292,15 @@ uint16_t videod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uin
   }
   TU_ASSERT(ctl_idx < CFG_TUD_VIDEO, 0);
 
-  void const *end = (void const*)itf_desc + max_len;
-  self->beg = itf_desc;
+  uint8_t const *end = (uint8_t const*)itf_desc + max_len;
+  self->beg = (uint8_t const*) itf_desc;
   self->len = max_len;
+
   /*------------- Video Control Interface -------------*/
   TU_VERIFY(_open_vc_itf(rhport, self, 0), 0);
   tusb_desc_vc_itf_t const *vc = _get_desc_vc(self);
   uint_fast8_t bInCollection   = vc->ctl.bInCollection;
+
   /* Find the end of the video interface descriptor */
   void const *cur = _next_desc_itf(itf_desc, end);
   for (uint8_t stm_idx = 0; stm_idx < bInCollection; ++stm_idx) {
@@ -1167,6 +1318,34 @@ uint16_t videod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uin
     stm->desc.beg = (uint16_t) ((uintptr_t)cur - (uintptr_t)itf_desc);
     cur = _next_desc_itf(cur, end);
     stm->desc.end = (uint16_t) ((uintptr_t)cur - (uintptr_t)itf_desc);
+    stm->state = VS_STATE_PROBING;
+#ifdef TUP_DCD_EDPT_ISO_ALLOC
+    /* Allocate ISO endpoints */
+    uint16_t ep_size = 0;
+    uint8_t ep_addr = 0;
+    uint8_t const *p_desc = (uint8_t const*)itf_desc + stm->desc.beg;
+    uint8_t const *p_desc_end = (uint8_t const*)itf_desc + stm->desc.end;
+    while (p_desc < p_desc_end) {
+      if (tu_desc_type(p_desc) == TUSB_DESC_ENDPOINT) {
+        tusb_desc_endpoint_t const *desc_ep = (tusb_desc_endpoint_t const *) p_desc;
+        if (desc_ep->bmAttributes.xfer == TUSB_XFER_ISOCHRONOUS) {
+              ep_addr = desc_ep->bEndpointAddress;
+              ep_size = TU_MAX(tu_edpt_packet_size(desc_ep), ep_size);
+        }
+      }
+      p_desc = tu_desc_next(p_desc);
+    }
+    if(ep_addr > 0 && ep_size > 0) usbd_edpt_iso_alloc(rhport, ep_addr, ep_size);
+#endif
+    if (0 == stm_idx && 1 == bInCollection) {
+      /* If there is only one streaming interface and no alternate settings,
+       * host may not issue set_interface so open the streaming interface here. */
+      uint8_t const *sbeg = (uint8_t const*)itf_desc + stm->desc.beg;
+      uint8_t const *send = (uint8_t const*)itf_desc + stm->desc.end;
+      if (send == _find_desc_itf(sbeg, send, _desc_itfnum(sbeg), 1)) {
+        TU_VERIFY(_open_vs_itf(rhport, stm, 0), 0);
+      }
+    }
   }
   self->len = (uint16_t) ((uintptr_t)cur - (uintptr_t)itf_desc);
   return (uint16_t) ((uintptr_t)cur - (uintptr_t)itf_desc);
@@ -1175,12 +1354,10 @@ uint16_t videod_open(uint8_t rhport, tusb_desc_interface_t const * itf_desc, uin
 // Invoked when a control transfer occurred on an interface of this class
 // Driver response accordingly to the request and the transfer stage (setup/data/ack)
 // return false to stall control endpoint (e.g unsupported request)
-bool videod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
-{
+bool videod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request) {
   int err;
   TU_VERIFY(request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_INTERFACE);
   uint_fast8_t itfnum = tu_u16_low(request->wIndex);
-
   /* Identify which control interface to use */
   uint_fast8_t itf;
   for (itf = 0; itf < CFG_TUD_VIDEO; ++itf) {
@@ -1190,6 +1367,7 @@ bool videod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_
   }
 
   if (itf < CFG_TUD_VIDEO) {
+    TU_LOG_DRV("  VC[%d]: ", itf);
     err = handle_video_ctl_req(rhport, stage, request, itf);
     _videod_itf[itf].error_code = (uint8_t)err;
     if (err) return false;
@@ -1200,11 +1378,12 @@ bool videod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_
   for (itf = 0; itf < CFG_TUD_VIDEO_STREAMING; ++itf) {
     videod_streaming_interface_t *stm = &_videod_streaming_itf[itf];
     if (!stm->desc.beg) continue;
-    void const *desc = _videod_itf[stm->index_vc].beg;
+    uint8_t const *desc = _videod_itf[stm->index_vc].beg;
     if (itfnum == _desc_itfnum(desc + stm->desc.beg)) break;
   }
 
   if (itf < CFG_TUD_VIDEO_STREAMING) {
+    TU_LOG_DRV("  VS[%d]: ", itf);
     err = handle_video_stm_req(rhport, stage, request, itf);
     _videod_streaming_itf[itf].error_code = (uint8_t)err;
     if (err) return false;
@@ -1213,8 +1392,7 @@ bool videod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_
   return false;
 }
 
-bool videod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
-{
+bool videod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes) {
   (void)result; (void)xferred_bytes;
 
   /* find streaming handle */
@@ -1226,16 +1404,17 @@ bool videod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint3
     uint_fast16_t const ep_ofs = stm->desc.ep[0];
     if (!ep_ofs) continue;
     ctl = &_videod_itf[stm->index_vc];
-    void const *desc = ctl->beg;
+    uint8_t const *desc = ctl->beg;
     if (ep_addr == _desc_ep_addr(desc + ep_ofs)) break;
   }
-
   TU_ASSERT(itf < CFG_TUD_VIDEO_STREAMING);
+  videod_streaming_epbuf_t *stm_epbuf = &_videod_streaming_epbuf[itf];
+
   if (stm->offset < stm->bufsize) {
     /* Claim the endpoint */
     TU_VERIFY( usbd_edpt_claim(rhport, ep_addr), 0);
-    uint_fast16_t pkt_len = _prepare_in_payload(stm);
-    TU_ASSERT( usbd_edpt_xfer(rhport, ep_addr, stm->ep_buf, (uint16_t) pkt_len), 0);
+    uint_fast16_t pkt_len = _prepare_in_payload(stm, stm_epbuf->buf);
+    TU_ASSERT( usbd_edpt_xfer(rhport, ep_addr, stm_epbuf->buf, (uint16_t) pkt_len), 0);
   } else {
     stm->buffer  = NULL;
     stm->bufsize = 0;
diff --git a/src/class/video/video_device.h b/src/class/video/video_device.h
index ee2fcb9d5..648a221d5 100644
--- a/src/class/video/video_device.h
+++ b/src/class/video/video_device.h
@@ -40,6 +40,8 @@ extern "C" {
 // CFG_TUD_VIDEO > 1
 //--------------------------------------------------------------------+
 
+bool tud_video_n_connected(uint_fast8_t ctl_idx);
+
 /** Return true if streaming
  *
  * @param[in] ctl_idx    Destination control interface index
@@ -85,6 +87,7 @@ TU_ATTR_WEAK int tud_video_commit_cb(uint_fast8_t ctl_idx, uint_fast8_t stm_idx,
 // INTERNAL USBD-CLASS DRIVER API
 //--------------------------------------------------------------------+
 void     videod_init           (void);
+bool     videod_deinit         (void);
 void     videod_reset          (uint8_t rhport);
 uint16_t videod_open           (uint8_t rhport, tusb_desc_interface_t const * itf_desc, uint16_t max_len);
 bool     videod_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);