feat: use fast memcpy for net

10 files changed, 100 insertions, 11 deletions

diff --git a/class/cdc/usbd_cdc_ecm.c b/class/cdc/usbd_cdc_ecm.c
index 1fca9cad..a2b02e98 100644
--- a/class/cdc/usbd_cdc_ecm.c
+++ b/class/cdc/usbd_cdc_ecm.c
@@ -183,7 +183,7 @@ struct pbuf *usbd_cdc_ecm_eth_rx(void)
         usbd_cdc_ecm_start_read_next();
         return NULL;
     }
-    memcpy(p->payload, (uint8_t *)g_cdc_ecm_rx_buffer, g_cdc_ecm_rx_data_length);
+    usb_memcpy(p->payload, (uint8_t *)g_cdc_ecm_rx_buffer, g_cdc_ecm_rx_data_length);
     p->len = g_cdc_ecm_rx_data_length;
 
     USB_LOG_DBG("rxlen:%d\r\n", g_cdc_ecm_rx_data_length);
@@ -206,7 +206,7 @@ int usbd_cdc_ecm_eth_tx(struct pbuf *p)
 
     buffer = g_cdc_ecm_tx_buffer;
     for (q = p; q != NULL; q = q->next) {
-        memcpy(buffer, q->payload, q->len);
+        usb_memcpy(buffer, q->payload, q->len);
         buffer += q->len;
     }
 
diff --git a/class/cdc/usbh_cdc_ecm.c b/class/cdc/usbh_cdc_ecm.c
index 5e3ae7bc..f7f9563f 100644
--- a/class/cdc/usbh_cdc_ecm.c
+++ b/class/cdc/usbh_cdc_ecm.c
@@ -287,7 +287,7 @@ int usbh_cdc_ecm_eth_output(uint8_t *buf, uint32_t buflen)
         return -USB_ERR_NOTCONN;
     }
 
-    memcpy(buffer, buf, buflen);
+    usb_memcpy(buffer, buf, buflen);
 
     USB_LOG_DBG("txlen:%d\r\n", buflen);
 
diff --git a/class/cdc/usbh_cdc_ncm.c b/class/cdc/usbh_cdc_ncm.c
index 4f13f791..ac220f57 100644
--- a/class/cdc/usbh_cdc_ncm.c
+++ b/class/cdc/usbh_cdc_ncm.c
@@ -356,7 +356,7 @@ int usbh_cdc_ncm_eth_output(uint8_t *buf, uint32_t buflen)
     ndp16_datagram->wDatagramLength = 0;
 
     buffer = &g_cdc_ncm_tx_buffer[16];
-    memcpy(buffer, buf, buflen);
+    usb_memcpy(buffer, buf, buflen);
 
     USB_LOG_DBG("txlen:%d\r\n", nth16->wBlockLength);
 
diff --git a/class/vendor/net/usbh_asix.c b/class/vendor/net/usbh_asix.c
index 62d78ec3..111277a7 100644
--- a/class/vendor/net/usbh_asix.c
+++ b/class/vendor/net/usbh_asix.c
@@ -721,7 +721,7 @@ int usbh_asix_eth_output(uint8_t *buf, uint32_t buflen)
     }
 
     buffer = &g_asix_tx_buffer[4];
-    memcpy(buffer, buf, buflen);
+    usb_memcpy(buffer, buf, buflen);
 
     g_asix_tx_buffer[0] = buflen & 0xff;
     g_asix_tx_buffer[1] = (buflen >> 8) & 0xff;
diff --git a/class/vendor/net/usbh_rtl8152.c b/class/vendor/net/usbh_rtl8152.c
index 3bd68a1d..df152744 100644
--- a/class/vendor/net/usbh_rtl8152.c
+++ b/class/vendor/net/usbh_rtl8152.c
@@ -2212,7 +2212,7 @@ int usbh_rtl8152_eth_output(uint8_t *buf, uint32_t buflen)
     tx_desc->opts2 = 0;
 
     buffer = g_rtl8152_tx_buffer + sizeof(struct tx_desc);
-    memcpy(buffer, buf, buflen);
+    usb_memcpy(buffer, buf, buflen);
 
     USB_LOG_DBG("txlen:%d\r\n", buflen + sizeof(struct tx_desc));
 
diff --git a/class/wireless/usbd_rndis.c b/class/wireless/usbd_rndis.c
index 67dc79db..c357b25a 100644
--- a/class/wireless/usbd_rndis.c
+++ b/class/wireless/usbd_rndis.c
@@ -488,7 +488,7 @@ struct pbuf *usbd_rndis_eth_rx(void)
     if (p == NULL) {
         return NULL;
     }
-    memcpy(p->payload, (uint8_t *)g_rndis_rx_data_buffer, g_rndis_rx_data_length);
+    usb_memcpy(p->payload, (uint8_t *)g_rndis_rx_data_buffer, g_rndis_rx_data_length);
     p->len = g_rndis_rx_data_length;
 
     USB_LOG_DBG("rxlen:%d\r\n", g_rndis_rx_data_length);
@@ -518,7 +518,7 @@ int usbd_rndis_eth_tx(struct pbuf *p)
 
     buffer = (uint8_t *)(g_rndis_tx_buffer + sizeof(rndis_data_packet_t));
     for (q = p; q != NULL; q = q->next) {
-        memcpy(buffer, q->payload, q->len);
+        usb_memcpy(buffer, q->payload, q->len);
         buffer += q->len;
     }
 
diff --git a/class/wireless/usbh_rndis.c b/class/wireless/usbh_rndis.c
index 969e6e53..f25cde11 100644
--- a/class/wireless/usbh_rndis.c
+++ b/class/wireless/usbh_rndis.c
@@ -468,7 +468,7 @@ find_class:
 
                 /* Not word-aligned case */
                 if (pmg_offset & 0x3) {
-                    memcpy(&temp, pmsg, sizeof(rndis_data_packet_t));
+                    usb_memcpy(&temp, pmsg, sizeof(rndis_data_packet_t));
                     pmsg = &temp;
                 }
 
@@ -523,11 +523,11 @@ int usbh_rndis_eth_output(uint8_t *buf, uint32_t buflen)
     hdr->DataLength = buflen;
 
     buffer = (uint8_t *)(g_rndis_tx_buffer + sizeof(rndis_data_packet_t));
-    memcpy(buffer, buf, buflen);
+    usb_memcpy(buffer, buf, buflen);
 
     len = hdr->MessageLength;
     /* if message length is the multiple of wMaxPacketSize, we should add a short packet to tell device transfer is over. */
-    if (!(hdr->MessageLength % g_rndis_class.bulkout->wMaxPacketSize)) {
+    if (!(len % g_rndis_class.bulkout->wMaxPacketSize)) {
         len += 1;
     }
 
diff --git a/common/usb_memcpy.h b/common/usb_memcpy.h
new file mode 100644
index 00000000..05f40ae3
--- /dev/null
+++ b/common/usb_memcpy.h
@@ -0,0 +1,87 @@
+/*
+ * Copyright (c) 2024, sakumisu
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+#include <stdint.h>
+#include <stddef.h>
+
+#define ALIGN_UP_DWORD(x) ((uint32_t)(x) & (sizeof(uint32_t) - 1))
+
+static inline void dword2array(char *addr, uint32_t w)
+{
+    addr[0] = w;
+    addr[1] = w >> 8;
+    addr[2] = w >> 16;
+    addr[3] = w >> 24;
+}
+
+static inline void *usb_memcpy(void *s1, const void *s2, size_t n)
+{
+    char *b1 = s1;
+    const char *b2 = s2;
+    uint32_t *w1;
+    const uint32_t *w2;
+
+    if (ALIGN_UP_DWORD(b1) == ALIGN_UP_DWORD(b2)) {
+        while (ALIGN_UP_DWORD(b1) != 0 && n > 0) {
+            *b1++ = *b2++;
+            --n;
+        }
+
+        w1 = (uint32_t *)b1;
+        w2 = (const uint32_t *)b2;
+
+        while (n >= 4 * sizeof(uint32_t)) {
+            *w1++ = *w2++;
+            *w1++ = *w2++;
+            *w1++ = *w2++;
+            *w1++ = *w2++;
+            n -= 4 * sizeof(uint32_t);
+        }
+
+        while (n >= sizeof(uint32_t)) {
+            *w1++ = *w2++;
+            n -= sizeof(uint32_t);
+        }
+
+        b1 = (char *)w1;
+        b2 = (const char *)w2;
+
+        while (n--) {
+            *b1++ = *b2++;
+        }
+    } else {
+        while (n > 0 && ALIGN_UP_DWORD(b2) != 0) {
+            *b1++ = *b2++;
+            --n;
+        }
+
+        w2 = (const uint32_t *)b2;
+
+        while (n >= 4 * sizeof(uint32_t)) {
+         dword2array(b1, *w2++);
+            b1 += sizeof(uint32_t);
+         dword2array(b1, *w2++);
+            b1 += sizeof(uint32_t);
+         dword2array(b1, *w2++);
+            b1 += sizeof(uint32_t);
+         dword2array(b1, *w2++);
+            b1 += sizeof(uint32_t);
+            n -= 4 * sizeof(uint32_t);
+        }
+
+        while (n >= sizeof(uint32_t)) {
+         dword2array(b1, *w2++);
+            b1 += sizeof(uint32_t);
+            n -= sizeof(uint32_t);
+        }
+
+        b2 = (const char *)w2;
+
+        while (n--) {
+            *b1++ = *b2++;
+        }
+    }
+    return s1;
+}
\ No newline at end of file
diff --git a/core/usbd_core.h b/core/usbd_core.h
index a1a943ec..01335f2f 100644
--- a/core/usbd_core.h
+++ b/core/usbd_core.h
@@ -22,6 +22,7 @@ extern "C" {
 #include "usb_list.h"
 #include "usb_log.h"
 #include "usb_dc.h"
+#include "usb_memcpy.h"
 
 enum usbd_event_type {
     /* USB DCD IRQ */
diff --git a/core/usbh_core.h b/core/usbh_core.h
index 9ce081fc..731133b5 100644
--- a/core/usbh_core.h
+++ b/core/usbh_core.h
@@ -20,6 +20,7 @@
 #include "usb_hc.h"
 #include "usb_osal.h"
 #include "usbh_hub.h"
+#include "usb_memcpy.h"
 
 #ifdef __cplusplus
 extern "C" {