1 /*
2 * Copyright (c) 2024, sakumisu
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6 #ifndef USB_MEMCPY_H
7 #define USB_MEMCPY_H
8
9 #include <stdint.h>
10 #include <stddef.h>
11
12 #define ALIGN_UP_DWORD(x) ((uint32_t)(uintptr_t)(x) & (sizeof(uint32_t) - 1))
13
dword2array(char * addr,uint32_t w)14 static inline void dword2array(char *addr, uint32_t w)
15 {
16 addr[0] = w;
17 addr[1] = w >> 8;
18 addr[2] = w >> 16;
19 addr[3] = w >> 24;
20 }
21
usb_memcpy(void * s1,const void * s2,size_t n)22 static inline void *usb_memcpy(void *s1, const void *s2, size_t n)
23 {
24 char *b1 = (char *)s1;
25 const char *b2 = (const char *)s2;
26 uint32_t *w1;
27 const uint32_t *w2;
28
29 if (ALIGN_UP_DWORD(b1) == ALIGN_UP_DWORD(b2)) {
30 while (ALIGN_UP_DWORD(b1) != 0 && n > 0) {
31 *b1++ = *b2++;
32 --n;
33 }
34
35 w1 = (uint32_t *)b1;
36 w2 = (const uint32_t *)b2;
37
38 while (n >= 4 * sizeof(uint32_t)) {
39 *w1++ = *w2++;
40 *w1++ = *w2++;
41 *w1++ = *w2++;
42 *w1++ = *w2++;
43 n -= 4 * sizeof(uint32_t);
44 }
45
46 while (n >= sizeof(uint32_t)) {
47 *w1++ = *w2++;
48 n -= sizeof(uint32_t);
49 }
50
51 b1 = (char *)w1;
52 b2 = (const char *)w2;
53
54 while (n--) {
55 *b1++ = *b2++;
56 }
57 } else {
58 while (n > 0 && ALIGN_UP_DWORD(b2) != 0) {
59 *b1++ = *b2++;
60 --n;
61 }
62
63 w2 = (const uint32_t *)b2;
64
65 while (n >= 4 * sizeof(uint32_t)) {
66 dword2array(b1, *w2++);
67 b1 += sizeof(uint32_t);
68 dword2array(b1, *w2++);
69 b1 += sizeof(uint32_t);
70 dword2array(b1, *w2++);
71 b1 += sizeof(uint32_t);
72 dword2array(b1, *w2++);
73 b1 += sizeof(uint32_t);
74 n -= 4 * sizeof(uint32_t);
75 }
76
77 while (n >= sizeof(uint32_t)) {
78 dword2array(b1, *w2++);
79 b1 += sizeof(uint32_t);
80 n -= sizeof(uint32_t);
81 }
82
83 b2 = (const char *)w2;
84
85 while (n--) {
86 *b1++ = *b2++;
87 }
88 }
89 return s1;
90 }
91
92 #ifndef CONFIG_USB_MEMCPY_DISABLE
93 #define memcpy usb_memcpy
94 #endif
95
96 #endif
97