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 +#include + +#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" {