Change to control complete cb

3 files changed, 24 insertions, 40 deletions

diff --git a/src/device/dcd.h b/src/device/dcd.h
index 7755ef107..2248cd392 100644
--- a/src/device/dcd.h
+++ b/src/device/dcd.h
@@ -58,7 +58,6 @@ typedef enum
 
   DCD_EVENT_SETUP_RECEIVED,
   DCD_EVENT_XFER_COMPLETE,
-  DCD_EVENT_TEST_MODE,
 
   // Not an DCD event, just a convenient way to defer ISR function
   USBD_EVENT_FUNC_CALL,
@@ -71,7 +70,7 @@ typedef struct TU_ATTR_ALIGNED(4)
   uint8_t rhport;
   uint8_t event_id;
 
-  union
+   union
   {
     // BUS RESET
     struct {
@@ -98,11 +97,6 @@ typedef struct TU_ATTR_ALIGNED(4)
       void (*func) (void*);
       void* param;
     }func_call;
-
-    // TEST MODE
-    struct {
-      uint8_t selector;
-    }test_mode;
   };
 } dcd_event_t;
 
@@ -164,8 +158,8 @@ void dcd_disconnect(uint8_t rhport) TU_ATTR_WEAK;
 // Enable/Disable Start-of-frame interrupt. Default is disabled
 void dcd_sof_enable(uint8_t rhport, bool en);
 
-// Check if the test mode is supported
-bool dcd_test_mode_supported(test_mode_t test_selector) TU_ATTR_WEAK;
+// Check if the test mode is supported, returns true is test mode selector is supported
+bool dcd_check_test_mode_support(test_mode_t test_selector) TU_ATTR_WEAK;
 
 // Put device into a test mode (needs power cycle to quit)
 void dcd_enter_test_mode(uint8_t rhport, test_mode_t test_selector) TU_ATTR_WEAK;
@@ -261,13 +255,6 @@ static inline void dcd_event_sof(uint8_t rhport, uint32_t frame_count, bool in_i
   dcd_event_handler(&event, in_isr);
 }
 
-TU_ATTR_ALWAYS_INLINE static inline void dcd_event_enter_test_mode(uint8_t rhport, uint8_t test_selector, bool in_isr)
-{
-  dcd_event_t event = { .rhport = rhport, .event_id = DCD_EVENT_TEST_MODE };
-  event.test_mode.selector = test_selector;
-  dcd_event_handler(&event, in_isr);
-}
-
 #ifdef __cplusplus
  }
 #endif
diff --git a/src/device/usbd.c b/src/device/usbd.c
index 90fde92b5..fbdba4d2f 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -285,6 +285,7 @@ tu_static osal_queue_t _usbd_q;
 static bool process_control_request(uint8_t rhport, tusb_control_request_t const * p_request);
 static bool process_set_config(uint8_t rhport, uint8_t cfg_num);
 static bool process_get_descriptor(uint8_t rhport, tusb_control_request_t const * p_request);
+static bool process_test_mode_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
 
 // from usbd_control.c
 void usbd_control_reset(void);
@@ -587,11 +588,6 @@ void tud_task_ext(uint32_t timeout_ms, bool in_isr)
         if ( event.func_call.func ) event.func_call.func(event.func_call.param);
       break;
 
-      case DCD_EVENT_TEST_MODE:
-        TU_LOG_USBD(": Enter Test Mode with selector index = %d)\r\n", event.test_mode.selector);
-        if (dcd_enter_test_mode) dcd_enter_test_mode(event.rhport, event.test_mode.selector);
-      break;
-
       case DCD_EVENT_SOF:
       default:
         TU_BREAKPOINT();
@@ -740,35 +736,29 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
               // Host may enable remote wake up before suspending especially HID device
               _usbd_dev.remote_wakeup_en = true;
               tud_control_status(rhport, p_request);
-
             break;
 
             // Support for TEST_MODE
             case TUSB_REQ_FEATURE_TEST_MODE:
-              // Only handle the test mode is supported and valid
-              if (!dcd_enter_test_mode || !dcd_test_mode_supported || 0 != tu_u16_low(p_request->wIndex))
-              {
-                return false;
-              }
+              // Only handle the test mode if supported and valid
+              TU_VERIFY(dcd_enter_test_mode && dcd_check_test_mode_support && 0 == tu_u16_low(p_request->wIndex));
 
               uint8_t selector = tu_u16_high(p_request->wIndex);
 
               // Stall request if the selected test mode isn't supported
-              if (!dcd_test_mode_supported(selector))
+              if (!dcd_check_test_mode_support(selector))
               {
                 TU_LOG_USBD("    Unsupported Test Mode (test selector index: %d)\r\n", selector);
 
                 return false;
               }
 
-              TU_LOG_USBD("    Schedule Test Mode (test selector index: %d)\r\n", selector);
-
               // Acknowledge request
               tud_control_status(rhport, p_request);
 
-              // Schedule the execution of the test mode so that the request can be answered
-              dcd_event_enter_test_mode(rhport, selector, false);
+              TU_LOG_USBD("    Enter Test Mode (test selector index: %d)\r\n", selector);
 
+              usbd_control_set_complete_callback(process_test_mode_cb);
             break;
 
             // Stall unsupported feature selector
@@ -1115,6 +1105,18 @@ static bool process_get_descriptor(uint8_t rhport, tusb_control_request_t const
   }
 }
 
+bool process_test_mode_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request)
+{
+  // At this point it should already be ensured that dcd_enter_test_mode() is defined
+
+  // Only enter the test mode after the request for it has completed
+  TU_VERIFY(CONTROL_STAGE_ACK == stage);
+
+  dcd_enter_test_mode(rhport, tu_u16_high(request->wIndex));
+
+  return true;
+}
+
 //--------------------------------------------------------------------+
 // DCD Event Handler
 //--------------------------------------------------------------------+
diff --git a/src/portable/synopsys/dwc2/dcd_dwc2.c b/src/portable/synopsys/dwc2/dcd_dwc2.c
index ee4b2b28d..aa4a8bfb2 100644
--- a/src/portable/synopsys/dwc2/dcd_dwc2.c
+++ b/src/portable/synopsys/dwc2/dcd_dwc2.c
@@ -1353,7 +1353,7 @@ void dcd_int_handler(uint8_t rhport)
 
 #if defined(TUP_USBIP_DWC2_TEST_MODE_SUPPORT)
 
-bool dcd_test_mode_supported(test_mode_t test_selector) {
+bool dcd_check_test_mode_support(test_mode_t test_selector) {
   // Check if test mode selector is unsupported
   if (TEST_FORCE_ENABLE < test_selector || TEST_J > test_selector) {
     return false;
@@ -1363,15 +1363,10 @@ bool dcd_test_mode_supported(test_mode_t test_selector) {
 }
 
 void dcd_enter_test_mode(uint8_t rhport, test_mode_t test_selector) {
-  // Disable test mode if not supported
-  if (!dcd_test_mode_supported(test_selector)) {
+  // Disable test mode if not supported as a fall back
+  if (!dcd_check_test_mode_support(test_selector)) {
     test_selector = 0;
   }
-
-  // Delay the entering a bit so there is enough time to acknowledge request
-  uint32_t count = SystemCoreClock / 20000;
-  while (count--) __NOP();
-
   // Get port address...
   dwc2_regs_t* dwc2 = DWC2_REG(rhport);