From bba4b46fa4d680c73f067579f54a13455930856d Mon Sep 17 00:00:00 2001
From: Zhihong Chen <zhihong.chen@hpmicro.com>
Date: Mon, 11 Mar 2024 16:32:32 +0800
Subject: [PATCH] update usb test mode

Signed-off-by: Zhihong Chen <zhihong.chen@hpmicro.com>
---
 common/usb_dc.h  |  9 +++++++++
 core/usbd_core.c | 24 ++++++++++--------------
 2 files changed, 19 insertions(+), 14 deletions(-)

diff --git a/common/usb_dc.h b/common/usb_dc.h
index 3e195bc2..ca6c10ee 100644
--- a/common/usb_dc.h
+++ b/common/usb_dc.h
@@ -178,6 +178,15 @@ void usbd_event_ep_in_complete_handler(uint8_t busid, uint8_t ep, uint32_t nbyte
  */
 void usbd_event_ep_out_complete_handler(uint8_t busid, uint8_t ep, uint32_t nbytes);
 
+#ifdef CONFIG_USBDEV_TEST_MODE
+/**
+ * @brief Usb execute test mode
+ * @param[in]  busid     device busid
+ * @param[in]  test_mode usb test mode
+ */
+void usbd_execute_test_mode(uint8_t busid, uint8_t test_mode);
+#endif
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/core/usbd_core.c b/core/usbd_core.c
index 695361f2..3d94a979 100644
--- a/core/usbd_core.c
+++ b/core/usbd_core.c
@@ -51,7 +51,7 @@ USB_NOCACHE_RAM_SECTION struct usbd_core_priv {
     uint8_t configuration;
     uint8_t speed;
 #ifdef CONFIG_USBDEV_TEST_MODE
-    bool test_mode;
+    bool test_req;
 #endif
     struct usbd_interface *intf[8];
     uint8_t intf_offset;
@@ -498,8 +498,7 @@ static bool usbd_std_device_req_handler(uint8_t busid, struct usb_setup_packet *
                 }
             } else if (value == USB_FEATURE_TEST_MODE) {
 #ifdef CONFIG_USBDEV_TEST_MODE
-                g_usbd_core[busid].test_mode = true;
-                usbd_execute_test_mode(busid, setup);
+                g_usbd_core[busid].test_req = true;
 #endif
             }
             *len = 0;
@@ -973,10 +972,6 @@ void usbd_event_reset_handler(uint8_t busid)
 {
     usbd_set_address(busid, 0);
     g_usbd_core[busid].configuration = 0;
-
-#ifdef CONFIG_USBDEV_TEST_MODE
-    g_usbd_core[busid].test_mode = false;
-#endif
     struct usb_endpoint_descriptor ep0;
 
     ep0.bLength = 7;
@@ -1027,13 +1022,7 @@ void usbd_event_ep0_setup_complete_handler(uint8_t busid, uint8_t *psetup)
         usbd_ep_set_stall(busid, USB_CONTROL_IN_EP0);
         return;
     }
-#ifdef CONFIG_USBDEV_TEST_MODE
-    /* send status in test mode, so do not execute downward, just return */
-    if (g_usbd_core[busid].test_mode) {
-        g_usbd_core[busid].test_mode = false;
-        return;
-    }
-#endif
+
     /* Send smallest of requested and offered length */
     g_usbd_core[busid].ep0_data_buf_residue = MIN(g_usbd_core[busid].ep0_data_buf_len, setup->wLength);
     if (g_usbd_core[busid].ep0_data_buf_residue > CONFIG_USBDEV_REQUEST_BUFFER_LEN) {
@@ -1082,6 +1071,13 @@ void usbd_event_ep0_in_complete_handler(uint8_t busid, uint8_t ep, uint32_t nbyt
                 /* if all data has sent completely, start reading out status */
                 usbd_ep_start_read(busid, USB_CONTROL_OUT_EP0, NULL, 0);
             }
+
+#ifdef CONFIG_USBDEV_TEST_MODE
+            if (g_usbd_core[busid].test_req) {
+                usbd_execute_test_mode(busid, HI_BYTE(setup->wIndex));
+                g_usbd_core[busid].test_req = false;
+            }
+#endif
         }
     }
 }