diff --git a/src/device/dcd.h b/src/device/dcd.h
index bd0ea852e..f07bda2d3 100644
--- a/src/device/dcd.h
+++ b/src/device/dcd.h
@@ -124,10 +124,14 @@ void dcd_event_xfer_complete (uint8_t rhport, uint8_t ep_addr, uint32_t xferred_
 
 /*------------------------------------------------------------------*/
 /* Endpoint API
- * Note:
- * - Address of control endpoint OUT is 0x00, In is 0x80
- * - When stalling control endpoint both control OUT and IN must be stalled
- * (according to USB spec, stalled control is only recovered with setup token)
+ *  - open        : Configure endpoint's registers
+ *  - xfer        : Submit a transfer. When complete dcd_event_xfer_complete
+ *                  must be called to notify the stack
+ *  - busy        : Check if endpoint transferring is complete (TODO remove)
+ *  - stall       : stall ep. When control endpoint (addr = 0) is stalled,
+ *                  both direction (IN & OUT) of control ep must be stalled.
+ *  - clear_stall : clear stall
+ *  - stalled     : check if stalled ( TODO remove )
  *------------------------------------------------------------------*/
 bool dcd_edpt_open        (uint8_t rhport, tusb_desc_endpoint_t const * p_endpoint_desc);
 bool dcd_edpt_xfer        (uint8_t rhport, uint8_t ep_addr, uint8_t * buffer, uint16_t total_bytes);
diff --git a/src/device/usbd.c b/src/device/usbd.c
index eac1a6a20..f39605987 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -174,7 +174,7 @@ static bool process_set_config(uint8_t rhport, uint8_t config_number);
 static void const* get_descriptor(tusb_control_request_t const * p_request, uint16_t* desc_len);
 
 void usbd_control_reset (uint8_t rhport);
-tusb_error_t usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, tusb_event_t event, uint32_t xferred_bytes);
+bool usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, tusb_event_t event, uint32_t xferred_bytes);
 void usbd_control_set_complete_callback( bool (*fp) (uint8_t, tusb_control_request_t const * ) );
 
 //--------------------------------------------------------------------+
@@ -338,7 +338,10 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
     switch ( p_request->bRequest )
     {
       case TUSB_REQ_SET_ADDRESS:
+        // response with status first before changing device address
+        usbd_control_status(rhport, p_request);
         dcd_set_address(rhport, (uint8_t) p_request->wValue);
+        return true; // skip the rest
       break;
 
       case TUSB_REQ_GET_CONFIGURATION:
@@ -362,7 +365,9 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
         if ( data_buf == NULL || data_len == 0 ) return false;
       break;
 
-      default: return false;
+      default:
+        TU_BREAKPOINT();
+      return false;
     }
 
     usbd_control_xfer(rhport, p_request, data_buf, data_len);
@@ -405,12 +410,15 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
         usbd_control_status(rhport, p_request);
       break;
 
-      default: return false;
+      default:
+        TU_BREAKPOINT();
+      return false;
     }
   }
   else
   {
     //------------- Unsupported Request -------------//
+    TU_BREAKPOINT();
     return false;
   }
 
@@ -549,10 +557,13 @@ void dcd_event_handler(dcd_event_t const * event, bool in_isr)
   {
     case DCD_EVENT_BUS_RESET:
     case DCD_EVENT_UNPLUGGED:
-    case DCD_EVENT_SOF:
       osal_queue_send(_usbd_q, event, in_isr);
     break;
 
+    case DCD_EVENT_SOF:
+      // nothing to do now
+    break;
+
     case DCD_EVENT_SUSPENDED:
       // TODO support suspended
     break;
diff --git a/src/device/usbd_control.c b/src/device/usbd_control.c
index a9687a3e6..8564d1ae9 100644
--- a/src/device/usbd_control.c
+++ b/src/device/usbd_control.c
@@ -126,7 +126,7 @@ bool usbd_control_xfer(uint8_t rhport, tusb_control_request_t const * request, v
 }
 
 // callback when a transaction complete on DATA stage of control endpoint
-tusb_error_t usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, tusb_event_t event, uint32_t xferred_bytes)
+bool usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, tusb_event_t event, uint32_t xferred_bytes)
 {
   if ( _control_state.request.bmRequestType_bit.direction == TUSB_DIR_OUT )
   {
@@ -151,7 +151,7 @@ tusb_error_t usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, tusb_event_t
     if ( is_ok )
     {
       // Send status
-      TU_ASSERT( usbd_control_status(rhport, &_control_state.request), TUSB_ERROR_FAILED );
+      TU_ASSERT( usbd_control_status(rhport, &_control_state.request) );
     }else
     {
       // stall due to callback
@@ -161,10 +161,10 @@ tusb_error_t usbd_control_xfer_cb (uint8_t rhport, uint8_t ep_addr, tusb_event_t
   else
   {
     // More data to transfer
-    TU_ASSERT(start_control_data_xact(rhport), TUSB_ERROR_FAILED);
+    TU_ASSERT( start_control_data_xact(rhport) );
   }
 
-  return TUSB_ERROR_NONE;
+  return true;
 }
 
 #endif
diff --git a/src/portable/microchip/samd51/dcd_samd51.c b/src/portable/microchip/samd51/dcd_samd51.c
index e67fbd295..971210464 100644
--- a/src/portable/microchip/samd51/dcd_samd51.c
+++ b/src/portable/microchip/samd51/dcd_samd51.c
@@ -56,9 +56,8 @@ enum
   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];
+static UsbDeviceDescBank sram_registers[8][2];
+static ATTR_ALIGNED(4) uint8_t setup_packet[8];
 
 volatile uint32_t setup_count = 0;
 
@@ -74,7 +73,8 @@ static void bus_reset(void) {
     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);
+    // Prepare for setup packet
+    dcd_edpt_xfer(0, 0, setup_packet, sizeof(setup_packet));
     setup_count = 0;
 }
 
@@ -105,7 +105,7 @@ 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;
@@ -119,20 +119,9 @@ void dcd_set_config (uint8_t rhport, uint8_t config_num)
 }
 
 /*------------------------------------------------------------------*/
-/* Control
+/* DCD Endpoint
  *------------------------------------------------------------------*/
 
-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;
@@ -161,7 +150,6 @@ bool dcd_edpt_open (uint8_t rhport, tusb_desc_endpoint_t const * desc_edpt)
     ep->EPCFG.bit.EPTYPE1 = desc_edpt->bmAttributes.xfer + 1;
     ep->EPINTENSET.bit.TRCPT1 = true;
   }
-  __ISB(); __DSB();
 
   return true;
 }
@@ -219,9 +207,12 @@ void dcd_edpt_stall (uint8_t rhport, uint8_t ep_addr)
       ep->EPSTATUSSET.reg = USB_DEVICE_EPSTATUSSET_STALLRQ1;
   } else {
       ep->EPSTATUSSET.reg = USB_DEVICE_EPSTATUSSET_STALLRQ0;
-  }
 
-  __ISB(); __DSB();
+      // for control, stall both IN & OUT
+      if (ep_addr == 0) {
+        ep->EPSTATUSSET.reg = USB_DEVICE_EPSTATUSSET_STALLRQ1;
+      }
+  }
 }
 
 void dcd_edpt_clear_stall (uint8_t rhport, uint8_t ep_addr)
@@ -260,12 +251,12 @@ 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;
     }
@@ -330,9 +321,12 @@ void transfer_complete(uint8_t direction) {
             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);
+
+        // just finished status stage (total size = 0), prepare for next setup packet
+        if (epnum == 0 && total_transfer_size == 0) {
+            dcd_edpt_xfer(0, 0, setup_packet, sizeof(setup_packet));
         }
+
         if (direction == TUSB_DIR_IN) {
             ep->EPINTFLAG.reg = USB_DEVICE_EPINTFLAG_TRCPT1;
         } else {