Merge pull request #2416 from Rocky04/test-mode-support

Add DWC2 Test Mode Support

5 files changed, 100 insertions, 7 deletions

diff --git a/src/common/tusb_mcu.h b/src/common/tusb_mcu.h
index 4e3b89262..3a7d6ad33 100644
--- a/src/common/tusb_mcu.h
+++ b/src/common/tusb_mcu.h
@@ -195,6 +195,7 @@
 #elif TU_CHECK_MCU(OPT_MCU_STM32F4)
   #define TUP_USBIP_DWC2
   #define TUP_USBIP_DWC2_STM32
+  #define TUP_USBIP_DWC2_TEST_MODE
 
   // For most mcu, FS has 4, HS has 6. TODO 446/469/479 HS has 9
   #define TUP_DCD_ENDPOINT_MAX    6
@@ -209,6 +210,7 @@
   // MCU with on-chip HS Phy
   #if defined(STM32F723xx) || defined(STM32F730xx) || defined(STM32F733xx)
     #define TUP_RHPORT_HIGHSPEED  1 // Port0: FS, Port1: HS
+    #define TUP_USBIP_DWC2_TEST_MODE
   #endif
 
 #elif TU_CHECK_MCU(OPT_MCU_STM32H7)
@@ -277,6 +279,7 @@
       defined(STM32U5F7xx) || defined(STM32U5F9xx) || defined(STM32U5G7xx) || defined(STM32U5G9xx)
     #define TUP_DCD_ENDPOINT_MAX  9
     #define TUP_RHPORT_HIGHSPEED  1
+    #define TUP_USBIP_DWC2_TEST_MODE
   #else
     #define TUP_DCD_ENDPOINT_MAX  6
   #endif
diff --git a/src/device/dcd.h b/src/device/dcd.h
index d4f105aa3..9447d6d9d 100644
--- a/src/device/dcd.h
+++ b/src/device/dcd.h
@@ -97,6 +97,14 @@ typedef struct TU_ATTR_ALIGNED(4) {
   };
 } dcd_event_t;
 
+typedef enum {
+  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 +157,13 @@ 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);
 
+#if CFG_TUD_TEST_MODE
+// 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;
+#endif
 //--------------------------------------------------------------------+
 // Endpoint API
 //--------------------------------------------------------------------+
diff --git a/src/device/usbd.c b/src/device/usbd.c
index e33f39bcf..25d890dc7 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -313,7 +313,9 @@ TU_ATTR_ALWAYS_INLINE static inline bool queue_event(dcd_event_t const * event,
 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);
-
+#if CFG_TUD_TEST_MODE
+static bool process_test_mode_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
+#endif
 // from usbd_control.c
 void usbd_control_reset(void);
 void usbd_control_set_request(tusb_control_request_t const *request);
@@ -753,14 +755,47 @@ 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);
+            break;
 
-          // Host may enable remote wake up before suspending especially HID device
-          _usbd_dev.remote_wakeup_en = true;
-          tud_control_status(rhport, p_request);
+#if CFG_TUD_TEST_MODE
+            // Support for TEST_MODE
+            case TUSB_REQ_FEATURE_TEST_MODE: {
+              // 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_check_test_mode_support((test_mode_t)selector))
+              {
+                TU_LOG_USBD("    Unsupported Test Mode (test selector index: %d)\r\n", selector);
+
+                return false;
+              }
+
+              // Acknowledge request
+              tud_control_status(rhport, p_request);
+
+              TU_LOG_USBD("    Enter Test Mode (test selector index: %d)\r\n", selector);
+
+              usbd_control_set_complete_callback(process_test_mode_cb);
+              break;
+            }
+#endif /* CFG_TUD_TEST_MODE */
+
+            // Stall unsupported feature selector
+            default: return false;
+          }
         break;
 
         case TUSB_REQ_CLEAR_FEATURE:
@@ -1088,6 +1123,20 @@ static bool process_get_descriptor(uint8_t rhport, tusb_control_request_t const
   }
 }
 
+#if CFG_TUD_TEST_MODE
+static 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, (test_mode_t)tu_u16_high(request->wIndex));
+
+  return true;
+}
+#endif /* CFG_TUD_TEST_MODE */
+
 //--------------------------------------------------------------------+
 // DCD Event Handler
 //--------------------------------------------------------------------+
diff --git a/src/portable/synopsys/dwc2/dcd_dwc2.c b/src/portable/synopsys/dwc2/dcd_dwc2.c
index 692096fc8..dbcd586c5 100644
--- a/src/portable/synopsys/dwc2/dcd_dwc2.c
+++ b/src/portable/synopsys/dwc2/dcd_dwc2.c
@@ -1195,4 +1195,25 @@ void dcd_int_handler(uint8_t rhport) {
   //  }
 }
 
+#if defined(TUP_USBIP_DWC2_TEST_MODE) && CFG_TUD_TEST_MODE
+
+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;
+  }
+
+  return true;
+}
+
+void dcd_enter_test_mode(uint8_t rhport, test_mode_t test_selector) {
+  // 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 && CFG_TUD_TEST_MODE */
+
 #endif
diff --git a/src/tusb_option.h b/src/tusb_option.h
index 8d5527936..058427e0e 100644
--- a/src/tusb_option.h
+++ b/src/tusb_option.h
@@ -364,6 +364,11 @@
   #define CFG_TUD_INTERFACE_MAX   16
 #endif
 
+// USB 2.0 compliance test mode support
+#ifndef CFG_TUD_TEST_MODE
+  #define CFG_TUD_TEST_MODE       0
+#endif
+
 //------------- Device Class Driver -------------//
 #ifndef CFG_TUD_BTH
   #define CFG_TUD_BTH             0