Added memory alignment

1 files changed, 32 insertions, 2 deletions

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)