diff --git a/avr/lcd.asm b/avr/lcd.asm
index 0347f1a..beefc0f 100644
--- a/avr/lcd.asm
+++ b/avr/lcd.asm
@@ -24,8 +24,6 @@
 ; ***************************************************************************
 ; defines
 
-.equ LCD_TWI_ADDRESS         = 0x3c
-
 .equ LCD_WIDTH               = 128
 .equ LCD_HEIGHT              = 64
 
@@ -86,6 +84,12 @@ LCD_Init:
   ldi zh, HIGH(lcdHelloMsg)
   rcall LCD_PrintFromFlash
 
+  ; set cursor to next line here, sometimes this doesn't work correctly
+  ; when called later. TODO: find out what goes wrong here...
+  ldi r18, 0
+  ldi r19, 2
+  rcall LCD_SetCursor
+
   sec
   ret
 
@@ -117,7 +121,7 @@ LCD_Fini:
 ; - R19: Y
 ; OUT:
 ; - CFLAG: set if okay, cleared otherwise
-; REGS: r1, r2, r16 (R16, R17, R18, R22)
+; REGS: r1, r2, r15, r16 (R16, R17, R18, R22)
 
 LCD_SetCursor:
   in r15, SREG
@@ -173,42 +177,45 @@ LCD_SetCursor_error:
 
 LCD_Fill:
   in r15, SREG
-  cli
-  mov r20, r16
+  push r15
+    cli
+    mov r20, r16
 
-  ldi r21, 0 ; Y
+    ldi r21, 0 ; Y
 LCD_Fill_loopY:
-    clr r18
-    mov r19, r21
-    rcall LCD_SetCursor
-    brcc LCD_Fill_error
-
-    rcall twiStart
-    ldi r16, (LCD_TWI_ADDRESS*2)
-    rcall twiSendByteExpectAck   ; (R16, R17, R18, R22)
-    brcc LCD_Fill_error
-    ldi r16, LCD_DATA_MODE
-    rcall twiSendByteExpectAck   ; (R16, R17, R18, R22)
-    brcc LCD_Fill_error
-  
-    ldi r19, 0
-LCD_Fill_loopX:
-      mov r16, r20
-      rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
-      rcall twiSendByte
+      clr r18
+      mov r19, r21
+      rcall LCD_SetCursor
       brcc LCD_Fill_error
-      inc r19
-      cpi r19, LCD_WIDTH
-    brcs LCD_Fill_loopX
-    rcall twiStop
-    inc r21
-    cpi r21, LCD_PAGE_COUNT
-  brcs LCD_Fill_loopY
+  
+      rcall twiStart
+      ldi r16, (LCD_TWI_ADDRESS*2)
+      rcall twiSendByteExpectAck   ; (R16, R17, R18, R22)
+      brcc LCD_Fill_error
+      ldi r16, LCD_DATA_MODE
+      rcall twiSendByteExpectAck   ; (R16, R17, R18, R22)
+      brcc LCD_Fill_error
+    
+      ldi r19, 0
+LCD_Fill_loopX:
+        mov r16, r20
+        rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
+        rcall twiSendByte
+        brcc LCD_Fill_error
+        inc r19
+        cpi r19, LCD_WIDTH
+      brcs LCD_Fill_loopX
+      rcall twiStop
+      inc r21
+      cpi r21, LCD_PAGE_COUNT
+    brcs LCD_Fill_loopY
+  pop r15
   out SREG, r15
   sec
   ret
 LCD_Fill_error:
-  rcall twiStop
+    rcall twiStop
+  pop r15
   out SREG, r15
   clc
   ret
@@ -228,36 +235,39 @@ LCD_Fill_error:
 
 LCD_PrintFromFlash:
   in r15, SREG
-  cli
-  lsl zl
-  rol zh
-  rcall twiStart
-  ldi r16, (LCD_TWI_ADDRESS*2)
-  rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
-  brcc LCD_PrintFromFlash_error
-  ldi r16, LCD_DATA_MODE
-  rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
-  brcc LCD_PrintFromFlash_error
+  push r15
+    cli
+    lsl zl
+    rol zh
+    rcall twiStart
+    ldi r16, (LCD_TWI_ADDRESS*2)
+    rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
+    brcc LCD_PrintFromFlash_error
+    ldi r16, LCD_DATA_MODE
+    rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
+    brcc LCD_PrintFromFlash_error
 
 LCD_PrintFromFlash_loop:
-    lpm r16, z+
-    tst r16
-    breq LCD_PrintFromFlash_end
-    push zh
-      push zl
-        rcall lcdPrintOneChar
-      pop zl
-    pop zh
-    brcc LCD_PrintFromFlash_error
-  rjmp LCD_PrintFromFlash_loop
+      lpm r16, z+
+      tst r16
+      breq LCD_PrintFromFlash_end
+      push zh
+        push zl
+          rcall lcdPrintOneChar
+        pop zl
+      pop zh
+      brcc LCD_PrintFromFlash_error
+    rjmp LCD_PrintFromFlash_loop
 LCD_PrintFromFlash_end:
-  rcall twiStop
+    rcall twiStop
+  pop r15
   out SREG, r15
   sec
   ret
   
 LCD_PrintFromFlash_error:
-  rcall twiStop
+    rcall twiStop
+  pop r15
   out SREG, r15
   clc
   ret
@@ -302,7 +312,7 @@ LCD_PrintChar_error:
 ; ---------------------------------------------------------------------------
 ; LCD_PrintHexByte
 ;
-; Convert a give byte into HEX and write it to the current position.
+; Convert a given byte into HEX and write it to the current position.
 ;
 ; IN:
 ; - R16: byte to convert to hex
@@ -311,24 +321,27 @@ LCD_PrintChar_error:
 
 LCD_PrintHexByte:
   in r15, SREG
-  cli
-  mov r20, r16
-  rcall twiStart
-  ldi r16, (LCD_TWI_ADDRESS*2)
-  rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
-  brcc LCD_PrintHexByte_error
-  ldi r16, LCD_DATA_MODE
-  rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
-  brcc LCD_PrintHexByte_error
-  mov r16, r20
-  rcall lcdPrintHexByte
-  brcc LCD_PrintHexByte_error
-  rcall twiStop
+  push r15
+    cli
+    mov r20, r16
+    rcall twiStart
+    ldi r16, (LCD_TWI_ADDRESS*2)
+    rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
+    brcc LCD_PrintHexByte_error
+    ldi r16, LCD_DATA_MODE
+    rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
+    brcc LCD_PrintHexByte_error
+    mov r16, r20
+    rcall lcdPrintHexByte
+    brcc LCD_PrintHexByte_error
+    rcall twiStop
+  pop r15
   out SREG, r15
   sec
   ret
 LCD_PrintHexByte_error:
-  rcall twiStop
+    rcall twiStop
+  pop r15
   out SREG, r15
   clc
   ret
@@ -349,29 +362,33 @@ LCD_PrintHexByte_error:
 
 LCD_PrintHexWord:
   in r15, SREG
-  cli
-  push r18
-    push r19
-      rcall twiStart
-      ldi r16, (LCD_TWI_ADDRESS*2)
-      rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
-      brcc LCD_PrintHexWord_error
-      ldi r16, LCD_DATA_MODE
-      rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
-      brcc LCD_PrintHexWord_error
-    pop r19
-  pop r18
-    
-  rcall lcdPrintHexWord
-  brcc LCD_PrintHexWord_error
-  rcall twiStop
+  push r15
+    cli
+    push r18
+      push r19
+        rcall twiStart
+        ldi r16, (LCD_TWI_ADDRESS*2)
+        rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
+        brcc LCD_PrintHexWord_errorPop
+        ldi r16, LCD_DATA_MODE
+        rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
+        brcc LCD_PrintHexWord_errorPop
+      pop r19
+    pop r18
+      
+    rcall lcdPrintHexWord
+    brcc LCD_PrintHexWord_errorNoPop
+    rcall twiStop
+  pop r15
   out SREG, r15
   sec
   ret
-LCD_PrintHexWord_error:
-      rcall twiStop
-    pop r19
-  pop r18
+LCD_PrintHexWord_errorPop:
+        rcall twiStop
+      pop r19
+    pop r18
+LCD_PrintHexWord_errorNoPop:
+  pop r15
   out SREG, r15
   clc
   ret