diff --git a/src/class/cdc/cdc_device.c b/src/class/cdc/cdc_device.c
index f3d4622f..81d7fa52 100644
--- a/src/class/cdc/cdc_device.c
+++ b/src/class/cdc/cdc_device.c
@@ -45,6 +45,7 @@
 // INCLUDE
 //--------------------------------------------------------------------+
 #include "cdc_device.h"
+#include "device/control.h"
 #include "device/usbd_pvt.h"
 
 //--------------------------------------------------------------------+
@@ -287,7 +288,7 @@ tusb_error_t cdcd_open(uint8_t rhport, tusb_desc_interface_t const * p_interface
   return TUSB_ERROR_NONE;
 }
 
-tusb_error_t cdcd_control_request_st(uint8_t rhport, tusb_control_request_t const * p_request)
+tusb_error_t cdcd_control_request(uint8_t rhport, tusb_control_request_t const * p_request, uint16_t bytes_already_sent)
 {
   //------------- Class Specific Request -------------//
   if (p_request->bmRequestType_bit.type != TUSB_REQ_TYPE_CLASS) return TUSB_ERROR_DCD_CONTROL_REQUEST_NOT_SUPPORT;
@@ -299,7 +300,7 @@ tusb_error_t cdcd_control_request_st(uint8_t rhport, tusb_control_request_t cons
   if ( (CDC_REQUEST_GET_LINE_CODING == p_request->bRequest) || (CDC_REQUEST_SET_LINE_CODING == p_request->bRequest) )
   {
     uint16_t len = tu_min16(sizeof(cdc_line_coding_t), p_request->wLength);
-    usbd_control_xfer_st(rhport, p_request->bmRequestType_bit.direction, (uint8_t*) &p_cdc->line_coding, len);
+    dcd_edpt_xfer(rhport, TUSB_DIR_IN_MASK, (uint8_t*) &p_cdc->line_coding, len);
 
     // Invoke callback
     if (CDC_REQUEST_SET_LINE_CODING == p_request->bRequest)
@@ -309,8 +310,6 @@ tusb_error_t cdcd_control_request_st(uint8_t rhport, tusb_control_request_t cons
   }
   else if (CDC_REQUEST_SET_CONTROL_LINE_STATE == p_request->bRequest )
   {
-    dcd_control_status(rhport, p_request->bmRequestType_bit.direction); // ACK control request
-
     // 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)
@@ -323,7 +322,7 @@ tusb_error_t cdcd_control_request_st(uint8_t rhport, tusb_control_request_t cons
   }
   else
   {
-    dcd_control_stall(rhport); // stall unsupported request
+    return TUSB_ERROR_FAILED; // stall unsupported request
   }
   return TUSB_ERROR_NONE;
 }
diff --git a/src/class/cdc/cdc_device.h b/src/class/cdc/cdc_device.h
index 2e03793a..88d37a83 100644
--- a/src/class/cdc/cdc_device.h
+++ b/src/class/cdc/cdc_device.h
@@ -112,7 +112,7 @@ 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_st (uint8_t rhport, tusb_control_request_t const * p_request);
+tusb_error_t cdcd_control_request (uint8_t rhport, tusb_control_request_t const * p_request, uint16_t bytes_already_sent);
 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/hid/hid_device.c b/src/class/hid/hid_device.c
index 3e74950e..aa7a8008 100644
--- a/src/class/hid/hid_device.c
+++ b/src/class/hid/hid_device.c
@@ -46,6 +46,7 @@
 //--------------------------------------------------------------------+
 #include "common/tusb_common.h"
 #include "hid_device.h"
+#include "device/control.h"
 #include "device/usbd_pvt.h"
 
 //--------------------------------------------------------------------+
@@ -402,7 +403,7 @@ 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_st(uint8_t rhport, tusb_control_request_t const * p_request)
+tusb_error_t hidd_control_request(uint8_t rhport, tusb_control_request_t const * p_request, uint16_t bytes_already_sent)
 {
   hidd_interface_t* p_hid = get_interface_by_itfnum( (uint8_t) p_request->wIndex );
   TU_ASSERT(p_hid, TUSB_ERROR_FAILED);
@@ -416,14 +417,17 @@ tusb_error_t hidd_control_request_st(uint8_t rhport, tusb_control_request_t cons
 
     if (p_request->bRequest == TUSB_REQ_GET_DESCRIPTOR && desc_type == HID_DESC_TYPE_REPORT)
     {
-      // use device control buffer
-      TU_ASSERT ( p_hid->desc_len <= CFG_TUD_CTRL_BUFSIZE );
-      memcpy(_usbd_ctrl_buf, p_hid->desc_report, p_hid->desc_len);
+      // 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);
 
-      usbd_control_xfer_st(rhport, p_request->bmRequestType_bit.direction, _usbd_ctrl_buf, p_hid->desc_len);
+      dcd_edpt_xfer(rhport, TUSB_DIR_IN_MASK, _shared_control_buffer, remaining_bytes);
     }else
     {
-      dcd_control_stall(rhport);
+      return TUSB_ERROR_FAILED;
     }
   }
   //------------- Class Specific Request -------------//
@@ -446,11 +450,11 @@ tusb_error_t hidd_control_request_st(uint8_t rhport, tusb_control_request_t cons
       }
 
       TU_ASSERT( xferlen > 0 );
-      usbd_control_xfer_st(rhport, p_request->bmRequestType_bit.direction, p_hid->report_buf, xferlen);
+      dcd_edpt_xfer(rhport, TUSB_DIR_IN_MASK, _shared_control_buffer, xferlen);
     }
     else if ( HID_REQ_CONTROL_SET_REPORT == p_request->bRequest )
     {
-      usbd_control_xfer_st(rhport, p_request->bmRequestType_bit.direction, _usbd_ctrl_buf, p_request->wLength);
+      dcd_edpt_xfer(rhport, 0, _shared_control_buffer, p_request->wLength);
 
       // wValue = Report Type | Report ID
       uint8_t const report_type = tu_u16_high(p_request->wValue);
@@ -458,37 +462,35 @@ tusb_error_t hidd_control_request_st(uint8_t rhport, tusb_control_request_t cons
 
       if ( p_hid->set_report_cb )
       {
-        p_hid->set_report_cb(report_id, (hid_report_type_t) report_type, _usbd_ctrl_buf, p_request->wLength);
+        p_hid->set_report_cb(report_id, (hid_report_type_t) report_type, _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);
-      dcd_control_status(rhport, p_request->bmRequestType_bit.direction);
     }
     else if (HID_REQ_CONTROL_GET_IDLE == p_request->bRequest)
     {
       // TODO idle rate of report
-      _usbd_ctrl_buf[0] = p_hid->idle_rate;
-      usbd_control_xfer_st(rhport, p_request->bmRequestType_bit.direction, _usbd_ctrl_buf, 1);
+      _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 )
     {
-      _usbd_ctrl_buf[0] = 1-p_hid->boot_protocol;   // 0 is Boot, 1 is Report protocol
-      usbd_control_xfer_st(rhport, p_request->bmRequestType_bit.direction, _usbd_ctrl_buf, 1);
+      _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
-      dcd_control_status(rhport, p_request->bmRequestType_bit.direction);
     }else
     {
-      dcd_control_stall(rhport);
+      return TUSB_ERROR_FAILED;
     }
   }else
   {
-    dcd_control_stall(rhport);
+    return TUSB_ERROR_FAILED;
   }
   return TUSB_ERROR_NONE;
 }
diff --git a/src/class/hid/hid_device.h b/src/class/hid/hid_device.h
index d5b9f383..10aea10a 100644
--- a/src/class/hid/hid_device.h
+++ b/src/class/hid/hid_device.h
@@ -378,7 +378,7 @@ 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_st(uint8_t rhport, tusb_control_request_t const * p_request);
+tusb_error_t hidd_control_request(uint8_t rhport, tusb_control_request_t const * p_request, uint16_t bytes_already_sent);
 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);
 
@@ -389,5 +389,3 @@ 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 aa9ebbbc..3c9997b9 100644
--- a/src/class/msc/msc_device.c
+++ b/src/class/msc/msc_device.c
@@ -47,6 +47,7 @@
 
 #include "common/tusb_common.h"
 #include "msc_device.h"
+#include "device/control.h"
 #include "device/usbd_pvt.h"
 
 //--------------------------------------------------------------------+
@@ -146,22 +147,22 @@ 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_st(uint8_t rhport, tusb_control_request_t const * p_request)
+tusb_error_t mscd_control_request(uint8_t rhport, tusb_control_request_t const * p_request, uint16_t bytes_already_sent)
 {
   TU_ASSERT(p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_CLASS, TUSB_ERROR_DCD_CONTROL_REQUEST_NOT_SUPPORT);
 
   if(MSC_REQ_RESET == p_request->bRequest)
   {
-    dcd_control_status(rhport, p_request->bmRequestType_bit.direction);
+    // TODO: Actually reset.
   }
   else if (MSC_REQ_GET_MAX_LUN == p_request->bRequest)
   {
     // returned MAX LUN is minus 1 by specs
-    _usbd_ctrl_buf[0] = CFG_TUD_MSC_MAXLUN-1;
-    usbd_control_xfer_st(rhport, p_request->bmRequestType_bit.direction, _usbd_ctrl_buf, 1);
+    _shared_control_buffer[0] = CFG_TUD_MSC_MAXLUN-1;
+    dcd_edpt_xfer(rhport, TUSB_DIR_IN_MASK, _shared_control_buffer, 1);
   }else
   {
-    dcd_control_stall(rhport); // stall unsupported request
+    return TUSB_ERROR_FAILED; // stall unsupported request
   }
   return TUSB_ERROR_NONE;
 }
diff --git a/src/class/msc/msc_device.h b/src/class/msc/msc_device.h
index 92ab4dde..a5bc54ff 100644
--- a/src/class/msc/msc_device.h
+++ b/src/class/msc/msc_device.h
@@ -198,7 +198,7 @@ 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_st(uint8_t rhport, tusb_control_request_t const * p_request);
+tusb_error_t mscd_control_request(uint8_t rhport, tusb_control_request_t const * p_request, uint16_t bytes_already_sent);
 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/device/control.c b/src/device/control.c
new file mode 100644
index 00000000..4944a999
--- /dev/null
+++ b/src/device/control.c
@@ -0,0 +1,252 @@
+/**************************************************************************/
+/*!
+    @file     usbd.c
+    @author   hathach (tinyusb.org)
+
+    @section LICENSE
+
+    Software License Agreement (BSD License)
+
+    Copyright (c) 2013, hathach (tinyusb.org)
+    All rights reserved.
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions are met:
+    1. Redistributions of source code must retain the above copyright
+    notice, this list of conditions and the following disclaimer.
+    2. Redistributions in binary form must reproduce the above copyright
+    notice, this list of conditions and the following disclaimer in the
+    documentation and/or other materials provided with the distribution.
+    3. Neither the name of the copyright holders nor the
+    names of its contributors may be used to endorse or promote products
+    derived from this software without specific prior written permission.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+    EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+    ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+    This file is part of the tinyusb stack.
+*/
+/**************************************************************************/
+
+#include "tusb_option.h"
+
+#if TUSB_OPT_DEVICE_ENABLED
+
+#define _TINY_USB_SOURCE_FILE_
+
+#include "tusb.h"
+#include "control.h"
+#include "device/usbd_pvt.h"
+
+control_t control_state;
+
+void controld_reset(uint8_t rhport) {
+    control_state.current_stage = CONTROL_STAGE_SETUP;
+}
+
+void controld_init(void) {
+}
+
+// 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)
+{
+  // status direction is reversed to one in the setup packet
+  return dcd_edpt_xfer(rhport, 1-dir, NULL, 0);
+}
+
+static inline void dcd_control_stall(uint8_t rhport)
+{
+  dcd_edpt_stall(rhport, 0 | TUSB_DIR_IN_MASK);
+}
+
+
+// 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)
+{
+  (void) rhport;
+
+  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;
+
+  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, 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;
+  }
+
+  TU_ASSERT( desc_data != NULL, 0);
+
+  // up to Host's length
+  len = tu_min16(p_request->wLength, len );
+  (*pp_buffer) = desc_data;
+
+  return 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);
+    } 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)
+{
+  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;
+  }
+
+  //------------- 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);
+
+        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;
+  }
+  return error;
+}
+
+#endif
diff --git a/src/device/control.h b/src/device/control.h
new file mode 100644
index 00000000..24f0d0a1
--- /dev/null
+++ b/src/device/control.h
@@ -0,0 +1,95 @@
+/**************************************************************************/
+/*!
+    @file     usbd.h
+    @author   hathach (tinyusb.org)
+
+    @section LICENSE
+
+    Software License Agreement (BSD License)
+
+    Copyright (c) 2013, hathach (tinyusb.org)
+    All rights reserved.
+
+    Redistribution and use in source and binary forms, with or without
+    modification, are permitted provided that the following conditions are met:
+    1. Redistributions of source code must retain the above copyright
+    notice, this list of conditions and the following disclaimer.
+    2. Redistributions in binary form must reproduce the above copyright
+    notice, this list of conditions and the following disclaimer in the
+    documentation and/or other materials provided with the distribution.
+    3. Neither the name of the copyright holders nor the
+    names of its contributors may be used to endorse or promote products
+    derived from this software without specific prior written permission.
+
+    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
+    EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
+    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+    ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+    This file is part of the tinyusb stack.
+*/
+/**************************************************************************/
+
+/** \ingroup group_usbd
+ *  @{ */
+
+#ifndef _TUSB_CONTROL_H_
+#define _TUSB_CONTROL_H_
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+#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;
+
+CFG_TUSB_ATTR_USBRAM CFG_TUSB_MEM_ALIGN 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);
+
+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
+//--------------------------------------------------------------------+
+
+void controld_init(void);
+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
+ }
+#endif
+
+#endif /* _TUSB_CONTROL_H_ */
+
+/** @} */
diff --git a/src/device/dcd.h b/src/device/dcd.h
index c1715a5b..0c976ede 100644
--- a/src/device/dcd.h
+++ b/src/device/dcd.h
@@ -135,22 +135,6 @@ void dcd_edpt_stall       (uint8_t rhport, uint8_t ep_addr);
 void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr);
 bool dcd_edpt_stalled     (uint8_t rhport, uint8_t ep_addr);
 
-//------------- Control Endpoint -------------//
-bool dcd_control_xfer     (uint8_t rhport, uint8_t dir, uint8_t * buffer, uint16_t length);
-
-// 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)
-{
-  // status direction is reversed to one in the setup packet
-  return dcd_control_xfer(rhport, 1-dir, NULL, 0);
-}
-
-static inline void dcd_control_stall(uint8_t rhport)
-{
-  dcd_edpt_stall(rhport, 0 | TUSB_DIR_IN_MASK);
-}
-
 #ifdef __cplusplus
  }
 #endif
diff --git a/src/device/usbd.c b/src/device/usbd.c
index 8155b585..3c77fefb 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -36,12 +36,15 @@
 */
 /**************************************************************************/
 
+// This top level class manages the bus state and delegates events to class-specific drivers.
+
 #include "tusb_option.h"
 
 #if TUSB_OPT_DEVICE_ENABLED
 
 #define _TINY_USB_SOURCE_FILE_
 
+#include "control.h"
 #include "tusb.h"
 #include "usbd.h"
 #include "device/usbd_pvt.h"
@@ -72,7 +75,6 @@ typedef struct {
   uint8_t ep2drv[2][8];
 }usbd_device_t;
 
-CFG_TUSB_ATTR_USBRAM CFG_TUSB_MEM_ALIGN uint8_t _usbd_ctrl_buf[CFG_TUD_CTRL_BUFSIZE];
 static usbd_device_t _usbd_dev;
 
 
@@ -92,7 +94,8 @@ typedef struct {
 
   void         (* init           ) (void);
   tusb_error_t (* open           ) (uint8_t rhport, tusb_desc_interface_t const * desc_intf, uint16_t* p_length);
-  tusb_error_t (* control_req_st ) (uint8_t rhport, tusb_control_request_t const *);
+  // 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);
   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);
@@ -100,52 +103,61 @@ typedef struct {
 
 static usbd_class_driver_t const usbd_class_drivers[] =
 {
+    {
+        .class_code      = TUSB_CLASS_UNSPECIFIED,
+        .init            = controld_init,
+        .open            = NULL,
+        .control_request = NULL,
+        .xfer_cb         = controld_xfer_cb,
+        .sof             = NULL,
+        .reset           = controld_reset
+    },
   #if CFG_TUD_CDC
     {
-        .class_code     = TUSB_CLASS_CDC,
-        .init           = cdcd_init,
-        .open           = cdcd_open,
-        .control_req_st = cdcd_control_request_st,
-        .xfer_cb        = cdcd_xfer_cb,
-        .sof            = NULL,
-        .reset          = cdcd_reset
+        .class_code      = TUSB_CLASS_CDC,
+        .init            = cdcd_init,
+        .open            = cdcd_open,
+        .control_request = cdcd_control_request,
+        .xfer_cb         = cdcd_xfer_cb,
+        .sof             = NULL,
+        .reset           = cdcd_reset
     },
   #endif
 
   #if CFG_TUD_MSC
     {
-        .class_code     = TUSB_CLASS_MSC,
-        .init           = mscd_init,
-        .open           = mscd_open,
-        .control_req_st = mscd_control_request_st,
-        .xfer_cb        = mscd_xfer_cb,
-        .sof            = NULL,
-        .reset          = mscd_reset
+        .class_code      = TUSB_CLASS_MSC,
+        .init            = mscd_init,
+        .open            = mscd_open,
+        .control_request = mscd_control_request,
+        .xfer_cb         = mscd_xfer_cb,
+        .sof             = NULL,
+        .reset           = mscd_reset
     },
   #endif
 
 
   #if CFG_TUD_HID
     {
-        .class_code     = TUSB_CLASS_HID,
-        .init           = hidd_init,
-        .open           = hidd_open,
-        .control_req_st = hidd_control_request_st,
-        .xfer_cb        = hidd_xfer_cb,
-        .sof            = NULL,
-        .reset          = hidd_reset
+        .class_code      = TUSB_CLASS_HID,
+        .init            = hidd_init,
+        .open            = hidd_open,
+        .control_request = hidd_control_request,
+        .xfer_cb         = hidd_xfer_cb,
+        .sof             = NULL,
+        .reset           = hidd_reset
     },
   #endif
 
   #if CFG_TUD_CUSTOM_CLASS
     {
-        .class_code     = TUSB_CLASS_VENDOR_SPECIFIC,
-        .init           = cusd_init,
-        .open           = cusd_open,
-        .control_req_st = cusd_control_request_st,
-        .xfer_cb        = cusd_xfer_cb,
-        .sof            = NULL,
-        .reset          = cusd_reset
+        .class_code      = TUSB_CLASS_VENDOR_SPECIFIC,
+        .init            = cusd_init,
+        .open            = cusd_open,
+        .control_request = cusd_control_request,
+        .xfer_cb         = cusd_xfer_cb,
+        .sof             = NULL,
+        .reset           = cusd_reset
     },
   #endif
 };
@@ -162,16 +174,10 @@ OSAL_TASK_DEF(_usbd_task_def, "usbd", usbd_task, CFG_TUD_TASK_PRIO, CFG_TUD_TASK
 OSAL_QUEUE_DEF(_usbd_qdef, CFG_TUD_TASK_QUEUE_SZ, dcd_event_t);
 static osal_queue_t _usbd_q;
 
-/*------------- control transfer semaphore -------------*/
-static osal_semaphore_def_t _usbd_sem_def;
-osal_semaphore_t _usbd_ctrl_sem;
-
 //--------------------------------------------------------------------+
 // INTERNAL FUNCTION
 //--------------------------------------------------------------------+
 static void mark_interface_endpoint(uint8_t const* p_desc, uint16_t desc_len, uint8_t driver_id);
-static tusb_error_t proc_set_config_req(uint8_t rhport, uint8_t config_number);
-static uint16_t get_descriptor(uint8_t rhport, tusb_control_request_t const * const p_request, uint8_t const ** pp_buffer);
 
 //--------------------------------------------------------------------+
 // APPLICATION API
@@ -184,7 +190,6 @@ bool tud_mounted(void)
 //--------------------------------------------------------------------+
 // IMPLEMENTATION
 //--------------------------------------------------------------------+
-static tusb_error_t proc_control_request_st(uint8_t rhport, tusb_control_request_t const * const p_request);
 static tusb_error_t usbd_main_st(void);
 
 tusb_error_t usbd_init (void)
@@ -201,9 +206,6 @@ tusb_error_t usbd_init (void)
   _usbd_q = osal_queue_create(&_usbd_qdef);
   TU_VERIFY(_usbd_q, TUSB_ERROR_OSAL_QUEUE_FAILED);
 
-  _usbd_ctrl_sem = osal_semaphore_create(&_usbd_sem_def);
-  TU_VERIFY(_usbd_q, TUSB_ERROR_OSAL_SEMAPHORE_FAILED);
-
   osal_task_create(&_usbd_task_def);
 
   //------------- class init -------------//
@@ -217,6 +219,9 @@ static void usbd_reset(uint8_t rhport)
   tu_varclr(&_usbd_dev);
   memset(_usbd_dev.itf2drv, 0xff, sizeof(_usbd_dev.itf2drv)); // invalid mapping
   memset(_usbd_dev.ep2drv , 0xff, sizeof(_usbd_dev.ep2drv )); // invalid mapping
+  // Always map the 0th endpoint to the control driver.
+  _usbd_dev.ep2drv[TUSB_DIR_IN][0] = 0;
+  _usbd_dev.ep2drv[TUSB_DIR_OUT][0] = 0;
 
   for (uint8_t i = 0; i < USBD_CLASS_DRIVER_COUNT; i++)
   {
@@ -239,8 +244,6 @@ void usbd_task( void* param)
   OSAL_TASK_END
 }
 
-extern uint32_t setup_count;
-
 static tusb_error_t usbd_main_st(void)
 {
   dcd_event_t event;
@@ -257,7 +260,8 @@ static tusb_error_t usbd_main_st(void)
 
     if ( DCD_EVENT_SETUP_RECEIVED == event.event_id )
     {
-      proc_control_request_st(event.rhport, &event.setup_received);
+      // Setup tokens are unique to the Control endpointso we delegate to it directly.
+      controld_process_setup_request(event.rhport, &event.setup_received);
     }
     else if (DCD_EVENT_XFER_COMPLETE == event.event_id)
     {
@@ -274,13 +278,11 @@ static tusb_error_t usbd_main_st(void)
     {
       usbd_reset(event.rhport);
       osal_queue_reset(_usbd_q);
-      osal_semaphore_reset(_usbd_ctrl_sem);
     }
     else if (DCD_EVENT_UNPLUGGED == event.event_id)
     {
       usbd_reset(event.rhport);
       osal_queue_reset(_usbd_q);
-      osal_semaphore_reset(_usbd_ctrl_sem);
 
       tud_umount_cb(); // invoke callback
     }
@@ -307,113 +309,18 @@ static tusb_error_t usbd_main_st(void)
     return err;
 }
 
-//--------------------------------------------------------------------+
-// CONTROL REQUEST
-//--------------------------------------------------------------------+
-static tusb_error_t proc_control_request_st(uint8_t rhport, tusb_control_request_t const * const p_request)
-{
-  tusb_error_t error = TUSB_ERROR_NONE;
-
-  //------------- 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 )
-  {
-    if ( TUSB_REQ_GET_DESCRIPTOR == p_request->bRequest )
+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)
     {
-      uint8_t  const * buffer = NULL;
-      uint16_t const   len    = get_descriptor(rhport, p_request, &buffer);
-
-
-      if ( len )
-      {
-        TU_ASSERT( len <= CFG_TUD_CTRL_BUFSIZE );
-        memcpy(_usbd_ctrl_buf, buffer, len);
-        usbd_control_xfer_st(rhport, p_request->bmRequestType_bit.direction, _usbd_ctrl_buf, len);
-      }else
-      {
-        dcd_control_stall(rhport); // stall unsupported descriptor
-      }
+      return usbd_class_drivers[_usbd_dev.itf2drv[interface]].control_request(rhport, p_request, bytes_already_sent);
     }
-    else if (TUSB_REQ_GET_CONFIGURATION == p_request->bRequest )
-    {
-      memcpy(_usbd_ctrl_buf, &_usbd_dev.config_num, 1);
-      usbd_control_xfer_st(rhport, p_request->bmRequestType_bit.direction, _usbd_ctrl_buf, 1);
-    }
-    else if ( TUSB_REQ_SET_ADDRESS == p_request->bRequest )
-    {
-      dcd_set_address(rhport, (uint8_t) p_request->wValue);
-
-      #if CFG_TUSB_MCU != OPT_MCU_NRF5X // nrf5x auto handle set address, we must not return status
-      dcd_control_status(rhport, p_request->bmRequestType_bit.direction);
-      #endif
-    }
-    else if ( TUSB_REQ_SET_CONFIGURATION == p_request->bRequest )
-    {
-      proc_set_config_req(rhport, (uint8_t) p_request->wValue);
-      dcd_control_status(rhport, p_request->bmRequestType_bit.direction);
-    }
-    else
-    {
-      dcd_control_stall(rhport); // Stall unsupported request
-    }
-  }
-
-  //------------- Class/Interface Specific Request -------------//
-  else if ( TUSB_REQ_RCPT_INTERFACE == p_request->bmRequestType_bit.recipient )
-  {
-    if (_usbd_dev.itf2drv[ tu_u16_low(p_request->wIndex) ] < USBD_CLASS_DRIVER_COUNT)
-    {
-      error = usbd_class_drivers[ _usbd_dev.itf2drv[ tu_u16_low(p_request->wIndex) ] ].control_req_st(rhport, p_request);
-    }else
-    {
-      dcd_control_stall(rhport); // Stall unsupported request
-    }
-  }
-
-  //------------- Endpoint Request -------------//
-  else if ( TUSB_REQ_RCPT_ENDPOINT == p_request->bmRequestType_bit.recipient &&
-            TUSB_REQ_TYPE_STANDARD == p_request->bmRequestType_bit.type)
-  {
-    if (TUSB_REQ_GET_STATUS == p_request->bRequest )
-    {
-      uint16_t status = dcd_edpt_stalled(rhport, tu_u16_low(p_request->wIndex)) ? 0x0001 : 0x0000;
-      memcpy(_usbd_ctrl_buf, &status, 2);
-
-      usbd_control_xfer_st(rhport, p_request->bmRequestType_bit.direction, _usbd_ctrl_buf, 2);
-    }
-    else if (TUSB_REQ_CLEAR_FEATURE == p_request->bRequest )
-    {
-      // only endpoint feature is halted/stalled
-      dcd_edpt_clear_stall(rhport, tu_u16_low(p_request->wIndex));
-      dcd_control_status(rhport, p_request->bmRequestType_bit.direction);
-    }
-    else if (TUSB_REQ_SET_FEATURE == p_request->bRequest )
-    {
-      // only endpoint feature is halted/stalled
-      dcd_edpt_stall(rhport, tu_u16_low(p_request->wIndex));
-      dcd_control_status(rhport, p_request->bmRequestType_bit.direction);
-    }
-    else
-    {
-      dcd_control_stall(rhport); // Stall unsupported request
-    }
-  }
-
-  //------------- Unsupported Request -------------//
-  else
-  {
-    dcd_control_stall(rhport); // Stall unsupported request
-  }
-  if (error != TUSB_ERROR_NONE) {
-    dcd_control_stall(rhport); // Stall errored requests
-  }
-  return error;
+    return TUSB_ERROR_FAILED;
 }
 
 // Process Set Configure Request
 // TODO Host (windows) can get HID report descriptor before set configured
 // may need to open interface before set configured
-static tusb_error_t proc_set_config_req(uint8_t rhport, uint8_t config_number)
+tusb_error_t tud_control_set_config_cb(uint8_t rhport, uint8_t config_number)
 {
   dcd_set_config(rhport, config_number);
 
@@ -465,65 +372,6 @@ static tusb_error_t proc_set_config_req(uint8_t rhport, uint8_t config_number)
   return TUSB_ERROR_NONE;
 }
 
-// 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)
-{
-  (void) rhport;
-
-  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;
-
-  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, 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;
-  }
-
-  TU_ASSERT( desc_data != NULL, 0);
-
-  // up to Host's length
-  len = tu_min16(p_request->wLength, len );
-  (*pp_buffer) = desc_data;
-
-  return len;
-}
-
 // Helper marking endpoint of interface belongs to class driver
 static void mark_interface_endpoint(uint8_t const* p_desc, uint16_t desc_len, uint8_t driver_id)
 {
@@ -569,18 +417,7 @@ void dcd_event_handler(dcd_event_t const * event, bool in_isr)
     break;
 
     case DCD_EVENT_XFER_COMPLETE:
-      if (event->xfer_complete.ep_addr == 0)
-      {
-        // only signal data stage, skip status (zero byte)
-        if (event->xfer_complete.len)
-        {
-          (void) event->xfer_complete.result; // TODO handle control error/stalled
-          osal_semaphore_post( _usbd_ctrl_sem, in_isr);
-        }
-      }else
-      {
-        osal_queue_send(_usbd_q, event, in_isr);
-      }
+      osal_queue_send(_usbd_q, event, in_isr);
       TU_ASSERT(event->xfer_complete.result == DCD_XFER_SUCCESS,);
     break;
 
@@ -621,15 +458,6 @@ void dcd_event_xfer_complete (uint8_t rhport, uint8_t ep_addr, uint32_t xferred_
 //--------------------------------------------------------------------+
 // Helper
 //--------------------------------------------------------------------+
-uint32_t usbd_control_xfer_st(uint8_t _rhport, uint8_t _dir, uint8_t* _buffer, uint16_t _len) {
-    uint32_t err = TUSB_ERROR_NONE;
-    if (_len) {
-      dcd_control_xfer(_rhport, _dir, (uint8_t*) _buffer, _len);
-    }
-
-    dcd_control_status(_rhport, _dir);
-    return err;
-}
 
 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)
 {
diff --git a/src/device/usbd_pvt.h b/src/device/usbd_pvt.h
index ecb71f77..bbbe0a60 100644
--- a/src/device/usbd_pvt.h
+++ b/src/device/usbd_pvt.h
@@ -45,10 +45,6 @@
  extern "C" {
 #endif
 
-// for used by usbd_control_xfer_st() only, must not be used directly
-extern osal_semaphore_t _usbd_ctrl_sem;
-extern uint8_t _usbd_ctrl_buf[CFG_TUD_CTRL_BUFSIZE];
-
 // Either point to tud_desc_set or usbd_auto_desc_set depending on CFG_TUD_DESC_AUTO
 extern tud_desc_set_t const* usbd_desc_set;
 
@@ -64,8 +60,6 @@ void         usbd_task (void* param);
 // 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);
 
-uint32_t usbd_control_xfer_st(uint8_t _rhport, uint8_t _dir, uint8_t* _buffer, uint16_t _len);
-
 /*------------------------------------------------------------------*/
 /* Other Helpers
  *------------------------------------------------------------------*/
diff --git a/src/portable/nordic/nrf5x/dcd_nrf5x.c b/src/portable/nordic/nrf5x/dcd_nrf5x.c
index 7eeab130..1e3dc4cb 100644
--- a/src/portable/nordic/nrf5x/dcd_nrf5x.c
+++ b/src/portable/nordic/nrf5x/dcd_nrf5x.c
@@ -203,9 +203,7 @@ static void xact_control_start(void)
 
 bool dcd_control_xfer (uint8_t rhport, uint8_t dir, uint8_t * buffer, uint16_t length)
 {
-
   (void) rhport;
-  osal_semaphore_wait( _usbd_ctrl_sem, OSAL_TIMEOUT_CONTROL_XFER);
 
   if ( length )
   {
@@ -222,7 +220,9 @@ bool dcd_control_xfer (uint8_t rhport, uint8_t dir, uint8_t * buffer, uint16_t l
     NRF_USBD->EPIN[0].MAXCNT     = 0;
     // Status Phase also require Easy DMA has to be free as well !!!!
     NRF_USBD->TASKS_EP0STATUS = 1;
-    osal_semaphore_post(_usbd_ctrl_sem, false);
+
+    // The nRF doesn't interrupt on status transmit so we queue up a success response.
+    dcd_event_xfer_complete(0, 0, 0, DCD_XFER_SUCCESS, false);
   }
 
   return true;