From 4982fcf40492c4a13ade258f25b76b59f2578533 Mon Sep 17 00:00:00 2001
From: Shuchita Khare <shuchitak@xmos.com>
Date: Fri, 12 Jan 2024 16:31:50 +0000
Subject: [PATCH] Support for TUSB_REQ_FEATURE_TEST_MODE

---
 src/device/dcd.h      |  2 ++
 src/device/usbd.c     | 39 ++++++++++++++++++++++++++++++++++-----
 src/device/usbd.h     | 11 +++++++++++
 src/device/usbd_pvt.h |  3 +++
 4 files changed, 50 insertions(+), 5 deletions(-)

diff --git a/src/device/dcd.h b/src/device/dcd.h
index 69c26bcf4a..0127d3c427 100644
--- a/src/device/dcd.h
+++ b/src/device/dcd.h
@@ -146,6 +146,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);
 
+// Transition device to test mode
+void dcd_set_test_mode(uint16_t test_mode) TU_ATTR_WEAK;
 //--------------------------------------------------------------------+
 // Endpoint API
 //--------------------------------------------------------------------+
diff --git a/src/device/usbd.c b/src/device/usbd.c
index 5c94ebcc5d..34f4ba2a68 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -719,13 +719,34 @@ static bool process_control_request(uint8_t rhport, tusb_control_request_t const
 
         case TUSB_REQ_SET_FEATURE:
           // Only support remote wakeup for device feature
-          TU_VERIFY(TUSB_REQ_FEATURE_REMOTE_WAKEUP == p_request->wValue);
+          if(p_request->wValue == TUSB_REQ_FEATURE_REMOTE_WAKEUP)
+          {
+            TU_LOG(USBD_DBG, "    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);
+          }
+          else if((p_request->wValue == TUSB_REQ_FEATURE_TEST_MODE) && (p_request->wLength == 0))
+          {
+            /* Inspect for Test Selector (high byte of wIndex, lower byte must be zero) */
+            switch(p_request->wIndex)
+            {
+              case TUSB_USB_WINDEX_TEST_J:
+              case TUSB_USB_WINDEX_TEST_K:
+              case TUSB_USB_WINDEX_TEST_SE0_NAK:
+              case TUSB_USB_WINDEX_TEST_PACKET:
+              {
+                tud_control_status(rhport, p_request);
+                if(tud_test_mode_req_status_xfer_cb)
+                {
+                  usbd_control_set_complete_callback(tud_test_mode_req_status_xfer_cb);
+                }
+              }
+              break;
+            }
 
-          // Host may enable remote wake up before suspending especially HID device
-          _usbd_dev.remote_wakeup_en = true;
-          tud_control_status(rhport, p_request);
+          }
         break;
 
         case TUSB_REQ_CLEAR_FEATURE:
@@ -1403,4 +1424,12 @@ bool usbd_edpt_iso_activate(uint8_t rhport, tusb_desc_endpoint_t const * desc_ep
   return dcd_edpt_iso_activate(rhport, desc_ep);
 }
 
+// Set device in test mode in response to TUSB_REQ_FEATURE_TEST_MODE
+bool usbd_set_test_mode(uint16_t test_mode)
+{
+  TU_VERIFY(dcd_set_test_mode);
+  dcd_set_test_mode(test_mode);
+  return true;
+}
+
 #endif
diff --git a/src/device/usbd.h b/src/device/usbd.h
index 3ab6c813f9..7c0cdc2462 100644
--- a/src/device/usbd.h
+++ b/src/device/usbd.h
@@ -152,6 +152,9 @@ void tud_event_hook_cb(uint8_t rhport, uint32_t eventid, bool in_isr);
 // Invoked when received control request with VENDOR TYPE
 TU_ATTR_WEAK bool tud_vendor_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
 
+// Invoked when ZLP xfer stage of TUSB_REQ_FEATURE_TEST_MODE request completes
+TU_ATTR_WEAK bool tud_test_mode_req_status_xfer_cb(uint8_t rhport, uint8_t stage, tusb_control_request_t const * request);
+
 //--------------------------------------------------------------------+
 // Binary Device Object Store (BOS) Descriptor Templates
 //--------------------------------------------------------------------+
@@ -855,6 +858,14 @@ TU_ATTR_WEAK bool tud_vendor_control_xfer_cb(uint8_t rhport, uint8_t stage, tusb
   /* Endpoint Out */\
   7, TUSB_DESC_ENDPOINT, _epout, TUSB_XFER_BULK, U16_TO_U8S_LE(_epsize), 0
 
+// Test selector defines for Test mode
+#define TUSB_USB_WINDEX_TEST_J               (0x1<<8)
+#define TUSB_USB_WINDEX_TEST_K               (0x2<<8)
+#define TUSB_USB_WINDEX_TEST_SE0_NAK         (0x3<<8)
+#define TUSB_USB_WINDEX_TEST_PACKET          (0x4<<8)
+#define TUSB_USB_WINDEX_TEST_FORCE_ENABLE    (0x5<<8)
+
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/device/usbd_pvt.h b/src/device/usbd_pvt.h
index 9039bc9d63..ff2ce1680d 100644
--- a/src/device/usbd_pvt.h
+++ b/src/device/usbd_pvt.h
@@ -112,6 +112,9 @@ bool usbd_edpt_ready(uint8_t rhport, uint8_t ep_addr) {
 // Enable SOF interrupt
 void usbd_sof_enable(uint8_t rhport, bool en);
 
+// Set device in test mode in response to TUSB_REQ_FEATURE_TEST_MODE
+bool usbd_set_test_mode(uint16_t test_mode);
+
 /*------------------------------------------------------------------*/
 /* Helper
  *------------------------------------------------------------------*/