From 22628b176d993daa3fb770ed94250a227ced1ff9 Mon Sep 17 00:00:00 2001
From: Andreas Baumann <mail@andreasbaumann.cc>
Date: Tue, 1 Aug 2017 13:39:16 +0200
Subject: memory_manager_allocate takes an alignment parameter now added C11
 aligned_alloc to stdlib

---
 src/kernel/memorymanagement.c | 15 ++++++++++-----
 src/kernel/memorymanagement.h |  2 +-
 src/libc/stdlib.c             | 13 ++++++++++++-
 src/libc/stdlib.h             |  2 ++
 4 files changed, 25 insertions(+), 7 deletions(-)

diff --git a/src/kernel/memorymanagement.c b/src/kernel/memorymanagement.c
index 49c825c..1e18849 100644
--- a/src/kernel/memorymanagement.c
+++ b/src/kernel/memorymanagement.c
@@ -24,12 +24,12 @@ void memory_manager_init( memory_manager_t *memory_manager, size_t offset, size_
 	memory_manager->first = first;
 }
 
-void *memory_manager_allocate( memory_manager_t *memory_manager, size_t size )
+void *memory_manager_allocate( memory_manager_t *memory_manager, size_t size, size_t alignment )
 {	
 	// find next free chunk with a proper size
 	memory_chunk_t *result = NULL;
 	for( memory_chunk_t *chunk = memory_manager->first; chunk != NULL; chunk = chunk->next ) {
-		if( chunk->size > size + sizeof( memory_chunk_t ) && !chunk->allocated ) {
+		if( chunk->size > size + alignment + sizeof( memory_chunk_t ) && !chunk->allocated ) {
 			result = chunk;
 			break;
 		}
@@ -41,9 +41,9 @@ void *memory_manager_allocate( memory_manager_t *memory_manager, size_t size )
 	
 	// split of a new unallocated chunk at the end of the retrieved
 	// chunk, put in into the right place in the list of chunks
-	memory_chunk_t *tmp = (memory_chunk_t *)(( (uint32_t)result) + sizeof( memory_chunk_t ) + size );
+	memory_chunk_t *tmp = (memory_chunk_t *)(( (uint32_t)result) + sizeof( memory_chunk_t ) + size + alignment );
 	tmp->allocated = false;
-	tmp->size = result->size - size - sizeof( memory_chunk_t );
+	tmp->size = result->size - size - alignment - sizeof( memory_chunk_t );
 	tmp->prev = result;
 	tmp->next = result->next;
 	if( tmp->next != NULL ) {
@@ -55,8 +55,13 @@ void *memory_manager_allocate( memory_manager_t *memory_manager, size_t size )
 	
 	// return a pointer after the internal data structure as pointer
 	// for the user, mark the chunk as used	
-	void *p = (void *)(( (uint32_t)result) + sizeof( memory_chunk_t ) );
+	void *p = (void *)( ( (uint32_t)result) + sizeof( memory_chunk_t ) );
 	
+	// align the pointer
+	if( alignment != 1 && (uint32_t)p % alignment != 0 ) {
+		p = (void *)( (uint32_t)p + alignment - ( (uint32_t)p % alignment ) );
+	}
+
 	return p;
 }
 
diff --git a/src/kernel/memorymanagement.h b/src/kernel/memorymanagement.h
index 6dee417..efaa5e2 100644
--- a/src/kernel/memorymanagement.h
+++ b/src/kernel/memorymanagement.h
@@ -20,7 +20,7 @@ typedef struct {
 } memory_manager_t;
 
 void memory_manager_init( memory_manager_t *memory_manager, size_t offset, size_t size );
-void *memory_manager_allocate( memory_manager_t *memory_manager, size_t size );
+void *memory_manager_allocate( memory_manager_t *memory_manager, size_t size, size_t alignment );
 void memory_manager_deallocate( memory_manager_t *memory_manager, void **p );
 size_t memory_manager_stats_used( memory_manager_t *memory_manager );
 size_t memory_manager_stats_free( memory_manager_t *memory_manager );
diff --git a/src/libc/stdlib.c b/src/libc/stdlib.c
index 4fea56e..1316152 100644
--- a/src/libc/stdlib.c
+++ b/src/libc/stdlib.c
@@ -86,7 +86,7 @@ static memory_manager_t *stdlib_memory_manager = NULL;
   
 void *malloc( size_t size )
 {
-	return memory_manager_allocate( stdlib_memory_manager, size );
+	return memory_manager_allocate( stdlib_memory_manager, size, 1 );
 }
 
 void free( void *p )
@@ -94,7 +94,18 @@ void free( void *p )
 	memory_manager_deallocate( stdlib_memory_manager, &p );
 }
 
+void *aligned_alloc( size_t alignment, size_t size )
+{
+	if( size % alignment != 0 ) {
+		kernel_panic( "Illegal call to 'aligned_alloc' with size %d and alignment %d!",
+			size, alignment );
+	}
+	
+	return memory_manager_allocate( stdlib_memory_manager, size, alignment );
+}
+
 void __stdlib_set_memory_manager( memory_manager_t *memory_manager )
 {
 	stdlib_memory_manager = memory_manager;
 }
+
diff --git a/src/libc/stdlib.h b/src/libc/stdlib.h
index f7cfc8c..5f17583 100644
--- a/src/libc/stdlib.h
+++ b/src/libc/stdlib.h
@@ -13,6 +13,8 @@ void __attribute__( (noreturn ) ) abort( void );
 void *malloc( size_t size );
 void free( void *p );
 
+void *aligned_alloc( size_t alignment, size_t size );
+
 void __stdlib_set_memory_manager( memory_manager_t *memory_manager );
 
 #endif // STDLIB_H
-- 
cgit v1.2.3-54-g00ecf