From 1755bba509edf2d5bc49cadecb6e956497732b37 Mon Sep 17 00:00:00 2001
From: Rocky04 <rockerfaeller04@gmx.de>
Date: Mon, 15 Jan 2024 12:47:13 +0000
Subject: Add DWC2 Test Mode SUpport

---
 src/device/dcd.h  | 28 ++++++++++++++++++++++++++++
 src/device/usbd.c | 53 +++++++++++++++++++++++++++++++++++++++++++++++------
 2 files changed, 75 insertions(+), 6 deletions(-)

(limited to 'src/device')

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:
-- 
cgit v1.3.1