Provide word-aligned access to memcpy(3) and memset(3)

1 files changed, 55 insertions, 11 deletions

diff --git a/libpsx/src/libc/string.c b/libpsx/src/libc/string.c
index 7a21224..ab85a42 100644
--- a/libpsx/src/libc/string.c
+++ b/libpsx/src/libc/string.c
@@ -10,14 +10,40 @@
 #include <unistd.h>
 #include <ctype.h>
 
-void *memcpy(void *dst, const void *src, size_t len)
+void *memcpy(void *const dst, const void *const src, size_t len)
 {
-	void *dst2 = dst;
-	
-	while(len--)
-		*(((unsigned char*)dst++)) = *(((unsigned char*)src++));
-		
-	return dst2;
+	union
+	{
+		const void *v;
+		const unsigned char *c;
+		const unsigned int *w;
+	} srcl;
+
+	union
+	{
+		void *v;
+		unsigned char *c;
+		unsigned int *w;
+	} dstl;
+
+	srcl.v = src;
+	dstl.v = dst;
+
+	/* Avoid unaligned access before doing word access. */
+	while (len && (((ptrdiff_t)dstl.v & (sizeof (int) - 1))
+		|| ((ptrdiff_t)srcl.v & (sizeof (int) - 1))))
+	{
+		*dstl.c++ = *srcl.c++;
+		len--;
+	}
+
+	for (; len >= sizeof (int); len -= sizeof (int))
+		*dstl.w++ = *srcl.w++;
+
+	while (len--)
+		*dstl.c++ = *srcl.c++;
+
+	return dst;
 }
 
 void *memccpy(void *dst, const void *src, int c, size_t len)
@@ -37,11 +63,29 @@ void *memccpy(void *dst, const void *src, int c, size_t len)
 
 void *memset(void *dst , int c , size_t len)
 {
-	unsigned char *dstc = (unsigned char*)dst;
-	int x;
+	union
+	{
+		void *v;
+		unsigned char *c;
+		unsigned int *w;
+	} dstl;
+
+	const int cw = c | c << 8 | c << 16 | c << 24;
+
+	dstl.v = dst;
+
+	/* Avoid unaligned access before doing word access. */
+	while (len && ((ptrdiff_t)dstl.v & (sizeof (int) - 1)))
+	{
+		*dstl.c++ = c;
+		len--;
+	}
+
+	for (; len >= sizeof (int); len -= sizeof (int))
+		*dstl.w++ = cw;
 
-	for(x = 0; x < len; x++)
-		dstc[x] = c;
+	while (len--)
+		*dstl.c++ = c;
 
 	return dst;
 }