diff --git a/CMakeLists.txt b/CMakeLists.txt
index d52ae91..7e55ad6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,6 +10,7 @@ set(HEADERS
     endian.h
     struct.h
     string.h
+    heap.h
 )
 
 set(SOURCES
@@ -20,6 +21,7 @@ set(SOURCES
     endian.c
     struct.c
     string.c
+    heap.c
 )
 
 add_library(cutil STATIC ${SOURCES} ${HEADERS}
diff --git a/cutypes.h b/cutypes.h
index e3754d5..08923d8 100644
--- a/cutypes.h
+++ b/cutypes.h
@@ -120,6 +120,8 @@ typedef signed int iword;
 #   warning "unknown bus width for arch " CU_ARCH
 #endif
 
+#define CU_WORD_SIZE (CU_WORD_BITS/8)
+
 typedef unsigned int uint;
 #define CU_UINT_SIZE sizeof(uint)
 
@@ -138,4 +140,6 @@ typedef iword ssize_t;
 #   include <stddef.h>
 #endif
 
+#define cu_round2_up(val,bit) (((val>>bit) + !!(val&((1<<bit)-1)) ) << bit)
+
 #endif
\ No newline at end of file
diff --git a/heap.c b/heap.c
new file mode 100644
index 0000000..b5c9eba
--- /dev/null
+++ b/heap.c
@@ -0,0 +1,124 @@
+#include "heap.h"
+
+static mblock_t* mblock_get_next(mblock_t* block)
+{
+    return (mblock_t*)((u8*)block+(block->size & ~7));
+}
+
+static mblock_t* mblock_by_data(void* data)
+{
+    return (mblock_t*)((u8*)data - MBLOCK_SIZE);
+}
+
+static u64 mblock_get_size(mblock_t* block)
+{
+    return block->size & ~7;
+}
+
+void heap_init(mheap_t* heap, void* data, uword size)
+{
+    heap->data = data;
+    heap->size = size;
+}
+
+void heap_join(mblock_t* start, int dir)
+{
+    mblock_t* cur;
+    u64 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 = blk_size & ~7;
+        if(cur)
+        {
+            if(cur->next) cur->next->prev = start;
+        }
+    }
+    else if(dir < 0)
+    {
+        if(cur)
+        {
+            cur = cur->next;
+            cur->size = blk_size & ~7;
+            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 = (req_size)|MBLOCK_ATTR_ALLOC;
+    split = mblock_get_next(block);
+    
+    //Init prev and next
+    split->prev = block;
+    split->next = block->next;
+    block->next = split;
+    
+    split->size = (tot_size-req_size) & ~7;
+}
+
+void* heap_alloc(mheap_t* heap, size_t size)
+{
+    mblock_t* start,*block,*prev;
+    size = cu_round2_up((size + MBLOCK_SIZE), MBLOCK_ALIGN);
+    
+    /*for(i = 0; i < heap->heapsz; i++)
+    {
+        start = (mblock_t*)((u8*)heap->heap+(i<<21));
+        if(!(start->size & MBLOCK_ATTR_ALLOC_2MB)) break;
+    }
+    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
+    {
+        block->size = size|MBLOCK_ATTR_ALLOC;
+        block->prev = prev;
+        block->next = mblock_get_next(block);
+    }
+    else heap_split(block,size);
+    
+    if(prev && !prev->next) prev->next = block;
+    
+    return block->data;
+}
+
+void heap_free(mheap_t* heap, void* mem)
+{
+    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);
+}
diff --git a/heap.h b/heap.h
new file mode 100644
index 0000000..12c6116
--- /dev/null
+++ b/heap.h
@@ -0,0 +1,34 @@
+#ifndef __CUTIL_HEAP_H
+#define __CUTIL_HEAP_H
+
+#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;
+    uword size;
+} mheap_t;
+
+#define MBLOCK_SIZE             (CU_WORD_SIZE*3)
+#define MBLOCK_ATTR_ALLOC       (1<<0)
+#define IS_MBLOCK_ALLOC(mblock) (mblock->size & MBLOCK_ATTR_ALLOC)
+
+#ifdef CU_64BIT
+#   define  MBLOCK_ALIGN    5
+#else
+#   define  MBLOCK_ALIGN    4
+#endif
+
+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_free(mheap_t* heap, void* mem);
+
+#endif
\ No newline at end of file
diff --git a/string.h b/string.h
index 50be60a..5eada47 100644
--- a/string.h
+++ b/string.h
@@ -32,7 +32,7 @@ void cu_iprints(char* str, iword number);
 void cu_uprints(char* str, uword number);
 void cu_xprints(char* str, uword number);
 
-iword cu_atoi(char* str,int base);
+iword cu_atoi(char* str, int base);
 uword cu_atou(char* str, int base);
 
 // all integer arguments must be iword or uword, strings - pointers
diff --git a/test.c b/test.c
index 895d048..2c52092 100644
--- a/test.c
+++ b/test.c
@@ -10,6 +10,7 @@
 #include "struct.h"
 #include "va_list.h"
 #include "string.h"
+#include "heap.h"
 
 typedef uint8_t item_t[14];
 
@@ -185,6 +186,21 @@ int main()
     cu_sscanf((char*)str4, "\"%s\" %d", str5, 8, &val5);
     printf("cu_sscanf\t%s\t%u\n", str5, val5);
 
+    printf("[heap]\n");
+    static u8 heap_data[4096];
+    mheap_t heap;
+    heap_init(&heap, heap_data, 4096);
+
+    void* m1 = heap_alloc(&heap, 8);
+    void* m2 = heap_alloc(&heap, 32);
+    void* m3 = heap_alloc(&heap, 9);
+    heap_free(&heap, m2);
+    void* m4 = heap_alloc(&heap, 8);
+    printf("heap_alloc\t%p\n", m1);
+    printf("heap_alloc\t%p\n", m2);
+    printf("heap_alloc\t%p\n", m3);
+    printf("heap_alloc\t%p\n", m4);
+
     cutil_exit();
     return 0;
 }