From bfec0b2f0091d72e800f2a64342f53868ce76ed3 Mon Sep 17 00:00:00 2001
From: Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
Date: Sun, 20 Nov 2016 03:02:19 -0500
Subject: [PATCH] added missing implementation for aligned memory allocator

---
 include/villas/memory.h |  2 +-
 lib/memory.c            | 29 +++++++++++++++++++++--------
 tests/memory.c          |  5 ++---
 3 files changed, 24 insertions(+), 12 deletions(-)

diff --git a/include/villas/memory.h b/include/villas/memory.h
index e546c106b..0390a0b92 100644
--- a/include/villas/memory.h
+++ b/include/villas/memory.h
@@ -15,7 +15,7 @@
 
 #define HUGEPAGESIZE	(1 << 21)
 
-typedef void *(*memzone_allocator_t)(size_t len);
+typedef void *(*memzone_allocator_t)(size_t len, size_t alignment);
 typedef int (*memzone_deallocator_t)(void *ptr, size_t len);
 
 enum memtype_flags {
diff --git a/lib/memory.c b/lib/memory.c
index 2e3120450..85ef52d7f 100644
--- a/lib/memory.c
+++ b/lib/memory.c
@@ -34,15 +34,20 @@ int memory_init()
 
 void * memory_alloc(const struct memtype *m, size_t len)
 {
-	debug(DBG_MEM | 2, "Allocating %#zx bytes of %s memory", len, m->name);
-	return m->alloc(len);
+	void *ptr = m->alloc(len, sizeof(void *));
+		
+	debug(DBG_MEM | 2, "Allocated %#zx bytes of %s memory: %p", len, m->name, ptr);
+
+	return ptr;
 }
 
 void * memory_alloc_aligned(const struct memtype *m, size_t len, size_t alignment)
 {
-	debug(DBG_MEM | 2, "Allocating %#zx bytes of %#zx-byte-aligned %s memory", len, alignment, m->name);
-	warn("%s: not implemented yet!", __FUNCTION__);
-	return memory_alloc(m, len);
+	void *ptr = m->alloc(len, alignment);
+	
+	debug(DBG_MEM | 2, "Allocated %#zx bytes of %#zx-byte-aligned %s memory: %p", len, alignment, m->name, ptr);
+	
+	return ptr;
 }
 
 int memory_free(const struct memtype *m, void *ptr, size_t len)
@@ -51,9 +56,17 @@ int memory_free(const struct memtype *m, void *ptr, size_t len)
 	return m->free(ptr, len);
 }
 
-static void * memory_heap_alloc(size_t len)
+static void * memory_heap_alloc(size_t len, size_t alignment)
 {
-	return malloc(len);
+	void *ptr;
+	int ret;
+	
+	if (alignment < sizeof(void *))
+		alignment = sizeof(void *);
+	
+	ret = posix_memalign(&ptr, alignment, len);
+	
+	return ret ? NULL : ptr;
 }
 
 int memory_heap_free(void *ptr, size_t len)
@@ -64,7 +77,7 @@ int memory_heap_free(void *ptr, size_t len)
 }
 
 /** Allocate memory backed by hugepages with malloc() like interface */
-static void * memory_hugepage_alloc(size_t len)
+static void * memory_hugepage_alloc(size_t len, size_t alignment)
 {
 	int prot = PROT_READ | PROT_WRITE;
 	int flags = MAP_PRIVATE | MAP_ANONYMOUS;
diff --git a/tests/memory.c b/tests/memory.c
index 234e6defd..6c56d1be8 100644
--- a/tests/memory.c
+++ b/tests/memory.c
@@ -15,8 +15,7 @@
 #include "utils.h"
 
 TheoryDataPoints(memory, aligned) = {
-//	DataPoints(size_t, 1, 32, 55, 1 << 10, 1 << 20),
-	DataPoints(size_t, 1<<12),
+	DataPoints(size_t, 1, 32, 55, 1 << 10, 1 << 20),
 	DataPoints(size_t, 1, 8, 1 << 12),
 	DataPoints(const struct memtype *, &memtype_heap, &memtype_hugepage)
 };
@@ -28,7 +27,7 @@ Theory((size_t len, size_t align, const struct memtype *m), memory, aligned) {
 	ptr = memory_alloc_aligned(m, len, align);
 	cr_assert_neq(ptr, NULL, "Failed to allocate memory");
 	
-	//cr_assert(IS_ALIGNED(ptr, align));
+	cr_assert(IS_ALIGNED(ptr, align));
 
 	if (m == &memtype_hugepage) {
 		cr_assert(IS_ALIGNED(ptr, HUGEPAGESIZE));