diff --git a/src/class/cdc/cdc_device.c b/src/class/cdc/cdc_device.c
index a6f1f1fb..f3d4622f 100644
--- a/src/class/cdc/cdc_device.c
+++ b/src/class/cdc/cdc_device.c
@@ -289,8 +289,6 @@ tusb_error_t cdcd_open(uint8_t rhport, tusb_desc_interface_t const * p_interface
 
 tusb_error_t cdcd_control_request_st(uint8_t rhport, tusb_control_request_t const * p_request)
 {
-  OSAL_SUBTASK_BEGIN
-
   //------------- Class Specific Request -------------//
   if (p_request->bmRequestType_bit.type != TUSB_REQ_TYPE_CLASS) return TUSB_ERROR_DCD_CONTROL_REQUEST_NOT_SUPPORT;
 
@@ -301,7 +299,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, &p_cdc->line_coding, len);
+    usbd_control_xfer_st(rhport, p_request->bmRequestType_bit.direction, (uint8_t*) &p_cdc->line_coding, len);
 
     // Invoke callback
     if (CDC_REQUEST_SET_LINE_CODING == p_request->bRequest)
@@ -327,8 +325,7 @@ tusb_error_t cdcd_control_request_st(uint8_t rhport, tusb_control_request_t cons
   {
     dcd_control_stall(rhport); // stall unsupported request
   }
-
-  OSAL_SUBTASK_END
+  return TUSB_ERROR_NONE;
 }
 
 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/hid/hid_device.c b/src/class/hid/hid_device.c
index d8696f13..3e74950e 100644
--- a/src/class/hid/hid_device.c
+++ b/src/class/hid/hid_device.c
@@ -407,8 +407,6 @@ tusb_error_t hidd_control_request_st(uint8_t rhport, tusb_control_request_t cons
   hidd_interface_t* p_hid = get_interface_by_itfnum( (uint8_t) p_request->wIndex );
   TU_ASSERT(p_hid, TUSB_ERROR_FAILED);
 
-  OSAL_SUBTASK_BEGIN
-
   //------------- STD Request -------------//
   if (p_request->bmRequestType_bit.type == TUSB_REQ_TYPE_STANDARD)
   {
@@ -419,7 +417,7 @@ 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
-      STASK_ASSERT ( p_hid->desc_len <= CFG_TUD_CTRL_BUFSIZE );
+      TU_ASSERT ( p_hid->desc_len <= CFG_TUD_CTRL_BUFSIZE );
       memcpy(_usbd_ctrl_buf, p_hid->desc_report, p_hid->desc_len);
 
       usbd_control_xfer_st(rhport, p_request->bmRequestType_bit.direction, _usbd_ctrl_buf, p_hid->desc_len);
@@ -447,7 +445,7 @@ tusb_error_t hidd_control_request_st(uint8_t rhport, tusb_control_request_t cons
         xferlen = p_request->wLength;
       }
 
-      STASK_ASSERT( xferlen > 0 );
+      TU_ASSERT( xferlen > 0 );
       usbd_control_xfer_st(rhport, p_request->bmRequestType_bit.direction, p_hid->report_buf, xferlen);
     }
     else if ( HID_REQ_CONTROL_SET_REPORT == p_request->bRequest )
@@ -492,8 +490,7 @@ tusb_error_t hidd_control_request_st(uint8_t rhport, tusb_control_request_t cons
   {
     dcd_control_stall(rhport);
   }
-
-  OSAL_SUBTASK_END
+  return TUSB_ERROR_NONE;
 }
 
 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/msc/msc.h b/src/class/msc/msc.h
index 59df6bed..a0abc250 100644
--- a/src/class/msc/msc.h
+++ b/src/class/msc/msc.h
@@ -279,11 +279,13 @@ typedef struct ATTR_PACKED
 
 TU_VERIFY_STATIC( sizeof(scsi_mode_sense6_t) == 6, "size is not correct");
 
+// This is only a Mode parameter header(6).
 typedef struct ATTR_PACKED
 {
   uint8_t data_len;
   uint8_t medium_type;
-  uint8_t device_specific_para;
+  bool write_protected : 1;
+  uint8_t reserved : 7;
   uint8_t block_descriptor_len;
 } scsi_mode_sense6_resp_t;
 
diff --git a/src/class/msc/msc_device.c b/src/class/msc/msc_device.c
index 5a047b01..aa9ebbbc 100644
--- a/src/class/msc/msc_device.c
+++ b/src/class/msc/msc_device.c
@@ -59,31 +59,7 @@ enum
   MSC_STAGE_STATUS
 };
 
-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 mscd_interface_t _mscd_itf;
 CFG_TUSB_ATTR_USBRAM CFG_TUSB_MEM_ALIGN static uint8_t _mscd_buf[CFG_TUD_MSC_BUFSIZE];
 
 //--------------------------------------------------------------------+
@@ -172,8 +148,6 @@ tusb_error_t mscd_open(uint8_t rhport, tusb_desc_interface_t const * p_desc_itf,
 
 tusb_error_t mscd_control_request_st(uint8_t rhport, tusb_control_request_t const * p_request)
 {
-  OSAL_SUBTASK_BEGIN
-
   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)
@@ -189,10 +163,19 @@ tusb_error_t mscd_control_request_st(uint8_t rhport, tusb_control_request_t cons
   {
     dcd_control_stall(rhport); // stall unsupported request
   }
-
-  OSAL_SUBTASK_END
+  return TUSB_ERROR_NONE;
 }
 
+// For backwards compatibility we support static block counts.
+#if defined(CFG_TUD_MSC_BLOCK_NUM) && defined(CFG_TUD_MSC_BLOCK_SZ)
+ATTR_WEAK bool tud_lun_capacity_cb(uint8_t lun, uint32_t* last_valid_sector, uint16_t* block_size) {
+    (void) lun;
+    *last_valid_sector = CFG_TUD_MSC_BLOCK_NUM-1;
+    *block_size = CFG_TUD_MSC_BLOCK_SZ;
+    return true;
+}
+#endif
+
 // return length of response (copied to buffer), -1 if it is not an built-in commands
 int32_t proc_builtin_scsi(msc_cbw_t const * p_cbw, uint8_t* buffer, uint32_t bufsize)
 {
@@ -202,11 +185,13 @@ int32_t proc_builtin_scsi(msc_cbw_t const * p_cbw, uint8_t* buffer, uint32_t buf
   {
     case SCSI_CMD_READ_CAPACITY_10:
     {
-      scsi_read_capacity10_resp_t read_capa10 =
-      {
-          .last_lba   = ENDIAN_BE(CFG_TUD_MSC_BLOCK_NUM-1), // read capacity
-          .block_size = ENDIAN_BE(CFG_TUD_MSC_BLOCK_SZ)
-      };
+      scsi_read_capacity10_resp_t read_capa10;
+
+      uint32_t last_valid_sector;
+      uint16_t block_size;
+      tud_lun_capacity_cb(p_cbw->lun, &last_valid_sector, &block_size);
+      read_capa10.last_lba = ENDIAN_BE(last_valid_sector); // read capacity
+      read_capa10.block_size = ENDIAN_BE(block_size);
 
       ret = sizeof(read_capa10);
       memcpy(buffer, &read_capa10, ret);
@@ -218,11 +203,17 @@ int32_t proc_builtin_scsi(msc_cbw_t const * p_cbw, uint8_t* buffer, uint32_t buf
       scsi_read_format_capacity_data_t read_fmt_capa =
       {
           .list_length     = 8,
-          .block_num       = ENDIAN_BE(CFG_TUD_MSC_BLOCK_NUM),  // write capacity
+          .block_num       = 0,
           .descriptor_type = 2,                                 // formatted media
-          .block_size_u16  = ENDIAN_BE16(CFG_TUD_MSC_BLOCK_SZ)
+          .block_size_u16  = 0
       };
 
+      uint32_t last_valid_sector;
+      uint16_t block_size;
+      tud_lun_capacity_cb(p_cbw->lun, &last_valid_sector, &block_size);
+      read_fmt_capa.block_num = ENDIAN_BE(last_valid_sector+1);
+      read_fmt_capa.block_size_u16 = ENDIAN_BE16(block_size);
+
       ret = sizeof(read_fmt_capa);
       memcpy(buffer, &read_fmt_capa, ret);
     }
@@ -251,13 +242,21 @@ int32_t proc_builtin_scsi(msc_cbw_t const * p_cbw, uint8_t* buffer, uint32_t buf
 
     case SCSI_CMD_MODE_SENSE_6:
     {
-      scsi_mode_sense6_resp_t const mode_resp = {
-        .data_len = 3,
-        .medium_type = 0,
-        .device_specific_para = 0,
-        .block_descriptor_len = 0    // no block descriptor are included
+      scsi_mode_sense6_resp_t mode_resp =
+      {
+          .data_len = 3,
+          .medium_type = 0,
+          .write_protected = false,
+          .reserved = 0,
+          .block_descriptor_len = 0  // no block descriptor are included
       };
 
+      bool writable = true;
+      if (tud_msc_is_writable_cb) {
+          writable = tud_msc_is_writable_cb(p_cbw->lun);
+      }
+      mode_resp.write_protected = !writable;
+
       ret = sizeof(mode_resp);
       memcpy(buffer, &mode_resp, ret);
     }
@@ -291,7 +290,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, tusb_event_t event, uint32_t xferred_bytes)
+tusb_error_t mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, uint8_t event, uint32_t xferred_bytes)
 {
   mscd_interface_t* p_msc = &_mscd_itf;
   msc_cbw_t const * p_cbw = &p_msc->cbw;
@@ -425,7 +424,7 @@ tusb_error_t mscd_xfer_cb(uint8_t rhport, uint8_t ep_addr, tusb_event_t event, u
           }else
           {
             // Application consume less than what we got (including zero)
-            if ( nbytes < xferred_bytes )
+            if ( nbytes < (int32_t) xferred_bytes )
             {
               if ( nbytes > 0 )
               {
diff --git a/src/class/msc/msc_device.h b/src/class/msc/msc_device.h
index 8403dfed..92ab4dde 100644
--- a/src/class/msc/msc_device.h
+++ b/src/class/msc/msc_device.h
@@ -55,14 +55,6 @@ TU_VERIFY_STATIC(CFG_TUD_MSC_BUFSIZE < UINT16_MAX, "Size is not correct");
   #error MSC Device: Incorrect setting of MAX LUN
 #endif
 
-#ifndef CFG_TUD_MSC_BLOCK_NUM
-  #error CFG_TUD_MSC_BLOCK_NUM must be defined
-#endif
-
-#ifndef CFG_TUD_MSC_BLOCK_SZ
-  #error CFG_TUD_MSC_BLOCK_SZ must be defined
-#endif
-
 #ifndef CFG_TUD_MSC_BUFSIZE
   #error CFG_TUD_MSC_BUFSIZE must be defined, value of CFG_TUD_MSC_BLOCK_SZ should work well, the more the better
 #endif
@@ -89,6 +81,32 @@ 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
@@ -138,7 +156,7 @@ int32_t tud_msc_read10_cb (uint8_t lun, uint32_t lba, uint32_t offset, void* buf
  * \retval      negative    Indicate error writing disk I/O. Tinyusb will \b STALL the corresponding
  *                          endpoint and return failed status in command status wrapper phase.
  */
-int32_t tud_msc_write10_cb (uint8_t lun, uint32_t lba, uint32_t offset, void* buffer, uint32_t bufsize);
+int32_t tud_msc_write10_cb (uint8_t lun, uint32_t lba, uint32_t offset, uint8_t* buffer, uint32_t bufsize);
 
 /**
  * Callback invoked when received an SCSI command not in built-in list below.
@@ -164,6 +182,12 @@ ATTR_WEAK void tud_msc_read10_complete_cb(uint8_t lun);
 ATTR_WEAK void tud_msc_write10_complete_cb(uint8_t lun);
 ATTR_WEAK void tud_msc_scsi_complete_cb(uint8_t lun, uint8_t const scsi_cmd[16]);
 
+// Hook to make a mass storage device read-only.
+ATTR_WEAK bool tud_msc_is_writable_cb(uint8_t lun);
+
+// Override for dynamic LUN sizes.
+ATTR_WEAK bool tud_lun_capacity_cb(uint8_t lun, uint32_t* last_valid_sector, uint16_t* block_size);
+
 /** @} */
 /** @} */
 
@@ -185,4 +209,3 @@ void mscd_reset(uint8_t rhport);
 #endif
 
 #endif /* _TUSB_MSC_DEVICE_H_ */
-
diff --git a/src/device/dcd.h b/src/device/dcd.h
index 87837df2..c1715a5b 100644
--- a/src/device/dcd.h
+++ b/src/device/dcd.h
@@ -113,32 +113,13 @@ void dcd_disconnect       (uint8_t rhport) ATTR_WEAK;
 void dcd_event_handler(dcd_event_t const * event, bool in_isr);
 
 // helper to send bus signal event
-static inline void dcd_event_bus_signal (uint8_t rhport, dcd_eventid_t eid, bool in_isr)
-{
-  dcd_event_t event = { .rhport = 0, .event_id = eid, };
-  dcd_event_handler(&event, in_isr);
-}
+void dcd_event_bus_signal (uint8_t rhport, dcd_eventid_t eid, bool in_isr);
 
 // helper to send setup received
-static inline void dcd_event_setup_recieved(uint8_t rhport, uint8_t const * setup, bool in_isr)
-{
-  dcd_event_t event = { .rhport = 0, .event_id = DCD_EVENT_SETUP_RECEIVED };
-  memcpy(&event.setup_received, setup, 8);
-
-  dcd_event_handler(&event, true);
-}
+void dcd_event_setup_received(uint8_t rhport, uint8_t const * setup, bool in_isr);
 
 // helper to send transfer complete event
-static inline void dcd_event_xfer_complete (uint8_t rhport, uint8_t ep_addr, uint32_t xferred_bytes, uint8_t result, bool in_isr)
-{
-  dcd_event_t event = { .rhport = 0, .event_id = DCD_EVENT_XFER_COMPLETE };
-
-  event.xfer_complete.ep_addr = ep_addr;
-  event.xfer_complete.len     = xferred_bytes;
-  event.xfer_complete.result  = result;
-
-  dcd_event_handler(&event, in_isr);
-}
+void dcd_event_xfer_complete (uint8_t rhport, uint8_t ep_addr, uint32_t xferred_bytes, uint8_t result, bool in_isr);
 
 
 /*------------------------------------------------------------------*/
@@ -167,7 +148,7 @@ static inline bool dcd_control_status(uint8_t rhport, uint8_t dir)
 
 static inline void dcd_control_stall(uint8_t rhport)
 {
-  dcd_edpt_stall(rhport, 0);
+  dcd_edpt_stall(rhport, 0 | TUSB_DIR_IN_MASK);
 }
 
 #ifdef __cplusplus
diff --git a/src/device/usbd.c b/src/device/usbd.c
index 1c2fa5d5..8155b585 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -239,25 +239,25 @@ void usbd_task( void* param)
   OSAL_TASK_END
 }
 
+extern uint32_t setup_count;
+
 static tusb_error_t usbd_main_st(void)
 {
-  static dcd_event_t event;
-
-  OSAL_SUBTASK_BEGIN
-
+  dcd_event_t event;
+  tusb_error_t err = TUSB_ERROR_NONE;
   // Loop until there is no more events in the queue
-  while (1)
+  while (_usbd_q->count > 0)
   {
-    uint32_t err;
-
-    err = TUSB_ERROR_NONE;
     tu_memclr(&event, sizeof(dcd_event_t));
 
-    osal_queue_receive(_usbd_q, &event, OSAL_TIMEOUT_WAIT_FOREVER, &err);
+    err = osal_queue_receive(_usbd_q, &event);
+    if (err != TUSB_ERROR_NONE) {
+        break;
+    }
 
     if ( DCD_EVENT_SETUP_RECEIVED == event.event_id )
     {
-      STASK_INVOKE( proc_control_request_st(event.rhport, &event.setup_received), err );
+      proc_control_request_st(event.rhport, &event.setup_received);
     }
     else if (DCD_EVENT_XFER_COMPLETE == event.event_id)
     {
@@ -267,7 +267,7 @@ static tusb_error_t usbd_main_st(void)
 
       if (drv_id < USBD_CLASS_DRIVER_COUNT)
       {
-        usbd_class_drivers[drv_id].xfer_cb( event.rhport, ep_addr, (tusb_event_t) event.xfer_complete.result, event.xfer_complete.len);
+        usbd_class_drivers[drv_id].xfer_cb( event.rhport, ep_addr, event.xfer_complete.result, event.xfer_complete.len);
       }
     }
     else if (DCD_EVENT_BUS_RESET == event.event_id)
@@ -304,7 +304,7 @@ static tusb_error_t usbd_main_st(void)
     }
   }
 
-  OSAL_SUBTASK_END
+    return err;
 }
 
 //--------------------------------------------------------------------+
@@ -312,10 +312,7 @@ static tusb_error_t usbd_main_st(void)
 //--------------------------------------------------------------------+
 static tusb_error_t proc_control_request_st(uint8_t rhport, tusb_control_request_t const * const p_request)
 {
-  OSAL_SUBTASK_BEGIN
-
-  ATTR_UNUSED tusb_error_t error;
-  error = TUSB_ERROR_NONE;
+  tusb_error_t error = TUSB_ERROR_NONE;
 
   //------------- Standard Request e.g in enumeration -------------//
   if( TUSB_REQ_RCPT_DEVICE    == p_request->bmRequestType_bit.recipient &&
@@ -326,9 +323,10 @@ static tusb_error_t proc_control_request_st(uint8_t rhport, tusb_control_request
       uint8_t  const * buffer = NULL;
       uint16_t const   len    = get_descriptor(rhport, p_request, &buffer);
 
+
       if ( len )
       {
-        STASK_ASSERT( len <= CFG_TUD_CTRL_BUFSIZE );
+        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
@@ -365,7 +363,7 @@ static tusb_error_t proc_control_request_st(uint8_t rhport, tusb_control_request
   {
     if (_usbd_dev.itf2drv[ tu_u16_low(p_request->wIndex) ] < USBD_CLASS_DRIVER_COUNT)
     {
-      STASK_INVOKE( usbd_class_drivers[ _usbd_dev.itf2drv[ tu_u16_low(p_request->wIndex) ] ].control_req_st(rhport, p_request), error );
+      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
@@ -406,8 +404,10 @@ static tusb_error_t proc_control_request_st(uint8_t rhport, tusb_control_request
   {
     dcd_control_stall(rhport); // Stall unsupported request
   }
-
-  OSAL_SUBTASK_END
+  if (error != TUSB_ERROR_NONE) {
+    dcd_control_stall(rhport); // Stall errored requests
+  }
+  return error;
 }
 
 // Process Set Configure Request
@@ -548,8 +548,6 @@ static void mark_interface_endpoint(uint8_t const* p_desc, uint16_t desc_len, ui
 //--------------------------------------------------------------------+
 void dcd_event_handler(dcd_event_t const * event, bool in_isr)
 {
-  uint8_t const rhport = event->rhport;
-
   switch (event->event_id)
   {
     case DCD_EVENT_BUS_RESET:
@@ -590,9 +588,49 @@ 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)
+{
+  dcd_event_t event = { .rhport = 0, .event_id = eid, };
+  dcd_event_handler(&event, in_isr);
+}
+
+// helper to send setup received
+void dcd_event_setup_received(uint8_t rhport, uint8_t const * setup, bool in_isr)
+{
+  dcd_event_t event = { .rhport = 0, .event_id = DCD_EVENT_SETUP_RECEIVED };
+  memcpy(&event.setup_received, setup, 8);
+
+  dcd_event_handler(&event, true);
+}
+
+// helper to send transfer complete event
+void dcd_event_xfer_complete (uint8_t rhport, uint8_t ep_addr, uint32_t xferred_bytes, uint8_t result, bool in_isr)
+{
+  dcd_event_t event = { .rhport = 0, .event_id = DCD_EVENT_XFER_COMPLETE };
+
+  event.xfer_complete.ep_addr = ep_addr;
+  event.xfer_complete.len     = xferred_bytes;
+  event.xfer_complete.result  = result;
+
+  dcd_event_handler(&event, in_isr);
+}
+
 //--------------------------------------------------------------------+
 // Helper
 //--------------------------------------------------------------------+
+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)
 {
   for(int i=0; i<2; i++)
diff --git a/src/device/usbd_pvt.h b/src/device/usbd_pvt.h
index 0b6b56d7..ecb71f77 100644
--- a/src/device/usbd_pvt.h
+++ b/src/device/usbd_pvt.h
@@ -64,20 +64,7 @@ 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);
 
-// Carry out Data and Status stage of control transfer
-// Must be call in a subtask (_st) function
-#define usbd_control_xfer_st(_rhport, _dir, _buffer, _len)                    \
-  do {                                                                        \
-    if (_len) {                                                               \
-      uint32_t err;                                                           \
-      dcd_control_xfer(_rhport, _dir, (uint8_t*) _buffer, _len);              \
-      osal_semaphore_wait( _usbd_ctrl_sem, OSAL_TIMEOUT_CONTROL_XFER, &err ); \
-      STASK_ASSERT_ERR( err );                                                \
-    }                                                                         \
-    dcd_control_status(_rhport, _dir);                                        \
-    /* No need to wait for status phase to complete */                        \
-  }while(0)
-
+uint32_t usbd_control_xfer_st(uint8_t _rhport, uint8_t _dir, uint8_t* _buffer, uint16_t _len);
 
 /*------------------------------------------------------------------*/
 /* Other Helpers
diff --git a/src/osal/osal_freertos.h b/src/osal/osal_freertos.h
index ae988ec1..f2975922 100644
--- a/src/osal/osal_freertos.h
+++ b/src/osal/osal_freertos.h
@@ -183,4 +183,3 @@ static inline void osal_queue_reset(osal_queue_t const queue_hdl)
 
 /** @} */
 /** @} */
-
diff --git a/src/osal/osal_none.h b/src/osal/osal_none.h
index 796ddc15..6e67c8ea 100644
--- a/src/osal/osal_none.h
+++ b/src/osal/osal_none.h
@@ -60,8 +60,6 @@
 //
 //   OSAL_TASK_LOOP_ENG
 // }
-//
-// NOTE: no switch statement is allowed in Task and subtask
 //--------------------------------------------------------------------+
 #define OSAL_TASK_DEF(_name, _str, _func, _prio, _stack_sz)  osal_task_def_t _name;
 
@@ -73,52 +71,8 @@ static inline bool osal_task_create(osal_task_def_t* taskdef)
   return true;
 }
 
-#define TASK_RESTART                             \
-  _state = 0
-
-#define osal_task_delay(_msec)                      \
-  do {                                              \
-    _timeout = tusb_hal_millis();                   \
-    _state = __LINE__; case __LINE__:               \
-      if ( _timeout + (_msec) > tusb_hal_millis() ) \
-        return TUSB_ERROR_OSAL_WAITING;             \
-  }while(0)
-
 //--------------------------------------------------------------------+
-// SUBTASK (a sub function that uses OS blocking services & called by a task
-//--------------------------------------------------------------------+
-#define OSAL_SUBTASK_BEGIN                       \
-  static uint16_t _state = 0;                    \
-  ATTR_UNUSED static uint32_t _timeout = 0;      \
-  (void) _timeout;                               \
-  switch(_state) {                               \
-    case 0: {
-
-#define OSAL_SUBTASK_END                         \
-  default: TASK_RESTART; break;                  \
-  }}                                             \
-  return TUSB_ERROR_NONE;
-
-#define STASK_INVOKE(_subtask, _status)                                         \
-  do {                                                                          \
-    _state = __LINE__; case __LINE__:                                           \
-    {                                                                           \
-      (_status) = _subtask; /* invoke sub task */                               \
-      if (TUSB_ERROR_OSAL_WAITING == (_status)) return TUSB_ERROR_OSAL_WAITING; \
-    }                                                                           \
-  }while(0)
-
-//------------- Sub Task Assert -------------//
-#define STASK_RETURN(error)     do { TASK_RESTART; return error; } while(0)
-
-#define STASK_ASSERT_ERR(_err)                TU_VERIFY_ERR_HDLR(_err, TU_BREAKPOINT(); TASK_RESTART, TUSB_ERROR_FAILED)
-#define STASK_ASSERT_ERR_HDLR(_err, _func)    TU_VERIFY_ERR_HDLR(_err, TU_BREAKPOINT(); _func; TASK_RESTART, TUSB_ERROR_FAILED )
-
-#define STASK_ASSERT(_cond)                   TU_VERIFY_HDLR(_cond, TU_BREAKPOINT(); TASK_RESTART, TUSB_ERROR_FAILED)
-#define STASK_ASSERT_HDLR(_cond, _func)       TU_VERIFY_HDLR(_cond, TU_BREAKPOINT(); _func; TASK_RESTART, TUSB_ERROR_FAILED)
-
-//--------------------------------------------------------------------+
-// Semaphore API
+// Binary Semaphore API
 //--------------------------------------------------------------------+
 typedef struct
 {
@@ -135,7 +89,6 @@ static inline osal_semaphore_t osal_semaphore_create(osal_semaphore_def_t* semde
 
 static inline bool osal_semaphore_post(osal_semaphore_t sem_hdl, bool in_isr)
 {
-  (void) in_isr;
   sem_hdl->count++;
   return true;
 }
@@ -145,22 +98,21 @@ static inline void osal_semaphore_reset(osal_semaphore_t sem_hdl)
   sem_hdl->count = 0;
 }
 
-#define osal_semaphore_wait(_sem_hdl, _msec, _err)                                               \
-  do {                                                                                           \
-    _timeout = tusb_hal_millis();                                                                \
-    _state = __LINE__; case __LINE__:                                                            \
-    if( (_sem_hdl)->count == 0 ) {                                                               \
-      if ( ((_msec) != OSAL_TIMEOUT_WAIT_FOREVER) && (_timeout + (_msec) <= tusb_hal_millis()) ) \
-        *(_err) = TUSB_ERROR_OSAL_TIMEOUT;                                                       \
-      else                                                                                       \
-        return TUSB_ERROR_OSAL_WAITING;                                                          \
-    } else{                                                                                      \
-      /* Enter critical ? */                                                                     \
-      (_sem_hdl)->count--;                                                                       \
-      /* Exit critical ? */                                                                      \
-      *(_err) = TUSB_ERROR_NONE;                                                                 \
-    }                                                                                            \
-  }while(0)
+static inline tusb_error_t osal_semaphore_wait(osal_semaphore_t sem_hdl, uint32_t msec) {
+  (void) msec;
+  while (true) {
+      while (sem_hdl->count == 0) {
+      }
+      // tusb_hal_int_disable_all();
+      if (sem_hdl->count == 0) {
+          sem_hdl->count--;
+          // tusb_hal_int_enable_all();
+          break;
+      }
+      // tusb_hal_int_enable_all();
+  }
+  return TUSB_ERROR_NONE;
+}
 
 //--------------------------------------------------------------------+
 // MUTEX API
@@ -218,22 +170,13 @@ static inline void osal_queue_reset(osal_queue_t const queue_hdl)
   queue_hdl->count = queue_hdl->rd_idx = queue_hdl->wr_idx = 0;
 }
 
-#define osal_queue_receive(_q_hdl, p_data, _msec, _err)                                           \
-  do {                                                                                            \
-    _timeout = tusb_hal_millis();                                                                 \
-    _state = __LINE__; case __LINE__:                                                             \
-    if( (_q_hdl)->count == 0 ) {                                                                  \
-      if ( ((_msec) != OSAL_TIMEOUT_WAIT_FOREVER) && ( _timeout + (_msec) <= tusb_hal_millis()) ) \
-        *(_err) = TUSB_ERROR_OSAL_TIMEOUT;                                                        \
-      else                                                                                        \
-        return TUSB_ERROR_OSAL_WAITING;                                                           \
-    } else{                                                                                       \
-      /* Enter critical ? */                                                                      \
-      tu_fifo_read(_q_hdl, p_data);                                                               \
-      /* Exit critical ? */                                                                       \
-      *(_err) = TUSB_ERROR_NONE;                                                                  \
-    }                                                                                             \
-  }while(0)
+static inline tusb_error_t osal_queue_receive(osal_queue_t const queue_hdl, void* data) {
+  if (!tu_fifo_read(queue_hdl, data)) {
+    return TUSB_ERROR_OSAL_WAITING;
+  }
+  return TUSB_ERROR_NONE;
+}
+
 
 #ifdef __cplusplus
  }
diff --git a/src/portable/microchip/samd21/dcd.c b/src/portable/microchip/samd21/dcd.c
new file mode 100644
index 00000000..c9e88ad1
--- /dev/null
+++ b/src/portable/microchip/samd21/dcd.c
@@ -0,0 +1,341 @@
+/**************************************************************************/
+/*!
+    @file     dcd_nrf5x.c
+    @author   hathach
+
+    @section LICENSE
+
+    Software License Agreement (BSD License)
+
+    Copyright (c) 2018, Scott Shawcroft for Adafruit Industries
+    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 && CFG_TUSB_MCU == OPT_MCU_SAMD51
+
+#include "device/dcd.h"
+
+#include "device/usbd.h"
+#include "device/usbd_pvt.h" // to use defer function helper
+
+#include "class/msc/msc_device.h"
+
+#include "sam.h"
+
+/*------------------------------------------------------------------*/
+/* MACRO TYPEDEF CONSTANT ENUM
+ *------------------------------------------------------------------*/
+enum
+{
+  // Max allowed by USB specs
+  MAX_PACKET_SIZE   = 64,
+};
+
+UsbDeviceDescBank sram_registers[8][2];
+ATTR_ALIGNED(4) uint8_t control_out_buffer[64];
+ATTR_ALIGNED(4) uint8_t control_in_buffer[64];
+
+volatile uint32_t setup_count = 0;
+
+// Setup the control endpoint 0.
+static void bus_reset(void) {
+    // Max size of packets is 64 bytes.
+    UsbDeviceDescBank* bank_out = &sram_registers[0][TUSB_DIR_OUT];
+    bank_out->PCKSIZE.bit.SIZE = 0x3;
+    UsbDeviceDescBank* bank_in = &sram_registers[0][TUSB_DIR_IN];
+    bank_in->PCKSIZE.bit.SIZE = 0x3;
+
+    UsbDeviceEndpoint* ep = &USB->DEVICE.DeviceEndpoint[0];
+    ep->EPCFG.reg = USB_DEVICE_EPCFG_EPTYPE0(0x1) | USB_DEVICE_EPCFG_EPTYPE1(0x1);
+    ep->EPINTENSET.reg = USB_DEVICE_EPINTENSET_TRCPT0 | USB_DEVICE_EPINTENSET_TRCPT1 | USB_DEVICE_EPINTENSET_RXSTP;
+
+    dcd_edpt_xfer(0, 0, control_out_buffer, 64);
+    setup_count = 0;
+}
+
+
+/*------------------------------------------------------------------*/
+/* Controller API
+ *------------------------------------------------------------------*/
+bool dcd_init (uint8_t rhport)
+{
+  (void) rhport;
+  USB->DEVICE.DESCADD.reg = (uint32_t) &sram_registers;
+  USB->DEVICE.CTRLB.reg = USB_DEVICE_CTRLB_SPDCONF_FS;
+  USB->DEVICE.CTRLA.reg = USB_CTRLA_MODE_DEVICE | USB_CTRLA_ENABLE;
+  USB->DEVICE.INTENSET.reg = USB_DEVICE_INTENSET_SOF | USB_DEVICE_INTENSET_EORST;
+
+  return true;
+}
+
+void dcd_connect (uint8_t rhport)
+{
+
+}
+void dcd_disconnect (uint8_t rhport)
+{
+
+}
+
+void dcd_set_address (uint8_t rhport, uint8_t dev_addr)
+{
+  (void) rhport;
+  dcd_edpt_xfer (0, TUSB_DIR_IN_MASK, NULL, 0);
+  // Wait for EP0 to finish before switching the address.
+  while (USB->DEVICE.DeviceEndpoint[0].EPSTATUS.bit.BK1RDY == 1) {}
+  USB->DEVICE.DADD.reg = USB_DEVICE_DADD_DADD(dev_addr) | USB_DEVICE_DADD_ADDEN;
+}
+
+void dcd_set_config (uint8_t rhport, uint8_t config_num)
+{
+  (void) rhport;
+  (void) config_num;
+  // Nothing to do
+}
+
+/*------------------------------------------------------------------*/
+/* Control
+ *------------------------------------------------------------------*/
+
+bool dcd_control_xfer (uint8_t rhport, uint8_t dir, uint8_t * buffer, uint16_t length)
+{
+  (void) rhport;
+  uint8_t ep_addr = 0;
+  if (dir == TUSB_DIR_IN) {
+      ep_addr |= TUSB_DIR_IN_MASK;
+  }
+
+  return dcd_edpt_xfer (rhport, ep_addr, buffer, length);
+}
+
+bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
+{
+  (void) rhport;
+
+  uint8_t const epnum = edpt_number(desc_edpt->bEndpointAddress);
+  uint8_t const dir   = edpt_dir(desc_edpt->bEndpointAddress);
+
+  UsbDeviceDescBank* bank = &sram_registers[epnum][dir];
+  uint32_t size_value = 0;
+  while (size_value < 7) {
+    if (1 << (size_value + 3) == desc_edpt->wMaxPacketSize.size) {
+      break;
+    }
+    size_value++;
+  }
+  bank->PCKSIZE.bit.SIZE = size_value;
+
+  UsbDeviceEndpoint* ep = &USB->DEVICE.DeviceEndpoint[epnum];
+
+  if ( dir == TUSB_DIR_OUT )
+  {
+    ep->EPCFG.bit.EPTYPE0 = desc_edpt->bmAttributes.xfer + 1;
+    ep->EPINTENSET.bit.TRCPT0 = true;
+  }else
+  {
+    ep->EPCFG.bit.EPTYPE1 = desc_edpt->bmAttributes.xfer + 1;
+    ep->EPINTENSET.bit.TRCPT1 = true;
+  }
+  __ISB(); __DSB();
+
+  return true;
+}
+
+bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
+{
+  (void) rhport;
+
+  uint8_t const epnum = edpt_number(ep_addr);
+  uint8_t const dir   = edpt_dir(ep_addr);
+
+  UsbDeviceDescBank* bank = &sram_registers[epnum][dir];
+  UsbDeviceEndpoint* ep = &USB->DEVICE.DeviceEndpoint[epnum];
+
+  bank->ADDR.reg = (uint32_t) buffer;
+  if ( dir == TUSB_DIR_OUT )
+  {
+    bank->PCKSIZE.bit.MULTI_PACKET_SIZE = total_bytes;
+    bank->PCKSIZE.bit.BYTE_COUNT = 0;
+    ep->EPSTATUSCLR.reg |= USB_DEVICE_EPSTATUSCLR_BK0RDY;
+    ep->EPINTFLAG.reg |= USB_DEVICE_EPINTFLAG_TRFAIL0;
+  } else
+  {
+    bank->PCKSIZE.bit.MULTI_PACKET_SIZE = 0;
+    bank->PCKSIZE.bit.BYTE_COUNT = total_bytes;
+    // bank->PCKSIZE.bit.AUTO_ZLP = 1;
+    ep->EPSTATUSSET.reg |= USB_DEVICE_EPSTATUSSET_BK1RDY;
+    ep->EPINTFLAG.reg |= USB_DEVICE_EPINTFLAG_TRFAIL1;
+  }
+
+  return true;
+}
+
+bool dcd_edpt_stalled (uint8_t rhport, uint8_t ep_addr)
+{
+  (void) rhport;
+
+  // control is never got halted
+  if ( ep_addr == 0 ) {
+      return false;
+  }
+
+  uint8_t const epnum = edpt_number(ep_addr);
+  UsbDeviceEndpoint* ep = &USB->DEVICE.DeviceEndpoint[epnum];
+  return (edpt_dir(ep_addr) == TUSB_DIR_IN ) ? ep->EPINTFLAG.bit.STALL1 : ep->EPINTFLAG.bit.STALL0;
+}
+
+void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
+{
+  (void) rhport;
+
+  uint8_t const epnum = edpt_number(ep_addr);
+  UsbDeviceEndpoint* ep = &USB->DEVICE.DeviceEndpoint[epnum];
+
+  if (edpt_dir(ep_addr) == TUSB_DIR_IN) {
+      ep->EPSTATUSSET.reg = USB_DEVICE_EPSTATUSSET_STALLRQ1;
+  } else {
+      ep->EPSTATUSSET.reg = USB_DEVICE_EPSTATUSSET_STALLRQ0;
+  }
+
+  __ISB(); __DSB();
+}
+
+void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
+{
+  (void) rhport;
+
+  uint8_t const epnum = edpt_number(ep_addr);
+  UsbDeviceEndpoint* ep = &USB->DEVICE.DeviceEndpoint[epnum];
+
+  if (edpt_dir(ep_addr) == TUSB_DIR_IN) {
+    ep->EPSTATUSCLR.reg = USB_DEVICE_EPSTATUSCLR_STALLRQ1;
+  } else {
+    ep->EPSTATUSCLR.reg = USB_DEVICE_EPSTATUSCLR_STALLRQ0;
+  }
+}
+
+bool dcd_edpt_busy (uint8_t rhport, uint8_t ep_addr)
+{
+  (void) rhport;
+
+  // USBD shouldn't check control endpoint state
+  if ( 0 == ep_addr ) return false;
+
+  uint8_t const epnum = edpt_number(ep_addr);
+  UsbDeviceEndpoint* ep = &USB->DEVICE.DeviceEndpoint[epnum];
+
+  if (edpt_dir(ep_addr) == TUSB_DIR_IN) {
+    return ep->EPINTFLAG.bit.TRCPT1 == 0 && ep->EPSTATUS.bit.BK1RDY == 1;
+  }
+  return ep->EPINTFLAG.bit.TRCPT0 == 0 && ep->EPSTATUS.bit.BK0RDY == 1;
+}
+
+/*------------------------------------------------------------------*/
+
+static bool maybe_handle_setup_packet(void) {
+    if (USB->DEVICE.DeviceEndpoint[0].EPINTFLAG.bit.RXSTP)
+    {
+        USB->DEVICE.DeviceEndpoint[0].EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_RXSTP;
+
+        // This copies the data elsewhere so we can reuse the buffer.
+        dcd_event_setup_received(0, (uint8_t*) sram_registers[0][0].ADDR.reg, true);
+        dcd_edpt_xfer(0, 0, control_out_buffer, 64);
+        setup_count += 1;
+        return true;
+    }
+    return false;
+}
+
+void maybe_transfer_complete(void) {
+    uint32_t epints = USB->DEVICE.EPINTSMRY.reg;
+    for (uint8_t epnum = 0; epnum < USB_EPT_NUM; epnum++) {
+        if ((epints & (1 << epnum)) == 0) {
+            continue;
+        }
+
+        if (maybe_handle_setup_packet()) {
+            continue;
+        }
+        UsbDeviceEndpoint* ep = &USB->DEVICE.DeviceEndpoint[epnum];
+
+        uint32_t epintflag = ep->EPINTFLAG.reg;
+
+        // Handle IN completions
+        if ((epintflag & USB_DEVICE_EPINTFLAG_TRCPT1) != 0) {
+            ep->EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_TRCPT1;
+
+            UsbDeviceDescBank* bank = &sram_registers[epnum][TUSB_DIR_IN];
+            uint16_t total_transfer_size = bank->PCKSIZE.bit.BYTE_COUNT;
+
+            uint8_t ep_addr = epnum | TUSB_DIR_IN_MASK;
+            dcd_event_xfer_complete(0, ep_addr, total_transfer_size, DCD_XFER_SUCCESS, true);
+        }
+
+        // Handle OUT completions
+        if ((epintflag & USB_DEVICE_EPINTFLAG_TRCPT0) != 0) {
+            ep->EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_TRCPT0;
+
+            UsbDeviceDescBank* bank = &sram_registers[epnum][TUSB_DIR_OUT];
+            uint16_t total_transfer_size = bank->PCKSIZE.bit.BYTE_COUNT;
+
+            uint8_t ep_addr = epnum;
+            dcd_event_xfer_complete(0, ep_addr, total_transfer_size, DCD_XFER_SUCCESS, true);
+            if (epnum == 0) {
+                dcd_edpt_xfer(0, 0, control_out_buffer, 64);
+            }
+        }
+    }
+}
+
+void USB_Handler(void) {
+  uint32_t int_status = USB->DEVICE.INTFLAG.reg;
+
+  /*------------- Interrupt Processing -------------*/
+  if ( int_status & USB_DEVICE_INTFLAG_EORST )
+  {
+    USB->DEVICE.INTFLAG.reg = USB_DEVICE_INTENCLR_EORST;
+    bus_reset();
+    dcd_event_bus_signal(0, DCD_EVENT_BUS_RESET, true);
+  }
+
+  if ( int_status & USB_DEVICE_INTFLAG_SOF )
+  {
+    USB->DEVICE.INTFLAG.reg = USB_DEVICE_INTFLAG_SOF;
+    dcd_event_bus_signal(0, DCD_EVENT_SOF, true);
+  }
+
+  // Setup packet received.
+  maybe_handle_setup_packet();
+
+  // Handle complete transfer
+  maybe_transfer_complete();
+}
+
+#endif
diff --git a/src/portable/microchip/samd21/hal.c b/src/portable/microchip/samd21/hal.c
new file mode 100644
index 00000000..524840be
--- /dev/null
+++ b/src/portable/microchip/samd21/hal.c
@@ -0,0 +1,82 @@
+/**************************************************************************/
+/*!
+    @file     hal_nrf5x.c
+    @author   hathach
+
+    @section LICENSE
+
+    Software License Agreement (BSD License)
+
+    Copyright (c) 2018, 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 && CFG_TUSB_MCU == OPT_MCU_SAMD21
+
+#include "sam.h"
+
+#include "tusb_hal.h"
+
+/*------------------------------------------------------------------*/
+/* MACRO TYPEDEF CONSTANT ENUM
+ *------------------------------------------------------------------*/
+#define USB_NVIC_PRIO   7
+
+void tusb_hal_nrf_power_event(uint32_t event);
+
+/*------------------------------------------------------------------*/
+/* TUSB HAL
+ *------------------------------------------------------------------*/
+bool tusb_hal_init(void)
+{
+  USB->DEVICE.PADCAL.bit.TRANSP = (*((uint32_t*) USB_FUSES_TRANSP_ADDR) & USB_FUSES_TRANSP_Msk) >> USB_FUSES_TRANSP_Pos;
+  USB->DEVICE.PADCAL.bit.TRANSN = (*((uint32_t*) USB_FUSES_TRANSN_ADDR) & USB_FUSES_TRANSN_Msk) >> USB_FUSES_TRANSN_Pos;
+  USB->DEVICE.PADCAL.bit.TRIM = (*((uint32_t*) USB_FUSES_TRIM_ADDR) & USB_FUSES_TRIM_Msk) >> USB_FUSES_TRIM_Pos;
+
+  USB->DEVICE.QOSCTRL.bit.CQOS = USB_QOSCTRL_CQOS_HIGH_Val;
+  USB->DEVICE.QOSCTRL.bit.DQOS = USB_QOSCTRL_DQOS_HIGH_Val;
+
+  tusb_hal_int_enable(0);
+  return true;
+}
+
+void tusb_hal_int_enable(uint8_t rhport)
+{
+  (void) rhport;
+  NVIC_EnableIRQ(USB_IRQn);
+}
+
+void tusb_hal_int_disable(uint8_t rhport)
+{
+  (void) rhport;
+  NVIC_DisableIRQ(USB_IRQn);
+}
+
+#endif
diff --git a/src/portable/microchip/samd51/dcd.c b/src/portable/microchip/samd51/dcd.c
new file mode 100644
index 00000000..e67fbd29
--- /dev/null
+++ b/src/portable/microchip/samd51/dcd.c
@@ -0,0 +1,360 @@
+/**************************************************************************/
+/*!
+    @file     dcd_nrf5x.c
+    @author   hathach
+
+    @section LICENSE
+
+    Software License Agreement (BSD License)
+
+    Copyright (c) 2018, Scott Shawcroft for Adafruit Industries
+    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 && CFG_TUSB_MCU == OPT_MCU_SAMD51
+
+#include "device/dcd.h"
+
+#include "device/usbd.h"
+#include "device/usbd_pvt.h" // to use defer function helper
+
+#include "sam.h"
+
+/*------------------------------------------------------------------*/
+/* MACRO TYPEDEF CONSTANT ENUM
+ *------------------------------------------------------------------*/
+enum
+{
+  // Max allowed by USB specs
+  MAX_PACKET_SIZE   = 64,
+};
+
+UsbDeviceDescBank sram_registers[8][2];
+ATTR_ALIGNED(4) uint8_t control_out_buffer[64];
+ATTR_ALIGNED(4) uint8_t control_in_buffer[64];
+
+volatile uint32_t setup_count = 0;
+
+// Setup the control endpoint 0.
+static void bus_reset(void) {
+    // Max size of packets is 64 bytes.
+    UsbDeviceDescBank* bank_out = &sram_registers[0][TUSB_DIR_OUT];
+    bank_out->PCKSIZE.bit.SIZE = 0x3;
+    UsbDeviceDescBank* bank_in = &sram_registers[0][TUSB_DIR_IN];
+    bank_in->PCKSIZE.bit.SIZE = 0x3;
+
+    UsbDeviceEndpoint* ep = &USB->DEVICE.DeviceEndpoint[0];
+    ep->EPCFG.reg = USB_DEVICE_EPCFG_EPTYPE0(0x1) | USB_DEVICE_EPCFG_EPTYPE1(0x1);
+    ep->EPINTENSET.reg = USB_DEVICE_EPINTENSET_TRCPT0 | USB_DEVICE_EPINTENSET_TRCPT1 | USB_DEVICE_EPINTENSET_RXSTP;
+
+    dcd_edpt_xfer(0, 0, control_out_buffer, 64);
+    setup_count = 0;
+}
+
+
+/*------------------------------------------------------------------*/
+/* Controller API
+ *------------------------------------------------------------------*/
+bool dcd_init (uint8_t rhport)
+{
+  (void) rhport;
+  USB->DEVICE.DESCADD.reg = (uint32_t) &sram_registers;
+  USB->DEVICE.CTRLB.reg = USB_DEVICE_CTRLB_SPDCONF_FS;
+  USB->DEVICE.CTRLA.reg = USB_CTRLA_MODE_DEVICE | USB_CTRLA_ENABLE;
+  USB->DEVICE.INTENSET.reg = USB_DEVICE_INTENSET_SOF | USB_DEVICE_INTENSET_EORST;
+
+  return true;
+}
+
+void dcd_connect (uint8_t rhport)
+{
+
+}
+void dcd_disconnect (uint8_t rhport)
+{
+
+}
+
+void dcd_set_address (uint8_t rhport, uint8_t dev_addr)
+{
+  (void) rhport;
+  dcd_edpt_xfer (0, TUSB_DIR_IN_MASK, NULL, 0);
+  // Wait for EP0 to finish before switching the address.
+  while (USB->DEVICE.DeviceEndpoint[0].EPSTATUS.bit.BK1RDY == 1) {}
+  USB->DEVICE.DADD.reg = USB_DEVICE_DADD_DADD(dev_addr) | USB_DEVICE_DADD_ADDEN;
+}
+
+void dcd_set_config (uint8_t rhport, uint8_t config_num)
+{
+  (void) rhport;
+  (void) config_num;
+  // Nothing to do
+}
+
+/*------------------------------------------------------------------*/
+/* Control
+ *------------------------------------------------------------------*/
+
+bool dcd_control_xfer (uint8_t rhport, uint8_t dir, uint8_t * buffer, uint16_t length)
+{
+  (void) rhport;
+  uint8_t ep_addr = 0;
+  if (dir == TUSB_DIR_IN) {
+      ep_addr |= TUSB_DIR_IN_MASK;
+  }
+
+  return dcd_edpt_xfer (rhport, ep_addr, buffer, length);
+}
+
+bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
+{
+  (void) rhport;
+
+  uint8_t const epnum = edpt_number(desc_edpt->bEndpointAddress);
+  uint8_t const dir   = edpt_dir(desc_edpt->bEndpointAddress);
+
+  UsbDeviceDescBank* bank = &sram_registers[epnum][dir];
+  uint32_t size_value = 0;
+  while (size_value < 7) {
+    if (1 << (size_value + 3) == desc_edpt->wMaxPacketSize.size) {
+      break;
+    }
+    size_value++;
+  }
+  bank->PCKSIZE.bit.SIZE = size_value;
+
+  UsbDeviceEndpoint* ep = &USB->DEVICE.DeviceEndpoint[epnum];
+
+  if ( dir == TUSB_DIR_OUT )
+  {
+    ep->EPCFG.bit.EPTYPE0 = desc_edpt->bmAttributes.xfer + 1;
+    ep->EPINTENSET.bit.TRCPT0 = true;
+  }else
+  {
+    ep->EPCFG.bit.EPTYPE1 = desc_edpt->bmAttributes.xfer + 1;
+    ep->EPINTENSET.bit.TRCPT1 = true;
+  }
+  __ISB(); __DSB();
+
+  return true;
+}
+
+bool dcd_edpt_xfer (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes)
+{
+  (void) rhport;
+
+  uint8_t const epnum = edpt_number(ep_addr);
+  uint8_t const dir   = edpt_dir(ep_addr);
+
+  UsbDeviceDescBank* bank = &sram_registers[epnum][dir];
+  UsbDeviceEndpoint* ep = &USB->DEVICE.DeviceEndpoint[epnum];
+
+  bank->ADDR.reg = (uint32_t) buffer;
+  if ( dir == TUSB_DIR_OUT )
+  {
+    bank->PCKSIZE.bit.MULTI_PACKET_SIZE = total_bytes;
+    bank->PCKSIZE.bit.BYTE_COUNT = 0;
+    ep->EPSTATUSCLR.reg |= USB_DEVICE_EPSTATUSCLR_BK0RDY;
+    ep->EPINTFLAG.reg |= USB_DEVICE_EPINTFLAG_TRFAIL0;
+  } else
+  {
+    bank->PCKSIZE.bit.MULTI_PACKET_SIZE = 0;
+    bank->PCKSIZE.bit.BYTE_COUNT = total_bytes;
+    ep->EPSTATUSSET.reg |= USB_DEVICE_EPSTATUSSET_BK1RDY;
+    ep->EPINTFLAG.reg |= USB_DEVICE_EPINTFLAG_TRFAIL1;
+  }
+
+  return true;
+}
+
+bool dcd_edpt_stalled (uint8_t rhport, uint8_t ep_addr)
+{
+  (void) rhport;
+
+  // control is never got halted
+  if ( ep_addr == 0 ) {
+      return false;
+  }
+
+  uint8_t const epnum = edpt_number(ep_addr);
+  UsbDeviceEndpoint* ep = &USB->DEVICE.DeviceEndpoint[epnum];
+  return (edpt_dir(ep_addr) == TUSB_DIR_IN ) ? ep->EPINTFLAG.bit.STALL1 : ep->EPINTFLAG.bit.STALL0;
+}
+
+void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
+{
+  (void) rhport;
+
+  uint8_t const epnum = edpt_number(ep_addr);
+  UsbDeviceEndpoint* ep = &USB->DEVICE.DeviceEndpoint[epnum];
+
+  if (edpt_dir(ep_addr) == TUSB_DIR_IN) {
+      ep->EPSTATUSSET.reg = USB_DEVICE_EPSTATUSSET_STALLRQ1;
+  } else {
+      ep->EPSTATUSSET.reg = USB_DEVICE_EPSTATUSSET_STALLRQ0;
+  }
+
+  __ISB(); __DSB();
+}
+
+void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
+{
+  (void) rhport;
+
+  uint8_t const epnum = edpt_number(ep_addr);
+  UsbDeviceEndpoint* ep = &USB->DEVICE.DeviceEndpoint[epnum];
+
+  if (edpt_dir(ep_addr) == TUSB_DIR_IN) {
+    ep->EPSTATUSCLR.reg = USB_DEVICE_EPSTATUSCLR_STALLRQ1;
+  } else {
+    ep->EPSTATUSCLR.reg = USB_DEVICE_EPSTATUSCLR_STALLRQ0;
+  }
+}
+
+bool dcd_edpt_busy (uint8_t rhport, uint8_t ep_addr)
+{
+  (void) rhport;
+
+  // USBD shouldn't check control endpoint state
+  if ( 0 == ep_addr ) return false;
+
+  uint8_t const epnum = edpt_number(ep_addr);
+  UsbDeviceEndpoint* ep = &USB->DEVICE.DeviceEndpoint[epnum];
+
+  if (edpt_dir(ep_addr) == TUSB_DIR_IN) {
+    return ep->EPINTFLAG.bit.TRCPT1 == 0 && ep->EPSTATUS.bit.BK1RDY == 1;
+  }
+  return ep->EPINTFLAG.bit.TRCPT0 == 0 && ep->EPSTATUS.bit.BK0RDY == 1;
+}
+
+/*------------------------------------------------------------------*/
+
+static bool maybe_handle_setup_packet(void) {
+    if (USB->DEVICE.DeviceEndpoint[0].EPINTFLAG.bit.RXSTP)
+    {
+        USB->DEVICE.DeviceEndpoint[0].EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_RXSTP;
+        // uint8_t* buf = (uint8_t*) sram_registers[0][0].ADDR.reg;
+        //
+        // if (buf[6] == 0x12) asm("bkpt");
+        // This copies the data elsewhere so we can reuse the buffer.
+        dcd_event_setup_received(0, (uint8_t*) sram_registers[0][0].ADDR.reg, true);
+        dcd_edpt_xfer(0, 0, control_out_buffer, 64);
+        setup_count += 1;
+        return true;
+    }
+    return false;
+}
+/*
+ *------------------------------------------------------------------*/
+/* USB_EORSM_DNRSM, USB_EORST_RST, USB_LPMSUSP_DDISC, USB_LPM_DCONN,
+USB_MSOF, USB_RAMACER, USB_RXSTP_TXSTP_0, USB_RXSTP_TXSTP_1,
+USB_RXSTP_TXSTP_2, USB_RXSTP_TXSTP_3, USB_RXSTP_TXSTP_4,
+USB_RXSTP_TXSTP_5, USB_RXSTP_TXSTP_6, USB_RXSTP_TXSTP_7,
+USB_STALL0_STALL_0, USB_STALL0_STALL_1, USB_STALL0_STALL_2,
+USB_STALL0_STALL_3, USB_STALL0_STALL_4, USB_STALL0_STALL_5,
+USB_STALL0_STALL_6, USB_STALL0_STALL_7, USB_STALL1_0, USB_STALL1_1,
+USB_STALL1_2, USB_STALL1_3, USB_STALL1_4, USB_STALL1_5, USB_STALL1_6,
+USB_STALL1_7, USB_SUSPEND, USB_TRFAIL0_TRFAIL_0, USB_TRFAIL0_TRFAIL_1,
+USB_TRFAIL0_TRFAIL_2, USB_TRFAIL0_TRFAIL_3, USB_TRFAIL0_TRFAIL_4,
+USB_TRFAIL0_TRFAIL_5, USB_TRFAIL0_TRFAIL_6, USB_TRFAIL0_TRFAIL_7,
+USB_TRFAIL1_PERR_0, USB_TRFAIL1_PERR_1, USB_TRFAIL1_PERR_2,
+USB_TRFAIL1_PERR_3, USB_TRFAIL1_PERR_4, USB_TRFAIL1_PERR_5,
+USB_TRFAIL1_PERR_6, USB_TRFAIL1_PERR_7, USB_UPRSM, USB_WAKEUP */
+void USB_0_Handler(void) {
+  uint32_t int_status = USB->DEVICE.INTFLAG.reg;
+
+  /*------------- Interrupt Processing -------------*/
+  if ( int_status & USB_DEVICE_INTFLAG_EORST )
+  {
+    USB->DEVICE.INTFLAG.reg = USB_DEVICE_INTENCLR_EORST;
+    bus_reset();
+    dcd_event_bus_signal(0, DCD_EVENT_BUS_RESET, true);
+  }
+
+  // Setup packet received.
+  maybe_handle_setup_packet();
+}
+/* USB_SOF_HSOF */
+void USB_1_Handler(void) {
+    USB->DEVICE.INTFLAG.reg = USB_DEVICE_INTFLAG_SOF;
+    dcd_event_bus_signal(0, DCD_EVENT_SOF, true);
+}
+
+void transfer_complete(uint8_t direction) {
+        // uint8_t* buf = (uint8_t*) sram_registers[0][0].ADDR.reg;
+        //
+        // if (buf[6] == 0x12 || setup_count == 2) asm("bkpt");
+    uint32_t epints = USB->DEVICE.EPINTSMRY.reg;
+    for (uint8_t epnum = 0; epnum < USB_EPT_NUM; epnum++) {
+        if ((epints & (1 << epnum)) == 0) {
+            continue;
+        }
+
+        if (direction == TUSB_DIR_OUT && maybe_handle_setup_packet()) {
+            continue;
+        }
+        UsbDeviceEndpoint* ep = &USB->DEVICE.DeviceEndpoint[epnum];
+
+        UsbDeviceDescBank* bank = &sram_registers[epnum][direction];
+        uint16_t total_transfer_size = bank->PCKSIZE.bit.BYTE_COUNT;
+
+        uint8_t ep_addr = epnum;
+        if (direction == TUSB_DIR_IN) {
+            ep_addr |= TUSB_DIR_IN_MASK;
+        }
+        dcd_event_xfer_complete(0, ep_addr, total_transfer_size, DCD_XFER_SUCCESS, true);
+        if (epnum == 0 && direction == TUSB_DIR_OUT) {
+            dcd_edpt_xfer(0, 0, control_out_buffer, 64);
+        }
+        if (direction == TUSB_DIR_IN) {
+            ep->EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_TRCPT1;
+        } else {
+            ep->EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_TRCPT0;
+        }
+    }
+}
+
+// Bank zero is for OUT and SETUP transactions.
+/* USB_TRCPT0_0, USB_TRCPT0_1, USB_TRCPT0_2,
+USB_TRCPT0_3, USB_TRCPT0_4, USB_TRCPT0_5,
+USB_TRCPT0_6, USB_TRCPT0_7 */
+void USB_2_Handler(void) {
+    transfer_complete(TUSB_DIR_OUT);
+}
+
+// Bank one is used for IN transactions.
+/* USB_TRCPT1_0, USB_TRCPT1_1, USB_TRCPT1_2,
+USB_TRCPT1_3, USB_TRCPT1_4, USB_TRCPT1_5,
+USB_TRCPT1_6, USB_TRCPT1_7 */
+void USB_3_Handler(void) {
+    transfer_complete(TUSB_DIR_IN);
+}
+
+#endif
diff --git a/src/portable/microchip/samd51/hal.c b/src/portable/microchip/samd51/hal.c
new file mode 100644
index 00000000..d8c71a7f
--- /dev/null
+++ b/src/portable/microchip/samd51/hal.c
@@ -0,0 +1,88 @@
+/**************************************************************************/
+/*!
+    @file     hal_nrf5x.c
+    @author   hathach
+
+    @section LICENSE
+
+    Software License Agreement (BSD License)
+
+    Copyright (c) 2018, 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 && CFG_TUSB_MCU == OPT_MCU_SAMD51
+
+#include "sam.h"
+
+#include "tusb_hal.h"
+
+/*------------------------------------------------------------------*/
+/* MACRO TYPEDEF CONSTANT ENUM
+ *------------------------------------------------------------------*/
+#define USB_NVIC_PRIO   7
+
+void tusb_hal_nrf_power_event(uint32_t event);
+
+/*------------------------------------------------------------------*/
+/* TUSB HAL
+ *------------------------------------------------------------------*/
+bool tusb_hal_init(void)
+{
+  USB->DEVICE.PADCAL.bit.TRANSP = (*((uint32_t*) USB_FUSES_TRANSP_ADDR) & USB_FUSES_TRANSP_Msk) >> USB_FUSES_TRANSP_Pos;
+  USB->DEVICE.PADCAL.bit.TRANSN = (*((uint32_t*) USB_FUSES_TRANSN_ADDR) & USB_FUSES_TRANSN_Msk) >> USB_FUSES_TRANSN_Pos;
+  USB->DEVICE.PADCAL.bit.TRIM = (*((uint32_t*) USB_FUSES_TRIM_ADDR) & USB_FUSES_TRIM_Msk) >> USB_FUSES_TRIM_Pos;
+
+  USB->DEVICE.QOSCTRL.bit.CQOS = 3;
+  USB->DEVICE.QOSCTRL.bit.DQOS = 3;
+
+  tusb_hal_int_enable(0);
+  return true;
+}
+
+void tusb_hal_int_enable(uint8_t rhport)
+{
+  (void) rhport;
+  NVIC_EnableIRQ(USB_0_IRQn);
+  NVIC_EnableIRQ(USB_1_IRQn);
+  NVIC_EnableIRQ(USB_2_IRQn);
+  NVIC_EnableIRQ(USB_3_IRQn);
+}
+
+void tusb_hal_int_disable(uint8_t rhport)
+{
+  (void) rhport;
+  NVIC_DisableIRQ(USB_3_IRQn);
+  NVIC_DisableIRQ(USB_2_IRQn);
+  NVIC_DisableIRQ(USB_1_IRQn);
+  NVIC_DisableIRQ(USB_0_IRQn);
+}
+
+#endif
diff --git a/src/portable/nordic/nrf5x/dcd_nrf5x.c b/src/portable/nordic/nrf5x/dcd_nrf5x.c
index 69e00124..7eeab130 100644
--- a/src/portable/nordic/nrf5x/dcd_nrf5x.c
+++ b/src/portable/nordic/nrf5x/dcd_nrf5x.c
@@ -203,7 +203,9 @@ 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 )
   {
@@ -216,9 +218,11 @@ bool dcd_control_xfer (uint8_t rhport, uint8_t dir, uint8_t * buffer, uint16_t l
     xact_control_start();
   }else
   {
+    NRF_USBD->EPIN[0].PTR        = 0;
+    NRF_USBD->EPIN[0].MAXCNT     = 0;
     // Status Phase also require Easy DMA has to be free as well !!!!
-    edpt_dma_start(&NRF_USBD->TASKS_EP0STATUS);
-    edpt_dma_end();
+    NRF_USBD->TASKS_EP0STATUS = 1;
+    osal_semaphore_post(_usbd_ctrl_sem, false);
   }
 
   return true;
@@ -434,7 +438,7 @@ void USBD_IRQHandler(void)
         NRF_USBD->BMREQUESTTYPE , NRF_USBD->BREQUEST, NRF_USBD->WVALUEL , NRF_USBD->WVALUEH,
         NRF_USBD->WINDEXL       , NRF_USBD->WINDEXH , NRF_USBD->WLENGTHL, NRF_USBD->WLENGTHH
     };
-    dcd_event_setup_recieved(0, setup, true);
+    dcd_event_setup_received(0, setup, true);
   }
 
   if ( int_status & USBD_INTEN_EP0DATADONE_Msk )
diff --git a/src/tusb_option.h b/src/tusb_option.h
index 84636712..d7a6c6fe 100644
--- a/src/tusb_option.h
+++ b/src/tusb_option.h
@@ -147,6 +147,8 @@
 
   #ifndef CFG_TUD_ENUM_BUFFER_SIZE
     #define CFG_TUD_CTRL_BUFSIZE 256
+  #else
+    #define CFG_TUD_CTRL_BUFSIZE CFG_TUD_ENUM_BUFFER_SIZE
   #endif
 
   #ifndef CFG_TUD_DESC_AUTO