1 files changed, 21 insertions, 7 deletions
diff --git a/stmhal/string0.c b/stmhal/string0.c
index c605e0a9e..2f28de097 100644
--- a/stmhal/string0.c
+++ b/stmhal/string0.c
@@ -27,14 +27,28 @@
 #include <stdint.h>
 #include "std.h"
 
-void *memcpy(void *dest, const void *src, size_t n) {
-    // TODO align and copy 32 bits at a time
-    uint8_t *d = dest;
-    const uint8_t *s = src;
-    for (; n > 0; n--) {
-        *d++ = *s++;
+#define likely(x) __builtin_expect((x), 1)
+
+void *memcpy(void *dst, const void *src, size_t n) {
+    if (likely(!(n&3) && !((long)dst&3) && !((long)src&3))) {
+        //aligned access, copy words
+        long *d = dst;
+        const long *s = src;
+
+        for (n=(n>>2); n; n--) {
+            *d++ = *s++;
+        }
+    } else {
+        //unaligned access, copy bytes
+        char *d = dst;
+        const char *s = src;
+
+        for (; n; n--) {
+            *d++ = *s++;
+        }
     }
-    return dest;
+
+    return dst;
 }
 
 void *memmove(void *dest, const void *src, size_t n) {