diff --git a/src/malloc.c b/src/malloc.c
index 21966aa..118014e 100644
--- a/src/malloc.c
+++ b/src/malloc.c
@@ -8,15 +8,14 @@
 {
 	if (context->free_list_head)
 	{
-		rtmalloc_block_link_t* i;
+		rtmalloc_block_link_t* prev = NULL;
+		for(rtmalloc_block_link_t* i = context->free_list_head; i && (i < block); prev = i, i = i->next);
 
-		for(i = context->free_list_head; i && (i->next < block); i = i->next);
-
-		if (i)
+		if (prev)
 		{
-			//insert the block after i
-			block->next = i->next;
-			i->next = block;
+			//insert the block after prev
+			block->next = prev->next;
+			prev->next = block;
 
 			//join with the next block if possible
 			if (((uint8_t*)block + context->link_size + block->size) == (uint8_t*)block->next)
@@ -25,11 +24,11 @@
 				block->next = block->next->next;
 			}
 
-			//join i with block if possible
-			if (((uint8_t*)i + context->link_size + i->size) == (uint8_t*)block)
+			//join prev with block if possible
+			if (((uint8_t*)prev + context->link_size + prev->size) == (uint8_t*)block)
 			{
-				i->size += context->link_size + block->size;
-				i->next = block->next;
+				prev->size += context->link_size + block->size;
+				prev->next = block->next;
 			}
 		}
 		else