5 files changed, 217 insertions, 268 deletions

diff --git a/apps/neo/keyboard.c b/apps/neo/keyboard.c
index 7433b4b6c7..d81d7629b3 100644
--- a/apps/neo/keyboard.c
+++ b/apps/neo/keyboard.c
@@ -94,7 +94,7 @@ int kbd_input(char* text, int buflen)
   lcd_puts(0, 0, kbdbuffer);
   kbd_show_legend(screen);
   lcd_cursor(cursorpos, 0);
-  lcd_write(true, LCD_BLINKCUR);
+  lcd_write_command(LCD_BLINKCUR);
   
   pstart = pcursor = kbdbuffer;
 
@@ -140,7 +140,7 @@ int kbd_input(char* text, int buflen)
               case BUTTON_IR|NEO_IR_BUTTON_STOP:
                   
                   /* Remove blinking cursor */
-                  lcd_write(true,LCD_OFFCUR);
+                  lcd_write_command(LCD_OFFCUR);
                   done = true;
           }
       }
diff --git a/firmware/drivers/lcd-player.c b/firmware/drivers/lcd-player.c
index 90e1d48e00..d040342862 100644
--- a/firmware/drivers/lcd-player.c
+++ b/firmware/drivers/lcd-player.c
@@ -111,6 +111,8 @@ static char lcd_cram;
 static char lcd_pram;
 static char lcd_iram;
 
+static unsigned char lcd_data_byte; /* global write buffer */
+
 unsigned short buffer_xlcd[11][2];
 unsigned short buffer_lcd_mirror[11][2];
 
@@ -122,7 +124,7 @@ unsigned char hardware_buffer_lcd[11][2];
 static void lcd_free_pat(int map_ch)
 {
     int x, y;
-    unsigned short substitute_char;
+    unsigned char substitute_char;
 
     int pat;
     pat=extended_chars_mapped[map_ch];
@@ -140,8 +142,8 @@ static void lcd_free_pat(int map_ch)
 #ifdef SIMULATOR
                     hardware_buffer_lcd[x][y]=substitute_char;
 #else
-                    lcd_write(true, LCD_CURSOR(x, y));
-                    lcd_write(false, substitute_char);
+                    lcd_write_command(LCD_CURSOR(x, y));
+                    lcd_write_data(&substitute_char, 1);
 #endif
                 }
             }
@@ -240,8 +242,8 @@ void xlcd_update(void)
 #ifdef SIMULATOR
             hardware_buffer_lcd[x][y]=hw_ch;
 #else
-            lcd_write(true,LCD_CURSOR(x,y));
-            lcd_write(false, hw_ch);
+            lcd_write_command(LCD_CURSOR(x,y));
+            lcd_write_data(&hw_ch, 1);
 #endif
         }
     }
@@ -274,8 +276,9 @@ bool lcdx_putc(int x, int y, unsigned short ch)
 #ifdef SIMULATOR
     hardware_buffer_lcd[x][y]=lcd_char;
 #else
-    lcd_write(true, LCD_CURSOR(x, y));
-    lcd_write(false, lcd_char);
+    lcd_data_byte = (unsigned char) lcd_char;
+    lcd_write_command(LCD_CURSOR(x, y));
+    lcd_write_data(&lcd_data_byte, 1);
 #endif
     return false;
 }
@@ -405,16 +408,14 @@ void lcd_define_pattern(int pat, char *pattern)
 #ifndef SIMULATOR
 void lcd_define_hw_pattern (int which,char *pattern,int length)
 {
-    int i;
-    lcd_write(true,lcd_pram | which);
-    for (i=0;i<length;i++)
-        lcd_write(false,pattern[i]);
+    lcd_write_command(lcd_pram | which);
+    lcd_write_data(pattern, length);
 }
 
 void lcd_double_height(bool on)
 {
     if(new_lcd)
-        lcd_write(true,on?9:8);
+        lcd_write_command(on?9:8);
 }
 
 static char icon_pos[] =
@@ -463,20 +464,21 @@ void lcd_icon(int icon, bool enable)
     pos = icon_pos[icon];
     mask = icon_mask[icon];
     
-    lcd_write(true, LCD_ICON(pos));
+    lcd_write_command(LCD_ICON(pos));
     
     if(enable)
         icon_mirror[pos] |= mask;
     else
         icon_mirror[pos] &= ~mask;
     
-    lcd_write(false, icon_mirror[pos]);
+    lcd_write_data(&icon_mirror[pos], 1);
 }
 
 void lcd_set_contrast(int val)
 {
-    lcd_write(true, lcd_contrast_set);
-    lcd_write(false, 31-val);
+    lcd_data_byte = (unsigned char) (31 - val);
+    lcd_write_command(lcd_contrast_set);
+    lcd_write_data(&lcd_data_byte, 1);
 }
 #endif /* SIMULATOR */
 
@@ -763,7 +765,7 @@ void lcd_cursor(int x, int y)
             value=0x80|(x+0x54);
             break;
     }
-    lcd_write(true,value);
+    lcd_write_command(value);
 }
 #endif
 
diff --git a/firmware/drivers/lcd-recorder.c b/firmware/drivers/lcd-recorder.c
index f7eb529fea..8cf5f12b8b 100644
--- a/firmware/drivers/lcd-recorder.c
+++ b/firmware/drivers/lcd-recorder.c
@@ -143,19 +143,19 @@ void lcd_init (void)
     PBIOR |= 0x000f; /* IOR = 1 */
 
     /* inits like the original firmware */
-    lcd_write(true, LCD_SOFTWARE_RESET);
-    lcd_write(true, LCD_SET_INTERNAL_REGULATOR_RESISTOR_RATIO + 4);
-    lcd_write(true, LCD_SET_1OVER4_BIAS_RATIO + 0); /* force 1/4 bias: 0 */
-    lcd_write(true, LCD_SET_POWER_CONTROL_REGISTER + 7); /* power control register: op-amp=1, regulator=1, booster=1 */
-    lcd_write(true, LCD_SET_DISPLAY_ON);
-    lcd_write(true, LCD_SET_NORMAL_DISPLAY);
-    lcd_write(true, LCD_SET_SEGMENT_REMAP + 1); /* mirror horizontal: 1 */
-    lcd_write(true, LCD_SET_COM_OUTPUT_SCAN_DIRECTION + 8); /* mirror vertical: 1 */
-    lcd_write(true, LCD_SET_DISPLAY_START_LINE + 0);
+    lcd_write_command(LCD_SOFTWARE_RESET);
+    lcd_write_command(LCD_SET_INTERNAL_REGULATOR_RESISTOR_RATIO + 4);
+    lcd_write_command(LCD_SET_1OVER4_BIAS_RATIO + 0); /* force 1/4 bias: 0 */
+    lcd_write_command(LCD_SET_POWER_CONTROL_REGISTER + 7); /* power control register: op-amp=1, regulator=1, booster=1 */
+    lcd_write_command(LCD_SET_DISPLAY_ON);
+    lcd_write_command(LCD_SET_NORMAL_DISPLAY);
+    lcd_write_command(LCD_SET_SEGMENT_REMAP + 1); /* mirror horizontal: 1 */
+    lcd_write_command(LCD_SET_COM_OUTPUT_SCAN_DIRECTION + 8); /* mirror vertical: 1 */
+    lcd_write_command(LCD_SET_DISPLAY_START_LINE + 0);
     lcd_set_contrast(lcd_default_contrast());
-    lcd_write(true, LCD_SET_PAGE_ADDRESS);
-    lcd_write(true, LCD_SET_LOWER_COLUMN_ADDRESS + 0);
-    lcd_write(true, LCD_SET_HIGHER_COLUMN_ADDRESS + 0);
+    lcd_write_command(LCD_SET_PAGE_ADDRESS);
+    lcd_write_command(LCD_SET_LOWER_COLUMN_ADDRESS + 0);
+    lcd_write_command(LCD_SET_HIGHER_COLUMN_ADDRESS + 0);
 
     lcd_clear_display();
     lcd_update();
@@ -171,9 +171,9 @@ void lcd_blit (unsigned char* p_data, int x, int y, int width, int height, int s
     /* Copy display bitmap to hardware */
     while (height--)
     {
-        lcd_write (true, LCD_CNTL_PAGE | (y++ & 0xf));
-        lcd_write (true, LCD_CNTL_HIGHCOL | (((x+xoffset)>>4) & 0xf));
-        lcd_write (true, LCD_CNTL_LOWCOL | ((x+xoffset) & 0xf));
+        lcd_write_command (LCD_CNTL_PAGE | (y++ & 0xf));
+        lcd_write_command (LCD_CNTL_HIGHCOL | (((x+xoffset)>>4) & 0xf));
+        lcd_write_command (LCD_CNTL_LOWCOL | ((x+xoffset) & 0xf));
 
         lcd_write_data(p_data, width);
         p_data += stride;
@@ -193,9 +193,9 @@ void lcd_update (void)
     /* Copy display bitmap to hardware */
     for (y = 0; y < LCD_HEIGHT/8; y++)
     {
-        lcd_write (true, LCD_CNTL_PAGE | (y & 0xf));
-        lcd_write (true, LCD_CNTL_HIGHCOL | ((xoffset>>4) & 0xf));
-        lcd_write (true, LCD_CNTL_LOWCOL | (xoffset & 0xf));
+        lcd_write_command (LCD_CNTL_PAGE | (y & 0xf));
+        lcd_write_command (LCD_CNTL_HIGHCOL | ((xoffset>>4) & 0xf));
+        lcd_write_command (LCD_CNTL_LOWCOL | (xoffset & 0xf));
 
         lcd_write_data (lcd_framebuffer[y], LCD_WIDTH);
     }
@@ -224,9 +224,9 @@ void lcd_update_rect (int x_start, int y,
     /* Copy specified rectange bitmap to hardware */
     for (; y <= ymax; y++)
     {
-        lcd_write (true, LCD_CNTL_PAGE | (y & 0xf));
-        lcd_write (true, LCD_CNTL_HIGHCOL | (((x_start+xoffset)>>4) & 0xf));
-        lcd_write (true, LCD_CNTL_LOWCOL | ((x_start+xoffset) & 0xf));
+        lcd_write_command (LCD_CNTL_PAGE | (y & 0xf));
+        lcd_write_command (LCD_CNTL_HIGHCOL | (((x_start+xoffset)>>4) & 0xf));
+        lcd_write_command (LCD_CNTL_LOWCOL | ((x_start+xoffset) & 0xf));
 
         lcd_write_data (&lcd_framebuffer[y][x_start], width);
     }
@@ -234,16 +234,16 @@ void lcd_update_rect (int x_start, int y,
 
 void lcd_set_contrast(int val)
 {
-    lcd_write(true, LCD_CNTL_CONTRAST);
-    lcd_write(true, val);
+    lcd_write_command(LCD_CNTL_CONTRAST);
+    lcd_write_command(val);
 }
 
 void lcd_set_invert_display(bool yesno)
 {
     if (yesno) 
-        lcd_write(true, LCD_SET_REVERSE_DISPLAY);
+        lcd_write_command(LCD_SET_REVERSE_DISPLAY);
     else 
-        lcd_write(true, LCD_SET_NORMAL_DISPLAY);
+        lcd_write_command(LCD_SET_NORMAL_DISPLAY);
 }
 
 /* turn the display upside down (call lcd_update() afterwards) */
@@ -251,14 +251,14 @@ void lcd_set_flip(bool yesno)
 {
     if (yesno) 
     {
-        lcd_write(true, LCD_SET_SEGMENT_REMAP);
-        lcd_write(true, LCD_SET_COM_OUTPUT_SCAN_DIRECTION);
+        lcd_write_command(LCD_SET_SEGMENT_REMAP);
+        lcd_write_command(LCD_SET_COM_OUTPUT_SCAN_DIRECTION);
         xoffset = 132 - LCD_WIDTH; /* 132 colums minus the 112 we have */
     }
     else 
     {
-        lcd_write(true, LCD_SET_SEGMENT_REMAP | 0x01);
-        lcd_write(true, LCD_SET_COM_OUTPUT_SCAN_DIRECTION | 0x08);
+        lcd_write_command(LCD_SET_SEGMENT_REMAP | 0x01);
+        lcd_write_command(LCD_SET_COM_OUTPUT_SCAN_DIRECTION | 0x08);
         xoffset = 0;
     }
 }
@@ -274,7 +274,7 @@ void lcd_set_flip(bool yesno)
  */
 void lcd_roll(int lines)
 {
-    lcd_write(true, LCD_SET_DISPLAY_START_LINE | (lines & (LCD_HEIGHT-1)));
+    lcd_write_command(LCD_SET_DISPLAY_START_LINE | (lines & (LCD_HEIGHT-1)));
 }
 
 #endif /* SIMULATOR */
diff --git a/firmware/drivers/lcd.c b/firmware/drivers/lcd.c
index 252652b29a..0df831e259 100644
--- a/firmware/drivers/lcd.c
+++ b/firmware/drivers/lcd.c
@@ -82,104 +82,50 @@
  *
  */ 
 
-void lcd_write(bool command, int byte) __attribute__ ((section (".icode")));
-void lcd_write(bool command, int byte)
+void lcd_write_command(int byte) __attribute__ ((section (".icode")));
+void lcd_write_command(int byte)
 {
-    asm("and.b %0, @(r0,gbr)"
-         :
-         : /* %0 */ "I"(~(LCD_CS|LCD_DS|LCD_SD|LCD_SC)),
-           /* %1 */ "z"(LCDR));
-
-    if (command)
-        asm ("shll8 %0\n"
-             "0:      \n\t"
-             "and.b %2,@(r0,gbr)\n\t"
-             "shll  %0\n\t"  
-             "bf    1f\n\t"
-             "or.b  %3,@(r0,gbr)\n"
-             "1:      \n\t"
-             "or.b  %4,@(r0,gbr)\n"
-             "add   #-1,%1\n\t"
-             "cmp/pl %1\n\t"
-             "bt    0b"
-             :
-             : /* %0 */ "r"(((unsigned)byte)<<16),
-             /* %1 */ "r"(8),
-             /* %2 */ "I"(~(LCD_SC|LCD_SD|LCD_DS)),
-             /* %3 */ "I"(LCD_SD),
-             /* %4 */ "I"(LCD_SC),
-             /* %5 */ "z"(LCDR));
-    else
-        asm ("shll8  %0\n"
-             "0:       \n\t"
-             "and.b  %2, @(r0,gbr)\n\t"
-             "shll   %0\n\t"  
-             "bf     1f\n\t"
-             "or.b   %3, @(r0,gbr)\n"
-             "1:       \n\t"
-             "or.b   %4, @(r0,gbr)\n"
-             "and.b  %2, @(r0,gbr)\n\t"
-             "shll   %0\n\t"  
-             "bf     1f\n\t"
-             "or.b   %3, @(r0,gbr)\n"
-             "1:       \n\t"
-             "or.b   %4, @(r0,gbr)\n"
-             "and.b  %2, @(r0,gbr)\n\t"
-             "shll   %0\n\t"  
-             "bf     1f\n\t"
-             "or.b   %3, @(r0,gbr)\n"
-             "1:       \n\t"
-             "or.b   %4, @(r0,gbr)\n"
-             "and.b  %2, @(r0,gbr)\n\t"
-             "shll   %0\n\t"  
-             "bf     1f\n\t"
-             "or.b   %3, @(r0,gbr)\n"
-             "1:       \n\t"
-             "or.b   %4, @(r0,gbr)\n"
-             "and.b  %2, @(r0,gbr)\n\t"
-             "shll   %0\n\t"  
-             "bf     1f\n\t"
-             "or.b   %3, @(r0,gbr)\n"
-             "1:       \n\t"
-             "or.b   %4, @(r0,gbr)\n"
-             "and.b  %2, @(r0,gbr)\n\t"
-             "shll   %0\n\t"  
-             "bf     1f\n\t"
-             "or.b   %3, @(r0,gbr)\n"
-             "1:       \n\t"
-             "or.b   %4, @(r0,gbr)\n"
-             "and.b  %2, @(r0,gbr)\n\t"
-             "shll   %0\n\t"  
-             "bf     1f\n\t"
-             "or.b   %3, @(r0,gbr)\n"
-             "1:       \n\t"
-             "or.b   %4, @(r0,gbr)\n"
-             "and.b  %2, @(r0,gbr)\n\t"
-             "shll   %0\n\t"  
-             "bf     1f\n\t"
-             "or.b   %3, @(r0,gbr)\n"
-             "1:       \n\t"
-             "or.b   %4, @(r0,gbr)\n"
-             :
-             : /* %0 */ "r"(((unsigned)byte)<<16),
-             /* %1 */ "r"(8),
-             /* %2 */ "I"(~(LCD_SC|LCD_SD)),
-             /* %3 */ "I"(LCD_SD|LCD_DS),
-             /* %4 */ "I"(LCD_SC|LCD_DS),
-             /* %5 */ "z"(LCDR));
-
-    asm("or.b %0, @(r0,gbr)"
-         :
-         : /* %0 */ "I"(LCD_CS|LCD_DS|LCD_SD|LCD_SC),
-           /* %1 */ "z"(LCDR));
+    asm (
+        "and.b   %0, @(r0,gbr)"
+        : /* outputs */
+        : /* inputs */
+        /* %0 */ "I"(~(LCD_CS|LCD_DS|LCD_SD|LCD_SC)),
+        /* %1 = r0 */ "z"(LCDR)
+    );
+
+    asm (
+        "0:                      \n"
+        "and.b   %2,@(r0,gbr)    \n"
+        "shll    %0              \n"
+        "bf      1f              \n"
+        "or.b    %3,@(r0,gbr)    \n"
+        "1:                      \n"
+        "or.b    %4,@(r0,gbr)    \n"
+        "add     #-1,%1          \n"
+        "cmp/pl  %1              \n"
+        "bt      0b              \n"
+        : /* outputs */
+        : /* inputs */
+        /* %0 */ "r"(((unsigned)byte)<<24),
+        /* %1 */ "r"(8),
+        /* %2 */ "I"(~(LCD_SC|LCD_SD|LCD_DS)),
+        /* %3 */ "I"(LCD_SD),
+        /* %4 */ "I"(LCD_SC),
+        /* %5 = r0 */ "z"(LCDR)
+    );
+
+    asm (
+        "or.b    %0, @(r0,gbr)"
+        : /* outputs */
+        : /* inputs */
+        /* %0 */ "I"(LCD_CS|LCD_DS|LCD_SD|LCD_SC),
+        /* %1 = r0 */ "z"(LCDR)
+    );
 }
 
 
 /* A high performance function to write data to the display, 
-   one or multiple bytes.
-   Ultimately, all calls to lcd_write(false, xxx) should be substituted by 
-   this, it will be most efficient if the LCD buffer is tilted to have the
-   X row as consecutive bytes, so we can write a whole row */
+   one or multiple bytes. */
 void lcd_write_data(unsigned char* p_bytes, int count) __attribute__ ((section (".icode")));
 
 #ifdef HAVE_LCD_CHARCELLS
@@ -199,16 +145,16 @@ void lcd_write_data(unsigned char* p_bytes, int count)
 
         /* precalculate the values for later bit toggling, init data write */
         asm (
-            "mov.b  @%2,%0\n" /* sda1 = PBDRL */
-            "or     %4,%0\n"  /* sda1 |= LCD_DS | LCD_SD     DS and SD high, */
-            "and    %3,%0\n"  /* sda1 &= ~(LCD_CS | LCD_SC)  CS and SC low   */
-            "mov    %0,%1\n"  /* sda1 -> clk0sda0 */
-            "and    %5,%1\n"  /* clk0sda0 &= ~LCD_SD  both low */
-            "mov.b  %1,@%2\n" /* PBDRL = clk0sda0 */
-            : // outputs
+            "mov.b   @%2,%0  \n" /* sda1 = PBDRL */
+            "or      %4,%0   \n" /* sda1 |= LCD_DS | LCD_SD     DS and SD high, */
+            "and     %3,%0   \n" /* sda1 &= ~(LCD_CS | LCD_SC)  CS and SC low   */
+            "mov     %0,%1   \n" /* sda1 -> clk0sda0 */
+            "and     %5,%1   \n" /* clk0sda0 &= ~LCD_SD  both low */
+            "mov.b   %1,@%2  \n" /* PBDRL = clk0sda0 */
+            : /* outputs */
             /* %0 */ "=r"(sda1),
             /* %1 */ "=r"(clk0sda0)
-            : // inputs
+            : /* inputs */
             /* %2 */ "r"(LCDR),
             /* %3 */ "r"(~(LCD_CS | LCD_SC)),
             /* %4 */ "r"(LCD_DS | LCD_SD),
@@ -217,72 +163,71 @@ void lcd_write_data(unsigned char* p_bytes, int count)
         
         /* unrolled loop to serialize the byte */
         asm (
-            "mov    %4,r0\n" /* we need &PBDRL in r0 for "or.b x,@(r0,gbr)" */
-            
-            "shll   %0\n" /* shift the MSB into carry */
-            "bf     1f\n"
-            "mov.b  %1,@%4\n" /* if it was a "1": set SD high, SC low still */
-            "1:       \n"
-            "or.b   %2, @(r0,gbr)\n" /* rise SC (independent of SD level) */
-            "shll   %0\n" /* shift for next round, now for longer hold time */
-            "mov.b  %3,@%4\n" /* SC and SD low again */
-
-            "bf     1f\n"
-            "mov.b  %1,@%4\n"
-            "1:       \n"
-            "or.b   %2, @(r0,gbr)\n"
-            "shll   %0\n"
-            "mov.b  %3,@%4\n"
-            
-            "bf     1f\n"
-            "mov.b  %1,@%4\n"
-            "1:       \n"
-            "or.b   %2, @(r0,gbr)\n"
-            "shll   %0\n"
-            "mov.b  %3,@%4\n"
-            
-            "bf     1f\n"
-            "mov.b  %1,@%4\n"
-            "1:       \n"
-            "or.b   %2, @(r0,gbr)\n"
-            "shll   %0\n"
-            "mov.b  %3,@%4\n"
-            
-            "bf     1f\n"
-            "mov.b  %1,@%4\n"
-            "1:       \n"
-            "or.b   %2, @(r0,gbr)\n"
-            "shll   %0\n"
-            "mov.b  %3,@%4\n"
-            
-            "bf     1f\n"
-            "mov.b  %1,@%4\n"
-            "1:       \n"
-            "or.b   %2, @(r0,gbr)\n"
-            "shll   %0\n"
-            "mov.b  %3,@%4\n"
-            
-            "bf     1f\n"
-            "mov.b  %1,@%4\n"
-            "1:       \n"
-            "or.b   %2, @(r0,gbr)\n"
-            "shll   %0\n"
-            "mov.b  %3,@%4\n"
-            
-            "bf     1f\n"
-            "mov.b  %1,@%4\n" /* set SD high, SC low still */
-            "1:       \n"
-            "or.b   %2, @(r0,gbr)\n" /* rise SC (independent of SD level) */
-
-            "or.b   %5, @(r0,gbr)\n" /* restore port */
-            :
-            : /* %0 */ "r"(byte),
+            "shll    %0              \n" /* shift the MSB into carry */
+
+            "bf      1f              \n"
+            "mov.b   %1,@%4          \n" /* if it was a "1": set SD high, SC low still */
+            "1:                      \n"
+            "or.b    %2,@(r0,gbr)    \n" /* rise SC (independent of SD level) */
+            "shll    %0              \n" /* shift for next round, now for longer hold time */
+            "mov.b   %3,@%4          \n" /* SC and SD low again */
+
+            "bf      1f              \n"
+            "mov.b   %1,@%4          \n"
+            "1:                      \n"
+            "or.b    %2,@(r0,gbr)    \n"
+            "shll    %0              \n"
+            "mov.b   %3,@%4          \n"
+
+            "bf      1f              \n"
+            "mov.b   %1,@%4          \n"
+            "1:                      \n"
+            "or.b    %2,@(r0,gbr)    \n"
+            "shll    %0              \n"
+            "mov.b   %3,@%4          \n"
+
+            "bf      1f              \n"
+            "mov.b   %1,@%4          \n"
+            "1:                      \n"
+            "or.b    %2,@(r0,gbr)    \n"
+            "shll    %0              \n"
+            "mov.b   %3,@%4          \n"
+
+            "bf      1f              \n"
+            "mov.b   %1,@%4          \n"
+            "1:                      \n"
+            "or.b    %2,@(r0,gbr)    \n"
+            "shll    %0              \n"
+            "mov.b   %3,@%4          \n"
+
+            "bf      1f              \n"
+            "mov.b   %1,@%4          \n"
+            "1:                      \n"
+            "or.b    %2,@(r0,gbr)    \n"
+            "shll    %0              \n"
+            "mov.b   %3,@%4          \n"
+
+            "bf      1f              \n"
+            "mov.b   %1,@%4          \n"
+            "1:                      \n"
+            "or.b    %2,@(r0,gbr)    \n"
+            "shll    %0              \n"
+            "mov.b   %3,@%4          \n"
+
+            "bf      1f              \n"
+            "mov.b   %1,@%4          \n" /* set SD high, SC low still */
+            "1:                      \n"
+            "or.b    %2,@(r0,gbr)    \n" /* rise SC (independent of SD level) */
+
+            "or.b    %5,@(r0,gbr)    \n" /* restore port */
+            : /* outputs */
+            : /* inputs */
+            /* %0 */ "r"(byte),
             /* %1 */ "r"(sda1),
             /* %2 */ "I"(LCD_SC),
             /* %3 */ "r"(clk0sda0),
-            /* %4 */ "r"(LCDR),
+            /* %4 = r0 */ "z"(LCDR),
             /* %5 */ "I"(LCD_CS|LCD_DS|LCD_SD|LCD_SC)
-            : "r0"
         );
 
         /* This is the place to reenable the interrupts, if we have disabled
@@ -309,11 +254,11 @@ void lcd_write_data(unsigned char* p_bytes, int count)
 
         /* precalculate the values for later bit toggling, init data write */
         asm (
-            "mov.b  @%1,r0\n" /* r0 = PBDRL */
-            "or     %3,r0\n"  /* r0 |= LCD_DS | LCD_SD     DS and SD high, */
-            "and    %2,r0\n"  /* r0 &= ~(LCD_CS | LCD_SC)  CS and SC low   */
-            "mov.b  r0,@%1\n" /* PBDRL = r0 */
-            "neg    r0,%0\n"  /* sda1 = 0-r0 */
+            "mov.b   @%1,r0  \n" /* r0 = PBDRL */
+            "or      %3,r0   \n" /* r0 |= LCD_DS | LCD_SD     DS and SD high, */
+            "and     %2,r0   \n" /* r0 &= ~(LCD_CS | LCD_SC)  CS and SC low   */
+            "mov.b   r0,@%1  \n" /* PBDRL = r0 */
+            "neg     r0,%0   \n" /* sda1 = 0-r0 */
             : /* outputs: */
             /* %0 */ "=r"(sda1)
             : /* inputs: */
@@ -326,56 +271,56 @@ void lcd_write_data(unsigned char* p_bytes, int count)
 
         /* unrolled loop to serialize the byte */
         asm (
-            "shll   %0    \n" /* shift the MSB into carry */
-            "negc   %1, r0\n" /* carry to SD, SC low */
-            "mov.b  r0,@%3\n" /* set data to port */
-            "or     %2, r0\n" /* rise SC (independent of SD level) */
-            "mov.b  r0,@%3\n" /* set to port */
-
-            "shll   %0    \n"
-            "negc   %1, r0\n"
-            "mov.b  r0,@%3\n"
-            "or     %2, r0\n"
-            "mov.b  r0,@%3\n"
-
-            "shll   %0    \n"
-            "negc   %1, r0\n"
-            "mov.b  r0,@%3\n"
-            "or     %2, r0\n"
-            "mov.b  r0,@%3\n"
-
-            "shll   %0    \n"
-            "negc   %1, r0\n"
-            "mov.b  r0,@%3\n"
-            "or     %2, r0\n"
-            "mov.b  r0,@%3\n"
-
-            "shll   %0    \n"
-            "negc   %1, r0\n"
-            "mov.b  r0,@%3\n"
-            "or     %2, r0\n"
-            "mov.b  r0,@%3\n"
-
-            "shll   %0    \n"
-            "negc   %1, r0\n"
-            "mov.b  r0,@%3\n"
-            "or     %2, r0\n"
-            "mov.b  r0,@%3\n"
-
-            "shll   %0    \n"
-            "negc   %1, r0\n"
-            "mov.b  r0,@%3\n"
-            "or     %2, r0\n"
-            "mov.b  r0,@%3\n"
-
-            "shll   %0    \n"
-            "negc   %1, r0\n"
-            "mov.b  r0,@%3\n"
-            "or     %2, r0\n"
-            "mov.b  r0,@%3\n"
-
-            "or     %4, r0\n" /* restore port */
-            "mov.b  r0,@%3\n"
+            "shll    %0      \n" /* shift the MSB into carry */
+            "negc    %1, r0  \n" /* carry to SD, SC low */
+            "mov.b   r0,@%3  \n" /* set data to port */
+            "or      %2, r0  \n" /* rise SC (independent of SD level) */
+            "mov.b   r0,@%3  \n" /* set to port */
+
+            "shll    %0      \n"
+            "negc    %1, r0  \n"
+            "mov.b   r0,@%3  \n"
+            "or      %2, r0  \n"
+            "mov.b   r0,@%3  \n"
+
+            "shll    %0      \n"
+            "negc    %1, r0  \n"
+            "mov.b   r0,@%3  \n"
+            "or      %2, r0  \n"
+            "mov.b   r0,@%3  \n"
+
+            "shll    %0      \n"
+            "negc    %1, r0  \n"
+            "mov.b   r0,@%3  \n"
+            "or      %2, r0  \n"
+            "mov.b   r0,@%3  \n"
+
+            "shll    %0      \n"
+            "negc    %1, r0  \n"
+            "mov.b   r0,@%3  \n"
+            "or      %2, r0  \n"
+            "mov.b   r0,@%3  \n"
+
+            "shll    %0      \n"
+            "negc    %1, r0  \n"
+            "mov.b   r0,@%3  \n"
+            "or      %2, r0  \n"
+            "mov.b   r0,@%3  \n"
+
+            "shll    %0      \n"
+            "negc    %1, r0  \n"
+            "mov.b   r0,@%3  \n"
+            "or      %2, r0  \n"
+            "mov.b   r0,@%3  \n"
+
+            "shll    %0      \n"
+            "negc    %1, r0  \n"
+            "mov.b   r0,@%3  \n"
+            "or      %2, r0  \n"
+            "mov.b   r0,@%3  \n"
+
+            "or      %4, r0  \n" /* restore port */
+            "mov.b   r0,@%3  \n"
             : /* outputs: */
             : /* inputs: */
             /* %0 */ "r"(byte),
@@ -393,3 +338,5 @@ void lcd_write_data(unsigned char* p_bytes, int count)
     } while (--count); /* tail loop is faster */
 }
 #endif /* #ifdef HAVE_LCD_CHARCELLS */
+
+
diff --git a/firmware/export/lcd.h b/firmware/export/lcd.h
index 733efdded3..20023bf1c6 100644
--- a/firmware/export/lcd.h
+++ b/firmware/export/lcd.h
@@ -43,7 +43,7 @@ extern void lcd_stop_scroll(void);
 extern void lcd_scroll_speed( int speed );
 extern void lcd_scroll_delay( int ms );
 extern void lcd_set_contrast(int val);
-extern void lcd_write( bool command, int byte );
+extern void lcd_write_command( int byte );
 extern void lcd_write_data( unsigned char* p_bytes, int count );
 extern int  lcd_default_contrast(void);