diff --git a/cutil.c b/cutil.c
index f54a94e..e9f1696 100644
--- a/cutil.c
+++ b/cutil.c
@@ -7,17 +7,20 @@ mheap_t cu_heap;
 
 void* _cu_malloc(size_t size)
 {
-    return heap_alloc(&cu_heap, size);
+    mblock_t* block =  heap_alloc(&cu_heap, size);
+    return block ? (u8*)block + sizeof(mblock_t) : NULL;
 }
 
 void* _cu_realloc(void* mem, size_t size)
 {
-    return heap_realloc(&cu_heap, mem, size);
+    mblock_t* block = heap_realloc(&cu_heap,
+        (mblock_t*)((u8*)mem - sizeof(mblock_t)), size);
+    return block ? (u8*)block + sizeof(mblock_t) : NULL;
 }
 
 void _cu_free(void* mem)
 {
-    return heap_free(&cu_heap, mem);
+    return heap_free((mblock_t*)((u8*)mem - sizeof(mblock_t)));
 }
 
 void* (*cu_malloc)(size_t) = _cu_malloc;
diff --git a/heap.c b/heap.c
index d372d9c..1663178 100644
--- a/heap.c
+++ b/heap.c
@@ -1,138 +1,93 @@
 #include "heap.h"
 #include "cutil.h"
 
-static mblock_t* mblock_get_next(mblock_t* block)
+void heap_init(mheap_t* heap, void* start, uword size)
 {
-    return (mblock_t*)((u8*)block+MBLOCK_ALIGN_SIZE(block->size));
-}
-
-static mblock_t* mblock_by_data(void* data)
-{
-    return (mblock_t*)((u8*)data - MBLOCK_SIZE);
-}
-
-static size_t mblock_get_size(mblock_t* block)
-{
-    return MBLOCK_ALIGN_SIZE(block->size & ~MBLOCK_ATTR_ALLOC);
-}
-
-void heap_init(mheap_t* heap, void* data, uword size)
-{
-    heap->data = data;
+    heap->start = start;
     heap->size = size;
+
+    mblock_t* block = (mblock_t*)heap->start;
+    block->prev = NULL;
+    block->size = size;
 }
 
-void heap_join(mblock_t* start, int dir)
+void heap_split(mblock_t* block, uword size)
 {
-    mblock_t* cur;
-    size_t blk_size;
-    
-    blk_size = 0;
-    cur = start;
-    while(cur != NULL && !IS_MBLOCK_ALLOC(cur))
-    {
-        blk_size += mblock_get_size(cur);
-        cur->size = 0;
-        if(dir > 0) cur = cur->next;
-        else if(dir < 0) cur = cur->prev;
-    }
-    
-    if(dir > 0)
-    {
-        start->next = cur;
-        start->size = MBLOCK_ALIGN_SIZE(blk_size);
-        if(cur)
-        {
-            if(cur->next) cur->next->prev = start;
-        }
-    }
-    else if(dir < 0)
-    {
-        if(cur)
-        {
-            cur = cur->next;
-            cur->size = MBLOCK_ALIGN_SIZE(blk_size);
-            cur->next = start->next;
-        }
-    }
-}
-
-void heap_split(mblock_t* block, size_t req_size)
-{
-    size_t tot_size = mblock_get_size(block);
-    mblock_t* split;
-    
-    block->size = MBLOCK_ALIGN_SIZE(req_size)|MBLOCK_ATTR_ALLOC;
-    split = mblock_get_next(block);
-    
-    //Init prev and next
+    mblock_t* split = (mblock_t*)((u8*)block + size);
     split->prev = block;
-    split->next = block->next;
-    block->next = split;
-    
-    split->size = MBLOCK_ALIGN_SIZE(tot_size-req_size);
+    split->size = block->size - size;
+    block->size = size;
 }
 
-void* heap_alloc(mheap_t* heap, size_t size)
+void heap_join(mblock_t* block, mblock_t* other)
 {
-    mblock_t* start,*block,*prev;
-    size = cu_round2_up((size + MBLOCK_SIZE), MBLOCK_ALIGN);
-    
-    /*for(i = 0; i < heap->heapsz; i++)
+    if (block < other)
     {
-        start = (mblock_t*)((u8*)heap->heap+(i<<21));
-        if(!(start->size & MBLOCK_ATTR_ALLOC_2MB)) break;
+        mblock_t* next = (mblock_t*)((u8*)other + other->size);
+        next->prev = block;
+        block->size += other->size;
     }
-    if(i == heap->heapsz) return NULL;*/
-    start = (mblock_t*)heap->data;
-    
-    //Find free block
-    prev = NULL;
-    block = start;
-    do {
-        if((!IS_MBLOCK_ALLOC(block) && mblock_get_size(block) >= size)
-            || block->size == 0)
-            break;
-        prev = block;
-        if(block->next) block = block->next;
-        else break;
-    } while(block);
-    if(prev == block) prev = NULL;
-    //Allocate
-    if(block->size == 0) //Unformatted
+    else if (block > other)
     {
-        block->size = size|MBLOCK_ATTR_ALLOC;
-        block->prev = prev;
-        block->next = mblock_get_next(block);
+        mblock_t* next = (mblock_t*)((u8*)block + block->size);
+        next->prev = other;
+        other->size += block->size;
     }
-    else heap_split(block,size);
-    
-    if(prev && !prev->next) prev->next = block;
-    
-    return block->data;
 }
 
-void* heap_realloc(mheap_t* heap, void* mem, size_t size)
+mblock_t* heap_alloc(mheap_t* heap, uword _size)
 {
-    mblock_t* block = (mblock_t*)((u8*)mem - MBLOCK_SIZE);
-    (void)heap;
-    
-    heap_free(heap, mem);
-    void* newMem = heap_alloc(heap, size);
-    if (mem != newMem)
-        cu_memcpy(newMem, mem, mblock_get_size(block));
-    
-    return newMem;
+    uword size = cu_round2_up(_size + sizeof(mblock_t), MBLOCK_ALIGN);
+    mblock_t* block = (mblock_t*)((u8*)heap->start);
+    mblock_t* end = (mblock_t*)((u8*)heap->start + heap->size);
+    while ((block->size & MBLOCK_ALLOCATED) || MBLOCK_SIZE(block->size) < size)
+    {
+        block = (mblock_t*)((u8*)block + MBLOCK_SIZE(block->size));
+        if (block >= end) return NULL;
+    }
+
+    if (block->size > size)
+    {
+        heap_split(block, size);
+    }
+
+    block->size |= MBLOCK_ALLOCATED;
+    return block;
 }
 
-void heap_free(mheap_t* heap, void* mem)
+void heap_free(mblock_t* block)
 {
-    mblock_t* block = (mblock_t*)((u8*)mem - MBLOCK_SIZE);
-    (void)heap;
-    
-    //Unlink
-    block->size &= ~MBLOCK_ATTR_ALLOC;
-    //Link free blocks prev and next
-    heap_join(block,1);
-    heap_join(block,-1);
+    block->size &= ~MBLOCK_ALLOCATED;
+    mblock_t* prev = block->prev;
+    mblock_t* next = (mblock_t*)((u8*)block + block->size);
+    if (!(prev->size & MBLOCK_ALLOCATED))
+        heap_join(block, prev);
+    if (!(next->size & MBLOCK_ALLOCATED))
+        heap_join(block, next);
+}
+
+mblock_t* heap_realloc(mheap_t* heap, mblock_t* block, uword _size)
+{
+    block->size &= ~MBLOCK_ALLOCATED;
+    uword size = cu_round2_up(_size, MBLOCK_ALIGN);
+    mblock_t* next = (mblock_t*)((u8*)block + block->size);
+    mblock_t* new;
+    if (!(next->size & MBLOCK_ALLOCATED) && next->size >= size)
+    {
+        new = block;
+        heap_join(new, next);
+        heap_split(new, size);
+        new->size |= MBLOCK_ALLOCATED;
+    }
+    else
+    {
+        heap_free(block);
+        new = heap_alloc(heap, size);
+        if (new)
+        {
+            cu_memcpy((u8*)new + sizeof(mblock_t),
+                (u8*)block + sizeof(mblock_t), size - sizeof(mblock_t));
+        }
+    }
+    return new;
 }
diff --git a/heap.h b/heap.h
index aa4cc02..0bd6300 100644
--- a/heap.h
+++ b/heap.h
@@ -3,35 +3,30 @@
 
 #include "cutypes.h"
 
-typedef struct mblock_s {
-    struct mblock_s* prev;
-    struct mblock_s* next;
-    uword size;
-    u8 data[1];
-} mblock_t;
-
 typedef struct {
-    u8* data;
+    void* start;
     uword size;
 } mheap_t;
 
+typedef struct {
+    void* prev;
+    uword size;
+} mblock_t;
+
+#define MBLOCK_ALLOCATED    (1<<0)
+#define MBLOCK_SIZE(size)   ((size) & ~MBLOCK_ALLOCATED)
+
 #ifdef CU_64BIT
-#   define  MBLOCK_ALIGN    5
-#else
 #   define  MBLOCK_ALIGN    4
+#else
+#   define  MBLOCK_ALIGN    3
 #endif
 
-#define MBLOCK_SIZE             (CU_WORD_SIZE*3)
-#define MBLOCK_SIZE_MASK        ((CU_WORD_BITS/8) - 1)
-#define MBLOCK_ALIGN_SIZE(size) (cu_round2_up((size), MBLOCK_ALIGN))
-#define MBLOCK_ATTR_ALLOC       (1<<0)
-#define IS_MBLOCK_ALLOC(mblock) (mblock->size & MBLOCK_ATTR_ALLOC)
-
-void heap_init(mheap_t* heap, void* data, uword size);
-void heap_join(mblock_t* start, int dir);
-void heap_split(mblock_t* block, size_t req_size);
-void* heap_alloc(mheap_t* heap, size_t size);
-void* heap_realloc(mheap_t* heap, void* mem, size_t size);
-void heap_free(mheap_t* heap, void* mem);
+void heap_init(mheap_t* heap, void* start, uword size);
+void heap_split(mblock_t* block, uword size);
+void heap_join(mblock_t* block, mblock_t* other);
+mblock_t* heap_alloc(mheap_t* heap, uword _size);
+void heap_free(mblock_t* block);
+mblock_t* heap_realloc(mheap_t* heap, mblock_t* block, uword _size);
 
 #endif
\ No newline at end of file
diff --git a/test.c b/test.c
index 763d490..68a1adf 100644
--- a/test.c
+++ b/test.c
@@ -207,7 +207,7 @@ int main()
     void* m1 = heap_alloc(&heap, 8);
     void* m2 = heap_alloc(&heap, 32);
     void* m3 = heap_alloc(&heap, 9);
-    heap_free(&heap, m2);
+    heap_free(m2);
     void* m4 = heap_alloc(&heap, 8);
     printf("heap_alloc\t%p\n", m1);
     printf("heap_alloc\t%p\n", m2);