diff --git a/yamalloc.c b/yamalloc.c
index 2999eed..2651bc8 100644
--- a/yamalloc.c
+++ b/yamalloc.c
@@ -23,77 +23,95 @@
 
 /* Macros */
 
-#define YA_IS_FREE(block) (((block)[0] & 1) == 0)
+#define YA_IS_ALLOC(block) ((block)[-1] & 1)
+#define YA_SZ_BLOCK(size)  ((size) & -2)
 #define YA_ROUND_DIV(n, m) (((n) + ((m)-1)) / (m))
-#define YA_ROUND(n, m) (YA_ROUND_DIV(n,m) * (m))
+#define YA_ROUND(n, m)     (YA_ROUND_DIV(n,m) * (m))
 
 /* Globals */
 
-intptr_t *heap_start = NULL;
-intptr_t *heap_end   = NULL;
+intptr_t *heap_start = NULL; // space for 2 words before
+intptr_t *heap_end   = NULL; // first block outside heap
 
 /* Local declarations */
 
-int heap_init();
-int heap_extend();
+intptr_t *heap_init();
+intptr_t *heap_extend(intptr_t size);
 
-void block_join(intptr_t *block);
+intptr_t *block_join(intptr_t *block);
 
 /* Function definitions */
 
 #ifdef YA_DEBUG
 void ya_print_blocks() {
-    for (intptr_t *ptr = heap_start; ptr < heap_end; ptr += ptr[0]) {
-        fprintf(stderr, "Block at %p, size %ld\n", ptr, ptr[0]);
+    intptr_t *block;
+    intptr_t block_size;
+    for (block = heap_start; block < heap_end; block += block_size) {
+        block_size = YA_SZ_BLOCK(block[-1]);
+        fprintf(stderr, "Block at %p  size = %ld  alloc = %d\n",
+                block, block_size, YA_IS_ALLOC(block));
     }
 }
 #endif
 
-void block_join_prev(intptr_t *block) {
-    intptr_t *prev = block - block[-1];
-    if (prev >= heap_start && YA_IS_FREE(prev)) {
-        intptr_t size = prev[0] + block[0];
-        prev[0] = size;
-        block[block[0] - 1] = size;
+intptr_t *block_join_prev(intptr_t *block) {
+    intptr_t prev_size = YA_SZ_BLOCK(block[-2]);
+    intptr_t *prev = block - prev_size;
+    if (prev <= heap_start || YA_IS_ALLOC(prev)) {
+        return block;
     }
+    intptr_t block_size = YA_SZ_BLOCK(block[-1]);
+    intptr_t size = prev_size + block_size;
+    prev[-1] = size;
+    block[block_size - 2] = size;
+    return prev;
 }
 
-void block_join_next(intptr_t *block) {
-    intptr_t *next = block + block[0];
-    if (next < heap_end && YA_IS_FREE(next)) {
-        intptr_t size = next[0] + block[0];
-        block[0] = size;
-        next[next[0] - 1] = size;
+intptr_t *block_join_next(intptr_t *block) {
+    intptr_t block_size = YA_SZ_BLOCK(block[-1]);
+    intptr_t *next = block + block_size;
+    if (next >= heap_end || YA_IS_ALLOC(next)) {
+        return block;
     }
+    intptr_t next_size = YA_SZ_BLOCK(next[-1]);
+    intptr_t size = next_size + block_size;
+    block[-1] = size;
+    next[next_size - 2] = size;
+    return block;
 }
 
-void block_join(intptr_t *block) {
+intptr_t *block_join(intptr_t *block) {
     if (block > heap_start) {
-        block_join_prev(block);
+        block = block_join_prev(block);
     }
-    block_join_next(block);
+    return block_join_next(block);
 }
 
 void block_split(intptr_t *block, intptr_t size) {
-    intptr_t old_size = block[0];
-    block[0] = size;
-    block[size - 1] = size;
-    block[size] = old_size - size;
-    block[old_size - 1] = old_size - size;
+    intptr_t old_size = YA_SZ_BLOCK(block[-1]);
+    block[-1]           = size;
+    block[size - 2]     = size;
+    block[size - 1]     = old_size - size;
+    block[old_size - 2] = old_size - size;
 }
 
-int heap_init() {
-    heap_start = sbrk(0);    
-    if (heap_start == (void*) -1) {
+intptr_t *heap_init() {
+    intptr_t size_w = YA_SZ_CHUNK / YA_SZ_WORD;
+    void *ptr = sbrk(YA_SZ_WORD * (size_w + 2));
+    if (ptr == (void*) -1) {
         heap_start = NULL;
         heap_end = NULL;
-        return -1;
+        return NULL;
     }
-    heap_end = heap_start;
-    return 0;
+    heap_start  = ptr;
+    heap_start += 2; // space for the first block[-1] + dword alignment
+    heap_end = heap_start + size_w;
+    heap_start[-1] = size_w;
+    heap_end[-2]   = size_w;
+    return heap_start;
 }
 
-int heap_extend(intptr_t size_w) {
+intptr_t *heap_extend(intptr_t size_w) {
     fprintf(stderr, "heap_extend: old heap_start = %p, heap_end = %p\n",
             heap_start, heap_end);
     intptr_t size = YA_ROUND(size_w * YA_SZ_WORD, YA_SZ_CHUNK);
@@ -101,19 +119,14 @@ int heap_extend(intptr_t size_w) {
     fprintf(stderr, "heap_extend: %ld bytes, %ld words\n", size, size_w);
     void *ptr = sbrk(size);
     if (ptr == (void *) - 1) {
-        return -1;
+        return NULL;
     }
-    fprintf(stderr, "heap_extend: %ld bytes, %ld words\n", size, size_w);
-    heap_end[0] = size_w;
-    heap_end[size_w - 1] = size_w;
-    fprintf(stderr, "heap_extend: heap_end = %p, heap_end + size_w = %p\n",
-            heap_end, heap_end + size_w);
-    fprintf(stderr, "heap_extend: heap_end[0] = %ld, heap_end[size_w - 1] = %ld\n",
-            heap_end[0], heap_end[size_w - 1]);
-    heap_end += size_w;
+    intptr_t *block = ptr; // == heap_end
+    heap_end = block + size_w;
+    block[-1]    = size_w;
+    heap_end[-2] = size_w;
     fprintf(stderr, "heap_extend: new heap_end = %p\n", heap_end);
-    block_join(ptr);
-    return 0;
+    return block_join(ptr);
 }
 
 void *malloc(size_t size) {
@@ -121,34 +134,44 @@ void *malloc(size_t size) {
         return NULL;
     }
     if (heap_start == NULL || heap_end == NULL) {
-        if (heap_init() != 0) {
+        if (!heap_init()) {
             return NULL;
         }
     }
-    intptr_t size_w = YA_ROUND_DIV(size + 2, YA_SZ_WORD); // size in words
-    intptr_t align_dw = YA_ROUND(size_w, 2); // round to dword
+    intptr_t size_w = YA_ROUND_DIV(size, YA_SZ_WORD); // size in words
+    fprintf(stderr, "size_w = %ld\n", size_w);
+    size_w = 2 + YA_ROUND(size_w, 2); // round to dword and make space for tags
+    fprintf(stderr, "size_w = %ld\n", size_w);
     intptr_t *block;
-    for (block = heap_start; block < heap_end; block += block[0]) {
-        if (YA_IS_FREE(block) && block[0] >= align_dw) {
+    intptr_t block_size;
+    for (block = heap_start; block < heap_end; block += block_size) {
+        block_size = YA_SZ_BLOCK(block[-1]);
+        if (!YA_IS_ALLOC(block) && size_w <= block_size) {
             break;
         }
     }
-    fprintf(stderr, "size_w = %ld, align_dw = %ld\n", size_w, align_dw);
     if (block >= heap_end) { // could not find block, extend heap
-        if (heap_extend(align_dw) != 0) { 
-            return NULL;
-        }
-        block = heap_end - heap_end[-1];
-        if (!YA_IS_FREE(block) || block[0] < align_dw) {
+        block = heap_extend(size_w);
+        if (!block) { 
             return NULL;
         }
+        block_size = YA_SZ_BLOCK(block[-1]);
     }
-    if (block[0] > align_dw) {
-        block_split(block, align_dw);
+    if (size_w < block_size) {
+        block_split(block, size_w);
     }
-    block[0] |= 1;
-    return block + 1;
+    block[-1] |= 1;
+    block[block_size - 2] |= 1;
+    return block;
 }
 
 void free(void *ptr) {
+    intptr_t *block = ptr;
+    if (block < heap_start || block > heap_end || !YA_IS_ALLOC(block)) {
+        return; // TODO: provoke segfault
+    }
+    intptr_t block_size = YA_SZ_BLOCK(block[-1]);
+    block[-1]             &= -2; // erase allocated bit
+    block[block_size - 2] &= -2;
+    block_join(block);
 }
diff --git a/yatest.c b/yatest.c
index 77d2b45..ff85743 100644
--- a/yatest.c
+++ b/yatest.c
@@ -10,17 +10,34 @@
 
 void *print_malloc(size_t size) {
     void *ptr = malloc(size);
-    printf("malloc(%ld) = %p\n", size, ptr);
+    fprintf(stderr, "malloc(%ld) = %p\n", size, ptr);
     return ptr;
 }
 
+void print_free(void *ptr) {
+    fprintf(stderr, "free(%p)\n", ptr);
+    free(ptr);
+}
+
 int main(int argc, char **argv) {
+    void *a, *b, *c, *d;
+    ya_print_blocks();
+    a = print_malloc(4);
+    ya_print_blocks();
+    b = print_malloc(10);
+    ya_print_blocks();
+    c = print_malloc(10000);
+    ya_print_blocks();
+    d = print_malloc(2000);
+    ya_print_blocks();
+    print_free(a);
+    ya_print_blocks();
+    print_free(c);
     ya_print_blocks();
-    print_malloc(4);
-    print_malloc(10);
+    c = print_malloc(100);
     ya_print_blocks();
-    print_malloc(10000);
+    a = print_malloc(2);
     ya_print_blocks();
-    print_malloc(2000);
+    print_free(d);
     ya_print_blocks();
 }