diff --git a/avr/modules/heap/main.asm b/avr/modules/heap/main.asm
index 177ddbb..898e6ba 100644
--- a/avr/modules/heap/main.asm
+++ b/avr/modules/heap/main.asm
@@ -7,16 +7,27 @@
 ; * Please see toplevel file COPYING of that project for license details.   *
 ; ***************************************************************************
 
+
 ; Needed vars:
 ; - HEAP_START
 ; - HEAP_SIZE
 
 
-.equ HEAP_HEADER_BIT_USED = 1 
+; ***************************************************************************
+; defines
 
 
+
+.equ HEAP_HEADER_BIT_USED = 0 
+
+
+
+; ***************************************************************************
+; data
+
 .dseg
 
+
 heapPtr:     .byte 2
 heapUsed:    .byte 2
 heapFree:    .byte 2
@@ -24,6 +35,8 @@ heapDblFree: .byte 1
 
 
 
+; ***************************************************************************
+; code
 
 .cseg
 
@@ -43,7 +56,7 @@ Heap_Init:
   st X+, r16                 ; write start header
   st X+, r16
   sts heapPtr, xl            ; store global vars
-  sts heapPtr+1, xl
+  sts heapPtr+1, xh
   sts heapFree, r24
   sts heapFree+1, r25
   st X+, r24                 ; store header of first chunk
@@ -80,9 +93,10 @@ heapAllocFirstFit_loop:
     mov r16, r18                  ; heap end reached?
     or r16, r19
     breq heapAllocFirstFit_memFull
-    sbrc r17, HEAP_HEADER_BIT_USED
+    sbrc r18, HEAP_HEADER_BIT_USED
       rjmp heapAllocFirstFit_next ; jump if used
     ; current chunk free, check size
+	andi r18, 0xfc
     mov r16, r18
     mov r17, r19
     sub r16, r24
@@ -92,6 +106,7 @@ heapAllocFirstFit_loop:
     sec
     ret
 heapAllocFirstFit_next:
+	andi r18, 0xfc
     add xl, r18
     adc xh, r19
     adiw xh:xl, 2 ; skip footer
@@ -215,15 +230,20 @@ heapCoalecseUp:
   ; read footer of preceeding chunk 
   ld r25, -X
   ld r24, -X
-  ; R25:R24=footer of preceeding chunk (i.e. size of previous chunk)
+  ; R25:R24=footer of preceeding chunk (i.e. size of previous chunk), X points to previous footer
   mov r16, r24                   ; check: beginning of heap reached (header==0x0000)?
   or r16, r25
   breq heapCoalecseUp_ret        ; yes, jump
   sbrc r24, HEAP_HEADER_BIT_USED ; block used?
     rjmp heapCoalecseUp_ret      ; jump if used
   ; previous chunk also free, coalesce, X points to current chunk header
+  adiw xh:xl, 2
+; skip footer
   ld r16, X+
   ld r17, X+
+ sbrc r16, HEAP_HEADER_BIT_USED ; current block used?
+    rjmp heapCoalecseUp_ret      ; jump if used
+ 
   ; X now points to beginning of current chunk's data, R17:r16=size of current chunk, R25:R24=size of previous chunk
   ; to go to start of previous chunk header: sub this chunk header, previous chunk footer, previous chunk header and previous data
   sbiw xh:xl, 6
@@ -260,10 +280,10 @@ heapCoalecseUp_ret:
 ; @clobbers 
 
 heapUseChunk:
-  push xh
-    push xl
+  push xl
+    push xh
       tst r17
-      breq heapUseChunk_split
+      brne heapUseChunk_split
       cpi r16, 8                     ; at least 8 bytes left?
       brcs heapUseChunk_directAlloc  ; nope, use full chunk
 heapUseChunk_split: