diff --git a/inc/malloc.h b/inc/malloc.h
index abf9aa6..c85a629 100644
--- a/inc/malloc.h
+++ b/inc/malloc.h
@@ -1,8 +1,10 @@
 #ifndef _RTLIBC_MALLOC_H
 #define _RTLIBC_MALLOC_H
 
+
 #include <stdint.h>
 #include <stddef.h>
+#include <stdbool.h>
 
 
 #ifdef __cplusplus
@@ -14,29 +16,23 @@
 typedef struct rtmalloc_block_link_s
 {
 	size_t size;
-	struct rtmalloc_block_link_s *next;
+	struct rtmalloc_block_link_s* next;
 }rtmalloc_block_link_t;
 
 
 typedef struct
 {
-	size_t alignment;
 	size_t alignment_mask;
-	size_t allocated_mask;
+	size_t link_size;
 	
-	size_t size_free;
-	size_t size_free_min;
-	size_t min_block_size;
-
-	rtmalloc_block_link_t free_list_head;
-	rtmalloc_block_link_t *free_list_tail;
+	rtmalloc_block_link_t* free_list_head;
 }rtmalloc_context_t;
 
 
-void rtmalloc_init(rtmalloc_context_t *context, void *heap, size_t size, size_t alignment);
-void* rtmalloc(rtmalloc_context_t *context, size_t size);
-void rtfree(rtmalloc_context_t *context, void *p);
-size_t rtmsize(rtmalloc_context_t *context, void *p);
+bool rtmalloc_init(rtmalloc_context_t* context, void* heap, size_t size, size_t alignment);
+void* rtmalloc(rtmalloc_context_t* context, size_t size);
+void rtfree(rtmalloc_context_t* context, void* p);
+size_t rtmsize(rtmalloc_context_t* context, void* p);
 
 
 #ifdef __cplusplus
diff --git a/src/malloc.c b/src/malloc.c
index f01b9cd..ba6ba82 100644
--- a/src/malloc.c
+++ b/src/malloc.c
@@ -1,66 +1,78 @@
 #include <malloc.h>
 
 
-static void rtmalloc_insert_in_free_list(rtmalloc_context_t *context, rtmalloc_block_link_t *block)
+#define ALLOCATED_MASK ((size_t)1 << ((sizeof(size_t) << 3) - 1))
+
+
+static void rtmalloc_insert_in_free_list(rtmalloc_context_t* context, rtmalloc_block_link_t* block)
 {
-	rtmalloc_block_link_t *i;
-
-	for(i = &context->free_list_head; i->next < block; i = i->next);
-
-	//check if the block and the previous one are contiguous
-	if (((uint8_t*)i + i->size) == (uint8_t*)block)
+	if (context->free_list_head)
 	{
-		i->size += block->size;
-		block = i;
-	}
+		rtmalloc_block_link_t* i;
 
-	//check if is contigous with the next block
-	if (((uint8_t*)block + block->size) == (uint8_t*)i->next)
-	{
-		if (i->next != context->free_list_tail)
+		for(i = context->free_list_head; i && (i->next < block); i = i->next);
+
+		if (i)
 		{
-			block->size += i->next->size;
-			block->next = i->next->next;
+			//insert the block after i
+			block->next = i->next;
+			i->next = block;
+
+			//join with the next block if possible
+			if (((uint8_t*)block + context->link_size + block->size) == (uint8_t*)block->next)
+			{
+				block->size += context->link_size + block->next->size;
+				block->next = block->next->next;
+			}
+
+			//join i with block if possible
+			if (((uint8_t*)i + context->link_size + i->size) == (uint8_t*)block)
+			{
+				i->size += context->link_size + block->size;
+				i->next = block->next;
+			}
 		}
 		else
 		{
-			block->next = context->free_list_tail;
+			//insert the block at the beginning of the free list
+			block->next = context->free_list_head;
+			context->free_list_head = block;
+
+			if (((uint8_t*)block + context->link_size + block->size) == (uint8_t*)block->next)
+			{
+				block->size += context->link_size + block->next->size;
+				block->next = block->next->next;
+			}
 		}
 	}
 	else
 	{
-		block->next = i->next;
+		context->free_list_head = block;
 	}
-
-	if (i != block)
-		i->next = block;
 }
 
 
 
-void rtmalloc_init(rtmalloc_context_t *context, void *heap, size_t size, size_t alignment)
+bool rtmalloc_init(rtmalloc_context_t* context, void* heap, size_t size, size_t alignment)
 {
-	context->alignment = alignment;
-	context->alignment_mask = context->alignment - 1;
-	context->allocated_mask = (size_t)1 << ((sizeof(size_t) << 3) - 1);
-	context->min_block_size = (sizeof(rtmalloc_block_link_t) + context->alignment) & ~context->alignment_mask;
+	//check if alignment is a power of 2
+	if (alignment & (alignment - 1))
+		return false;
 
-	//align properly
+
+	context->alignment_mask = alignment - 1;
+	context->link_size = (sizeof(rtmalloc_block_link_t) + context->alignment_mask) & ~context->alignment_mask;
+
+	//align the available memory
 	void *aligned_heap = (void*)(((size_t)heap + context->alignment_mask) & ~context->alignment_mask);
-	size -= (aligned_heap - heap);
+	size -= ((uint8_t*)aligned_heap - (uint8_t*)heap);
 
-	context->free_list_head.size = 0;
-	context->free_list_head.next = aligned_heap;
+	context->free_list_head = (rtmalloc_block_link_t*)aligned_heap;
+	
+	context->free_list_head->size = size - context->link_size;
+	context->free_list_head->next = NULL;
 
-	context->free_list_tail = (rtmalloc_block_link_t*)(((size_t)aligned_heap + size - sizeof(rtmalloc_block_link_t)) & ~context->alignment_mask);
-	context->free_list_tail->size = 0;
-	context->free_list_tail->next = NULL;
-
-	context->size_free = (uint8_t*)context->free_list_tail - (uint8_t*)context->free_list_head.next;
-	context->size_free_min = context->size_free;
-
-	context->free_list_head.next->size = context->size_free;
-	context->free_list_head.next->next = context->free_list_tail;
+	return true;
 }
 
 
@@ -70,50 +82,44 @@
 {
 	void *p = NULL;
 
-	if ((size & context->allocated_mask) == 0)
+	if (((size & ALLOCATED_MASK) == 0) && (size > 0))
 	{
-		if (size > 0)
+		//calculate the size of data required
+		size = (size + context->alignment_mask) & ~context->alignment_mask;
+
+		rtmalloc_block_link_t *prev_block = NULL;
+		rtmalloc_block_link_t *block = context->free_list_head;
+
+		while(block && (block->size < size))
 		{
-			size += sizeof(rtmalloc_block_link_t);
-			size = (size + context->alignment_mask) & ~context->alignment_mask;
+			prev_block = block;
+			block = block->next;
 		}
 
-		if ((size > 0) && (size <= context->size_free))
+		if (block)
 		{
-			rtmalloc_block_link_t *prev_block = &context->free_list_head;
-			rtmalloc_block_link_t *block = context->free_list_head.next;
+			//get the memory pointer to return
+			p = (uint8_t*)block + context->link_size;
 
-			while((block->size < size) && block->next)
-			{
-				prev_block = block;
-				block = block->next;
-			}
-
-			if (block != context->free_list_tail)
-			{
-				//get the memory pointer to return
-				p = (uint8_t*)block + sizeof(rtmalloc_block_link_t);
-
-				//remove from list
+			//remove from list
+			if (prev_block)
 				prev_block->next = block->next;
+			else
+				context->free_list_head = block->next;
 
-				if ((block->size - size) > context->min_block_size)
-				{
-					//split a larger block
-					rtmalloc_block_link_t *new_block_link = (rtmalloc_block_link_t*)((uint8_t*)block + size);
-					new_block_link->size = block->size - size;
-					block->size = size;
+			//split if we have enough size for another allocation
+			if ((block->size - size) >= (2 * context->link_size))
+			{
+				//split a larger block
+				rtmalloc_block_link_t *new_block_link = (rtmalloc_block_link_t*)((uint8_t*)block + context->link_size + size);
+				new_block_link->size = block->size - context->link_size - size;
+				block->size = size;
 
-					rtmalloc_insert_in_free_list(context, new_block_link);
-				}
-
-				context->size_free -= block->size;
-				if (context->size_free < context->size_free_min)
-					context->size_free_min = context->size_free;
-
-				block->size |= context->allocated_mask;
-				block->next = NULL;
+				rtmalloc_insert_in_free_list(context, new_block_link);
 			}
+
+			block->size |= ALLOCATED_MASK;
+			block->next = NULL;
 		}
 	}
 
@@ -121,33 +127,32 @@
 }
 
 
-void rtfree(rtmalloc_context_t *context, void *p)
+void rtfree(rtmalloc_context_t* context, void* p)
 {
 	if (p)
 	{
-		rtmalloc_block_link_t *block = (rtmalloc_block_link_t*)((uint8_t*)p - sizeof(rtmalloc_block_link_t));
+		rtmalloc_block_link_t* block = (rtmalloc_block_link_t*)((uint8_t*)p - context->link_size);
 
-		if ((block->size & context->allocated_mask) && (block->next == NULL))
+		if ((block->size & ALLOCATED_MASK) && (block->next == NULL))
 		{
-			block->size &= ~context->allocated_mask;
+			block->size &= ~ALLOCATED_MASK;
 
-			context->size_free += block->size;
 			rtmalloc_insert_in_free_list(context, block);
 		}
 	}
 }
 
 
-size_t rtmsize(rtmalloc_context_t *context, void *p)
+size_t rtmsize(rtmalloc_context_t* context, void* p)
 {
 	size_t size = 0;
 
 	if (p)
 	{
-		rtmalloc_block_link_t *block = (rtmalloc_block_link_t*)((uint8_t*)p - sizeof(rtmalloc_block_link_t));
+		rtmalloc_block_link_t* block = (rtmalloc_block_link_t*)((uint8_t*)p - context->link_size);
 
-		if ((block->size & context->allocated_mask) && (block->next == NULL))
-			size = block->size & ~context->allocated_mask;
+		if ((block->size & ALLOCATED_MASK) && (block->next == NULL))
+			size = block->size & ~ALLOCATED_MASK;
 	}
 
 	return size;
@@ -155,19 +160,18 @@
 
 
 /*
-void rtmalloc_dump(malloc_context_t *context)
+void rtmalloc_dump(rtmalloc_context_t* context)
 {
-	printf("\nFree block list (%lu)", context->size_free);
+	debug_printf("\nFree block list");
 
-	malloc_block_link_t *block = context->free_list_head.next;
+	rtmalloc_block_link_t* block = context->free_list_head;
 
-	while(block != context->free_list_tail)
+	while(block)
 	{
-		printf("\n\t%p (%lu)", block, block->size);
+		debug_printf("\n\t%p (%lu)", block, block->size);
 		block = block->next;
 	}
 
-	printf("\n\n");
+	debug_printf("\n\n");
 }
 */
-