From 215f8603b14cc29e08d8a25e08f8eca44ed90826 Mon Sep 17 00:00:00 2001
From: hathach <thach@tinyusb.org>
Date: Fri, 16 Nov 2018 21:56:39 +0700
Subject: [PATCH] nrf5x: refactor device control transfer.

- make control transfer as part of usbd. Class driver must use
usbd_control_ API() instead of dcd_ api.
- change the signature of class driver's control_request
- allow control request complete to stall in staus stage
- move control request parser & handling to usbd.
---
 src/class/cdc/cdc_device.c       |  88 +++++-----
 src/class/cdc/cdc_device.h       |   4 +-
 src/class/custom/custom_device.c |   4 +-
 src/class/custom/custom_device.h |   4 +-
 src/class/hid/hid_device.c       | 143 ++++++++--------
 src/class/hid/hid_device.h       |   5 +-
 src/class/msc/msc_device.c       |  71 +++++---
 src/class/msc/msc_device.h       |  30 +---
 src/common/tusb_types.h          |   6 +-
 src/device/control.c             | 283 +++++++++++--------------------
 src/device/control.h             |  38 -----
 src/device/dcd.h                 |   4 +
 src/device/usbd.c                | 242 ++++++++++++++++++++------
 src/device/usbd_pvt.h            |  17 +-
 src/tusb_option.h                |   4 +-
 15 files changed, 491 insertions(+), 452 deletions(-)

diff --git a/src/class/cdc/cdc_device.c b/src/class/cdc/cdc_device.c
index bc16699f1..d8ade9c7f 100644
--- a/src/class/cdc/cdc_device.c
+++ b/src/class/cdc/cdc_device.c
@@ -45,7 +45,6 @@
 // INCLUDE
 //--------------------------------------------------------------------+
 #include "cdc_device.h"
-#include "device/control.h"
 #include "device/usbd_pvt.h"
 
 //--------------------------------------------------------------------+
@@ -63,7 +62,7 @@ typedef struct
 
   /*------------- From this point, data is not cleared by bus reset -------------*/
   char    wanted_char;
-  CFG_TUSB_MEM_ALIGN cdc_line_coding_t line_coding;
+  cdc_line_coding_t line_coding;
 
   // FIFO
   tu_fifo_t rx_ff;
@@ -199,23 +198,23 @@ void cdcd_init(void)
 
   for(uint8_t i=0; i<CFG_TUD_CDC; i++)
   {
-    cdcd_interface_t* ser = &_cdcd_itf[i];
+    cdcd_interface_t* p_cdc = &_cdcd_itf[i];
 
-    ser->wanted_char = -1;
+    p_cdc->wanted_char = -1;
 
     // default line coding is : stop bit = 1, parity = none, data bits = 8
-    ser->line_coding.bit_rate = 115200;
-    ser->line_coding.stop_bits = 0;
-    ser->line_coding.parity    = 0;
-    ser->line_coding.data_bits = 8;
+    p_cdc->line_coding.bit_rate = 115200;
+    p_cdc->line_coding.stop_bits = 0;
+    p_cdc->line_coding.parity    = 0;
+    p_cdc->line_coding.data_bits = 8;
 
     // config fifo
-    tu_fifo_config(&ser->rx_ff, ser->rx_ff_buf, CFG_TUD_CDC_RX_BUFSIZE, 1, true);
-    tu_fifo_config(&ser->tx_ff, ser->tx_ff_buf, CFG_TUD_CDC_TX_BUFSIZE, 1, false);
+    tu_fifo_config(&p_cdc->rx_ff, p_cdc->rx_ff_buf, CFG_TUD_CDC_RX_BUFSIZE, 1, true);
+    tu_fifo_config(&p_cdc->tx_ff, p_cdc->tx_ff_buf, CFG_TUD_CDC_TX_BUFSIZE, 1, false);
 
 #if CFG_FIFO_MUTEX
-    tu_fifo_config_mutex(&ser->rx_ff, osal_mutex_create(&ser->rx_ff_mutex));
-    tu_fifo_config_mutex(&ser->tx_ff, osal_mutex_create(&ser->tx_ff_mutex));
+    tu_fifo_config_mutex(&p_cdc->rx_ff, osal_mutex_create(&p_cdc->rx_ff_mutex));
+    tu_fifo_config_mutex(&p_cdc->tx_ff, osal_mutex_create(&p_cdc->tx_ff_mutex));
 #endif
   }
 }
@@ -299,57 +298,64 @@ tusb_error_t cdcd_open(uint8_t rhport, tusb_desc_interface_t const * p_interface
   return TUSB_ERROR_NONE;
 }
 
-void cdcd_control_request_complete(uint8_t rhport, tusb_control_request_t const * p_request)
+// Invoked when class request DATA stage is finished.
+// return false to stall control endpoint (e.g Host send non-sense DATA)
+bool cdcd_control_request_complete(uint8_t rhport, tusb_control_request_t const * request)
 {
   //------------- Class Specific Request -------------//
-  if (p_request->bmRequestType_bit.type != TUSB_REQ_TYPE_CLASS) return;
+  TU_VERIFY (request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS);
 
   // TODO Support multiple interfaces
   uint8_t const itf = 0;
   cdcd_interface_t* p_cdc = &_cdcd_itf[itf];
 
   // Invoke callback
-  if (CDC_REQUEST_SET_LINE_CODING == p_request->bRequest) {
+  if ( CDC_REQUEST_SET_LINE_CODING == request->bRequest )
+  {
     if ( tud_cdc_line_coding_cb ) tud_cdc_line_coding_cb(itf, &p_cdc->line_coding);
   }
+
+  return true;
 }
 
-tusb_error_t cdcd_control_request(uint8_t rhport, tusb_control_request_t const * p_request, uint16_t bytes_already_sent)
+// Handle class control request
+// return false to stall control endpoint (e.g unsupported request)
+bool cdcd_control_request(uint8_t rhport, tusb_control_request_t const * request)
 {
   //------------- Class Specific Request -------------//
-  if (p_request->bmRequestType_bit.type != TUSB_REQ_TYPE_CLASS) return TUSB_ERROR_DCD_CONTROL_REQUEST_NOT_SUPPORT;
+  TU_ASSERT(request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS);
 
   // TODO Support multiple interfaces
   uint8_t const itf = 0;
   cdcd_interface_t* p_cdc = &_cdcd_itf[itf];
 
-  if ((CDC_REQUEST_SET_LINE_CODING == p_request->bRequest) )
+  switch ( request->bRequest )
   {
-    uint16_t len = tu_min16(sizeof(cdc_line_coding_t), p_request->wLength);
-    dcd_edpt_xfer(rhport, 0, (uint8_t*) &p_cdc->line_coding, len);
-  }
-  else if ( (CDC_REQUEST_GET_LINE_CODING == p_request->bRequest))
-  {
-    uint16_t len = tu_min16(sizeof(cdc_line_coding_t), p_request->wLength);
-    dcd_edpt_xfer(rhport, TUSB_DIR_IN_MASK, (uint8_t*) &p_cdc->line_coding, len);
-  }
-  else if (CDC_REQUEST_SET_CONTROL_LINE_STATE == p_request->bRequest )
-  {
-    // 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)
-    // Bit 1: Carrier control for half-duplex modems.
-    //        This signal corresponds to V.24 signal 105 and RS-232 signal RTS (Request to Send)
-    p_cdc->line_state = (uint8_t) p_request->wValue;
+    case CDC_REQUEST_SET_LINE_CODING:
+      usbd_control_xfer(rhport, request, &p_cdc->line_coding, sizeof(cdc_line_coding_t));
+    break;
 
-    // Invoke callback
-    if ( tud_cdc_line_state_cb) tud_cdc_line_state_cb(itf, BIT_TEST_(p_request->wValue, 0), BIT_TEST_(p_request->wValue, 1));
+    case CDC_REQUEST_GET_LINE_CODING:
+      usbd_control_xfer(rhport, request, &p_cdc->line_coding, sizeof(cdc_line_coding_t));
+    break;
+
+    case CDC_REQUEST_SET_CONTROL_LINE_STATE:
+      // 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)
+      // Bit 1: Carrier control for half-duplex modems.
+      //        This signal corresponds to V.24 signal 105 and RS-232 signal RTS (Request to Send)
+      p_cdc->line_state = (uint8_t) request->wValue;
+
+      // Invoke callback
+      if ( tud_cdc_line_state_cb) tud_cdc_line_state_cb(itf, BIT_TEST_(request->wValue, 0), BIT_TEST_(request->wValue, 1));
+      usbd_control_status(rhport, request);
+    break;
+
+    default: return false; // stall unsupported request
   }
-  else
-  {
-    return TUSB_ERROR_FAILED; // stall unsupported request
-  }
-  return TUSB_ERROR_NONE;
+
+  return true;
 }
 
 tusb_error_t cdcd_xfer_cb(uint8_t rhport, uint8_t ep_addr, tusb_event_t event, uint32_t xferred_bytes)
diff --git a/src/class/cdc/cdc_device.h b/src/class/cdc/cdc_device.h
index 9d0b93c6d..749351a54 100644
--- a/src/class/cdc/cdc_device.h
+++ b/src/class/cdc/cdc_device.h
@@ -114,8 +114,8 @@ ATTR_WEAK void tud_cdc_line_coding_cb(uint8_t itf, cdc_line_coding_t const* p_li
 
 void         cdcd_init               (void);
 tusb_error_t cdcd_open               (uint8_t rhport, tusb_desc_interface_t const * p_interface_desc, uint16_t *p_length);
-tusb_error_t cdcd_control_request (uint8_t rhport, tusb_control_request_t const * p_request, uint16_t bytes_already_sent);
-void cdcd_control_request_complete (uint8_t rhport, tusb_control_request_t const * p_request);
+bool cdcd_control_request (uint8_t rhport, tusb_control_request_t const * p_request);
+bool cdcd_control_request_complete (uint8_t rhport, tusb_control_request_t const * p_request);
 tusb_error_t cdcd_xfer_cb            (uint8_t rhport, uint8_t edpt_addr, tusb_event_t event, uint32_t xferred_bytes);
 void         cdcd_reset              (uint8_t rhport);
 
diff --git a/src/class/custom/custom_device.c b/src/class/custom/custom_device.c
index 1b36f1331..cc440fc7e 100644
--- a/src/class/custom/custom_device.c
+++ b/src/class/custom/custom_device.c
@@ -89,9 +89,9 @@ tusb_error_t cusd_open(uint8_t rhport, tusb_desc_interface_t const * p_desc_itf,
   return TUSB_ERROR_NONE;
 }
 
-tusb_error_t cusd_control_request(uint8_t rhport, tusb_control_request_t const * p_request)
+bool cusd_control_request(uint8_t rhport, tusb_control_request_t const * p_request)
 {
-  return TUSB_ERROR_DCD_CONTROL_REQUEST_NOT_SUPPORT;
+  return false;
 }
 
 tusb_error_t cusd_xfer_cb(uint8_t rhport, uint8_t edpt_addr, tusb_event_t event, uint32_t xferred_bytes)
diff --git a/src/class/custom/custom_device.h b/src/class/custom/custom_device.h
index 116c6897d..704c4127b 100644
--- a/src/class/custom/custom_device.h
+++ b/src/class/custom/custom_device.h
@@ -64,8 +64,8 @@
 
 void cusd_init(void);
 tusb_error_t cusd_open(uint8_t rhport, tusb_desc_interface_t const * p_interface_desc, uint16_t *p_length);
-tusb_error_t cusd_control_request_st(uint8_t rhport, tusb_control_request_t const * p_request);
-void cusd_control_request_complete (uint8_t rhport, tusb_control_request_t const * p_request);
+bool cusd_control_request_st(uint8_t rhport, tusb_control_request_t const * p_request);
+bool cusd_control_request_complete (uint8_t rhport, tusb_control_request_t const * p_request);
 tusb_error_t cusd_xfer_cb(uint8_t rhport, uint8_t edpt_addr, tusb_event_t event, uint32_t xferred_bytes);
 void cusd_reset(uint8_t rhport);
 #endif
diff --git a/src/class/hid/hid_device.c b/src/class/hid/hid_device.c
index efa602ad6..b5586a488 100644
--- a/src/class/hid/hid_device.c
+++ b/src/class/hid/hid_device.c
@@ -46,7 +46,6 @@
 //--------------------------------------------------------------------+
 #include "common/tusb_common.h"
 #include "hid_device.h"
-#include "device/control.h"
 #include "device/usbd_pvt.h"
 
 //--------------------------------------------------------------------+
@@ -403,110 +402,112 @@ tusb_error_t hidd_open(uint8_t rhport, tusb_desc_interface_t const * desc_itf, u
   return TUSB_ERROR_NONE;
 }
 
-tusb_error_t hidd_control_request(uint8_t rhport, tusb_control_request_t const * p_request, uint16_t bytes_already_sent)
+// Handle class control request
+// return false to stall control endpoint (e.g unsupported request)
+bool hidd_control_request(uint8_t rhport, tusb_control_request_t const * p_request)
 {
   hidd_interface_t* p_hid = get_interface_by_itfnum( (uint8_t) p_request->wIndex );
-  TU_ASSERT(p_hid, TUSB_ERROR_FAILED);
+  TU_ASSERT(p_hid);
 
-  //------------- STD Request -------------//
   if (p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD)
   {
+    //------------- STD Request -------------//
     uint8_t const desc_type  = tu_u16_high(p_request->wValue);
     uint8_t const desc_index = tu_u16_low (p_request->wValue);
     (void) desc_index;
 
     if (p_request->bRequest == TUSB_REQ_GET_DESCRIPTOR && desc_type == HID_DESC_TYPE_REPORT)
     {
-      // TODO: Handle zero length packet.
-      uint16_t remaining_bytes = p_hid->desc_len - bytes_already_sent;
-      if (remaining_bytes > 64) {
-          remaining_bytes = 64;
-      }
-      memcpy(_shared_control_buffer, p_hid->desc_report + bytes_already_sent, remaining_bytes);
-
-      dcd_edpt_xfer(rhport, TUSB_DIR_IN_MASK, _shared_control_buffer, remaining_bytes);
+      usbd_control_xfer(rhport, p_request, p_hid->desc_report, p_hid->desc_len);
     }else
     {
-      return TUSB_ERROR_FAILED;
+      return false; // stall unsupported request
     }
   }
-  //------------- Class Specific Request -------------//
   else if (p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS)
   {
-    if( HID_REQ_CONTROL_GET_REPORT == p_request->bRequest )
+    //------------- Class Specific Request -------------//
+    switch( p_request->bRequest )
     {
-      // wValue = Report Type | Report ID
-      uint8_t const report_type = tu_u16_high(p_request->wValue);
-      uint8_t const report_id   = tu_u16_low(p_request->wValue);
+      case HID_REQ_CONTROL_GET_REPORT:
+      {
+        // wValue = Report Type | Report ID
+        uint8_t const report_type = tu_u16_high(p_request->wValue);
+        uint8_t const report_id   = tu_u16_low(p_request->wValue);
 
-      uint16_t xferlen;
-      if ( p_hid->get_report_cb )
-      {
-        xferlen = p_hid->get_report_cb(report_id, (hid_report_type_t) report_type, p_hid->report_buf, p_request->wLength);
-      }else
-      {
-        // For boot Interface only: re-use report_buf -> report has no change
-        xferlen = p_request->wLength;
+        uint16_t xferlen;
+        if ( p_hid->get_report_cb )
+        {
+          xferlen = p_hid->get_report_cb(report_id, (hid_report_type_t) report_type, p_hid->report_buf, p_request->wLength);
+        }else
+        {
+          // For boot Interface only: re-use report_buf -> report has no change
+          xferlen = p_request->wLength;
+        }
+
+        TU_ASSERT( xferlen > 0 );
+        usbd_control_xfer(rhport, p_request, p_hid->report_buf, xferlen);
       }
+      break;
 
-      TU_ASSERT( xferlen > 0 );
-      dcd_edpt_xfer(rhport, TUSB_DIR_IN_MASK, _shared_control_buffer, xferlen);
-    }
-    else if ( HID_REQ_CONTROL_SET_REPORT == p_request->bRequest )
-    {
-      dcd_edpt_xfer(rhport, 0, _shared_control_buffer, p_request->wLength);
-    }
-    else if (HID_REQ_CONTROL_SET_IDLE == p_request->bRequest)
-    {
-      // TODO idle rate of report
-      p_hid->idle_rate = tu_u16_high(p_request->wValue);
-    }
-    else if (HID_REQ_CONTROL_GET_IDLE == p_request->bRequest)
-    {
-      // TODO idle rate of report
-      _shared_control_buffer[0] = p_hid->idle_rate;
-      dcd_edpt_xfer(rhport, TUSB_DIR_IN_MASK, _shared_control_buffer, 1);
-    }
-    else if (HID_REQ_CONTROL_GET_PROTOCOL == p_request->bRequest )
-    {
-      _shared_control_buffer[0] = 1-p_hid->boot_protocol;   // 0 is Boot, 1 is Report protocol
-      dcd_edpt_xfer(rhport, TUSB_DIR_IN_MASK, _shared_control_buffer, 1);
-    }
-    else if (HID_REQ_CONTROL_SET_PROTOCOL == p_request->bRequest )
-    {
-      p_hid->boot_protocol = 1 - p_request->wValue; // 0 is Boot, 1 is Report protocol
-    }else
-    {
-      return TUSB_ERROR_FAILED;
+      case  HID_REQ_CONTROL_SET_REPORT:
+        usbd_control_xfer(rhport, p_request, p_hid->report_buf, p_request->wLength);
+      break;
+
+      case HID_REQ_CONTROL_SET_IDLE:
+        // TODO idle rate of report
+        p_hid->idle_rate = tu_u16_high(p_request->wValue);
+        usbd_control_status(rhport, p_request);
+      break;
+
+      case HID_REQ_CONTROL_GET_IDLE:
+        // TODO idle rate of report
+        usbd_control_xfer(rhport, p_request, &p_hid->idle_rate, 1);
+      break;
+
+      case HID_REQ_CONTROL_GET_PROTOCOL:
+      {
+        uint8_t protocol = 1-p_hid->boot_protocol;   // 0 is Boot, 1 is Report protocol
+        usbd_control_xfer(rhport, p_request, &protocol, 1);
+      }
+      break;
+
+      case HID_REQ_CONTROL_SET_PROTOCOL:
+        p_hid->boot_protocol = 1 - p_request->wValue; // 0 is Boot, 1 is Report protocol
+        usbd_control_status(rhport, p_request);
+      break;
+
+      default: return false; // stall unsupported request
     }
   }else
   {
-    return TUSB_ERROR_FAILED;
+    return false; // stall unsupported request
   }
-  return TUSB_ERROR_NONE;
+
+  return true;
 }
 
-void hidd_control_request_complete(uint8_t rhport, tusb_control_request_t const * p_request)
+// Invoked when class request DATA stage is finished.
+// return false to stall control endpoint (e.g Host send non-sense DATA)
+bool hidd_control_request_complete(uint8_t rhport, tusb_control_request_t const * p_request)
 {
   hidd_interface_t* p_hid = get_interface_by_itfnum( (uint8_t) p_request->wIndex );
-  if (p_hid == NULL) {
-      return;
-  }
+  TU_ASSERT(p_hid);
 
-  if (p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS)
+  if (p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS &&
+      p_request->bRequest == HID_REQ_CONTROL_SET_REPORT)
   {
-    if ( HID_REQ_CONTROL_SET_REPORT == p_request->bRequest )
-    {
-      // wValue = Report Type | Report ID
-      uint8_t const report_type = tu_u16_high(p_request->wValue);
-      uint8_t const report_id   = tu_u16_low(p_request->wValue);
+    // wValue = Report Type | Report ID
+    uint8_t const report_type = tu_u16_high(p_request->wValue);
+    uint8_t const report_id   = tu_u16_low(p_request->wValue);
 
-      if ( p_hid->set_report_cb )
-      {
-        p_hid->set_report_cb(report_id, (hid_report_type_t) report_type, _shared_control_buffer, p_request->wLength);
-      }
+    if ( p_hid->set_report_cb )
+    {
+      p_hid->set_report_cb(report_id, (hid_report_type_t) report_type, p_hid->report_buf, p_request->wLength);
     }
   }
+
+  return true;
 }
 
 tusb_error_t hidd_xfer_cb(uint8_t rhport, uint8_t edpt_addr, tusb_event_t event, uint32_t xferred_bytes)
diff --git a/src/class/hid/hid_device.h b/src/class/hid/hid_device.h
index 1dc40af57..7f8cd0fa7 100644
--- a/src/class/hid/hid_device.h
+++ b/src/class/hid/hid_device.h
@@ -378,8 +378,8 @@ ATTR_WEAK void tud_hid_mouse_set_report_cb(uint8_t report_id, hid_report_type_t
 
 void hidd_init(void);
 tusb_error_t hidd_open(uint8_t rhport, tusb_desc_interface_t const * p_interface_desc, uint16_t *p_length);
-tusb_error_t hidd_control_request(uint8_t rhport, tusb_control_request_t const * p_request, uint16_t bytes_already_sent);
-void hidd_control_request_complete (uint8_t rhport, tusb_control_request_t const * p_request);
+bool hidd_control_request(uint8_t rhport, tusb_control_request_t const * p_request);
+bool hidd_control_request_complete (uint8_t rhport, tusb_control_request_t const * p_request);
 tusb_error_t hidd_xfer_cb(uint8_t rhport, uint8_t edpt_addr, tusb_event_t event, uint32_t xferred_bytes);
 void hidd_reset(uint8_t rhport);
 
@@ -390,3 +390,4 @@ void hidd_reset(uint8_t rhport);
 #endif
 
 #endif /* _TUSB_HID_DEVICE_H_ */
+
diff --git a/src/class/msc/msc_device.c b/src/class/msc/msc_device.c
index 32ad90102..92bc4c6c8 100644
--- a/src/class/msc/msc_device.c
+++ b/src/class/msc/msc_device.c
@@ -47,7 +47,6 @@
 
 #include "common/tusb_common.h"
 #include "msc_device.h"
-#include "device/control.h"
 #include "device/usbd_pvt.h"
 
 //--------------------------------------------------------------------+
@@ -60,7 +59,31 @@ enum
   MSC_STAGE_STATUS
 };
 
-CFG_TUSB_ATTR_USBRAM CFG_TUSB_MEM_ALIGN mscd_interface_t _mscd_itf;
+typedef struct {
+  CFG_TUSB_MEM_ALIGN msc_cbw_t  cbw;
+
+//#if defined (__ICCARM__) && (CFG_TUSB_MCU == OPT_MCU_LPC11UXX || CFG_TUSB_MCU == OPT_MCU_LPC13UXX)
+//  uint8_t padding1[64-sizeof(msc_cbw_t)]; // IAR cannot align struct's member
+//#endif
+
+  CFG_TUSB_MEM_ALIGN msc_csw_t csw;
+
+  uint8_t  itf_num;
+  uint8_t  ep_in;
+  uint8_t  ep_out;
+
+  // Bulk Only Transfer (BOT) Protocol
+  uint8_t  stage;
+  uint32_t total_len;
+  uint32_t xferred_len; // numbered of bytes transferred so far in the Data Stage
+
+  // Sense Response Data
+  uint8_t sense_key;
+  uint8_t add_sense_code;
+  uint8_t add_sense_qualifier;
+}mscd_interface_t;
+
+CFG_TUSB_ATTR_USBRAM CFG_TUSB_MEM_ALIGN static mscd_interface_t _mscd_itf;
 CFG_TUSB_ATTR_USBRAM CFG_TUSB_MEM_ALIGN static uint8_t _mscd_buf[CFG_TUD_MSC_BUFSIZE];
 
 //--------------------------------------------------------------------+
@@ -147,29 +170,39 @@ tusb_error_t mscd_open(uint8_t rhport, tusb_desc_interface_t const * p_desc_itf,
   return TUSB_ERROR_NONE;
 }
 
-tusb_error_t mscd_control_request(uint8_t rhport, tusb_control_request_t const * p_request, uint16_t bytes_already_sent)
+// Handle class control request
+// return false to stall control endpoint (e.g unsupported request)
+bool mscd_control_request(uint8_t rhport, tusb_control_request_t const * p_request)
 {
-  TU_ASSERT(p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS, TUSB_ERROR_DCD_CONTROL_REQUEST_NOT_SUPPORT);
+  TU_ASSERT(p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS);
 
-  if(MSC_REQ_RESET == p_request->bRequest)
+  switch ( p_request->bRequest )
   {
-    // TODO: Actually reset.
+    case MSC_REQ_RESET:
+      // TODO: Actually reset interface.
+      usbd_control_status(rhport, p_request);
+    break;
+
+    case MSC_REQ_GET_MAX_LUN:
+    {
+      // returned MAX LUN is minus 1 by specs
+      uint8_t maxlun = CFG_TUD_MSC_MAXLUN-1;
+      usbd_control_xfer(rhport, p_request, &maxlun, 1);
+    }
+    break;
+
+    default: return false; // stall unsupported request
   }
-  else if (MSC_REQ_GET_MAX_LUN == p_request->bRequest)
-  {
-    // returned MAX LUN is minus 1 by specs
-    _shared_control_buffer[0] = CFG_TUD_MSC_MAXLUN-1;
-    dcd_edpt_xfer(rhport, TUSB_DIR_IN_MASK, _shared_control_buffer, 1);
-  }else
-  {
-    return TUSB_ERROR_FAILED; // stall unsupported request
-  }
-  return TUSB_ERROR_NONE;
+
+  return true;
 }
 
-void mscd_control_request_complete(uint8_t rhport, tusb_control_request_t const * p_request)
+// Invoked when class request DATA stage is finished.
+// return false to stall control endpoint (e.g Host send non-sense DATA)
+bool mscd_control_request_complete(uint8_t rhport, tusb_control_request_t const * p_request)
 {
-  return;
+  // nothing to do
+  return true;
 }
 
 // For backwards compatibility we support static block counts.
@@ -296,7 +329,7 @@ int32_t proc_builtin_scsi(msc_cbw_t const * p_cbw, uint8_t* buffer, uint32_t buf
   return ret;
 }
 
-tusb_error_t mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, uint8_t event, uint32_t xferred_bytes)
+tusb_error_t mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, tusb_event_t event, uint32_t xferred_bytes)
 {
   mscd_interface_t* p_msc = &_mscd_itf;
   msc_cbw_t const * p_cbw = &p_msc->cbw;
diff --git a/src/class/msc/msc_device.h b/src/class/msc/msc_device.h
index ac3eade22..d08d9f9cb 100644
--- a/src/class/msc/msc_device.h
+++ b/src/class/msc/msc_device.h
@@ -81,32 +81,6 @@ TU_VERIFY_STATIC(CFG_TUD_MSC_BUFSIZE < UINT16_MAX, "Size is not correct");
  extern "C" {
 #endif
 
-typedef struct {
-  CFG_TUSB_MEM_ALIGN msc_cbw_t  cbw;
-
-//#if defined (__ICCARM__) && (CFG_TUSB_MCU == OPT_MCU_LPC11UXX || CFG_TUSB_MCU == OPT_MCU_LPC13UXX)
-//  uint8_t padding1[64-sizeof(msc_cbw_t)]; // IAR cannot align struct's member
-//#endif
-
-  CFG_TUSB_MEM_ALIGN msc_csw_t csw;
-
-  uint8_t  itf_num;
-  uint8_t  ep_in;
-  uint8_t  ep_out;
-
-  // Bulk Only Transfer (BOT) Protocol
-  uint8_t  stage;
-  uint32_t total_len;
-  uint32_t xferred_len; // numbered of bytes transferred so far in the Data Stage
-
-  // Sense Response Data
-  uint8_t sense_key;
-  uint8_t add_sense_code;
-  uint8_t add_sense_qualifier;
-}mscd_interface_t;
-
-extern mscd_interface_t _mscd_itf;
-
 /** \addtogroup ClassDriver_MSC
  *  @{
  * \defgroup MSC_Device Device
@@ -198,8 +172,8 @@ ATTR_WEAK bool tud_lun_capacity_cb(uint8_t lun, uint32_t* last_valid_sector, uin
 
 void mscd_init(void);
 tusb_error_t mscd_open(uint8_t rhport, tusb_desc_interface_t const * p_interface_desc, uint16_t *p_length);
-tusb_error_t mscd_control_request(uint8_t rhport, tusb_control_request_t const * p_request, uint16_t bytes_already_sent);
-void mscd_control_request_complete (uint8_t rhport, tusb_control_request_t const * p_request);
+bool mscd_control_request(uint8_t rhport, tusb_control_request_t const * p_request);
+bool mscd_control_request_complete (uint8_t rhport, tusb_control_request_t const * p_request);
 tusb_error_t mscd_xfer_cb(uint8_t rhport, uint8_t edpt_addr, tusb_event_t event, uint32_t xferred_bytes);
 void mscd_reset(uint8_t rhport);
 
diff --git a/src/common/tusb_types.h b/src/common/tusb_types.h
index 5c226f556..32632cd90 100644
--- a/src/common/tusb_types.h
+++ b/src/common/tusb_types.h
@@ -203,12 +203,14 @@ typedef enum
   TUSB_EVENT_XFER_STALLED,
 }tusb_event_t;
 
-enum {
+enum
+{
   DESC_OFFSET_LEN  = 0,
   DESC_OFFSET_TYPE = 1
 };
 
-enum {
+enum
+{
   INTERFACE_INVALID_NUMBER = 0xff
 };
 
diff --git a/src/device/control.c b/src/device/control.c
index 35ae536e1..7ab1e16ab 100644
--- a/src/device/control.c
+++ b/src/device/control.c
@@ -46,216 +46,125 @@
 #include "control.h"
 #include "device/usbd_pvt.h"
 
+enum
+{
+  EDPT_CTRL_OUT = 0x00,
+  EDPT_CTRL_IN  = 0x80
+};
+
+typedef struct {
+    tusb_control_request_t request;
+
+    void* buffer;
+    uint16_t total_len;
+    uint16_t total_transferred;
+
+    bool (*complete_cb) (uint8_t, tusb_control_request_t const * );
+} control_t;
+
 control_t control_state;
 
-CFG_TUSB_ATTR_USBRAM CFG_TUSB_MEM_ALIGN uint8_t _shared_control_buffer[64];
+CFG_TUSB_ATTR_USBRAM CFG_TUSB_MEM_ALIGN uint8_t _usbd_ctrl_buf[CFG_TUD_ENDOINT0_SIZE];
 
-void controld_reset(uint8_t rhport) {
-    control_state.current_stage = CONTROL_STAGE_SETUP;
+void usbd_control_reset (uint8_t rhport)
+{
+  tu_varclr(&control_state);
 }
 
-// Helper to send STATUS (zero length) packet
-// Note dir is value of direction bit in setup packet (i.e DATA stage direction)
-static inline bool dcd_control_status(uint8_t rhport, uint8_t dir)
+void usbd_control_stall(uint8_t rhport)
+{
+  dcd_edpt_stall(rhport, 0);
+}
+
+bool usbd_control_status(uint8_t rhport, tusb_control_request_t const * request)
 {
-  uint8_t ep_addr = 0;
-  // Invert the direction.
-  if (dir == TUSB_DIR_OUT) {
-    ep_addr |= TUSB_DIR_IN_MASK;
-  }
   // status direction is reversed to one in the setup packet
-  return dcd_edpt_xfer(rhport, ep_addr, NULL, 0);
-}
-
-static inline void dcd_control_stall(uint8_t rhport)
-{
-  dcd_edpt_stall(rhport, 0 | TUSB_DIR_IN_MASK);
+  return dcd_edpt_xfer(rhport, request->bmRequestType_bit.direction ? EDPT_CTRL_OUT : EDPT_CTRL_IN, NULL, 0);
 }
 
 
-// return len of descriptor and change pointer to descriptor's buffer
-static uint16_t get_descriptor(uint8_t rhport, tusb_control_request_t const * const p_request, uint8_t const ** pp_buffer)
+// Each transaction is up to endpoint0's max packet size
+static bool start_control_data_xact(uint8_t rhport)
 {
-  (void) rhport;
+  uint16_t const xact_len = tu_min16(control_state.total_len - control_state.total_transferred, CFG_TUD_ENDOINT0_SIZE);
 
-  tusb_desc_type_t const desc_type = (tusb_desc_type_t) tu_u16_high(p_request->wValue);
-  uint8_t const desc_index = tu_u16_low( p_request->wValue );
+  uint8_t ep_addr = EDPT_CTRL_OUT;
 
-  uint8_t const * desc_data = NULL ;
-  uint16_t len = 0;
-
-  switch(desc_type)
+  if ( control_state.request.bmRequestType_bit.direction == TUSB_DIR_IN )
   {
-    case TUSB_DESC_DEVICE:
-      desc_data = (uint8_t const *) usbd_desc_set->device;
-      len       = sizeof(tusb_desc_device_t);
-    break;
-
-    case TUSB_DESC_CONFIGURATION:
-      desc_data = (uint8_t const *) usbd_desc_set->config;
-      len       = ((tusb_desc_configuration_t const*) desc_data)->wTotalLength;
-    break;
-
-    case TUSB_DESC_STRING:
-      // String Descriptor always uses the desc set from user
-      if ( desc_index < tud_desc_set.string_count )
-      {
-        desc_data = tud_desc_set.string_arr[desc_index];
-        TU_VERIFY( desc_data != NULL, 0 );
-
-        len  = desc_data[0];  // first byte of descriptor is its size
-      }else
-      {
-        // out of range
-        /* The 0xee string is indeed a Microsoft USB extension.
-         * It can be used to tell Windows what driver it should use for the device !!!
-         */
-        return 0;
-      }
-    break;
-
-    case TUSB_DESC_DEVICE_QUALIFIER:
-      // TODO If not highspeed capable stall this request otherwise
-      // return the descriptor that could work in highspeed
-      return 0;
-    break;
-
-    default: return 0;
+    ep_addr = EDPT_CTRL_IN;
+    memcpy(_usbd_ctrl_buf, control_state.buffer, xact_len);
   }
 
-  TU_ASSERT( desc_data != NULL, 0);
-
-  // up to Host's length
-  len = tu_min16(p_request->wLength, len );
-  (*pp_buffer) = desc_data;
-
-  return len;
+  return dcd_edpt_xfer(rhport, ep_addr, _usbd_ctrl_buf, xact_len);
 }
 
-tusb_error_t controld_xfer_cb(uint8_t rhport, uint8_t edpt_addr, tusb_event_t event, uint32_t xferred_bytes) {
-    if (control_state.current_stage == CONTROL_STAGE_STATUS && xferred_bytes == 0) {
-        control_state.current_stage = CONTROL_STAGE_SETUP;
-        return TUSB_ERROR_NONE;
-    }
-    tusb_error_t error = TUSB_ERROR_NONE;
-    control_state.total_transferred += xferred_bytes;
-    tusb_control_request_t const *p_request = &control_state.current_request;
-
-    if (p_request->wLength == control_state.total_transferred || xferred_bytes < 64) {
-        control_state.current_stage = CONTROL_STAGE_STATUS;
-        dcd_control_status(rhport, p_request->bmRequestType_bit.direction);
-
-        // Do the user callback after queueing the STATUS packet because the callback could be slow.
-        if ( TUSB_REQ_RCPT_INTERFACE == p_request->bmRequestType_bit.recipient )
-        {
-          tud_control_interface_control_complete_cb(rhport, tu_u16_low(p_request->wIndex), p_request);
-        }
-    } else {
-        if (TUSB_REQ_RCPT_INTERFACE == p_request->bmRequestType_bit.recipient) {
-          error = tud_control_interface_control_cb(rhport, tu_u16_low(p_request->wIndex), p_request, control_state.total_transferred);
-        } else {
-          error = controld_process_control_request(rhport, p_request, control_state.total_transferred);
-        }
-    }
-    return error;
-}
-
-// This tracks the state of a control request.
-tusb_error_t controld_process_setup_request(uint8_t rhport, tusb_control_request_t const * p_request) {
-  tusb_error_t error = TUSB_ERROR_NONE;
-  memcpy(&control_state.current_request, p_request, sizeof(tusb_control_request_t));
-  if (p_request->wLength == 0) {
-      control_state.current_stage = CONTROL_STAGE_STATUS;
-  } else {
-      control_state.current_stage = CONTROL_STAGE_DATA;
-      control_state.total_transferred = 0;
-  }
-
-  if ( TUSB_REQ_RCPT_INTERFACE == p_request->bmRequestType_bit.recipient )
-  {
-    error = tud_control_interface_control_cb(rhport, tu_u16_low(p_request->wIndex), p_request, 0);
-  } else {
-    error = controld_process_control_request(rhport, p_request, 0);
-  }
-
-  if (error != TUSB_ERROR_NONE) {
-    dcd_control_stall(rhport); // Stall errored requests
-  } else if (control_state.current_stage == CONTROL_STAGE_STATUS) {
-    dcd_control_status(rhport, p_request->bmRequestType_bit.direction);
-  }
-  return error;
-}
-
-// This handles the actual request and its response.
-tusb_error_t controld_process_control_request(uint8_t rhport, tusb_control_request_t const * p_request, uint16_t bytes_already_sent)
+// TODO may find a better way
+void usbd_control_set_complete_callback( bool (*fp) (uint8_t, tusb_control_request_t const * ) )
 {
-  tusb_error_t error = TUSB_ERROR_NONE;
-  uint8_t ep_addr = 0;
-  if (p_request->bmRequestType_bit.direction == TUSB_DIR_IN) {
-      ep_addr |= TUSB_DIR_IN_MASK;
+  control_state.complete_cb = fp;
+}
+
+bool usbd_control_xfer(uint8_t rhport, tusb_control_request_t const * request, void* buffer, uint16_t len)
+{
+  control_state.request = (*request);
+  control_state.buffer = buffer;
+  control_state.total_len = tu_min16(len, request->wLength);
+  control_state.total_transferred = 0;
+
+  if ( buffer != NULL && len )
+  {
+    // Data stage
+    TU_ASSERT( start_control_data_xact(rhport) );
+  }else
+  {
+    // Status stage
+    TU_ASSERT( usbd_control_status(rhport, request) );
   }
 
-  //------------- Standard Request e.g in enumeration -------------//
-  if( TUSB_REQ_RCPT_DEVICE    == p_request->bmRequestType_bit.recipient &&
-      TUSB_REQ_TYPE_STANDARD  == p_request->bmRequestType_bit.type ) {
-    switch (p_request->bRequest) {
-      case TUSB_REQ_GET_DESCRIPTOR: {
-        uint8_t  const * buffer = NULL;
-        uint16_t const   len    = get_descriptor(rhport, p_request, &buffer);
+  return true;
+}
 
-        if (len) {
-          uint16_t remaining_bytes = len - bytes_already_sent;
-          if (remaining_bytes > 64) {
-              remaining_bytes = 64;
-          }
-          memcpy(_shared_control_buffer, buffer + bytes_already_sent, remaining_bytes);
-          dcd_edpt_xfer(rhport, ep_addr, _shared_control_buffer, remaining_bytes);
-        } else {
-          return TUSB_ERROR_FAILED;
-        }
-        break;
-      }
-      case TUSB_REQ_GET_CONFIGURATION:
-        memcpy(_shared_control_buffer, &control_state.config, 1);
-        dcd_edpt_xfer(rhport, ep_addr, _shared_control_buffer, 1);
-        break;
-      case TUSB_REQ_SET_ADDRESS:
-        dcd_set_address(rhport, (uint8_t) p_request->wValue);
-        break;
-      case TUSB_REQ_SET_CONFIGURATION:
-        control_state.config = p_request->wValue;
-        tud_control_set_config_cb (rhport, control_state.config);
-        break;
-      default:
-        return TUSB_ERROR_FAILED;
-    }
-  } else if (p_request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_ENDPOINT &&
-             p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD) {
-    //------------- Endpoint Request -------------//
-    switch (p_request->bRequest) {
-      case TUSB_REQ_GET_STATUS: {
-        uint16_t status = dcd_edpt_stalled(rhport, tu_u16_low(p_request->wIndex)) ? 0x0001 : 0x0000;
-        memcpy(_shared_control_buffer, &status, 2);
-
-        dcd_edpt_xfer(rhport, ep_addr, _shared_control_buffer, 2);
-        break;
-      }
-      case TUSB_REQ_CLEAR_FEATURE:
-        // only endpoint feature is halted/stalled
-        dcd_edpt_clear_stall(rhport, tu_u16_low(p_request->wIndex));
-        break;
-      case TUSB_REQ_SET_FEATURE:
-        // only endpoint feature is halted/stalled
-        dcd_edpt_stall(rhport, tu_u16_low(p_request->wIndex));
-        break;
-      default:
-        return TUSB_ERROR_FAILED;
-    }
-  } else {
-    //------------- Unsupported Request -------------//
-    return TUSB_ERROR_FAILED;
+// callback when a transaction complete on DATA stage of control endpoint
+tusb_error_t usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, tusb_event_t event, uint32_t xferred_bytes)
+{
+  if ( control_state.request.bmRequestType_bit.direction == TUSB_DIR_OUT )
+  {
+    memcpy(control_state.buffer, _usbd_ctrl_buf, xferred_bytes);
   }
-  return error;
+
+  control_state.total_transferred += xferred_bytes;
+  control_state.buffer += xferred_bytes;
+
+  if ( control_state.total_len == control_state.total_transferred || xferred_bytes < CFG_TUD_ENDOINT0_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 ( control_state.complete_cb )
+    {
+      is_ok = control_state.complete_cb(rhport, &control_state.request);
+    }
+
+    if ( is_ok )
+    {
+      // Send status
+      TU_ASSERT( usbd_control_status(rhport, &control_state.request), TUSB_ERROR_FAILED );
+    }else
+    {
+      // stall due to callback
+      usbd_control_stall(rhport);
+    }
+  }
+  else
+  {
+    // More data to transfer
+    TU_ASSERT(start_control_data_xact(rhport), TUSB_ERROR_FAILED);
+  }
+
+  return TUSB_ERROR_NONE;
 }
 
 #endif
diff --git a/src/device/control.h b/src/device/control.h
index e8dcf76f9..e1709bcee 100644
--- a/src/device/control.h
+++ b/src/device/control.h
@@ -48,44 +48,6 @@
 
 #include "tusb.h"
 
-typedef enum {
-    CONTROL_STAGE_SETUP, // Waiting for a setup token.
-    CONTROL_STAGE_DATA, // In the process of sending or receiving data.
-    CONTROL_STAGE_STATUS // In the process of transmitting the STATUS ZLP.
-} control_stage_t;
-
-typedef struct {
-    control_stage_t current_stage;
-    tusb_control_request_t current_request;
-    uint16_t total_transferred;
-    uint8_t config;
-} control_t;
-
-extern uint8_t _shared_control_buffer[64];
-
-tusb_error_t controld_process_setup_request(uint8_t rhport, tusb_control_request_t const * const p_request);
-
-// Callback when the configuration of the device is changed.
-tusb_error_t tud_control_set_config_cb(uint8_t rhport, uint8_t config_number);
-
-// Called when the DATA stage of a control transaction is complete.
-void tud_control_interface_control_complete_cb(uint8_t rhport, uint8_t interface, tusb_control_request_t const * const p_request);
-
-tusb_error_t tud_control_interface_control_cb(uint8_t rhport, uint8_t interface, tusb_control_request_t const * const p_request, uint16_t bytes_already_sent);
-
-//--------------------------------------------------------------------+
-// INTERNAL API
-//--------------------------------------------------------------------+
-tusb_error_t controld_open(uint8_t rhport, tusb_desc_interface_t const * p_interface_desc, uint16_t *p_length);
-
-// This tracks the state of a control request.
-tusb_error_t controld_process_setup_request(uint8_t rhport, tusb_control_request_t const * p_request);
-
-// This handles the actual request and its response.
-tusb_error_t controld_process_control_request(uint8_t rhport, tusb_control_request_t const * p_request, uint16_t bytes_already_sent);
-
-tusb_error_t controld_xfer_cb(uint8_t rhport, uint8_t edpt_addr, tusb_event_t event, uint32_t xferred_bytes);
-void controld_reset(uint8_t rhport);
 
 #ifdef __cplusplus
  }
diff --git a/src/device/dcd.h b/src/device/dcd.h
index ff836f405..bd0ea852e 100644
--- a/src/device/dcd.h
+++ b/src/device/dcd.h
@@ -124,6 +124,10 @@ void dcd_event_xfer_complete (uint8_t rhport, uint8_t ep_addr, uint32_t xferred_
 
 /*------------------------------------------------------------------*/
 /* Endpoint API
+ * Note:
+ * - Address of control endpoint OUT is 0x00, In is 0x80
+ * - When stalling control endpoint both control OUT and IN must be stalled
+ * (according to USB spec, stalled control is only recovered with setup token)
  *------------------------------------------------------------------*/
 bool dcd_edpt_open        (uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc);
 bool dcd_edpt_xfer        (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes);
diff --git a/src/device/usbd.c b/src/device/usbd.c
index 13cc7f332..e9ac24c4b 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -68,11 +68,10 @@
 typedef struct {
   uint8_t config_num;
 
-  // map interface number to driver (0xff is invalid)
-  uint8_t itf2drv[16];
+  uint8_t itf2drv[16];  // map interface number to driver (0xff is invalid)
+  uint8_t ep2drv[2][8]; // map endpoint to driver ( 0xff is invalid )
+
 
-  // map endpoint to driver ( 0xff is invalid )
-  uint8_t ep2drv[2][8];
 }usbd_device_t;
 
 static usbd_device_t _usbd_dev;
@@ -94,9 +93,8 @@ typedef struct {
 
   void         (* init           ) (void);
   tusb_error_t (* open           ) (uint8_t rhport, tusb_desc_interface_t const * desc_intf, uint16_t* p_length);
-  // Control request is called one or more times for a request and can queue multiple data packets.
-  tusb_error_t (* control_request ) (uint8_t rhport, tusb_control_request_t const *, uint16_t bytes_already_sent);
-  void (* control_request_complete ) (uint8_t rhport, tusb_control_request_t const *);
+  bool         (* control_request ) (uint8_t rhport, tusb_control_request_t const * request);
+  bool (* control_request_complete ) (uint8_t rhport, tusb_control_request_t const * request);
   tusb_error_t (* xfer_cb        ) (uint8_t rhport, uint8_t ep_addr, tusb_event_t, uint32_t);
   void         (* sof            ) (uint8_t rhport);
   void         (* reset          ) (uint8_t);
@@ -171,9 +169,16 @@ OSAL_QUEUE_DEF(_usbd_qdef, CFG_TUD_TASK_QUEUE_SZ, dcd_event_t);
 static osal_queue_t _usbd_q;
 
 //--------------------------------------------------------------------+
-// INTERNAL FUNCTION
+// Prototypes
 //--------------------------------------------------------------------+
 static void mark_interface_endpoint(uint8_t const* p_desc, uint16_t desc_len, uint8_t driver_id);
+static bool process_control_request(uint8_t rhport, tusb_control_request_t const * p_request);
+static bool process_set_config(uint8_t rhport, uint8_t config_number);
+static void const* get_descriptor(tusb_control_request_t const * p_request, uint16_t* desc_len);
+
+void usbd_control_reset (uint8_t rhport);
+tusb_error_t usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, tusb_event_t event, uint32_t xferred_bytes);
+void usbd_control_set_complete_callback( bool (*fp) (uint8_t, tusb_control_request_t const * ) );
 
 //--------------------------------------------------------------------+
 // APPLICATION API
@@ -184,7 +189,7 @@ bool tud_mounted(void)
 }
 
 //--------------------------------------------------------------------+
-// IMPLEMENTATION
+// USBD Task
 //--------------------------------------------------------------------+
 tusb_error_t usbd_init (void)
 {
@@ -214,7 +219,7 @@ static void usbd_reset(uint8_t rhport)
   memset(_usbd_dev.itf2drv, 0xff, sizeof(_usbd_dev.itf2drv)); // invalid mapping
   memset(_usbd_dev.ep2drv , 0xff, sizeof(_usbd_dev.ep2drv )); // invalid mapping
 
-  controld_reset(rhport);
+  usbd_control_reset(rhport);
 
   for (uint8_t i = 0; i < USBD_CLASS_DRIVER_COUNT; i++)
   {
@@ -222,20 +227,24 @@ static void usbd_reset(uint8_t rhport)
   }
 }
 
+// Main device task implementation
 static void usbd_task_body(void)
 {
-  dcd_event_t event;
-
   // Loop until there is no more events in the queue
   while (1)
   {
+    dcd_event_t event;
+
     if ( !osal_queue_receive(_usbd_q, &event) ) return;
 
     switch ( event.event_id )
     {
       case DCD_EVENT_SETUP_RECEIVED:
-        // Setup tokens are unique to the Control endpoint so we delegate to it directly.
-        controld_process_setup_request(event.rhport, &event.setup_received);
+        // Process control request, if failed control endpoint is stalled
+        if ( !process_control_request(event.rhport, &event.setup_received) )
+        {
+          usbd_control_stall(event.rhport);
+        }
       break;
 
       case DCD_EVENT_XFER_COMPLETE:
@@ -245,8 +254,8 @@ static void usbd_task_body(void)
 
         if ( 0 == edpt_number(ep_addr) )
         {
-          // control transfer
-          controld_xfer_cb(event.rhport, ep_addr, event.xfer_complete.result, event.xfer_complete.len);
+          // control transfer DATA stage callback
+          usbd_control_xfer_cb(event.rhport, ep_addr, event.xfer_complete.result, event.xfer_complete.len);
         }
         else
         {
@@ -312,66 +321,142 @@ void usbd_task( void* param)
 #endif
 }
 
-void tud_control_interface_control_complete_cb(uint8_t rhport, uint8_t interface, tusb_control_request_t const * const p_request) {
-    if (_usbd_dev.itf2drv[ interface ] < USBD_CLASS_DRIVER_COUNT)
-    {
-      const usbd_class_driver_t *driver = &usbd_class_drivers[_usbd_dev.itf2drv[interface]];
-      if (driver->control_request_complete != NULL) {
-        driver->control_request_complete(rhport, p_request);
-      }
-    }
-}
+//--------------------------------------------------------------------+
+// Control Request Parser & Handling
+//--------------------------------------------------------------------+
 
-tusb_error_t tud_control_interface_control_cb(uint8_t rhport, uint8_t interface, tusb_control_request_t const * const p_request, uint16_t bytes_already_sent) {
-    if (_usbd_dev.itf2drv[ interface ] < USBD_CLASS_DRIVER_COUNT)
+// This handles the actual request and its response.
+// return false will cause its caller to stall control endpoint
+static bool process_control_request(uint8_t rhport, tusb_control_request_t const * p_request)
+{
+  usbd_control_set_complete_callback(NULL);
+
+  if ( TUSB_REQ_RCPT_DEVICE == p_request->bmRequestType_bit.recipient &&
+       TUSB_REQ_TYPE_STANDARD == p_request->bmRequestType_bit.type )
+  {
+    //------------- Standard Device Requests e.g in enumeration -------------//
+    void* data_buf = NULL;
+    uint16_t data_len = 0;
+
+    switch ( p_request->bRequest )
     {
-      return usbd_class_drivers[_usbd_dev.itf2drv[interface]].control_request(rhport, p_request, bytes_already_sent);
+      case TUSB_REQ_SET_ADDRESS:
+        dcd_set_address(rhport, (uint8_t) p_request->wValue);
+      break;
+
+      case TUSB_REQ_GET_CONFIGURATION:
+        data_buf = &_usbd_dev.config_num;
+        data_len = 1;
+      break;
+
+      case TUSB_REQ_SET_CONFIGURATION:
+      {
+        uint8_t const config = (uint8_t) p_request->wValue;
+
+        dcd_set_config(rhport, config);
+        _usbd_dev.config_num = config;
+
+        TU_ASSERT( TUSB_ERROR_NONE == process_set_config(rhport, config) );
+      }
+      break;
+
+      case TUSB_REQ_GET_DESCRIPTOR:
+        data_buf = (void*) get_descriptor(p_request, &data_len);
+        if ( data_buf == NULL || data_len == 0 ) return false;
+      break;
+
+      default: return false;
     }
-    return TUSB_ERROR_FAILED;
+
+    usbd_control_xfer(rhport, p_request, data_buf, data_len);
+  }
+  else if ( TUSB_REQ_RCPT_INTERFACE == p_request->bmRequestType_bit.recipient )
+  {
+    //------------- Class/Interface Specific Request -------------//
+    uint8_t const itf = tu_u16_low(p_request->wIndex);
+    uint8_t const drvid = _usbd_dev.itf2drv[ itf ];
+
+    TU_VERIFY (drvid < USBD_CLASS_DRIVER_COUNT );
+
+    usbd_control_set_complete_callback(usbd_class_drivers[drvid].control_request_complete );
+
+    // control endpoint will be stalled if driver return false
+    return usbd_class_drivers[drvid].control_request(rhport, p_request);
+  }
+  else if ( p_request->bmRequestType_bit.recipient == TUSB_REQ_RCPT_ENDPOINT &&
+            p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD )
+  {
+    //------------- Endpoint Request -------------//
+    switch ( p_request->bRequest )
+    {
+      case TUSB_REQ_GET_STATUS:
+      {
+        uint16_t status = dcd_edpt_stalled(rhport, tu_u16_low(p_request->wIndex)) ? 0x0001 : 0x0000;
+        usbd_control_xfer(rhport, p_request, &status, 2);
+      }
+      break;
+
+      case TUSB_REQ_CLEAR_FEATURE:
+        // only endpoint feature is halted/stalled
+        dcd_edpt_clear_stall(rhport, tu_u16_low(p_request->wIndex));
+        usbd_control_status(rhport, p_request);
+      break;
+
+      case TUSB_REQ_SET_FEATURE:
+        // only endpoint feature is halted/stalled
+        dcd_edpt_stall(rhport, tu_u16_low(p_request->wIndex));
+        usbd_control_status(rhport, p_request);
+      break;
+
+      default: return false;
+    }
+  }
+  else
+  {
+    //------------- Unsupported Request -------------//
+    return false;
+  }
+
+  return true;
 }
 
 // Process Set Configure Request
-// TODO Host (windows) can get HID report descriptor before set configured
-// may need to open interface before set configured
-tusb_error_t tud_control_set_config_cb(uint8_t rhport, uint8_t config_number)
+// This function parse configuration descriptor & open drivers accordingly
+static bool process_set_config(uint8_t rhport, uint8_t config_number)
 {
-  dcd_set_config(rhport, config_number);
-
-  _usbd_dev.config_num = config_number;
-
-  //------------- parse configuration & open drivers -------------//
   uint8_t const * desc_cfg = (uint8_t const *) usbd_desc_set->config;
-  TU_ASSERT(desc_cfg != NULL, TUSB_ERROR_DESCRIPTOR_CORRUPTED);
+  TU_ASSERT(desc_cfg != NULL);
+
   uint8_t const * p_desc = desc_cfg + sizeof(tusb_desc_configuration_t);
   uint16_t const cfg_len = ((tusb_desc_configuration_t*)desc_cfg)->wTotalLength;
 
   while( p_desc < desc_cfg + cfg_len )
   {
+    // Each interface always starts with Interface or Association descriptor
     if ( TUSB_DESC_INTERFACE_ASSOCIATION == descriptor_type(p_desc) )
     {
       p_desc = descriptor_next(p_desc); // ignore Interface Association
     }else
     {
-      TU_ASSERT( TUSB_DESC_INTERFACE == descriptor_type(p_desc), TUSB_ERROR_NOT_SUPPORTED_YET );
+      TU_ASSERT( TUSB_DESC_INTERFACE == descriptor_type(p_desc) );
 
-      tusb_desc_interface_t* p_desc_itf = (tusb_desc_interface_t*) p_desc;
-      uint8_t const class_code = p_desc_itf->bInterfaceClass;
+      tusb_desc_interface_t* desc_itf = (tusb_desc_interface_t*) p_desc;
 
       // Check if class is supported
       uint8_t drv_id;
       for (drv_id = 0; drv_id < USBD_CLASS_DRIVER_COUNT; drv_id++)
       {
-        if ( usbd_class_drivers[drv_id].class_code == class_code ) break;
+        if ( usbd_class_drivers[drv_id].class_code == desc_itf->bInterfaceClass ) break;
       }
-      TU_ASSERT( drv_id < USBD_CLASS_DRIVER_COUNT, TUSB_ERROR_NOT_SUPPORTED_YET );
+      TU_ASSERT( drv_id < USBD_CLASS_DRIVER_COUNT ); // unsupported class
 
-      // Interface number must not be used
-      TU_ASSERT( 0xff == _usbd_dev.itf2drv[p_desc_itf->bInterfaceNumber], TUSB_ERROR_FAILED);
-      _usbd_dev.itf2drv[p_desc_itf->bInterfaceNumber] = drv_id;
+      // Interface number must not be used already TODO alternate interface
+      TU_ASSERT( 0xff == _usbd_dev.itf2drv[desc_itf->bInterfaceNumber] );
+      _usbd_dev.itf2drv[desc_itf->bInterfaceNumber] = drv_id;
 
       uint16_t len=0;
-      TU_ASSERT_ERR( usbd_class_drivers[drv_id].open( rhport, p_desc_itf, &len ) );
-      TU_ASSERT( len >= sizeof(tusb_desc_interface_t), TUSB_ERROR_FAILED );
+      TU_ASSERT_ERR( usbd_class_drivers[drv_id].open( rhport, desc_itf, &len ), false );
+      TU_ASSERT( len >= sizeof(tusb_desc_interface_t) );
 
       mark_interface_endpoint(p_desc, len, drv_id);
 
@@ -404,8 +489,62 @@ static void mark_interface_endpoint(uint8_t const* p_desc, uint16_t desc_len, ui
   }
 }
 
+// return descriptor's buffer and update desc_len
+static void const* get_descriptor(tusb_control_request_t const * p_request, uint16_t* desc_len)
+{
+  tusb_desc_type_t const desc_type = (tusb_desc_type_t) tu_u16_high(p_request->wValue);
+  uint8_t const desc_index = tu_u16_low( p_request->wValue );
+
+  uint8_t const * desc_data = NULL;
+  uint16_t len = 0;
+
+  *desc_len = 0;
+
+  switch(desc_type)
+  {
+    case TUSB_DESC_DEVICE:
+      desc_data = (uint8_t const *) usbd_desc_set->device;
+      len       = sizeof(tusb_desc_device_t);
+    break;
+
+    case TUSB_DESC_CONFIGURATION:
+      desc_data = (uint8_t const *) usbd_desc_set->config;
+      len       = ((tusb_desc_configuration_t const*) desc_data)->wTotalLength;
+    break;
+
+    case TUSB_DESC_STRING:
+      // String Descriptor always uses the desc set from user
+      if ( desc_index < tud_desc_set.string_count )
+      {
+        desc_data = tud_desc_set.string_arr[desc_index];
+        TU_VERIFY( desc_data != NULL, NULL );
+
+        len  = desc_data[0];  // first byte of descriptor is its size
+      }else
+      {
+        // out of range
+        /* The 0xEE index string is a Microsoft USB extension.
+         * It can be used to tell Windows what driver it should use for the device !!!
+         */
+        return NULL;
+      }
+    break;
+
+    case TUSB_DESC_DEVICE_QUALIFIER:
+      // TODO If not highspeed capable stall this request otherwise
+      // return the descriptor that could work in highspeed
+      return NULL;
+    break;
+
+    default: return NULL;
+  }
+
+  *desc_len = len;
+  return desc_data;
+}
+
 //--------------------------------------------------------------------+
-// USBD-DCD Callback API
+// DCD Event Handler
 //--------------------------------------------------------------------+
 void dcd_event_handler(dcd_event_t const * event, bool in_isr)
 {
@@ -430,6 +569,9 @@ void dcd_event_handler(dcd_event_t const * event, bool in_isr)
     break;
 
     case DCD_EVENT_XFER_COMPLETE:
+      // skip zero-length control status complete event, should dcd notifies us.
+      if ( 0 == edpt_number(event->xfer_complete.ep_addr) && event->xfer_complete.len == 0) break;
+
       osal_queue_send(_usbd_q, event, in_isr);
       TU_ASSERT(event->xfer_complete.result == DCD_XFER_SUCCESS,);
     break;
@@ -438,8 +580,6 @@ void dcd_event_handler(dcd_event_t const * event, bool in_isr)
   }
 }
 
-void dcd_event_handler(dcd_event_t const * event, bool in_isr);
-
 // helper to send bus signal event
 void dcd_event_bus_signal (uint8_t rhport, dcd_eventid_t eid, bool in_isr)
 {
diff --git a/src/device/usbd_pvt.h b/src/device/usbd_pvt.h
index bbbe0a604..30f010887 100644
--- a/src/device/usbd_pvt.h
+++ b/src/device/usbd_pvt.h
@@ -54,15 +54,24 @@ extern tud_desc_set_t const* usbd_desc_set;
 tusb_error_t usbd_init (void);
 void         usbd_task (void* param);
 
+
+// Carry out Data and Status stage of control transfer
+// - If len = 0, it is equivalent to sending status only
+// - If len > wLength : it will be truncated
+bool usbd_control_xfer(uint8_t rhport, tusb_control_request_t const * request, void* buffer, uint16_t len);
+
+// Send STATUS (zero length) packet
+bool usbd_control_status(uint8_t rhport, tusb_control_request_t const * request);
+
+// Stall control endpoint until new setup packet arrived
+void usbd_control_stall(uint8_t rhport);
+
 /*------------------------------------------------------------------*/
-/* Endpoint helper
+/* Helper
  *------------------------------------------------------------------*/
 // helper to parse an pair of In and Out endpoint descriptors. They must be consecutive
 tusb_error_t usbd_open_edpt_pair(uint8_t rhport, tusb_desc_endpoint_t const* p_desc_ep, uint8_t xfer_type, uint8_t* ep_out, uint8_t* ep_in);
 
-/*------------------------------------------------------------------*/
-/* Other Helpers
- *------------------------------------------------------------------*/
 void usbd_defer_func( osal_task_func_t func, void* param, bool in_isr );
 
 
diff --git a/src/tusb_option.h b/src/tusb_option.h
index d027ea0e6..f19ff4076 100644
--- a/src/tusb_option.h
+++ b/src/tusb_option.h
@@ -148,10 +148,8 @@
     #define CFG_TUD_ENDOINT0_SIZE    64
   #endif
 
-  #ifndef CFG_TUD_ENUM_BUFFER_SIZE
+  #ifndef CFG_TUD_CTRL_BUFSIZE
     #define CFG_TUD_CTRL_BUFSIZE 256
-  #else
-    #define CFG_TUD_CTRL_BUFSIZE CFG_TUD_ENUM_BUFFER_SIZE
   #endif
 
   #ifndef CFG_TUD_DESC_AUTO