optimise usbfs and usbhs driver

2 files changed, 186 insertions, 487 deletions

diff --git a/port/ch32/usb_dc_usbfs.c b/port/ch32/usb_dc_usbfs.c
index 752b357e..b7eec7e0 100644
--- a/port/ch32/usb_dc_usbfs.c
+++ b/port/ch32/usb_dc_usbfs.c
@@ -11,10 +11,10 @@
 
 #define USB_SET_TX_LEN(ep_idx, len)  (*(volatile uint16_t *)((uint32_t)(&USBFS_DEVICE->UEP0_TX_LEN) + 4 * ep_idx) = len)
 #define USB_GET_TX_LEN(ep_idx)       (*(volatile uint16_t *)((uint32_t)(&USBFS_DEVICE->UEP0_TX_LEN) + 4 * ep_idx))
-#define USB_SET_TX_CTRL(ep_idx, val) (*(volatile uint16_t *)((uint32_t)(&USBFS_DEVICE->UEP0_TX_CTRL) + 4 * ep_idx) = val)
-#define USB_GET_TX_CTRL(ep_idx)      (*(volatile uint16_t *)((uint32_t)(&USBFS_DEVICE->UEP0_TX_CTRL) + 4 * ep_idx))
-#define USB_SET_RX_CTRL(ep_idx, val) (*(volatile uint16_t *)((uint32_t)(&USBFS_DEVICE->UEP0_RX_CTRL) + 4 * ep_idx) = val)
-#define USB_GET_RX_CTRL(ep_idx)      (*(volatile uint16_t *)((uint32_t)(&USBFS_DEVICE->UEP0_RX_CTRL) + 4 * ep_idx))
+#define USB_SET_TX_CTRL(ep_idx, val) (*(volatile uint8_t *)((uint32_t)(&USBFS_DEVICE->UEP0_TX_CTRL) + 4 * ep_idx) = val)
+#define USB_GET_TX_CTRL(ep_idx)      (*(volatile uint8_t *)((uint32_t)(&USBFS_DEVICE->UEP0_TX_CTRL) + 4 * ep_idx))
+#define USB_SET_RX_CTRL(ep_idx, val) (*(volatile uint8_t *)((uint32_t)(&USBFS_DEVICE->UEP0_RX_CTRL) + 4 * ep_idx) = val)
+#define USB_GET_RX_CTRL(ep_idx)      (*(volatile uint8_t *)((uint32_t)(&USBFS_DEVICE->UEP0_RX_CTRL) + 4 * ep_idx))
 
 /* Endpoint state */
 struct ch32_usbfs_ep_state {
@@ -30,19 +30,13 @@ struct ch32_usbfs_ep_state {
 /* Driver state */
 struct ch32_usbfs_udc {
     volatile uint8_t dev_addr;
-    struct ch32_usbfs_ep_state in_ep[USB_NUM_BIDIR_ENDPOINTS];  /*!< IN endpoint parameters*/
-    struct ch32_usbfs_ep_state out_ep[USB_NUM_BIDIR_ENDPOINTS]; /*!< OUT endpoint parameters */
+    struct ch32_usbfs_ep_state in_ep[USB_NUM_BIDIR_ENDPOINTS];                            /*!< IN endpoint parameters*/
+    struct ch32_usbfs_ep_state out_ep[USB_NUM_BIDIR_ENDPOINTS];                           /*!< OUT endpoint parameters */
+    __attribute__((aligned(4))) uint8_t ep_databuf[USB_NUM_BIDIR_ENDPOINTS - 1][64 + 64]; //epx_out(64)+epx_in(64)
 } g_ch32_usbfs_udc;
 
-/* Endpoint Buffer */
-__attribute__((aligned(4))) uint8_t EP0_DatabufHD[64];      //ep0(64)
-__attribute__((aligned(4))) uint8_t EP1_DatabufHD[64 + 64]; //ep1_out(64)+ep1_in(64)
-__attribute__((aligned(4))) uint8_t EP2_DatabufHD[64 + 64]; //ep2_out(64)+ep2_in(64)
-__attribute__((aligned(4))) uint8_t EP3_DatabufHD[64 + 64]; //ep3_out(64)+ep3_in(64)
-__attribute__((aligned(4))) uint8_t EP4_DatabufHD[64 + 64]; //ep4_out(64)+ep4_in(64)
-__attribute__((aligned(4))) uint8_t EP5_DatabufHD[64 + 64]; //ep5_out(64)+ep5_in(64)
-__attribute__((aligned(4))) uint8_t EP6_DatabufHD[64 + 64]; //ep6_out(64)+ep6_in(64)
-__attribute__((aligned(4))) uint8_t EP7_DatabufHD[64 + 64]; //ep7_out(64)+ep7_in(64)
+/* Endpoint0 Buffer */
+__attribute__((aligned(4))) uint8_t EP0_DatabufHD[64]; //ep0(64)
 
 void USBD_IRQHandler(void) __attribute__((interrupt("WCH-Interrupt-fast")));
 
@@ -70,46 +64,13 @@ int usb_dc_init(void)
     USBFS_DEVICE->UEP7_MOD = USBFS_UEP7_RX_EN | USBFS_UEP7_TX_EN;
 
     USBFS_DEVICE->UEP0_DMA = (uint32_t)EP0_DatabufHD;
-    USBFS_DEVICE->UEP1_DMA = (uint32_t)EP1_DatabufHD;
-    USBFS_DEVICE->UEP2_DMA = (uint32_t)EP2_DatabufHD;
-    USBFS_DEVICE->UEP3_DMA = (uint32_t)EP3_DatabufHD;
-    USBFS_DEVICE->UEP4_DMA = (uint32_t)EP4_DatabufHD;
-    USBFS_DEVICE->UEP5_DMA = (uint32_t)EP5_DatabufHD;
-    USBFS_DEVICE->UEP6_DMA = (uint32_t)EP6_DatabufHD;
-    USBFS_DEVICE->UEP7_DMA = (uint32_t)EP7_DatabufHD;
-
-    USBFS_DEVICE->UEP0_RX_CTRL = USBFS_UEP_R_RES_ACK;
-    USBFS_DEVICE->UEP1_RX_CTRL = USBFS_UEP_R_RES_ACK | USBFS_UEP_AUTO_TOG;
-    ;
-    USBFS_DEVICE->UEP2_RX_CTRL = USBFS_UEP_R_RES_ACK | USBFS_UEP_AUTO_TOG;
-    ;
-    USBFS_DEVICE->UEP3_RX_CTRL = USBFS_UEP_R_RES_ACK | USBFS_UEP_AUTO_TOG;
-    ;
-    USBFS_DEVICE->UEP4_RX_CTRL = USBFS_UEP_R_RES_ACK | USBFS_UEP_AUTO_TOG;
-    ;
-    USBFS_DEVICE->UEP5_RX_CTRL = USBFS_UEP_R_RES_ACK | USBFS_UEP_AUTO_TOG;
-    ;
-    USBFS_DEVICE->UEP6_RX_CTRL = USBFS_UEP_R_RES_ACK | USBFS_UEP_AUTO_TOG;
-    ;
-    USBFS_DEVICE->UEP7_RX_CTRL = USBFS_UEP_R_RES_ACK | USBFS_UEP_AUTO_TOG;
-    ;
-
-    USBFS_DEVICE->UEP0_TX_CTRL = USBFS_UEP_T_RES_NAK;
-    USBFS_DEVICE->UEP1_TX_LEN = 0;
-    USBFS_DEVICE->UEP2_TX_LEN = 0;
-    USBFS_DEVICE->UEP3_TX_LEN = 0;
-    USBFS_DEVICE->UEP4_TX_LEN = 0;
-    USBFS_DEVICE->UEP5_TX_LEN = 0;
-    USBFS_DEVICE->UEP6_TX_LEN = 0;
-    USBFS_DEVICE->UEP7_TX_LEN = 0;
-
-    USBFS_DEVICE->UEP1_TX_CTRL = USBFS_UEP_T_RES_NAK | USBFS_UEP_AUTO_TOG;
-    USBFS_DEVICE->UEP2_TX_CTRL = USBFS_UEP_T_RES_NAK | USBFS_UEP_AUTO_TOG;
-    USBFS_DEVICE->UEP3_TX_CTRL = USBFS_UEP_T_RES_NAK | USBFS_UEP_AUTO_TOG;
-    USBFS_DEVICE->UEP4_TX_CTRL = USBFS_UEP_T_RES_NAK | USBFS_UEP_AUTO_TOG;
-    USBFS_DEVICE->UEP5_TX_CTRL = USBFS_UEP_T_RES_NAK | USBFS_UEP_AUTO_TOG;
-    USBFS_DEVICE->UEP6_TX_CTRL = USBFS_UEP_T_RES_NAK | USBFS_UEP_AUTO_TOG;
-    USBFS_DEVICE->UEP7_TX_CTRL = USBFS_UEP_T_RES_NAK | USBFS_UEP_AUTO_TOG;
+    USBFS_DEVICE->UEP1_DMA = (uint32_t)g_ch32_usbfs_udc.ep_databuf[0];
+    USBFS_DEVICE->UEP2_DMA = (uint32_t)g_ch32_usbfs_udc.ep_databuf[1];
+    USBFS_DEVICE->UEP3_DMA = (uint32_t)g_ch32_usbfs_udc.ep_databuf[2];
+    USBFS_DEVICE->UEP4_DMA = (uint32_t)g_ch32_usbfs_udc.ep_databuf[3];
+    USBFS_DEVICE->UEP5_DMA = (uint32_t)g_ch32_usbfs_udc.ep_databuf[4];
+    USBFS_DEVICE->UEP6_DMA = (uint32_t)g_ch32_usbfs_udc.ep_databuf[5];
+    USBFS_DEVICE->UEP7_DMA = (uint32_t)g_ch32_usbfs_udc.ep_databuf[6];
 
     USBFS_DEVICE->INT_FG = 0xFF;
     USBFS_DEVICE->INT_EN = USBFS_UIE_SUSPEND | USBFS_UIE_BUS_RST | USBFS_UIE_TRANSFER;
@@ -221,38 +182,11 @@ int usbd_ep_write(const uint8_t ep, const uint8_t *data, uint32_t data_len, uint
         }
     }
 
-    switch (ep_idx) {
-        case 0:
-            memcpy(&EP0_DatabufHD[0], data, data_len);
-            break;
-        case 1:
-            memcpy(&EP1_DatabufHD[64], data, data_len);
-            break;
-        case 2:
-            memcpy(&EP2_DatabufHD[64], data, data_len);
-            break;
-        case 3:
-            memcpy(&EP3_DatabufHD[64], data, data_len);
-            break;
-        case 4:
-            memcpy(&EP4_DatabufHD[64], data, data_len);
-            break;
-        case 5:
-            memcpy(&EP5_DatabufHD[64], data, data_len);
-            break;
-        case 6:
-            memcpy(&EP6_DatabufHD[64], data, data_len);
-            break;
-        case 7:
-            memcpy(&EP7_DatabufHD[64], data, data_len);
-            break;
-        default:
-            break;
-    }
-
     if (ep_idx == 0) {
+        memcpy(&EP0_DatabufHD[0], data, data_len);
         USB_SET_TX_LEN(ep_idx, data_len);
     } else {
+        memcpy(&g_ch32_usbfs_udc.ep_databuf[ep_idx - 1][64], data, data_len);
         USB_SET_TX_LEN(ep_idx, data_len);
         USB_SET_TX_CTRL(ep_idx, (USB_GET_TX_CTRL(ep_idx) & ~USBFS_UEP_T_RES_MASK) | USBFS_UEP_T_RES_ACK);
     }
@@ -282,38 +216,15 @@ int usbd_ep_read(const uint8_t ep, uint8_t *data, uint32_t max_data_len, uint32_
     read_count = USBFS_DEVICE->RX_LEN;
     read_count = MIN(read_count, max_data_len);
 
-    switch (ep_idx) {
-        case 0:
-            if ((max_data_len == 8) && !read_bytes) {
-                read_count = 8;
-                memcpy(data, &EP0_DatabufHD[0], 8);
-            } else {
-                memcpy(data, &EP0_DatabufHD[0], read_count);
-            }
-            break;
-        case 1:
-            memcpy(data, &EP1_DatabufHD[0], read_count);
-            break;
-        case 2:
-            memcpy(data, &EP2_DatabufHD[0], read_count);
-            break;
-        case 3:
-            memcpy(data, &EP3_DatabufHD[0], read_count);
-            break;
-        case 4:
-            memcpy(data, &EP4_DatabufHD[0], read_count);
-            break;
-        case 5:
-            memcpy(data, &EP5_DatabufHD[0], read_count);
-            break;
-        case 6:
-            memcpy(data, &EP6_DatabufHD[0], read_count);
-            break;
-        case 7:
-            memcpy(data, &EP7_DatabufHD[0], read_count);
-            break;
-        default:
-            break;
+    if (ep_idx == 0x00) {
+        if ((max_data_len == 8) && !read_bytes) {
+            read_count = 8;
+            memcpy(data, &EP0_DatabufHD[0], 8);
+        } else {
+            memcpy(data, &EP0_DatabufHD[0], read_count);
+        }
+    } else {
+        memcpy(data, &g_ch32_usbfs_udc.ep_databuf[ep_idx - 1][0], read_count);
     }
 
     if (read_bytes) {
@@ -325,12 +236,15 @@ int usbd_ep_read(const uint8_t ep, uint8_t *data, uint32_t max_data_len, uint32_
 
 void USBD_IRQHandler(void)
 {
+    uint32_t ep_idx, token;
     uint8_t intflag = 0;
 
     intflag = USBFS_DEVICE->INT_FG;
 
     if (intflag & USBFS_UIF_TRANSFER) {
-        switch (USBFS_DEVICE->INT_ST & USBFS_UIS_TOKEN_MASK) {
+        token = USBFS_DEVICE->INT_ST & USBFS_UIS_TOKEN_MASK;
+        ep_idx = USBFS_DEVICE->INT_ST & USBFS_UIS_ENDP_MASK;
+        switch (token) {
             case USBFS_UIS_TOKEN_SETUP:
                 USBFS_DEVICE->UEP0_TX_CTRL = USBFS_UEP_T_TOG | USBFS_UEP_T_RES_NAK;
 
@@ -341,120 +255,34 @@ void USBD_IRQHandler(void)
                 break;
 
             case USBFS_UIS_TOKEN_IN:
-                switch (USBFS_DEVICE->INT_ST & (USBFS_UIS_TOKEN_MASK | USBFS_UIS_ENDP_MASK)) {
-                    case USBFS_UIS_TOKEN_IN:
-
-                        usbd_event_notify_handler(USBD_EVENT_EP0_IN_NOTIFY, NULL);
-                        if (g_ch32_usbfs_udc.dev_addr > 0) {
-                            USBFS_DEVICE->DEV_ADDR = (USBFS_DEVICE->DEV_ADDR & USBFS_UDA_GP_BIT) | g_ch32_usbfs_udc.dev_addr;
-                            g_ch32_usbfs_udc.dev_addr = 0;
-                        }
-
-                        if (mps_over_flag) {
-                            mps_over_flag = 0;
-                            USBFS_DEVICE->UEP0_TX_CTRL ^= USBFS_UEP_T_TOG;
-                        } else {
-                            USBFS_DEVICE->UEP0_TX_CTRL = USBFS_UEP_T_RES_NAK;
-                            USBFS_DEVICE->UEP0_RX_CTRL = USBFS_UEP_R_RES_ACK;
-                        }
-                        break;
-
-                    case USBFS_UIS_TOKEN_IN | 1:
-                        USBFS_DEVICE->UEP1_TX_CTRL = (USBFS_DEVICE->UEP1_TX_CTRL & ~USBFS_UEP_T_RES_MASK) | USBFS_UEP_T_RES_NAK;
-                        usbd_event_notify_handler(USBD_EVENT_EP_IN_NOTIFY, (void *)(1 | 0x80));
-                        break;
-
-                    case USBFS_UIS_TOKEN_IN | 2:
-                        USBFS_DEVICE->UEP2_TX_CTRL = (USBFS_DEVICE->UEP2_TX_CTRL & ~USBFS_UEP_T_RES_MASK) | USBFS_UEP_T_RES_NAK;
-                        usbd_event_notify_handler(USBD_EVENT_EP_IN_NOTIFY, (void *)(2 | 0x80));
-                        break;
-
-                    case USBFS_UIS_TOKEN_IN | 3:
-                        USBFS_DEVICE->UEP3_TX_CTRL = (USBFS_DEVICE->UEP3_TX_CTRL & ~USBFS_UEP_T_RES_MASK) | USBFS_UEP_T_RES_NAK;
-                        usbd_event_notify_handler(USBD_EVENT_EP_IN_NOTIFY, (void *)(3 | 0x80));
-                        break;
-
-                    case USBFS_UIS_TOKEN_IN | 4:
-                        USBFS_DEVICE->UEP4_TX_CTRL = (USBFS_DEVICE->UEP4_TX_CTRL & ~USBFS_UEP_T_RES_MASK) | USBFS_UEP_T_RES_NAK;
-                        usbd_event_notify_handler(USBD_EVENT_EP_IN_NOTIFY, (void *)(4 | 0x80));
-                        break;
-
-                    case USBFS_UIS_TOKEN_IN | 5:
-                        USBFS_DEVICE->UEP5_TX_CTRL = (USBFS_DEVICE->UEP5_TX_CTRL & ~USBFS_UEP_T_RES_MASK) | USBFS_UEP_T_RES_NAK;
-                        usbd_event_notify_handler(USBD_EVENT_EP_IN_NOTIFY, (void *)(5 | 0x80));
-                        break;
-
-                    case USBFS_UIS_TOKEN_IN | 6:
-                        USBFS_DEVICE->UEP6_TX_CTRL = (USBFS_DEVICE->UEP6_TX_CTRL & ~USBFS_UEP_T_RES_MASK) | USBFS_UEP_T_RES_NAK;
-                        usbd_event_notify_handler(USBD_EVENT_EP_IN_NOTIFY, (void *)(6 | 0x80));
-                        break;
+                if (ep_idx == 0x00) {
+                    usbd_event_notify_handler(USBD_EVENT_EP0_IN_NOTIFY, NULL);
+                    if (g_ch32_usbfs_udc.dev_addr > 0) {
+                        USBFS_DEVICE->DEV_ADDR = (USBFS_DEVICE->DEV_ADDR & USBFS_UDA_GP_BIT) | g_ch32_usbfs_udc.dev_addr;
+                        g_ch32_usbfs_udc.dev_addr = 0;
+                    }
 
-                    case USBFS_UIS_TOKEN_IN | 7:
-                        USBFS_DEVICE->UEP7_TX_CTRL = (USBFS_DEVICE->UEP7_TX_CTRL & ~USBFS_UEP_T_RES_MASK) | USBFS_UEP_T_RES_NAK;
-                        usbd_event_notify_handler(USBD_EVENT_EP_IN_NOTIFY, (void *)(7 | 0x80));
-                        break;
-
-                    default:
-                        break;
+                    if (mps_over_flag) {
+                        mps_over_flag = 0;
+                        USBFS_DEVICE->UEP0_TX_CTRL ^= USBFS_UEP_T_TOG;
+                    } else {
+                        USBFS_DEVICE->UEP0_TX_CTRL = USBFS_UEP_T_RES_NAK;
+                        USBFS_DEVICE->UEP0_RX_CTRL = USBFS_UEP_R_RES_ACK;
+                    }
+                } else {
+                    USB_SET_TX_CTRL(ep_idx, (USB_GET_TX_CTRL(ep_idx) & ~USBFS_UEP_T_RES_MASK) | USBFS_UEP_T_RES_NAK);
+                    usbd_event_notify_handler(USBD_EVENT_EP_IN_NOTIFY, (void *)(ep_idx | 0x80));
                 }
                 break;
-
             case USBFS_UIS_TOKEN_OUT:
-                switch (USBFS_DEVICE->INT_ST & (USBFS_UIS_TOKEN_MASK | USBFS_UIS_ENDP_MASK)) {
-                    case USBFS_UIS_TOKEN_OUT:
-                        usbd_event_notify_handler(USBD_EVENT_EP0_OUT_NOTIFY, NULL);
-                        break;
-
-                    case USBFS_UIS_TOKEN_OUT | 1:
-                        if (USBFS_DEVICE->INT_ST & USBFS_UIS_TOG_OK) {
-                            USBFS_DEVICE->UEP1_RX_CTRL = (USBFS_DEVICE->UEP1_RX_CTRL & ~USBFS_UEP_R_RES_MASK) | USBFS_UEP_R_RES_NAK;
-                            usbd_event_notify_handler(USBD_EVENT_EP_OUT_NOTIFY, (void *)(1 & 0x7f));
-                        }
-                        break;
-
-                    case USBFS_UIS_TOKEN_OUT | 2:
-                        if (USBFS_DEVICE->INT_ST & USBFS_UIS_TOG_OK) {
-                            USBFS_DEVICE->UEP2_RX_CTRL = (USBFS_DEVICE->UEP2_RX_CTRL & ~USBFS_UEP_R_RES_MASK) | USBFS_UEP_R_RES_NAK;
-                            usbd_event_notify_handler(USBD_EVENT_EP_OUT_NOTIFY, (void *)(2 & 0x7f));
-                        }
-                        break;
-
-                    case USBFS_UIS_TOKEN_OUT | 3:
-                        if (USBFS_DEVICE->INT_ST & USBFS_UIS_TOG_OK) {
-                            USBFS_DEVICE->UEP3_RX_CTRL = (USBFS_DEVICE->UEP3_RX_CTRL & ~USBFS_UEP_R_RES_MASK) | USBFS_UEP_R_RES_NAK;
-                            usbd_event_notify_handler(USBD_EVENT_EP_OUT_NOTIFY, (void *)(3 & 0x7f));
-                        }
-                        break;
-
-                    case USBFS_UIS_TOKEN_OUT | 4:
-                        if (USBFS_DEVICE->INT_ST & USBFS_UIS_TOG_OK) {
-                            USBFS_DEVICE->UEP4_RX_CTRL = (USBFS_DEVICE->UEP4_RX_CTRL & ~USBFS_UEP_R_RES_MASK) | USBFS_UEP_R_RES_NAK;
-                            usbd_event_notify_handler(USBD_EVENT_EP_OUT_NOTIFY, (void *)(4 & 0x7f));
-                        }
-                        break;
-
-                    case USBFS_UIS_TOKEN_OUT | 5:
-                        if (USBFS_DEVICE->INT_ST & USBFS_UIS_TOG_OK) {
-                            USBFS_DEVICE->UEP5_RX_CTRL = (USBFS_DEVICE->UEP5_RX_CTRL & ~USBFS_UEP_R_RES_MASK) | USBFS_UEP_R_RES_NAK;
-                            usbd_event_notify_handler(USBD_EVENT_EP_OUT_NOTIFY, (void *)(5 & 0x7f));
-                        }
-                        break;
-
-                    case USBFS_UIS_TOKEN_OUT | 6:
-                        if (USBFS_DEVICE->INT_ST & USBFS_UIS_TOG_OK) {
-                            USBFS_DEVICE->UEP6_RX_CTRL = (USBFS_DEVICE->UEP6_RX_CTRL & ~USBFS_UEP_R_RES_MASK) | USBFS_UEP_R_RES_NAK;
-                            usbd_event_notify_handler(USBD_EVENT_EP_OUT_NOTIFY, (void *)(6 & 0x7f));
-                        }
-                        break;
-
-                    case USBFS_UIS_TOKEN_OUT | 7:
-                        if (USBFS_DEVICE->INT_ST & USBFS_UIS_TOG_OK) {
-                            USBFS_DEVICE->UEP7_RX_CTRL = (USBFS_DEVICE->UEP7_RX_CTRL & ~USBFS_UEP_R_RES_MASK) | USBFS_UEP_R_RES_NAK;
-                            usbd_event_notify_handler(USBD_EVENT_EP_OUT_NOTIFY, (void *)(7 & 0x7f));
-                        }
-                        break;
+                if (ep_idx == 0x00) {
+                    usbd_event_notify_handler(USBD_EVENT_EP0_OUT_NOTIFY, NULL);
+                } else {
+                    if (USBFS_DEVICE->INT_ST & USBFS_UIS_TOG_OK) {
+                        USB_SET_RX_CTRL(ep_idx, (USB_GET_RX_CTRL(ep_idx) & ~USBFS_UEP_R_RES_MASK) | USBFS_UEP_R_RES_NAK);
+                        usbd_event_notify_handler(USBD_EVENT_EP_OUT_NOTIFY, (void *)(ep_idx));
+                    }
                 }
-
                 break;
 
             case USBFS_UIS_TOKEN_SOF:
@@ -467,32 +295,15 @@ void USBD_IRQHandler(void)
 
         USBFS_DEVICE->INT_FG = USBFS_UIF_TRANSFER;
     } else if (intflag & USBFS_UIF_BUS_RST) {
-        USBFS_DEVICE->DEV_ADDR = 0;
-
+        USBFS_DEVICE->UEP0_TX_LEN = 0;
+        USBFS_DEVICE->UEP0_TX_CTRL = USBFS_UEP_T_RES_NAK;
         USBFS_DEVICE->UEP0_RX_CTRL = USBFS_UEP_R_RES_ACK;
-        USBFS_DEVICE->UEP1_RX_CTRL = USBFS_UEP_R_RES_ACK | USBFS_UEP_AUTO_TOG;
-        ;
-        USBFS_DEVICE->UEP2_RX_CTRL = USBFS_UEP_R_RES_ACK | USBFS_UEP_AUTO_TOG;
-        ;
-        USBFS_DEVICE->UEP3_RX_CTRL = USBFS_UEP_R_RES_ACK | USBFS_UEP_AUTO_TOG;
-        ;
-        USBFS_DEVICE->UEP4_RX_CTRL = USBFS_UEP_R_RES_ACK | USBFS_UEP_AUTO_TOG;
-        ;
-        USBFS_DEVICE->UEP5_RX_CTRL = USBFS_UEP_R_RES_ACK | USBFS_UEP_AUTO_TOG;
-        ;
-        USBFS_DEVICE->UEP6_RX_CTRL = USBFS_UEP_R_RES_ACK | USBFS_UEP_AUTO_TOG;
-        ;
-        USBFS_DEVICE->UEP7_RX_CTRL = USBFS_UEP_R_RES_ACK | USBFS_UEP_AUTO_TOG;
-        ;
 
-        USBFS_DEVICE->UEP0_TX_CTRL = USBFS_UEP_T_RES_NAK;
-        USBFS_DEVICE->UEP1_TX_CTRL = USBFS_UEP_T_RES_NAK | USBFS_UEP_AUTO_TOG;
-        USBFS_DEVICE->UEP2_TX_CTRL = USBFS_UEP_T_RES_NAK | USBFS_UEP_AUTO_TOG;
-        USBFS_DEVICE->UEP3_TX_CTRL = USBFS_UEP_T_RES_NAK | USBFS_UEP_AUTO_TOG;
-        USBFS_DEVICE->UEP4_TX_CTRL = USBFS_UEP_T_RES_NAK | USBFS_UEP_AUTO_TOG;
-        USBFS_DEVICE->UEP5_TX_CTRL = USBFS_UEP_T_RES_NAK | USBFS_UEP_AUTO_TOG;
-        USBFS_DEVICE->UEP6_TX_CTRL = USBFS_UEP_T_RES_NAK | USBFS_UEP_AUTO_TOG;
-        USBFS_DEVICE->UEP7_TX_CTRL = USBFS_UEP_T_RES_NAK | USBFS_UEP_AUTO_TOG;
+        for (uint8_t ep_idx = 1; ep_idx < USB_NUM_BIDIR_ENDPOINTS; ep_idx++) {
+            USB_SET_TX_LEN(ep_idx, 0);
+            USB_SET_TX_CTRL(ep_idx, USBFS_UEP_T_RES_NAK | USBFS_UEP_AUTO_TOG);
+            USB_SET_RX_CTRL(ep_idx, USBFS_UEP_R_RES_ACK | USBFS_UEP_AUTO_TOG);
+        }
 
         usbd_event_notify_handler(USBD_EVENT_RESET, NULL);
 
diff --git a/port/ch32/usb_dc_usbhs.c b/port/ch32/usb_dc_usbhs.c
index 8790d719..0e018fcd 100644
--- a/port/ch32/usb_dc_usbhs.c
+++ b/port/ch32/usb_dc_usbhs.c
@@ -6,11 +6,21 @@
 #endif
 
 #ifndef USB_NUM_BIDIR_ENDPOINTS
-#define USB_NUM_BIDIR_ENDPOINTS 8
+#define USB_NUM_BIDIR_ENDPOINTS 16
 #endif
 
+#define USB_SET_RX_DMA(ep_idx, addr) (*(volatile uint32_t *)((uint32_t)(&USBHS_DEVICE->UEP1_RX_DMA) + 4 * (ep_idx - 1)) = addr)
+#define USB_SET_TX_DMA(ep_idx, addr) (*(volatile uint32_t *)((uint32_t)(&USBHS_DEVICE->UEP1_TX_DMA) + 4 * (ep_idx - 1)) = addr)
+#define USB_SET_MAX_LEN(ep_idx, len) (*(volatile uint16_t *)((uint32_t)(&USBHS_DEVICE->UEP0_MAX_LEN) + 4 * ep_idx) = len)
+#define USB_SET_TX_LEN(ep_idx, len)  (*(volatile uint16_t *)((uint32_t)(&USBHS_DEVICE->UEP0_TX_LEN) + 4 * ep_idx) = len)
+#define USB_GET_TX_LEN(ep_idx)       (*(volatile uint16_t *)((uint32_t)(&USBHS_DEVICE->UEP0_TX_LEN) + 4 * ep_idx))
+#define USB_SET_TX_CTRL(ep_idx, val) (*(volatile uint8_t *)((uint32_t)(&USBHS_DEVICE->UEP0_TX_CTRL) + 4 * ep_idx) = val)
+#define USB_GET_TX_CTRL(ep_idx)      (*(volatile uint8_t *)((uint32_t)(&USBHS_DEVICE->UEP0_TX_CTRL) + 4 * ep_idx))
+#define USB_SET_RX_CTRL(ep_idx, val) (*(volatile uint8_t *)((uint32_t)(&USBHS_DEVICE->UEP0_RX_CTRL) + 4 * ep_idx) = val)
+#define USB_GET_RX_CTRL(ep_idx)      (*(volatile uint8_t *)((uint32_t)(&USBHS_DEVICE->UEP0_RX_CTRL) + 4 * ep_idx))
+
 /* Endpoint state */
-struct usb_dc_ep_state {
+struct ch32_usbhs_ep_state {
     /** Endpoint max packet size */
     uint16_t ep_mps;
     /** Endpoint Transfer Type.
@@ -21,11 +31,12 @@ struct usb_dc_ep_state {
 };
 
 /* Driver state */
-struct usb_dc_config_priv {
+struct ch32_usbhs_udc {
     volatile uint8_t dev_addr;
-    struct usb_dc_ep_state in_ep[USB_NUM_BIDIR_ENDPOINTS];  /*!< IN endpoint parameters*/
-    struct usb_dc_ep_state out_ep[USB_NUM_BIDIR_ENDPOINTS]; /*!< OUT endpoint parameters */
-} usb_dc_cfg;
+    struct ch32_usbhs_ep_state in_ep[USB_NUM_BIDIR_ENDPOINTS];                              /*!< IN endpoint parameters*/
+    struct ch32_usbhs_ep_state out_ep[USB_NUM_BIDIR_ENDPOINTS];                             /*!< OUT endpoint parameters */
+    __attribute__((aligned(4))) uint8_t ep_databuf[USB_NUM_BIDIR_ENDPOINTS - 1][512 + 512]; //epx_out(512)+epx_in(512)
+} g_ch32_usbhs_udc;
 
 // clang-format off
 /* Endpoint Buffer */
@@ -36,8 +47,9 @@ __attribute__ ((aligned(4))) uint8_t EP2_DatabufHD[512+512];  //ep2_out(64)+ep2_
 
 void USBHS_IRQHandler(void) __attribute__((interrupt("WCH-Interrupt-fast")));
 
+volatile bool ep0_data_toggle = 0x01;
 volatile uint8_t mps_over_flag = 0;
-volatile uint8_t USBHS_Dev_Endp0_Tog = 0x01; /* USB2.0�����豸�˵�0ͬ����־ */
+volatile bool epx_data_toggle[USB_NUM_BIDIR_ENDPOINTS - 1];
 
 __WEAK void usb_dc_low_level_init(void)
 {
@@ -49,116 +61,36 @@ __WEAK void usb_dc_low_level_deinit(void)
 
 int usb_dc_init(void)
 {
-    memset(&usb_dc_cfg, 0, sizeof(struct usb_dc_config_priv));
+    memset(&g_ch32_usbhs_udc, 0, sizeof(struct ch32_usbhs_udc));
 
     usb_dc_low_level_init();
 
     USBHS_DEVICE->HOST_CTRL = 0x00;
-    USBHS_DEVICE->HOST_CTRL = USBHS_SUSPEND_EN;
+    USBHS_DEVICE->HOST_CTRL = USBHS_PHY_SUSPENDM;
 
     USBHS_DEVICE->CONTROL = 0;
-#if 1
+#if CONFIG_USB_HS
     USBHS_DEVICE->CONTROL = USBHS_DMA_EN | USBHS_INT_BUSY_EN | USBHS_HIGH_SPEED;
 #else
     USBHS_DEVICE->CONTROL = USBHS_DMA_EN | USBHS_INT_BUSY_EN | USBHS_FULL_SPEED;
 #endif
 
+    USBHS_DEVICE->INT_FG = 0xff;
     USBHS_DEVICE->INT_EN = 0;
-    USBHS_DEVICE->INT_EN = USBHS_SETUP_ACT_EN | USBHS_TRANSFER_EN | USBHS_DETECT_EN | USBHS_SUSPEND_EN;
+    USBHS_DEVICE->INT_EN = USBHS_SETUP_ACT_EN | USBHS_TRANSFER_EN | USBHS_DETECT_EN;
 
     /* ALL endpoint enable */
     USBHS_DEVICE->ENDP_CONFIG = 0xffffffff;
 
-    USBHS_DEVICE->ENDP_CONFIG = USBHS_EP0_T_EN | USBHS_EP0_R_EN | USBHS_EP1_T_EN | USBHS_EP2_T_EN | USBHS_EP1_R_EN | USBHS_EP2_R_EN;
     USBHS_DEVICE->ENDP_TYPE = 0x00;
     USBHS_DEVICE->BUF_MODE = 0x00;
 
-    USBHS_DEVICE->UEP0_MAX_LEN = 64;
-    USBHS_DEVICE->UEP1_MAX_LEN = 512;
-    USBHS_DEVICE->UEP2_MAX_LEN = 512;
-    USBHS_DEVICE->UEP3_MAX_LEN = 512;
-    USBHS_DEVICE->UEP4_MAX_LEN = 512;
-    USBHS_DEVICE->UEP5_MAX_LEN = 512;
-    USBHS_DEVICE->UEP6_MAX_LEN = 512;
-    USBHS_DEVICE->UEP7_MAX_LEN = 512;
-    USBHS_DEVICE->UEP8_MAX_LEN = 512;
-    USBHS_DEVICE->UEP9_MAX_LEN = 512;
-    USBHS_DEVICE->UEP10_MAX_LEN = 512;
-    USBHS_DEVICE->UEP11_MAX_LEN = 512;
-    USBHS_DEVICE->UEP12_MAX_LEN = 512;
-    USBHS_DEVICE->UEP13_MAX_LEN = 512;
-    USBHS_DEVICE->UEP14_MAX_LEN = 512;
-    USBHS_DEVICE->UEP15_MAX_LEN = 512;
-
     USBHS_DEVICE->UEP0_DMA = (uint32_t)EP0_DatabufHD;
-    USBHS_DEVICE->UEP1_TX_DMA = (uint32_t)&EP1_DatabufHD[512];
-    USBHS_DEVICE->UEP1_RX_DMA = (uint32_t)&EP1_DatabufHD[0];
-    USBHS_DEVICE->UEP2_TX_DMA = (uint32_t)&EP2_DatabufHD[512];
-    USBHS_DEVICE->UEP2_RX_DMA = (uint32_t)&EP2_DatabufHD[0];
-
-    USBHS_DEVICE->UEP0_TX_LEN = 0;
-    USBHS_DEVICE->UEP0_TX_CTRL = USBHS_EP_T_RES_NAK;
-    USBHS_DEVICE->UEP0_RX_CTRL = USBHS_EP_R_RES_ACK;
-
-    USBHS_DEVICE->UEP1_TX_LEN = 0;
-    USBHS_DEVICE->UEP1_TX_CTRL = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK;
-    USBHS_DEVICE->UEP1_RX_CTRL = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK;
-
-    USBHS_DEVICE->UEP2_TX_LEN = 0;
-    USBHS_DEVICE->UEP2_TX_CTRL = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK;
-    USBHS_DEVICE->UEP2_RX_CTRL = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK;
-
-    USBHS_DEVICE->UEP3_TX_LEN = 0;
-    USBHS_DEVICE->UEP3_TX_CTRL = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK;
-    USBHS_DEVICE->UEP3_RX_CTRL = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK;
-
-    USBHS_DEVICE->UEP4_TX_LEN = 0;
-    USBHS_DEVICE->UEP4_TX_CTRL = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK;
-    USBHS_DEVICE->UEP4_RX_CTRL = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK;
-
-    USBHS_DEVICE->UEP5_TX_LEN = 0;
-    USBHS_DEVICE->UEP5_TX_CTRL = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK;
-    USBHS_DEVICE->UEP5_RX_CTRL = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK;
-
-    USBHS_DEVICE->UEP6_TX_LEN = 0;
-    USBHS_DEVICE->UEP6_TX_CTRL = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK;
-    USBHS_DEVICE->UEP6_RX_CTRL = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK;
-
-    USBHS_DEVICE->UEP7_TX_LEN = 0;
-    USBHS_DEVICE->UEP7_TX_CTRL = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK;
-    USBHS_DEVICE->UEP7_RX_CTRL = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK;
-
-    USBHS_DEVICE->UEP8_TX_LEN = 0;
-    USBHS_DEVICE->UEP8_TX_CTRL = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK;
-    USBHS_DEVICE->UEP8_RX_CTRL = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK;
-
-    USBHS_DEVICE->UEP9_TX_LEN = 0;
-    USBHS_DEVICE->UEP9_TX_CTRL = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK;
-    USBHS_DEVICE->UEP9_RX_CTRL = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK;
-
-    USBHS_DEVICE->UEP10_TX_LEN = 0;
-    USBHS_DEVICE->UEP10_TX_CTRL = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK;
-    USBHS_DEVICE->UEP10_RX_CTRL = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK;
 
-    USBHS_DEVICE->UEP11_TX_LEN = 0;
-    USBHS_DEVICE->UEP11_TX_CTRL = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK;
-    USBHS_DEVICE->UEP11_RX_CTRL = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK;
-
-    USBHS_DEVICE->UEP12_TX_LEN = 0;
-    USBHS_DEVICE->UEP12_TX_CTRL = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK;
-    USBHS_DEVICE->UEP12_RX_CTRL = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK;
-
-    USBHS_DEVICE->UEP13_TX_LEN = 0;
-    USBHS_DEVICE->UEP13_TX_CTRL = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK;
-    USBHS_DEVICE->UEP13_RX_CTRL = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK;
-
-    USBHS_DEVICE->UEP14_TX_LEN = 0;
-    USBHS_DEVICE->UEP14_TX_CTRL = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK;
-    USBHS_DEVICE->UEP14_RX_CTRL = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK;
-
-    USBHS_DEVICE->UEP15_TX_LEN = 0;
-    USBHS_DEVICE->UEP15_TX_CTRL = USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK;
-    USBHS_DEVICE->UEP15_RX_CTRL = USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK;
+    for (uint8_t ep_idx = 1; ep_idx < USB_NUM_BIDIR_ENDPOINTS; ep_idx++) {
+        USB_SET_RX_DMA(ep_idx, (uint32_t)&g_ch32_usbhs_udc.ep_databuf[ep_idx - 1][0]);
+        USB_SET_TX_DMA(ep_idx, (uint32_t)&g_ch32_usbhs_udc.ep_databuf[ep_idx - 1][512]);
+    }
 
     USBHS_DEVICE->CONTROL |= USBHS_DEV_PU_EN;
 
@@ -175,7 +107,7 @@ int usbd_set_address(const uint8_t addr)
     if (addr == 0) {
         USBHS_DEVICE->DEV_AD = addr & 0xff;
     }
-    usb_dc_cfg.dev_addr = addr;
+    g_ch32_usbhs_udc.dev_addr = addr;
     return 0;
 }
 
@@ -184,12 +116,15 @@ int usbd_ep_open(const struct usbd_endpoint_cfg *ep_cfg)
     uint8_t ep_idx = USB_EP_GET_IDX(ep_cfg->ep_addr);
 
     if (USB_EP_DIR_IS_OUT(ep_cfg->ep_addr)) {
-        usb_dc_cfg.out_ep[ep_idx].ep_mps = ep_cfg->ep_mps;
-        usb_dc_cfg.out_ep[ep_idx].ep_type = ep_cfg->ep_type;
+        g_ch32_usbhs_udc.out_ep[ep_idx].ep_mps = ep_cfg->ep_mps;
+        g_ch32_usbhs_udc.out_ep[ep_idx].ep_type = ep_cfg->ep_type;
+        USBHS_DEVICE->ENDP_CONFIG |= (1 << (ep_idx + 16));
     } else {
-        usb_dc_cfg.in_ep[ep_idx].ep_mps = ep_cfg->ep_mps;
-        usb_dc_cfg.in_ep[ep_idx].ep_type = ep_cfg->ep_type;
+        g_ch32_usbhs_udc.in_ep[ep_idx].ep_mps = ep_cfg->ep_mps;
+        g_ch32_usbhs_udc.in_ep[ep_idx].ep_type = ep_cfg->ep_type;
+        USBHS_DEVICE->ENDP_CONFIG |= (1 << (ep_idx));
     }
+    USB_SET_MAX_LEN(ep_idx, ep_cfg->ep_mps);
     return 0;
 }
 int usbd_ep_close(const uint8_t ep)
@@ -201,34 +136,16 @@ int usbd_ep_set_stall(const uint8_t ep)
     uint8_t ep_idx = USB_EP_GET_IDX(ep);
 
     if (USB_EP_DIR_IS_OUT(ep)) {
-        switch (ep_idx) {
-            case 0:
-                USBHS_DEVICE->UEP0_RX_CTRL = USBHS_EP_R_RES_STALL;
-                break;
-            case 1:
-                USBHS_DEVICE->UEP1_RX_CTRL = (USBHS_DEVICE->UEP1_RX_CTRL & ~USBHS_EP_R_RES_MASK) | USBHS_EP_R_RES_STALL;
-                break;
-            case 2:
-                USBHS_DEVICE->UEP2_RX_CTRL = (USBHS_DEVICE->UEP2_RX_CTRL & ~USBHS_EP_R_RES_MASK) | USBHS_EP_R_RES_STALL;
-                break;
-            default:
-                break;
+        if (ep_idx == 0) {
+            USBHS_DEVICE->UEP0_RX_CTRL = USBHS_EP_R_RES_STALL;
+        } else {
+            USB_SET_RX_CTRL(ep_idx, (USB_GET_RX_CTRL(ep_idx) & ~USBHS_EP_R_RES_MASK) | USBHS_EP_R_RES_STALL);
         }
-
     } else {
-        switch (ep_idx) {
-            case 0:
-                USBHS_DEVICE->UEP0_TX_LEN = 0;
-                USBHS_DEVICE->UEP0_TX_CTRL = USBHS_EP_T_RES_STALL;
-                break;
-            case 1:
-                USBHS_DEVICE->UEP1_TX_CTRL = (USBHS_DEVICE->UEP1_TX_CTRL & ~USBHS_EP_T_RES_MASK) | USBHS_EP_T_RES_STALL;
-                break;
-            case 2:
-                USBHS_DEVICE->UEP2_TX_CTRL = (USBHS_DEVICE->UEP2_TX_CTRL & ~USBHS_EP_T_RES_MASK) | USBHS_EP_T_RES_STALL;
-                break;
-            default:
-                break;
+        if (ep_idx == 0) {
+            USBHS_DEVICE->UEP0_TX_CTRL = USBHS_EP_T_RES_STALL;
+        } else {
+            USB_SET_TX_CTRL(ep_idx, (USB_GET_TX_CTRL(ep_idx) & ~USBHS_EP_T_RES_MASK) | USBHS_EP_T_RES_STALL);
         }
     }
 
@@ -240,40 +157,9 @@ int usbd_ep_clear_stall(const uint8_t ep)
     uint8_t ep_idx = USB_EP_GET_IDX(ep);
 
     if (USB_EP_DIR_IS_OUT(ep)) {
-        switch (ep_idx) {
-            case 0:
-
-                break;
-            case 1:
-                /* SET Endp1 Rx to USBHS_EP_R_RES_NAK;USBHS_EP_R_TOG_0 */
-                USBHS_DEVICE->UEP1_RX_CTRL = USBHS_EP_R_RES_ACK | USBHS_EP_R_TOG_0;
-                break;
-            case 2:
-                /* SET Endp2 Rx to USBHS_EP_R_RES_ACK;USBHS_EP_R_TOG_0 */
-                USBHS_DEVICE->UEP2_TX_CTRL = USBHS_EP_R_RES_ACK | USBHS_EP_R_TOG_0;
-                break;
-            default:
-                break;
-        }
-
+        USB_SET_RX_CTRL(ep_idx, USBHS_EP_R_RES_ACK | USBHS_EP_R_TOG_0);
     } else {
-        switch (ep_idx) {
-            case 0:
-
-                break;
-            case 1:
-                /* SET Endp1 Tx to USBHS_EP_T_RES_NAK;USBHS_EP_T_TOG_0;len = 0 */
-                USBHS_DEVICE->UEP1_TX_LEN = 0;
-                USBHS_DEVICE->UEP1_TX_CTRL = USBHS_EP_T_RES_NAK | USBHS_EP_T_TOG_0;
-                break;
-            case 2:
-                /* SET Endp2 Tx to USBHS_EP_T_RES_NAK;USBHS_EP_T_TOG_0;len = 0 */
-                USBHS_DEVICE->UEP2_TX_LEN = 0;
-                USBHS_DEVICE->UEP2_TX_CTRL = USBHS_EP_T_RES_NAK | USBHS_EP_T_TOG_0;
-                break;
-            default:
-                break;
-        }
+        USB_SET_TX_CTRL(ep_idx, USBHS_EP_T_RES_NAK | USBHS_EP_T_TOG_0);
     }
     return 0;
 }
@@ -290,52 +176,39 @@ int usbd_ep_write(const uint8_t ep, const uint8_t *data, uint32_t data_len, uint
         return -1;
     }
 
+    while (((USB_GET_TX_CTRL(ep_idx) & USBHS_EP_T_RES_MASK) == USBHS_EP_T_RES_ACK) && (ep_idx != 0)) {
+    }
+
     if (!data_len) {
-        switch (ep_idx) {
-            case 0:
-                USBHS_DEVICE->UEP0_TX_LEN = 0;
-                USBHS_DEVICE->UEP0_TX_CTRL = USBHS_EP_T_RES_ACK | USBHS_EP_T_TOG_1;
-                break;
-            case 1:
-                USBHS_DEVICE->UEP1_TX_LEN = 0;
-                break;
-            case 2:
-                USBHS_DEVICE->UEP2_TX_LEN = 0;
-                break;
-            default:
-                break;
+        if (ep_idx == 0) {
+            USB_SET_TX_LEN(ep_idx, 0);
+        } else {
+            USB_SET_TX_LEN(ep_idx, 0);
+            USB_SET_TX_CTRL(ep_idx, (USB_GET_TX_CTRL(ep_idx) & ~USBHS_EP_T_RES_MASK) | USBHS_EP_T_RES_ACK);
+            USB_SET_TX_CTRL(ep_idx, epx_data_toggle[ep_idx - 1] ? USBHS_EP_T_TOG_1 : USBHS_EP_T_TOG_0);
+            epx_data_toggle[ep_idx - 1] ^= 1;
         }
-
         return 0;
     }
 
-    if (data_len > usb_dc_cfg.in_ep[ep_idx].ep_mps) {
-        data_len = usb_dc_cfg.in_ep[ep_idx].ep_mps;
+    if (data_len > g_ch32_usbhs_udc.in_ep[ep_idx].ep_mps) {
+        data_len = g_ch32_usbhs_udc.in_ep[ep_idx].ep_mps;
+        if (ep_idx == 0) {
+            mps_over_flag = 1;
+        }
     }
 
-    switch (ep_idx) {
-        case 0:
-            memcpy(&EP0_DatabufHD[0], data, data_len);
-            USBHS_DEVICE->UEP0_TX_LEN = data_len;
-            USBHS_DEVICE->UEP0_TX_CTRL = USBHS_EP_T_RES_ACK | (USBHS_Dev_Endp0_Tog ? USBHS_EP_T_TOG_0 : USBHS_EP_T_TOG_1);
-            USBHS_Dev_Endp0_Tog ^= 1;
-            break;
-        case 1:
-            memcpy(&EP1_DatabufHD[512], data, data_len);
-            USBHS_DEVICE->UEP1_TX_LEN = data_len;
-            USBHS_DEVICE->UEP1_TX_CTRL = (USBHS_DEVICE->UEP1_TX_CTRL & ~(USBHS_EP_T_RES_MASK | USBHS_EP_T_LEN_MASK | USBHS_EP_T_TOG_MASK)) | USBHS_EP_T_RES_ACK;
-            //USBHS_DEVICE->UEP1_TX_CTRL |= ( USBHS_Endp1_T_Tog ? USBHS_EP_T_TOG_1 : USBHS_EP_T_TOG_0 );
-            break;
-        case 2:
-            memcpy(&EP2_DatabufHD[512], data, data_len);
-            USBHS_DEVICE->UEP2_TX_LEN = data_len;
-            USBHS_DEVICE->UEP2_TX_CTRL = (USBHS_DEVICE->UEP2_TX_CTRL & ~(USBHS_EP_T_RES_MASK | USBHS_EP_T_LEN_MASK | USBHS_EP_T_TOG_MASK)) | USBHS_EP_T_RES_ACK;
-            //USBHS_DEVICE->UEP2_TX_CTRL |= ( USBHS_Endp2_T_Tog ? USBHS_EP_T_TOG_1 : USBHS_EP_T_TOG_0 );
-            break;
-        default:
-            break;
-    }
+    if (ep_idx == 0) {
+        memcpy(&EP0_DatabufHD[0], data, data_len);
+        USB_SET_TX_LEN(ep_idx, data_len);
+    } else {
+        USB_SET_TX_LEN(ep_idx, data_len);
+        memcpy(&g_ch32_usbhs_udc.ep_databuf[ep_idx - 1][512], data, data_len);
 
+        USB_SET_TX_CTRL(ep_idx, (USB_GET_TX_CTRL(ep_idx) & ~(USBHS_EP_T_RES_MASK | USBHS_EP_T_TOG_MASK)) | USBHS_EP_T_RES_ACK);
+        USB_SET_TX_CTRL(ep_idx, epx_data_toggle[ep_idx - 1] ? USBHS_EP_T_TOG_1 : USBHS_EP_T_TOG_0);
+        epx_data_toggle[ep_idx - 1] ^= 1;
+    }
     if (ret_bytes) {
         *ret_bytes = data_len;
     }
@@ -353,29 +226,24 @@ int usbd_ep_read(const uint8_t ep, uint8_t *data, uint32_t max_data_len, uint32_
     }
 
     if (!max_data_len) {
+        if (ep_idx) {
+            USB_SET_RX_CTRL(ep_idx, (USB_GET_RX_CTRL(ep_idx) & ~USBHS_EP_R_RES_MASK) | USBHS_EP_R_RES_ACK);
+        }
         return 0;
     }
 
     read_count = USBHS_DEVICE->RX_LEN;
     read_count = MIN(read_count, max_data_len);
 
-    switch (ep_idx) {
-        case 0:
-            if ((max_data_len == 8) && !read_bytes) {
-                read_count = 8;
-                memcpy(data, &EP0_DatabufHD[0], 8);
-            } else {
-                memcpy(data, &EP0_DatabufHD[0], read_count);
-            }
-            break;
-        case 1:
-            memcpy(data, &EP1_DatabufHD[0], read_count);
-            break;
-        case 2:
-            memcpy(data, &EP2_DatabufHD[0], read_count);
-            break;
-        default:
-            break;
+    if (ep_idx == 0x00) {
+        if ((max_data_len == 8) && !read_bytes) {
+            read_count = 8;
+            memcpy(data, &EP0_DatabufHD[0], 8);
+        } else {
+            memcpy(data, &EP0_DatabufHD[0], read_count);
+        }
+    } else {
+        memcpy(data, &g_ch32_usbhs_udc.ep_databuf[ep_idx - 1][0], read_count);
     }
 
     if (read_bytes) {
@@ -387,44 +255,64 @@ int usbd_ep_read(const uint8_t ep, uint8_t *data, uint32_t max_data_len, uint32_
 
 void USBD_IRQHandler(void)
 {
-    uint32_t end_num, rx_token;
+    uint32_t ep_idx, token;
     uint8_t intflag = 0;
 
     intflag = USBHS_DEVICE->INT_FG;
 
     if (intflag & USBHS_TRANSFER_FLAG) {
-        end_num = (USBHS_DEVICE->INT_ST) & MASK_UIS_ENDP;
-        rx_token = (((USBHS_DEVICE->INT_ST) & MASK_UIS_TOKEN) >> 4) & 0x03;
-        if (end_num == 0) {
-            if (rx_token == PID_IN) {
+        ep_idx = (USBHS_DEVICE->INT_ST) & MASK_UIS_ENDP;
+        token = (((USBHS_DEVICE->INT_ST) & MASK_UIS_TOKEN) >> 4) & 0x03;
+
+        if (token == PID_IN) {
+            if (ep_idx == 0x00) {
                 usbd_event_notify_handler(USBD_EVENT_EP0_IN_NOTIFY, NULL);
-                if (usb_dc_cfg.dev_addr > 0) {
-                    USBHS_DEVICE->DEV_AD = usb_dc_cfg.dev_addr & 0xff;
-                    usb_dc_cfg.dev_addr = 0;
+                if (g_ch32_usbhs_udc.dev_addr > 0) {
+                    USBHS_DEVICE->DEV_AD = g_ch32_usbhs_udc.dev_addr & 0xff;
+                    g_ch32_usbhs_udc.dev_addr = 0;
                 }
-            } else if (rx_token == PID_OUT) {
-                usbd_event_notify_handler(USBD_EVENT_EP0_OUT_NOTIFY, NULL);
-                USBHS_DEVICE->UEP0_RX_CTRL = USBHS_EP_R_RES_ACK | USBHS_EP_R_TOG_1;
-            }
-        } else if (end_num == 1) {
-            if (rx_token == PID_IN) {
-                //USBHS_Endp1_Up_Flag = 0x00;
-                /* Ĭ�ϻ�NAK */
-                USBHS_DEVICE->UEP1_TX_CTRL = (USBHS_DEVICE->UEP1_TX_CTRL & ~(USBHS_EP_T_RES_MASK | USBHS_EP_T_TOG_MASK)) | USBHS_EP_T_RES_NAK | USBHS_EP_T_TOG_0;
-            } else if (rx_token == PID_OUT) {
+                if (mps_over_flag) {
+                    mps_over_flag = 0;
+                    ep0_data_toggle ^= 1;
+                    USBHS_DEVICE->UEP0_TX_CTRL = USBHS_EP_T_RES_ACK | (ep0_data_toggle ? USBHS_EP_T_TOG_0 : USBHS_EP_T_TOG_1);
+                } else {
+                    USBHS_DEVICE->UEP0_RX_CTRL = USBHS_EP_R_RES_ACK | USBHS_EP_R_TOG_1;
+                }
+            } else {
+                USB_SET_TX_CTRL(ep_idx, (USB_GET_TX_CTRL(ep_idx) & ~(USBHS_EP_T_RES_MASK | USBHS_EP_T_TOG_MASK)) | USBHS_EP_T_RES_NAK | USBHS_EP_T_TOG_0);
+                usbd_event_notify_handler(USBD_EVENT_EP_IN_NOTIFY, (void *)(ep_idx | 0x80));
             }
-        } else if (end_num == 2) {
-            if (rx_token == PID_IN) {
-            } else if (rx_token == PID_OUT) {
+        } else if (token == PID_OUT) {
+            if (ep_idx == 0x00) {
+                usbd_event_notify_handler(USBD_EVENT_EP0_OUT_NOTIFY, NULL);
+                USBHS_DEVICE->UEP0_TX_CTRL = USBHS_EP_T_RES_ACK | USBHS_EP_T_TOG_1;
+            } else {
+                if (USBHS_DEVICE->INT_ST & USBHS_DEV_UIS_TOG_OK) {
+                    USB_SET_RX_CTRL(ep_idx, (USB_GET_RX_CTRL(ep_idx) & ~USBHS_EP_R_RES_MASK) | USBHS_EP_R_RES_NAK);
+                    usbd_event_notify_handler(USBD_EVENT_EP_OUT_NOTIFY, (void *)(ep_idx & 0x7f));
+                }
             }
         }
         USBHS_DEVICE->INT_FG = USBHS_TRANSFER_FLAG;
     } else if (intflag & USBHS_SETUP_FLAG) {
         usbd_event_notify_handler(USBD_EVENT_SETUP_NOTIFY, NULL);
+        USBHS_DEVICE->UEP0_TX_CTRL = USBHS_EP_T_RES_ACK | USBHS_EP_T_TOG_1;
         USBHS_DEVICE->INT_FG = USBHS_SETUP_FLAG;
     } else if (intflag & USBHS_DETECT_FLAG) {
+        USBHS_DEVICE->ENDP_CONFIG = USBHS_EP0_T_EN | USBHS_EP0_R_EN;
+
+        USBHS_DEVICE->UEP0_TX_LEN = 0;
+        USBHS_DEVICE->UEP0_TX_CTRL = USBHS_EP_T_RES_NAK;
+        USBHS_DEVICE->UEP0_RX_CTRL = USBHS_EP_R_RES_ACK;
+
+        for (uint8_t ep_idx = 1; ep_idx < USB_NUM_BIDIR_ENDPOINTS; ep_idx++) {
+            USB_SET_TX_LEN(ep_idx, 0);
+            USB_SET_TX_CTRL(ep_idx, USBHS_EP_T_AUTOTOG | USBHS_EP_T_RES_NAK); // autotog does not work
+            USB_SET_RX_CTRL(ep_idx, USBHS_EP_R_AUTOTOG | USBHS_EP_R_RES_ACK);
+            epx_data_toggle[ep_idx - 1] = false;
+        }
+
         usbd_event_notify_handler(USBD_EVENT_RESET, NULL);
         USBHS_DEVICE->INT_FG = USBHS_DETECT_FLAG;
     }
-    printf("reset\r\n");
 }