From 6668ea8666377f9e2adfeb486c5048a85fa590d1 Mon Sep 17 00:00:00 2001
From: Marvin Borner
Date: Fri, 25 Sep 2020 19:28:42 +0200
Subject: Added memory alignment

---
 libc/mem.c | 34 ++++++++++++++++++++++++++++++++--
 1 file changed, 32 insertions(+), 2 deletions(-)

(limited to 'libc')

diff --git a/libc/mem.c b/libc/mem.c
index 4744a8b..e87089f 100644
--- a/libc/mem.c
+++ b/libc/mem.c
@@ -58,6 +58,30 @@ int memcmp(const void *s1, const void *s2, u32 n)
 
 #ifdef kernel
 
+#define ALIGNMENT 16ul
+#define ALIGN_TYPE char
+#define ALIGN_INFO sizeof(ALIGN_TYPE) * 16
+
+#define ALIGN(ptr)                                                                                 \
+	if (ALIGNMENT > 1) {                                                                       \
+		u32 diff;                                                                          \
+		ptr = (void *)((u32)ptr + ALIGN_INFO);                                             \
+		diff = (u32)ptr & (ALIGNMENT - 1);                                                 \
+		if (diff != 0) {                                                                   \
+			diff = ALIGNMENT - diff;                                                   \
+			ptr = (void *)((u32)ptr + diff);                                           \
+		}                                                                                  \
+		*((ALIGN_TYPE *)((u32)ptr - ALIGN_INFO)) = diff + ALIGN_INFO;                      \
+	}
+
+#define UNALIGN(ptr)                                                                               \
+	if (ALIGNMENT > 1) {                                                                       \
+		u32 diff = *((ALIGN_TYPE *)((u32)ptr - ALIGN_INFO));                               \
+		if (diff < (ALIGNMENT + ALIGN_INFO)) {                                             \
+			ptr = (void *)((u32)ptr - diff);                                           \
+		}                                                                                  \
+	}
+
 static u32 *heap;
 static u32 index;
 
@@ -86,15 +110,21 @@ void *malloc(u32 size)
 	if (size < 1)
 		return NULL;
 
+	size = size + ALIGNMENT + ALIGN_INFO;
+
 	heap[index] = size;
 	index += size + 1;
-	return (void *)(heap + index - size);
+
+	void *p = (void *)(heap + index - size);
+	ALIGN(p);
+
+	return p;
 }
 
 // TODO: Implement free, realloc and find_smallest_hole
 void free(void *ptr)
 {
-	(void)ptr;
+	UNALIGN(ptr);
 }
 
 void *realloc(void *ptr, u32 size)
-- 
cgit v1.2.3