Add DWC2 Test Mode SUpport

4 files changed, 107 insertions, 6 deletions

diff --git a/src/common/tusb_mcu.h b/src/common/tusb_mcu.h
index 35f94efbc..146accabf 100644
--- a/src/common/tusb_mcu.h
+++ b/src/common/tusb_mcu.h
@@ -195,6 +195,7 @@
 #elif TU_CHECK_MCU(OPT_MCU_STM32F7)
   #define TUP_USBIP_DWC2
   #define TUP_USBIP_DWC2_STM32
+  #define TUP_USBIP_DWC2_TEST_MODE_SUPPORT
 
   // FS has 6, HS has 9
   #define TUP_DCD_ENDPOINT_MAX    9
@@ -262,6 +263,7 @@
   #define TUP_USBIP_DWC2
   #define TUP_USBIP_DWC2_STM32
   #define TUP_DCD_ENDPOINT_MAX    6
+  #define TUP_USBIP_DWC2_TEST_MODE_SUPPORT
 
 #elif TU_CHECK_MCU(OPT_MCU_STM32L5)
   #define TUP_USBIP_FSDEV
diff --git a/src/device/dcd.h b/src/device/dcd.h
index 18a708347..7755ef107 100644
--- a/src/device/dcd.h
+++ b/src/device/dcd.h
@@ -58,6 +58,7 @@ 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,
@@ -97,9 +98,23 @@ typedef struct TU_ATTR_ALIGNED(4)
       void (*func) (void*);
       void* param;
     }func_call;
+
+    // TEST MODE
+    struct {
+      uint8_t selector;
+    }test_mode;
   };
 } dcd_event_t;
 
+typedef enum _test_mode_selector
+{
+  TEST_J = 1,
+  TEST_K,
+  TEST_SE0_NAK,
+  TEST_PACKET,
+  TEST_FORCE_ENABLE,
+} test_mode_t;
+
 //TU_VERIFY_STATIC(sizeof(dcd_event_t) <= 12, "size is not correct");
 
 //--------------------------------------------------------------------+
@@ -149,6 +164,12 @@ 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;
+
+// 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;
+
 //--------------------------------------------------------------------+
 // Endpoint API
 //--------------------------------------------------------------------+
@@ -240,6 +261,13 @@ 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 f0d9fba52..90fde92b5 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -587,6 +587,11 @@ 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();
@@ -725,14 +730,50 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
         break;
 
         case TUSB_REQ_SET_FEATURE:
-          // Only support remote wakeup for device feature
-          TU_VERIFY(TUSB_REQ_FEATURE_REMOTE_WAKEUP == p_request->wValue);
+          // Handle the feature selector
+          switch(p_request->wValue)
+          {
+            // Support for remote wakeup
+            case TUSB_REQ_FEATURE_REMOTE_WAKEUP:
+              TU_LOG_USBD("    Enable Remote Wakeup\r\n");
 
-          TU_LOG_USBD("    Enable Remote Wakeup\r\n");
+              // Host may enable remote wake up before suspending especially HID device
+              _usbd_dev.remote_wakeup_en = true;
+              tud_control_status(rhport, p_request);
 
-          // 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;
+              }
+
+              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))
+              {
+                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);
+
+            break;
+
+            // Stall unsupported feature selector
+            default: return false;
+          }
         break;
 
         case TUSB_REQ_CLEAR_FEATURE:
diff --git a/src/portable/synopsys/dwc2/dcd_dwc2.c b/src/portable/synopsys/dwc2/dcd_dwc2.c
index c6132a1f5..ee4b2b28d 100644
--- a/src/portable/synopsys/dwc2/dcd_dwc2.c
+++ b/src/portable/synopsys/dwc2/dcd_dwc2.c
@@ -1351,4 +1351,34 @@ 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) {
+  // Check if test mode selector is unsupported
+  if (TEST_FORCE_ENABLE < test_selector || TEST_J > test_selector) {
+    return false;
+  }
+
+  return true;
+}
+
+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)) {
+    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);
+
+  // Enable the test mode
+  dwc2->dctl = (dwc2->dctl & ~DCTL_TCTL_Msk) | (test_selector << DCTL_TCTL_Pos);
+}
+
+#endif /* TUP_USBIP_DWC2_TEST_MODE_SUPPORT */
+
 #endif