diff --git a/avr/modules/lcd/main.asm b/avr/modules/lcd/main.asm
index 7b21f79..120d72a 100644
--- a/avr/modules/lcd/main.asm
+++ b/avr/modules/lcd/main.asm
@@ -128,40 +128,59 @@ LCD_Fini:
 ;
 ; @return nothing
 ; @param r16 1=sleep, 0=awake
-; @clobbers r15, r16, r19 (R17, R18, R22)
+; @clobbers r15, r16, r20 (R17, R18, R22)
 ;
 
 LCD_Sleep:
-  ldi r16, 0xae ; set sleep
-  tst r19
+  ldi r20, 0xae ; set sleep
+  tst r16
   brne LCD_Sleep_send
-  ldi r16, 0xaf ; set wake
+  ldi r20, 0xaf ; set wake
 LCD_Sleep_send:
-  rjmp lcdOneByteCommand
+  ldi r23, LCD_MODE_MULTICMD
+  rjmp lcdOneByteMsg
 ; @end
 
 
 
 ; ---------------------------------------------------------------------------
-; @routine lcdOneByteCommand
+; @routine LCD_SetContrast @global
+;
+; @return nothing
+; @param r16 contrast value (0-255)
+; @clobbers r15, r16, r19 (R17, R18, R22)
+;
+
+LCD_SetContrast:
+  ldi r23, LCD_MODE_MULTICMD
+  ldi r20, 0x81
+  mov r21, r16
+  rjmp lcdTwoByteMsg
+; @end
+
+
+
+; ---------------------------------------------------------------------------
+; @routine lcdOneByteMsg
 ; Send a one byte command
 ;
 ; @return nothing
+; @param r23 packet type (LCD_MODE_MULTICMD, LCD_MODE_DATA etc)
 ; @param r20 first byte of command
 ; @clobbers r15, r16, r19 (R17, R18, R22)
 ;
 
-lcdOneByteCommand:
+lcdOneByteMsg:
   in r15, SREG
   push r15
     cli
 
-    rcall lcdBeginMultiCommand
-    brcc lcdOneByteCommand_error
+    rcall lcdBeginMsg
+    brcc lcdOneByteMsg_error
 
     mov r16, r20
     rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
-    brcc lcdOneByteCommand_error
+    brcc lcdOneByteMsg_error
 
     rcall twiStop              ; (R22)
   pop r15
@@ -169,37 +188,38 @@ lcdOneByteCommand:
   sec
   ret
   
-lcdOneByteCommand_error:
+lcdOneByteMsg_error:
   rjmp lcdTwiStopPopR15ClcRet     ; (R22)
 ; @end
 
 
 
 ; ---------------------------------------------------------------------------
-; @routine lcdTwoByteCommand
+; @routine lcdTwoByteMsg
 ; Send a two byte command
 ;
 ; @return nothing
+; @param r23 packet type (LCD_MODE_MULTICMD, LCD_MODE_DATA etc)
 ; @param r20 first byte of command
 ; @param r21 second byte of command
 ; @clobbers r15, r16, r19 (R17, R18, R22)
 ;
 
-lcdTwoByteCommand:
+lcdTwoByteMsg:
   in r15, SREG
   push r15
     cli
 
-    rcall lcdBeginMultiCommand
-    brcc lcdTwoByteCommand_error
+    rcall lcdBeginMsg
+    brcc lcdTwoByteMsg_error
 
     mov r16, r20
     rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
-    brcc lcdTwoByteCommand_error
+    brcc lcdTwoByteMsg_error
 
     mov r16, r21
     rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
-    brcc lcdTwoByteCommand_error
+    brcc lcdTwoByteMsg_error
 
     rcall twiStop              ; (R22)
   pop r15
@@ -207,30 +227,31 @@ lcdTwoByteCommand:
   sec
   ret
   
-lcdTwoByteCommand_error:
+lcdTwoByteMsg_error:
   rjmp lcdTwiStopPopR15ClcRet     ; (R22)
 ; @end
 
 
 
 ; ---------------------------------------------------------------------------
-; @routine lcdBeginMultiCommand
+; @routine lcdBeginMsg
 ; Send a two byte command
 ;
 ; @return nothing
-; @param r20 first byte of command
-; @param r21 second byte of command
+; @param r23 packet type (LCD_MODE_MULTICMD, LCD_MODE_DATA etc)
 ; @clobbers r16, (R17, R18, R22)
 ;
 
-lcdBeginMultiCommand:
+lcdBeginMsg:
   rcall twiStart             ; (R22)
   ldi r16, (LCD_TWI_ADDRESS*2)
   rcall twiSendByteExpectAck ; (R16, R17, R18, R22)
-  brcc lcdTwoByteCommand_error
+  brcc lcdBeginMsg_error
 
-  ldi r16, LCD_MODE_MULTICMD
+  mov r16, r23
   rjmp twiSendByteExpectAck ; (R16, R17, R18, R22)
+lcdBeginMsg_error:
+  ret
 ; @end
 
 
@@ -668,6 +689,7 @@ lcdPrintOneChar_error:
   ret
 
 
+
 ; ---------------------------------------------------------------------------
 ; lcdGetCharMatrix
 ;