diff --git a/examples/make.mk b/examples/make.mk
index 95b2593b..fc974513 100644
--- a/examples/make.mk
+++ b/examples/make.mk
@@ -82,11 +82,16 @@ CFLAGS += \
 
 # Debugging/Optimization
 ifeq ($(DEBUG), 1)
-  CFLAGS += -Og -ggdb -DCFG_TUSB_DEBUG=2
+  CFLAGS += -Og -ggdb
 else
-ifneq ($(BOARD), spresense)
-  CFLAGS += -flto -Os
-else
-  CFLAGS += -Os
+  ifneq ($(BOARD),spresense)
+    CFLAGS += -flto -Os
+  else
+    CFLAGS += -Os
+  endif
 endif
+
+# TUSB Logging option
+ifneq ($(LOG),)
+  CFLAGS += -DCFG_TUSB_DEBUG=$(LOG)
 endif
diff --git a/hw/bsp/stm32f070rbnucleo/board.mk b/hw/bsp/stm32f070rbnucleo/board.mk
index 42394d1a..f930f6a8 100644
--- a/hw/bsp/stm32f070rbnucleo/board.mk
+++ b/hw/bsp/stm32f070rbnucleo/board.mk
@@ -24,8 +24,8 @@ SRC_C += \
 	$(ST_HAL_DRIVER)/Src/stm32f0xx_hal_cortex.c \
 	$(ST_HAL_DRIVER)/Src/stm32f0xx_hal_rcc.c \
 	$(ST_HAL_DRIVER)/Src/stm32f0xx_hal_rcc_ex.c \
-	$(ST_HAL_DRIVER)/Src/stm32f0xx_hal_gpio.c
-
+	$(ST_HAL_DRIVER)/Src/stm32f0xx_hal_gpio.c \
+	$(ST_HAL_DRIVER)/Src/stm32f0xx_hal_uart.c
 SRC_S += \
 	$(ST_CMSIS)/Source/Templates/gcc/startup_stm32f070xb.s
 
diff --git a/hw/bsp/stm32f070rbnucleo/stm32f070rbnucleo.c b/hw/bsp/stm32f070rbnucleo/stm32f070rbnucleo.c
index e51eda92..084d9a92 100644
--- a/hw/bsp/stm32f070rbnucleo/stm32f070rbnucleo.c
+++ b/hw/bsp/stm32f070rbnucleo/stm32f070rbnucleo.c
@@ -37,6 +37,24 @@
 #define BUTTON_PIN            GPIO_PIN_13
 #define BUTTON_STATE_ACTIVE   0
 
+#define UARTx                 USART2
+#define UART_GPIO_PORT        GPIOA
+#define UART_GPIO_AF          GPIO_AF1_USART2
+#define UART_TX_PIN           GPIO_PIN_2
+#define UART_RX_PIN           GPIO_PIN_3
+
+UART_HandleTypeDef UartHandle;
+
+ // enable all LED, Button, Uart, USB clock
+static void all_rcc_clk_enable(void)
+{
+  __HAL_RCC_GPIOA_CLK_ENABLE();  // USB D+, D-
+  __HAL_RCC_GPIOC_CLK_ENABLE();  // LED
+  //__HAL_RCC_GPIOA_CLK_ENABLE();  // Button
+  //__HAL_RCC_GPIOA_CLK_ENABLE();  // Uart tx, rx
+  __HAL_RCC_USART2_CLK_ENABLE(); // Uart module
+}
+
 void board_init(void)
 {
   #if CFG_TUSB_OS  == OPT_OS_NONE
@@ -74,8 +92,9 @@ void board_init(void)
   // Notify runtime of frequency change.
   SystemCoreClockUpdate();
 
+  all_rcc_clk_enable();
+
   // LED
-  __HAL_RCC_GPIOA_CLK_ENABLE();
   GPIO_InitTypeDef  GPIO_InitStruct;
   GPIO_InitStruct.Pin = LED_PIN;
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
@@ -84,13 +103,30 @@ void board_init(void)
   HAL_GPIO_Init(LED_PORT, &GPIO_InitStruct);
 
   // Button
-  __HAL_RCC_GPIOC_CLK_ENABLE();
   GPIO_InitStruct.Pin = BUTTON_PIN;
   GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
   GPIO_InitStruct.Pull = GPIO_PULLDOWN;
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
   HAL_GPIO_Init(BUTTON_PORT, &GPIO_InitStruct);
 
+  // Uart
+  GPIO_InitStruct.Pin       = UART_TX_PIN | UART_RX_PIN;
+  GPIO_InitStruct.Mode      = GPIO_MODE_AF_PP;
+  GPIO_InitStruct.Pull      = GPIO_PULLUP;
+  GPIO_InitStruct.Speed     = GPIO_SPEED_FREQ_HIGH;
+  GPIO_InitStruct.Alternate = UART_GPIO_AF;
+  HAL_GPIO_Init(UART_GPIO_PORT, &GPIO_InitStruct);
+
+  UartHandle.Instance        = UARTx;
+  UartHandle.Init.BaudRate   = CFG_BOARD_UART_BAUDRATE;
+  UartHandle.Init.WordLength = UART_WORDLENGTH_8B;
+  UartHandle.Init.StopBits   = UART_STOPBITS_1;
+  UartHandle.Init.Parity     = UART_PARITY_NONE;
+  UartHandle.Init.HwFlowCtl  = UART_HWCONTROL_NONE;
+  UartHandle.Init.Mode       = UART_MODE_TX_RX;
+  UartHandle.Init.OverSampling = UART_OVERSAMPLING_16;
+  HAL_UART_Init(&UartHandle);
+
   // Start USB clock
   __HAL_RCC_USB_CLK_ENABLE();
 }
@@ -117,8 +153,8 @@ int board_uart_read(uint8_t* buf, int len)
 
 int board_uart_write(void const * buf, int len)
 {
-  (void) buf; (void) len;
-  return 0;
+  HAL_UART_Transmit(&UartHandle, (uint8_t*) buf, len, 0xffff);
+  return len;
 }
 
 #if CFG_TUSB_OS  == OPT_OS_NONE
diff --git a/hw/bsp/stm32f070rbnucleo/stm32f0xx_hal_conf.h b/hw/bsp/stm32f070rbnucleo/stm32f0xx_hal_conf.h
index 7e3721af..45fa015f 100644
--- a/hw/bsp/stm32f070rbnucleo/stm32f0xx_hal_conf.h
+++ b/hw/bsp/stm32f070rbnucleo/stm32f0xx_hal_conf.h
@@ -66,8 +66,8 @@
 /*#define HAL_RTC_MODULE_ENABLED   */
 /*#define HAL_SPI_MODULE_ENABLED   */
 /*#define HAL_TIM_MODULE_ENABLED   */
-/*#define HAL_UART_MODULE_ENABLED   */
-#define HAL_USART_MODULE_ENABLED   
+#define HAL_UART_MODULE_ENABLED
+/*#define HAL_USART_MODULE_ENABLED   */
 /*#define HAL_IRDA_MODULE_ENABLED   */
 /*#define HAL_SMARTCARD_MODULE_ENABLED   */
 /*#define HAL_SMBUS_MODULE_ENABLED   */
diff --git a/hw/bsp/stm32f072disco/board.mk b/hw/bsp/stm32f072disco/board.mk
index 6529763e..39715e83 100644
--- a/hw/bsp/stm32f072disco/board.mk
+++ b/hw/bsp/stm32f072disco/board.mk
@@ -24,7 +24,8 @@ SRC_C += \
 	$(ST_HAL_DRIVER)/Src/stm32f0xx_hal_cortex.c \
 	$(ST_HAL_DRIVER)/Src/stm32f0xx_hal_rcc.c \
 	$(ST_HAL_DRIVER)/Src/stm32f0xx_hal_rcc_ex.c \
-	$(ST_HAL_DRIVER)/Src/stm32f0xx_hal_gpio.c
+	$(ST_HAL_DRIVER)/Src/stm32f0xx_hal_gpio.c \
+	$(ST_HAL_DRIVER)/Src/stm32f0xx_hal_uart.c
 
 SRC_S += \
 	$(ST_CMSIS)/Source/Templates/gcc/startup_stm32f072xb.s
diff --git a/hw/bsp/stm32f072disco/stm32f072disco.c b/hw/bsp/stm32f072disco/stm32f072disco.c
index 46cb0949..aac1c553 100644
--- a/hw/bsp/stm32f072disco/stm32f072disco.c
+++ b/hw/bsp/stm32f072disco/stm32f072disco.c
@@ -37,6 +37,24 @@
 #define BUTTON_PIN            GPIO_PIN_0
 #define BUTTON_STATE_ACTIVE   1
 
+#define UARTx                 USART1
+#define UART_GPIO_PORT        GPIOA
+#define UART_GPIO_AF          GPIO_AF1_USART1
+#define UART_TX_PIN           GPIO_PIN_9
+#define UART_RX_PIN           GPIO_PIN_10
+
+
+UART_HandleTypeDef UartHandle;
+
+// enable all LED, Button, Uart, USB clock
+static void all_rcc_clk_enable(void)
+{
+  __HAL_RCC_GPIOA_CLK_ENABLE();  // USB D+, D-
+  __HAL_RCC_GPIOC_CLK_ENABLE();  // LED
+  //__HAL_RCC_GPIOA_CLK_ENABLE();  // Button
+  //__HAL_RCC_GPIOA_CLK_ENABLE();  // Uart tx, rx
+  __HAL_RCC_USART1_CLK_ENABLE(); // Uart module
+}
 
 /**
   * @brief  System Clock Configuration
@@ -83,12 +101,11 @@ void board_init(void)
   #endif
 
   SystemClock_Config();
-  
-  // Notify runtime of frequency change.
   SystemCoreClockUpdate();
+  
+  all_rcc_clk_enable();
 
   // LED
-  __HAL_RCC_GPIOC_CLK_ENABLE();
   GPIO_InitTypeDef  GPIO_InitStruct;
   GPIO_InitStruct.Pin = LED_PIN;
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
@@ -97,16 +114,32 @@ void board_init(void)
   HAL_GPIO_Init(LED_PORT, &GPIO_InitStruct);
 
   // Button
-  __HAL_RCC_GPIOA_CLK_ENABLE();
   GPIO_InitStruct.Pin = BUTTON_PIN;
   GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
   GPIO_InitStruct.Pull = GPIO_PULLDOWN;
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
   HAL_GPIO_Init(BUTTON_PORT, &GPIO_InitStruct);
 
+  // Uart
+  GPIO_InitStruct.Pin       = UART_TX_PIN | UART_RX_PIN;
+  GPIO_InitStruct.Mode      = GPIO_MODE_AF_PP;
+  GPIO_InitStruct.Pull      = GPIO_PULLUP;
+  GPIO_InitStruct.Speed     = GPIO_SPEED_FREQ_HIGH;
+  GPIO_InitStruct.Alternate = UART_GPIO_AF;
+  HAL_GPIO_Init(UART_GPIO_PORT, &GPIO_InitStruct);
+
+  UartHandle.Instance        = UARTx;
+  UartHandle.Init.BaudRate   = CFG_BOARD_UART_BAUDRATE;
+  UartHandle.Init.WordLength = UART_WORDLENGTH_8B;
+  UartHandle.Init.StopBits   = UART_STOPBITS_1;
+  UartHandle.Init.Parity     = UART_PARITY_NONE;
+  UartHandle.Init.HwFlowCtl  = UART_HWCONTROL_NONE;
+  UartHandle.Init.Mode       = UART_MODE_TX_RX;
+  UartHandle.Init.OverSampling = UART_OVERSAMPLING_16;
+  HAL_UART_Init(&UartHandle);
+
   // USB Pins
   // Configure USB DM and DP pins. This is optional, and maintained only for user guidance.
-  __HAL_RCC_GPIOA_CLK_ENABLE();
   GPIO_InitStruct.Pin = (GPIO_PIN_11 | GPIO_PIN_12);
   GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
   GPIO_InitStruct.Pull = GPIO_NOPULL;
@@ -139,8 +172,8 @@ int board_uart_read(uint8_t* buf, int len)
 
 int board_uart_write(void const * buf, int len)
 {
-  (void) buf; (void) len;
-  return 0;
+  HAL_UART_Transmit(&UartHandle, (uint8_t*) buf, len, 0xffff);
+  return len;
 }
 
 #if CFG_TUSB_OS  == OPT_OS_NONE
@@ -170,7 +203,8 @@ void HardFault_Handler (void)
   * @retval None
   */
 void assert_failed(char *file, uint32_t line)
-{ 
+{
+  (void) file; (void) line;
   /* USER CODE BEGIN 6 */
   /* User can add his own implementation to report the file name and line number,
      tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
diff --git a/hw/bsp/stm32f072disco/stm32f0xx_hal_conf.h b/hw/bsp/stm32f072disco/stm32f0xx_hal_conf.h
index 7e3721af..d001f252 100644
--- a/hw/bsp/stm32f072disco/stm32f0xx_hal_conf.h
+++ b/hw/bsp/stm32f072disco/stm32f0xx_hal_conf.h
@@ -66,8 +66,8 @@
 /*#define HAL_RTC_MODULE_ENABLED   */
 /*#define HAL_SPI_MODULE_ENABLED   */
 /*#define HAL_TIM_MODULE_ENABLED   */
-/*#define HAL_UART_MODULE_ENABLED   */
-#define HAL_USART_MODULE_ENABLED   
+#define HAL_UART_MODULE_ENABLED
+//#define HAL_USART_MODULE_ENABLED
 /*#define HAL_IRDA_MODULE_ENABLED   */
 /*#define HAL_SMARTCARD_MODULE_ENABLED   */
 /*#define HAL_SMBUS_MODULE_ENABLED   */
diff --git a/src/class/usbtmc/usbtmc_device.c b/src/class/usbtmc/usbtmc_device.c
index c8e742fc..91642275 100644
--- a/src/class/usbtmc/usbtmc_device.c
+++ b/src/class/usbtmc/usbtmc_device.c
@@ -656,7 +656,7 @@ bool usbtmcd_control_request_cb(uint8_t rhport, tusb_control_request_t const * r
     TU_VERIFY(request->wLength == sizeof(rsp));
     TU_VERIFY(request->wIndex == usbtmc_state.ep_bulk_out);
     TU_VERIFY(tud_usbtmc_check_abort_bulk_out_cb(&rsp));
-    TU_VERIFY(usbd_edpt_xfer(rhport, 0u, (void*)&rsp,sizeof(rsp)));
+    TU_VERIFY(tud_control_xfer(rhport, request, (void*)&rsp,sizeof(rsp)));
     return true;
   }
 
diff --git a/src/device/dcd.h b/src/device/dcd.h
index c88465cb..d6e8b202 100644
--- a/src/device/dcd.h
+++ b/src/device/dcd.h
@@ -124,50 +124,20 @@ void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr);
 void dcd_control_status_complete(uint8_t rhport) TU_ATTR_WEAK;
 
 //--------------------------------------------------------------------+
-// Event API
+// Event API (Implemented by device stack)
 //--------------------------------------------------------------------+
 
 // Called by DCD to notify device stack
 extern void dcd_event_handler(dcd_event_t const * event, bool in_isr);
 
 // helper to send bus signal event
-static inline void dcd_event_bus_signal (uint8_t rhport, dcd_eventid_t eid, bool in_isr);
+extern void dcd_event_bus_signal (uint8_t rhport, dcd_eventid_t eid, bool in_isr);
 
 // helper to send setup received
-static inline void dcd_event_setup_received(uint8_t rhport, uint8_t const * setup, bool in_isr);
+extern void dcd_event_setup_received(uint8_t rhport, uint8_t const * setup, bool in_isr);
 
 // helper to send transfer complete event
-static inline void dcd_event_xfer_complete (uint8_t rhport, uint8_t ep_addr, uint32_t xferred_bytes, uint8_t result, bool in_isr);
-
-
-//--------------------------------------------------------------------+
-// Inline helper
-//--------------------------------------------------------------------+
-
-static inline void dcd_event_bus_signal (uint8_t rhport, dcd_eventid_t eid, bool in_isr)
-{
-  dcd_event_t event = { .rhport = rhport, .event_id = eid, };
-  dcd_event_handler(&event, in_isr);
-}
-
-static inline void dcd_event_setup_received(uint8_t rhport, uint8_t const * setup, bool in_isr)
-{
-  dcd_event_t event = { .rhport = rhport, .event_id = DCD_EVENT_SETUP_RECEIVED };
-  memcpy(&event.setup_received, setup, 8);
-
-  dcd_event_handler(&event, in_isr);
-}
-
-static inline void dcd_event_xfer_complete (uint8_t rhport, uint8_t ep_addr, uint32_t xferred_bytes, uint8_t result, bool in_isr)
-{
-  dcd_event_t event = { .rhport = rhport, .event_id = DCD_EVENT_XFER_COMPLETE };
-
-  event.xfer_complete.ep_addr = ep_addr;
-  event.xfer_complete.len     = xferred_bytes;
-  event.xfer_complete.result  = result;
-
-  dcd_event_handler(&event, in_isr);
-}
+extern void dcd_event_xfer_complete (uint8_t rhport, uint8_t ep_addr, uint32_t xferred_bytes, uint8_t result, bool in_isr);
 
 #ifdef __cplusplus
  }
diff --git a/src/device/usbd.c b/src/device/usbd.c
index 360881f3..85ec4acd 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -377,7 +377,7 @@ void tud_task (void)
 
       case DCD_EVENT_SETUP_RECEIVED:
         TU_LOG2("  ");
-        TU_LOG2_MEM(&event.setup_received, 1, 8);
+        TU_LOG1_MEM(&event.setup_received, 1, 8);
 
         // Mark as connected after receiving 1st setup packet.
         // But it is easier to set it every time instead of wasting time to check then set
@@ -386,6 +386,7 @@ void tud_task (void)
         // Process control request
         if ( !process_control_request(event.rhport, &event.setup_received) )
         {
+          TU_LOG1("  Stall EP0\r\n");
           // Failed -> stall both control endpoint IN and OUT
           dcd_edpt_stall(event.rhport, 0);
           dcd_edpt_stall(event.rhport, 0 | TUSB_DIR_IN_MASK);
@@ -405,6 +406,7 @@ void tud_task (void)
 
         if ( 0 == epnum )
         {
+          TU_LOG1("  EP Addr = 0x%02X, len = %ld\r\n", ep_addr, event.xfer_complete.len);
           usbd_control_xfer_cb(event.rhport, ep_addr, event.xfer_complete.result, event.xfer_complete.len);
         }
         else
@@ -589,7 +591,7 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
             // stall control endpoint if driver return false
             usbd_control_set_complete_callback(usbd_class_drivers[drvid].control_complete);
             TU_LOG2("  %s control request\r\n", _usbd_driver_str[drvid]);
-            TU_ASSERT(usbd_class_drivers[drvid].control_request != NULL &&
+            TU_VERIFY(usbd_class_drivers[drvid].control_request != NULL &&
                       usbd_class_drivers[drvid].control_request(rhport, p_request));
           break;
         }
@@ -599,7 +601,7 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
         // stall control endpoint if driver return false
         usbd_control_set_complete_callback(usbd_class_drivers[drvid].control_complete);
         TU_LOG2("  %s control request\r\n", _usbd_driver_str[drvid]);
-        TU_ASSERT(usbd_class_drivers[drvid].control_request != NULL &&
+        TU_VERIFY(usbd_class_drivers[drvid].control_request != NULL &&
                   usbd_class_drivers[drvid].control_request(rhport, p_request));
       }
     }
@@ -898,6 +900,31 @@ void dcd_event_handler(dcd_event_t const * event, bool in_isr)
   }
 }
 
+void dcd_event_bus_signal (uint8_t rhport, dcd_eventid_t eid, bool in_isr)
+{
+  dcd_event_t event = { .rhport = rhport, .event_id = eid, };
+  dcd_event_handler(&event, in_isr);
+}
+
+void dcd_event_setup_received(uint8_t rhport, uint8_t const * setup, bool in_isr)
+{
+  dcd_event_t event = { .rhport = rhport, .event_id = DCD_EVENT_SETUP_RECEIVED };
+  memcpy(&event.setup_received, setup, 8);
+
+  dcd_event_handler(&event, in_isr);
+}
+
+void dcd_event_xfer_complete (uint8_t rhport, uint8_t ep_addr, uint32_t xferred_bytes, uint8_t result, bool in_isr)
+{
+  dcd_event_t event = { .rhport = rhport, .event_id = DCD_EVENT_XFER_COMPLETE };
+
+  event.xfer_complete.ep_addr = ep_addr;
+  event.xfer_complete.len     = xferred_bytes;
+  event.xfer_complete.result  = result;
+
+  dcd_event_handler(&event, in_isr);
+}
+
 //--------------------------------------------------------------------+
 // Helper
 //--------------------------------------------------------------------+
diff --git a/src/device/usbd_control.c b/src/device/usbd_control.c
index d37e9ec2..fb16a5d3 100644
--- a/src/device/usbd_control.c
+++ b/src/device/usbd_control.c
@@ -1,179 +1,189 @@
-/*
- * The MIT License (MIT)
- *
- * 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
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- *
- * This file is part of the TinyUSB stack.
- */
-
-#include "tusb_option.h"
-
-#if TUSB_OPT_DEVICE_ENABLED
-
-#include "tusb.h"
-#include "device/usbd_pvt.h"
-#include "dcd.h"
-
-enum
-{
-  EDPT_CTRL_OUT = 0x00,
-  EDPT_CTRL_IN  = 0x80
-};
-
-typedef struct
-{
-  tusb_control_request_t request;
-
-  uint8_t* buffer;
-  uint16_t data_len;
-  uint16_t total_xferred;
-
-  bool (*complete_cb) (uint8_t, tusb_control_request_t const *);
-} usbd_control_xfer_t;
-
-static usbd_control_xfer_t _ctrl_xfer;
-
-CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN uint8_t _usbd_ctrl_buf[CFG_TUD_ENDPOINT0_SIZE];
-
-
-//--------------------------------------------------------------------+
-// Application API
-//--------------------------------------------------------------------+
-
-bool tud_control_status(uint8_t rhport, tusb_control_request_t const * request)
-{
-  // status direction is reversed to one in the setup packet
-  return dcd_edpt_xfer(rhport, request->bmRequestType_bit.direction ? EDPT_CTRL_OUT : EDPT_CTRL_IN, NULL, 0);
-}
-
-// Transfer an transaction in Data Stage
-// Each transaction has up to Endpoint0's max packet size.
-// This function can also transfer an zero-length packet
-static bool _data_stage_xact(uint8_t rhport)
-{
-  uint16_t const xact_len = tu_min16(_ctrl_xfer.data_len - _ctrl_xfer.total_xferred, CFG_TUD_ENDPOINT0_SIZE);
-
-  uint8_t ep_addr = EDPT_CTRL_OUT;
-
-  if ( _ctrl_xfer.request.bmRequestType_bit.direction == TUSB_DIR_IN )
-  {
-    ep_addr = EDPT_CTRL_IN;
-    if ( xact_len ) memcpy(_usbd_ctrl_buf, _ctrl_xfer.buffer, xact_len);
-  }
-
-  return dcd_edpt_xfer(rhport, ep_addr, xact_len ? _usbd_ctrl_buf : NULL, xact_len);
-}
-
-bool tud_control_xfer(uint8_t rhport, tusb_control_request_t const * request, void* buffer, uint16_t len)
-{
-  _ctrl_xfer.request       = (*request);
-  _ctrl_xfer.buffer        = (uint8_t*) buffer;
-  _ctrl_xfer.total_xferred = 0;
-  _ctrl_xfer.data_len      = tu_min16(len, request->wLength);
-
-  if ( _ctrl_xfer.data_len )
-  {
-    TU_ASSERT(buffer);
-
-    // Data stage
-    TU_ASSERT( _data_stage_xact(rhport) );
-  }else
-  {
-    // Status stage
-    TU_ASSERT( tud_control_status(rhport, request) );
-  }
-
-  return true;
-}
-
-//--------------------------------------------------------------------+
-// USBD API
-//--------------------------------------------------------------------+
-
-void usbd_control_reset (uint8_t rhport)
-{
-  (void) rhport;
-  tu_varclr(&_ctrl_xfer);
-}
-
-// TODO may find a better way
-void usbd_control_set_complete_callback( bool (*fp) (uint8_t, tusb_control_request_t const * ) )
-{
-  _ctrl_xfer.complete_cb = fp;
-}
-
-// callback when a transaction complete on DATA stage of control endpoint
-bool usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
-{
-  (void) result;
-
-  // Endpoint Address is opposite to direction bit, this is Status Stage complete event
-  if ( tu_edpt_dir(ep_addr) != _ctrl_xfer.request.bmRequestType_bit.direction )
-  {
-    TU_ASSERT(0 == xferred_bytes);
-    if (dcd_control_status_complete) dcd_control_status_complete(rhport);
-    return true;
-  }
-
-  if ( _ctrl_xfer.request.bmRequestType_bit.direction == TUSB_DIR_OUT )
-  {
-    TU_VERIFY(_ctrl_xfer.buffer);
-    memcpy(_ctrl_xfer.buffer, _usbd_ctrl_buf, xferred_bytes);
-  }
-
-  _ctrl_xfer.total_xferred += xferred_bytes;
-  _ctrl_xfer.buffer += xferred_bytes;
-
-  // Data Stage is complete when all request's length are transferred or
-  // a short packet is sent including zero-length packet.
-  if ( (_ctrl_xfer.request.wLength == _ctrl_xfer.total_xferred) || xferred_bytes < CFG_TUD_ENDPOINT0_SIZE )
-  {
-    // DATA stage is complete
-    bool is_ok = true;
-
-    // invoke complete callback if set
-    // callback can still stall control in status phase e.g out data does not make sense
-    if ( _ctrl_xfer.complete_cb )
-    {
-      is_ok = _ctrl_xfer.complete_cb(rhport, &_ctrl_xfer.request);
-    }
-
-    if ( is_ok )
-    {
-      // Send status
-      TU_ASSERT( tud_control_status(rhport, &_ctrl_xfer.request) );
-    }else
-    {
-      // Stall both IN and OUT control endpoint
-      dcd_edpt_stall(rhport, EDPT_CTRL_OUT);
-      dcd_edpt_stall(rhport, EDPT_CTRL_IN);
-    }
-  }
-  else
-  {
-    // More data to transfer
-    TU_ASSERT( _data_stage_xact(rhport) );
-  }
-
-  return true;
-}
-
-#endif
+/*
+ * The MIT License (MIT)
+ *
+ * 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
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * This file is part of the TinyUSB stack.
+ */
+
+#include "tusb_option.h"
+
+#if TUSB_OPT_DEVICE_ENABLED
+
+#include "tusb.h"
+#include "device/usbd_pvt.h"
+#include "dcd.h"
+
+enum
+{
+  EDPT_CTRL_OUT = 0x00,
+  EDPT_CTRL_IN  = 0x80
+};
+
+typedef struct
+{
+  tusb_control_request_t request;
+
+  uint8_t* buffer;
+  uint16_t data_len;
+  uint16_t total_xferred;
+
+  bool (*complete_cb) (uint8_t, tusb_control_request_t const *);
+} usbd_control_xfer_t;
+
+static usbd_control_xfer_t _ctrl_xfer;
+
+CFG_TUSB_MEM_SECTION CFG_TUSB_MEM_ALIGN static uint8_t _usbd_ctrl_buf[CFG_TUD_ENDPOINT0_SIZE];
+
+
+//--------------------------------------------------------------------+
+// Application API
+//--------------------------------------------------------------------+
+
+static inline bool _status_stage_xact(uint8_t rhport, tusb_control_request_t const * request)
+{
+  // status direction is reversed to one in the setup packet
+  return dcd_edpt_xfer(rhport, request->bmRequestType_bit.direction ? EDPT_CTRL_OUT : EDPT_CTRL_IN, NULL, 0);
+}
+
+bool tud_control_status(uint8_t rhport, tusb_control_request_t const * request)
+{
+  _ctrl_xfer.request       = (*request);
+  _ctrl_xfer.buffer        = NULL;
+  _ctrl_xfer.total_xferred = 0;
+  _ctrl_xfer.data_len      = 0;
+
+  return _status_stage_xact(rhport, request);
+}
+
+// Transfer an transaction in Data Stage
+// Each transaction has up to Endpoint0's max packet size.
+// This function can also transfer an zero-length packet
+static bool _data_stage_xact(uint8_t rhport)
+{
+  uint16_t const xact_len = tu_min16(_ctrl_xfer.data_len - _ctrl_xfer.total_xferred, CFG_TUD_ENDPOINT0_SIZE);
+
+  uint8_t ep_addr = EDPT_CTRL_OUT;
+
+  if ( _ctrl_xfer.request.bmRequestType_bit.direction == TUSB_DIR_IN )
+  {
+    ep_addr = EDPT_CTRL_IN;
+    if ( xact_len ) memcpy(_usbd_ctrl_buf, _ctrl_xfer.buffer, xact_len);
+  }
+
+  return dcd_edpt_xfer(rhport, ep_addr, xact_len ? _usbd_ctrl_buf : NULL, xact_len);
+}
+
+bool tud_control_xfer(uint8_t rhport, tusb_control_request_t const * request, void* buffer, uint16_t len)
+{
+  _ctrl_xfer.request       = (*request);
+  _ctrl_xfer.buffer        = (uint8_t*) buffer;
+  _ctrl_xfer.total_xferred = 0;
+  _ctrl_xfer.data_len      = tu_min16(len, request->wLength);
+
+  if ( _ctrl_xfer.data_len )
+  {
+    TU_ASSERT(buffer);
+
+    // Data stage
+    TU_ASSERT( _data_stage_xact(rhport) );
+  }else
+  {
+    // Status stage
+    TU_ASSERT( _status_stage_xact(rhport, request) );
+  }
+
+  return true;
+}
+
+//--------------------------------------------------------------------+
+// USBD API
+//--------------------------------------------------------------------+
+
+void usbd_control_reset (uint8_t rhport)
+{
+  (void) rhport;
+  tu_varclr(&_ctrl_xfer);
+}
+
+// TODO may find a better way
+void usbd_control_set_complete_callback( bool (*fp) (uint8_t, tusb_control_request_t const * ) )
+{
+  _ctrl_xfer.complete_cb = fp;
+}
+
+// callback when a transaction complete on DATA stage of control endpoint
+bool usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint32_t xferred_bytes)
+{
+  (void) result;
+
+  // Endpoint Address is opposite to direction bit, this is Status Stage complete event
+  if ( tu_edpt_dir(ep_addr) != _ctrl_xfer.request.bmRequestType_bit.direction )
+  {
+    TU_ASSERT(0 == xferred_bytes);
+    if (dcd_control_status_complete) dcd_control_status_complete(rhport);
+    return true;
+  }
+
+  if ( _ctrl_xfer.request.bmRequestType_bit.direction == TUSB_DIR_OUT )
+  {
+    TU_VERIFY(_ctrl_xfer.buffer);
+    memcpy(_ctrl_xfer.buffer, _usbd_ctrl_buf, xferred_bytes);
+  }
+
+  _ctrl_xfer.total_xferred += xferred_bytes;
+  _ctrl_xfer.buffer += xferred_bytes;
+
+  // Data Stage is complete when all request's length are transferred or
+  // a short packet is sent including zero-length packet.
+  if ( (_ctrl_xfer.request.wLength == _ctrl_xfer.total_xferred) || xferred_bytes < CFG_TUD_ENDPOINT0_SIZE )
+  {
+    // DATA stage is complete
+    bool is_ok = true;
+
+    // invoke complete callback if set
+    // callback can still stall control in status phase e.g out data does not make sense
+    if ( _ctrl_xfer.complete_cb )
+    {
+      is_ok = _ctrl_xfer.complete_cb(rhport, &_ctrl_xfer.request);
+    }
+
+    if ( is_ok )
+    {
+      // Send status
+      TU_ASSERT( _status_stage_xact(rhport, &_ctrl_xfer.request) );
+    }else
+    {
+      // Stall both IN and OUT control endpoint
+      dcd_edpt_stall(rhport, EDPT_CTRL_OUT);
+      dcd_edpt_stall(rhport, EDPT_CTRL_IN);
+    }
+  }
+  else
+  {
+    // More data to transfer
+    TU_ASSERT( _data_stage_xact(rhport) );
+  }
+
+  return true;
+}
+
+#endif
diff --git a/src/osal/osal_none.h b/src/osal/osal_none.h
index fa581215..5868dfb3 100644
--- a/src/osal/osal_none.h
+++ b/src/osal/osal_none.h
@@ -198,6 +198,8 @@ static inline bool osal_queue_send(osal_queue_t const qhdl, void const * data, b
     _osal_q_unlock(qhdl);
   }
 
+  TU_ASSERT(success);
+
   return success;
 }