Greyscale library: Changed the internal data format once more (separated pixel values and phases), allowing for further optimisation of drawing, scrolling etc. * Optimised grey phase blitting in the core reduces CPU load on all architectures, most significantly on coldfire. Previous version was too slow to keep up at 45MHz, leading to unwanted graininess (update frequency was halved). Also fixed screendump on 2bpp targets with vertical pixel packing.

git-svn-id: svn://svn.rockbox.org/rockbox/trunk@16043 a1c6a512-1295-4272-9138-f99709370657

13 files changed, 476 insertions, 327 deletions

diff --git a/apps/plugin.h b/apps/plugin.h
index 849c10b..a3ec9c3 100644
--- a/apps/plugin.h
+++ b/apps/plugin.h
@@ -119,12 +119,12 @@
 #define PLUGIN_MAGIC 0x526F634B /* RocK */
 
 /* increase this every time the api struct changes */
-#define PLUGIN_API_VERSION 95
+#define PLUGIN_API_VERSION 96
 
 /* update this to latest version if a change to the api struct breaks
    backwards compatibility (and please take the opportunity to sort in any
    new function which are "waiting" at the end of the function table) */
-#define PLUGIN_MIN_API_VERSION 95
+#define PLUGIN_MIN_API_VERSION 96
 
 /* plugin return codes */
 enum plugin_status {
@@ -268,8 +268,9 @@ struct plugin_api {
                               int height);
 #endif
 #if defined(HAVE_LCD_BITMAP) && (LCD_DEPTH < 4) && !defined(SIMULATOR)
-    void (*lcd_grey_phase_blit)(const struct grey_data *data, int bx, int by,
-                                int bwidth, int bheight, int stride);
+    void (*lcd_grey_phase_blit)(unsigned char *values, unsigned char *phases,
+                                int bx, int by, int bwidth, int bheight,
+                                int stride);
 #endif
 #if defined(HAVE_LCD_COLOR)
     void (*lcd_yuv_blit)(unsigned char * const src[3],
diff --git a/apps/plugins/lib/grey.h b/apps/plugins/lib/grey.h
index 136794b..9a3bd7d 100644
--- a/apps/plugins/lib/grey.h
+++ b/apps/plugins/lib/grey.h
@@ -121,12 +121,12 @@ void grey_ub_scroll_down(int count);
 #endif
 
 #if LCD_PIXELFORMAT == HORIZONTAL_PACKING
-#define _GREY_X_ADVANCE sizeof(struct grey_data)
+#define _GREY_X_ADVANCE 1
 #else
 #if LCD_DEPTH == 1
-#define _GREY_X_ADVANCE (8*sizeof(struct grey_data))
+#define _GREY_X_ADVANCE 8
 #elif LCD_DEPTH == 2
-#define _GREY_X_ADVANCE (4*sizeof(struct grey_data))
+#define _GREY_X_ADVANCE 4
 #endif
 #endif /* LCD_PIXELFORMAT */
 
@@ -146,7 +146,8 @@ struct _grey_info
 #endif
     unsigned long flags;       /* various flags, see #defines */
 #ifndef SIMULATOR
-    struct grey_data *data;    /* start of greyscale display data */
+    unsigned char *values;     /* start of greyscale pixel values */
+    unsigned char *phases;     /* start of greyscale pixel phases */
 #endif
     unsigned char *buffer;     /* start of chunky pixel buffer (for buffered mode) */
     unsigned char gvalue[256]; /* calculated brightness -> greyvalue table */
diff --git a/apps/plugins/lib/grey_core.c b/apps/plugins/lib/grey_core.c
index b3ab640..43eed9b 100644
--- a/apps/plugins/lib/grey_core.c
+++ b/apps/plugins/lib/grey_core.c
@@ -222,11 +222,13 @@ static inline void _deferred_update(void)
 static void _timer_isr(void)
 {
 #if LCD_PIXELFORMAT == HORIZONTAL_PACKING
-    _grey_rb->lcd_grey_phase_blit(_grey_info.data, _grey_info.bx, _grey_info.y,
+    _grey_rb->lcd_grey_phase_blit(_grey_info.values, _grey_info.phases,
+                                  _grey_info.bx, _grey_info.y,
                                   _grey_info.bwidth, _grey_info.height,
                                   _grey_info.width);
 #else
-    _grey_rb->lcd_grey_phase_blit(_grey_info.data, _grey_info.x, _grey_info.by,
+    _grey_rb->lcd_grey_phase_blit(_grey_info.values, _grey_info.phases,
+                                  _grey_info.x, _grey_info.by,
                                   _grey_info.width, _grey_info.bheight,
                                   _grey_info.width);
 #endif
@@ -321,7 +323,7 @@ bool grey_init(struct plugin_api* newrb, unsigned char *gbuf, long gbuf_size,
     long plane_size, buftaken;
     unsigned data;
 #ifndef SIMULATOR
-    struct grey_data *grey_data, *grey_end;
+    unsigned *dst, *end;
 #endif
 
     _grey_rb = newrb;
@@ -343,35 +345,41 @@ bool grey_init(struct plugin_api* newrb, unsigned char *gbuf, long gbuf_size,
 #endif
 #endif
 
-    /* the buffer has to be long aligned */
-    buftaken = (-(long)gbuf) & 3;
-    gbuf += buftaken;
-
     plane_size = _GREY_MULUQ(width, height);
+#ifdef CPU_COLDFIRE
+    plane_size += (-plane_size) & 0xf;  /* All buffers should be line aligned */
+    buftaken    = (-(long)gbuf) & 0xf;
+#else
+    buftaken    = (-(long)gbuf) & 3;    /* All buffers must be long aligned. */
+#endif
+    gbuf += buftaken;
 
     if (buffered) /* chunky buffer */
     {
-        buftaken += plane_size;
         _grey_info.buffer = gbuf;
-        gbuf += plane_size;
+        gbuf     += plane_size;
+        buftaken += plane_size;
     }
-    buftaken += sizeof(struct grey_data) * plane_size;
-    if (buftaken > gbuf_size)
-        return false;
-
 #ifdef SIMULATOR
     _grey_info.buffer = gbuf;
 #else
-    grey_data = (struct grey_data *)gbuf;
-    grey_end = grey_data + plane_size;
-    _grey_info.data = grey_data;
+    _grey_info.values = gbuf;
+    gbuf += plane_size;
+    _grey_info.phases = gbuf;
+#endif
+    buftaken += 2 * plane_size;
 
-    while (grey_data < grey_end)
-    {
-        grey_data->phase = _grey_rb->rand() & 0xff;
-        grey_data->value = 128; /* init to white */
-        grey_data++;
-    }
+    if (buftaken > gbuf_size)
+        return false;
+        
+#ifndef SIMULATOR
+    _grey_rb->memset(_grey_info.values, 0x80, plane_size);
+    dst = (unsigned*)(_grey_info.phases);
+    end = (unsigned*)(_grey_info.phases + plane_size);
+
+    do
+        *dst++ = _grey_rb->rand();
+    while (dst < end);
 #endif
 
     _grey_info.x = 0;
@@ -393,8 +401,7 @@ bool grey_init(struct plugin_api* newrb, unsigned char *gbuf, long gbuf_size,
     _grey_info.drawmode = DRMODE_SOLID;
     _grey_info.curfont = FONT_SYSFIXED;
 
-
-    /* precalculate the value -> pattern index conversion table, taking 
+    /* precalculate the value -> pattern index conversion table, taking
        linearisation and gamma correction into account */
     for (i = 0; i < 256; i++)
     {
@@ -532,7 +539,7 @@ void grey_update_rect(int x, int y, int width, int height)
         int idx = _GREY_MULUQ(_grey_info.width, y & ~3) + (x << 2) + (~y & 3);
 #endif
 #endif /* LCD_PIXELFORMAT */
-        unsigned char *dst_row = &_grey_info.data[idx].value;
+        unsigned char *dst_row = _grey_info.values + idx;
         unsigned char *src_row = src;
         unsigned char *src_end = src + width;
         
@@ -684,8 +691,8 @@ static void grey_screendump_hook(int fd)
                 for (i = 0; i < 4; i++)
                     linebuf[x + i] = BMP_FIXEDCOLORS + *src++;
 #else
-                unsigned char *src = &_grey_info.data[_GREY_MULUQ(_grey_info.width,
-                                                               gy) + gx].value;
+                unsigned char *src = _grey_info.values
+                                   + _GREY_MULUQ(_grey_info.width, gy) + gx;
                 for (i = 0; i < 4; i++)
                 {
                     linebuf[x + i] = BMP_FIXEDCOLORS + *src;
@@ -722,8 +729,8 @@ static void grey_screendump_hook(int fd)
                                                                      gy) + gx];
 #else
                 linebuf[x] = BMP_FIXEDCOLORS
-                           + _grey_info.data[_GREY_MULUQ(_grey_info.width,
-                                       gy & ~7) + (gx << 3) + (~gy & 7)].value;
+                           + _grey_info.values[_GREY_MULUQ(_grey_info.width,
+                                             gy & ~7) + (gx << 3) + (~gy & 7)];
 #endif
             }
             else
@@ -749,8 +756,8 @@ static void grey_screendump_hook(int fd)
                                                                      gy) + gx];
 #else
                 linebuf[x] = BMP_FIXEDCOLORS
-                           + _grey_info.data[_GREY_MULUQ(_grey_info.width,
-                                        gy & ~3) + (gx << 2) + (~gy & 7)].value;
+                           + _grey_info.values[_GREY_MULUQ(_grey_info.width,
+                                             gy & ~3) + (gx << 2) + (~gy & 3)];
 #endif
             }
             else
diff --git a/apps/plugins/lib/grey_draw.c b/apps/plugins/lib/grey_draw.c
index e243b5f..ccb8dea 100644
--- a/apps/plugins/lib/grey_draw.c
+++ b/apps/plugins/lib/grey_draw.c
@@ -601,17 +601,10 @@ void grey_ub_gray_bitmap_part(const unsigned char *src, int src_x, int src_y,
 void grey_ub_clear_display(void)
 {
     int value = (_grey_info.drawmode & DRMODE_INVERSEVID) ?
-                 _grey_info.fg_val : _grey_info.bg_val;
-    unsigned char *dst = &_grey_info.data[0].value;
-    unsigned char *dst_end = dst + sizeof(struct grey_data) 
-                            * _GREY_MULUQ(_grey_info.width, _grey_info.height);
-    
-    do
-    {
-        *dst = value;
-        dst += sizeof(struct grey_data);
-    }
-    while (dst < dst_end);
+                     _grey_info.fg_val : _grey_info.bg_val;
+                     
+    _grey_rb->memset(_grey_info.values, value, 
+                     _GREY_MULUQ(_grey_info.width, _grey_info.height));
 }
 
 /* Draw a partial greyscale bitmap, canonical format */
@@ -654,7 +647,7 @@ void grey_ub_gray_bitmap_part(const unsigned char *src, int src_x, int src_y,
         int idx = _GREY_MULUQ(_grey_info.width, y & ~3) + (x << 2) + (~y & 3);
 #endif
 #endif /* LCD_PIXELFORMAT */
-        unsigned char *dst_row = &_grey_info.data[idx].value;
+        unsigned char *dst_row = _grey_info.values + idx;
         const unsigned char *src_row = src;
         const unsigned char *src_end = src + width;
 
diff --git a/apps/plugins/lib/grey_scroll.c b/apps/plugins/lib/grey_scroll.c
index 80496e7..4a18d7d 100644
--- a/apps/plugins/lib/grey_scroll.c
+++ b/apps/plugins/lib/grey_scroll.c
@@ -169,7 +169,7 @@ void grey_ub_scroll_left(int count)
         idx = _GREY_MULUQ(_grey_info.width, y & ~3) + (~y & 3);
 #endif
 #endif /* LCD_PIXELFORMAT */
-        dst = &_grey_info.data[idx].value;
+        dst = _grey_info.values + idx;
         src = dst + count * _GREY_X_ADVANCE;
         end = dst + _grey_info.width * _GREY_X_ADVANCE;
         
@@ -213,7 +213,7 @@ void grey_ub_scroll_right(int count)
         idx = _GREY_MULUQ(_grey_info.width, y & ~3) + (~y & 3);
 #endif
 #endif /* LCD_PIXELFORMAT */
-        start = &_grey_info.data[idx].value;
+        start = _grey_info.values + idx;
         dst = start + _grey_info.width * _GREY_X_ADVANCE;
         src = dst - count * _GREY_X_ADVANCE;
         
@@ -259,8 +259,8 @@ void grey_ub_scroll_up(int count)
         is = _GREY_MULUQ(_grey_info.width, ys & ~3) + (~ys & 3);
 #endif
 #endif /* LCD_PIXELFORMAT */
-        dst = &_grey_info.data[id].value;
-        src = &_grey_info.data[is].value;
+        dst = _grey_info.values + id;
+        src = _grey_info.values + is;
         dst_end = dst + _grey_info.width * _GREY_X_ADVANCE;
 
         do
@@ -282,7 +282,7 @@ void grey_ub_scroll_up(int count)
         id = _GREY_MULUQ(_grey_info.width, yd & ~3) + (~yd & 3);
 #endif
 #endif /* LCD_PIXELFORMAT */
-        dst = &_grey_info.data[id].value;
+        dst = _grey_info.values + id;
         dst_end = dst + _grey_info.width * _GREY_X_ADVANCE;
 
         do
@@ -320,8 +320,8 @@ void grey_ub_scroll_down(int count)
         is = _GREY_MULUQ(_grey_info.width, ys & ~3) + (~ys & 3);
 #endif
 #endif /* LCD_PIXELFORMAT */
-        dst = &_grey_info.data[id].value;
-        src = &_grey_info.data[is].value;
+        dst = _grey_info.values + id;
+        src = _grey_info.values + is;
         dst_end = dst + _grey_info.width * _GREY_X_ADVANCE;
 
         do
@@ -343,7 +343,7 @@ void grey_ub_scroll_down(int count)
         id = _GREY_MULUQ(_grey_info.width, yd & ~3) + (~yd & 3);
 #endif
 #endif /* LCD_PIXELFORMAT */
-        dst = &_grey_info.data[id].value;
+        dst = _grey_info.values + id;
         dst_end = dst + _grey_info.width * _GREY_X_ADVANCE;
 
         do
diff --git a/firmware/export/lcd.h b/firmware/export/lcd.h
index 57c9b1d..669ef67 100644
--- a/firmware/export/lcd.h
+++ b/firmware/export/lcd.h
@@ -131,13 +131,9 @@ extern void lcd_yuv_blit(unsigned char * const src[3],
                          int src_x, int src_y, int stride,
                          int x, int y, int width, int height);
 #else
-struct grey_data {
-    unsigned char phase; /* SH1 uses it signed (doesn't matter for high level) */
-    unsigned char value; /* 0..128 are allowed */
-} __attribute__((packed));
-extern void lcd_grey_data(const struct grey_data *data, int count); /* private */
-extern void lcd_grey_phase_blit(const struct grey_data *data, int bx, int by,
-                                int bwidth, int bheight, int stride);
+extern void lcd_grey_phase_blit(unsigned char *values, unsigned char *phases,
+                                int bx, int by, int bwidth, int bheight,
+                                int stride);
 #endif
 
 /* performance function */
diff --git a/firmware/target/arm/ipod/lcd-gray.c b/firmware/target/arm/ipod/lcd-gray.c
index c7f4074..5734480 100644
--- a/firmware/target/arm/ipod/lcd-gray.c
+++ b/firmware/target/arm/ipod/lcd-gray.c
@@ -301,118 +301,88 @@ void lcd_blit(const unsigned char* data, int bx, int y, int bwidth,
 
 /* Performance function that works with an external buffer
    note that bx and bwidth are in 8-pixel units! */
-void lcd_grey_phase_blit(const struct grey_data *data, int bx, int y, 
-                         int bwidth, int height, int stride)
+void lcd_grey_phase_blit(unsigned char *values, unsigned char *phases,
+                         int bx, int y, int bwidth, int height, int stride)
 {
-    const struct grey_data *addr;
-    int width;
-    
+    unsigned char *val, *ph;
+    int bw;
+
     while (height--) {
         lcd_cmd_and_data(R_RAM_ADDR_SET, (y++ << 5) + addr_offset - bx);
         lcd_prepare_cmd(R_RAM_DATA);
 
-        addr = data;
-        width = bwidth;
+        val = values;
+        ph = phases;
+        bw = bwidth;
         asm volatile (
         "10:                                 \n"
-            "ldmia   %[addr]!, {r0-r3}       \n" /* r0 = v1p1v0p0 ... */
+            "ldmia   %[ph], {r0-r1}          \n" /* Fetch 8 pixel phases */
+            "ldmia   %[val]!, {r2-r3}        \n" /* Fetch 8 pixel values */
 #ifdef IPOD_MINI2G
-            "mov     r5, #0x7600             \n"
+            "mov     r4, #0x7600             \n"
 #else
-            "mov     r5, #0                  \n"
+            "mov     r4, #0                  \n"
 #endif
-
-            "and     r4, r0, %[mask]         \n" /* r4 = --p1--p0 */
-            "and     r0, %[mask], r0, lsr #8 \n" /* r0 = --v1--v0 */
-            
-            "tst     r4, #0x80               \n"
-            "orreq   r5, r5, #0xc0           \n"
-            "tst     r4, #0x800000           \n"
-            "orreq   r5, r5, #0x30           \n"
-            "bic     r4, r4, %[clbt]         \n"
-            
-            "add     r4, r0, r4              \n" /* p0 += v0; p1 += v1; */
-            "strb    r4, [%[addr], #-16]     \n"
-            "mov     r4, r4, lsr #16         \n"
-            "strb    r4, [%[addr], #-14]     \n"
-
-            "and     r4, r1, %[mask]         \n"
-            "and     r1, %[mask], r1, lsr #8 \n"
-
-            "tst     r4, #0x80               \n"
-            "orreq   r5, r5, #0x0c           \n"
-            "tst     r4, #0x800000           \n"
-            "orreq   r5, r5, #0x03           \n"
-            "bic     r4, r4, %[clbt]         \n"
-            
-            "add     r4, r1, r4              \n"
-            "strb    r4, [%[addr], #-12]     \n"
-            "mov     r4, r4, lsr #16         \n"
-            "strb    r4, [%[addr], #-10]     \n"
+            "tst     r0, #0x80               \n"
+            "orreq   r4, r4, #0xc0           \n"
+            "tst     r0, #0x8000             \n"
+            "orreq   r4, r4, #0x30           \n"
+            "tst     r0, #0x800000           \n"
+            "orreq   r4, r4, #0x0c           \n"
+            "tst     r0, #0x80000000         \n"
+            "orreq   r4, r4, #0x03           \n"
+            "bic     r0, r0, %[clbt]         \n"
+            "add     r0, r0, r2              \n"
 
 #ifdef IPOD_MINI2G
-            "mov     r5, r5, lsl #8          \n"
+            "mov     r4, r4, lsl #8          \n"
 #else
         "1:                                  \n"
-            "ldr     r4, [%[lcdb]]           \n"
-            "tst     r4, #0x8000             \n"
+            "ldr     r2, [%[lcdb]]           \n"
+            "tst     r2, #0x8000             \n"
             "bne     1b                      \n"
 
-            "str     r5, [%[lcdb], #0x10]    \n"
-            "mov     r5, #0                  \n"
+            "str     r4, [%[lcdb], #0x10]    \n"
+            "mov     r4, #0                  \n"
 #endif
 
-            "and     r4, r2, %[mask]         \n"
-            "and     r2, %[mask], r2, lsr #8 \n"
-
-            "tst     r4, #0x80               \n"
-            "orreq   r5, r5, #0xc0           \n"
-            "tst     r4, #0x800000           \n"
-            "orreq   r5, r5, #0x30           \n"
-            "bic     r4, r4, %[clbt]         \n"
-            
-            "add     r4, r2, r4              \n"
-            "strb    r4, [%[addr], #-8]      \n"
-            "mov     r4, r4, lsr #16         \n"
-            "strb    r4, [%[addr], #-6]      \n"
-
-            "and     r4, r3, %[mask]         \n"
-            "and     r3, %[mask], r3, lsr #8 \n"
-
-            "tst     r4, #0x80               \n"
-            "orreq   r5, r5, #0x0c           \n"
-            "tst     r4, #0x800000           \n"
-            "orreq   r5, r5, #0x03           \n"
-            "bic     r4, r4, %[clbt]         \n"
-            
-            "add     r4, r3, r4              \n"
-            "strb    r4, [%[addr], #-4]      \n"
-            "mov     r4, r4, lsr #16         \n"
-            "strb    r4, [%[addr], #-2]      \n"
-            
+            "tst     r1, #0x80               \n"
+            "orreq   r4, r4, #0xc0           \n"
+            "tst     r1, #0x8000             \n"
+            "orreq   r4, r4, #0x30           \n"
+            "tst     r1, #0x800000           \n"
+            "orreq   r4, r4, #0x0c           \n"
+            "tst     r1, #0x80000000         \n"
+            "orreq   r4, r4, #0x03           \n"
+            "bic     r1, r1, %[clbt]         \n"
+            "add     r1, r1, r3              \n"
+
+            "stmia   %[ph]!, {r0-r1}         \n"
+
         "1:                                  \n"
-            "ldr     r4, [%[lcdb]]           \n"
-            "tst     r4, #0x8000             \n"
+            "ldr     r2, [%[lcdb]]           \n"
+            "tst     r2, #0x8000             \n"
             "bne     1b                      \n"
 #ifdef IPOD_MINI2G
-            "str     r5, [%[lcdb], #0x08]    \n"
+            "str     r4, [%[lcdb], #0x08]    \n"
 #else
-            "str     r5, [%[lcdb], #0x10]    \n"
+            "str     r4, [%[lcdb], #0x10]    \n"
 #endif
 
-            "subs    %[wdth], %[wdth], #1    \n"
+            "subs    %[bw], %[bw], #1        \n"
             "bne     10b                     \n"
             : /* outputs */
-            [addr]"+r"(addr),
-            [wdth]"+r"(width)
+            [val]"+r"(val),
+            [ph] "+r"(ph),
+            [bw] "+r"(bw)
             : /* inputs */
-            [mask]"r"(0x00ff00ff),
-            [clbt]"r"(0x00800080),
+            [clbt]"r"(0x80808080),
             [lcdb]"r"(LCD1_BASE)
             : /* clobbers */
-            "r0", "r1", "r2", "r3", "r4", "r5"
+            "r0", "r1", "r2", "r3", "r4"
         );
-        data += stride;
+        values += stride;
+        phases += stride;
     }
 }
 
diff --git a/firmware/target/coldfire/iaudio/m5/lcd-as-m5.S b/firmware/target/coldfire/iaudio/m5/lcd-as-m5.S
index 7e89815..0ec98e4 100644
--- a/firmware/target/coldfire/iaudio/m5/lcd-as-m5.S
+++ b/firmware/target/coldfire/iaudio/m5/lcd-as-m5.S
@@ -88,57 +88,146 @@ lcd_write_data:
     .type       lcd_grey_data,@function
 
 lcd_grey_data:
-    lea.l   (-4*4, %sp), %sp
-    movem.l %d2-%d5, (%sp)
-    movem.l (4*4+4, %sp), %a0-%a1 /* Data pointer */
-    move.l  %a1, %d0            /* Length */
-    lea     0xf0008002, %a1     /* LCD data port */
-    move.l  #0xff00ff00, %d2    /* mask for splitting value/phase pairs */
-
-.greyloop:
-    movem.l (%a0), %d4-%d5      /* fetch 4 pixel phase/value pairs at once */
-                                /* %d4 = p0v0p1v1, %d5 = p2v2p3v3 */
-    move.l  %d2, %d3            /* copy mask */
-    and.l   %d4, %d3            /* %d3 = p0--p1-- */
-    eor.l   %d3, %d4            /* %d4 = --v0--v1 */
-    lsr.l   #8, %d3             /* %d3 = --p0--p1 */
-
-    bclr.l  #23, %d3            /* Z = !(p0 & 0x80); p0 &= ~0x80; */
-    seq.b   %d1                 /* %d1 = ........................00000000 */
-    lsl.l   #2, %d1             /* %d1 = ......................00000000.. */
-    bclr.l  #7, %d3             /* Z = !(p1 & 0x80); p1 &= ~0x80; */
-    seq.b   %d1                 /* %d1 = ......................0011111111 */
-    lsl.l   #2, %d1             /* %d1 = ....................0011111111.. */
+    lea.l   (-9*4, %sp), %sp
+    movem.l %d2-%d5/%a2-%a6, (%sp)  /* free some registers */
+    movem.l (9*4+4, %sp), %a0-%a2   /* values, phases, length */
+    lea.l   (%a1, %a2.l*4), %a2     /* end address */
+    lea     0xf0008002, %a3     /* LCD data port */
+
+    moveq.l #15, %d3
+    add.l   %a1, %d3
+    and.l   #0xfffffff0, %d3    /* first line bound */
+    move.l  %a2, %d1
+    and.l   #0xfffffff0, %d1    /* last line bound */
+    cmp.l   %d3, %d1
+    bls.w   .g_tloop            /* no lines to copy - jump to tail loop */
+    cmp.l   %a1, %d0
+    bls.s   .g_lloop            /* no head blocks - jump to line loop */
+
+.g_hloop:
+    move.l  (%a1), %d2          /* fetch 4 pixel phases */
+
+    bclr.l  #31, %d2            /* Z = !(p0 & 0x80); p0 &= ~0x80; */
+    seq.b   %d0                 /* %d0 = ........................00000000 */
+    lsl.l   #2, %d0             /* %d0 = ......................00000000.. */
+    bclr.l  #23, %d2            /* Z = !(p1 & 0x80); p1 &= ~0x80; */
+    seq.b   %d0                 /* %d0 = ......................0011111111 */
+    lsl.l   #2, %d0             /* %d0 = ....................0011111111.. */
+    bclr.l  #15, %d2            /* Z = !(p2 & 0x80); p2 &= ~0x80; */
+    seq.b   %d0                 /* %d0 = ....................001122222222 */
+    lsl.l   #2, %d0             /* %d0 = ..................001122222222.. */
+    bclr.l  #7, %d2             /* Z = !(p3 & 0x80); p3 &= ~0x80; */
+    seq.b   %d0                 /* %d0 = ..................00112233333333 */
+    lsr.l   #6, %d0             /* %d0 = ........................00112233 */
+    move.w  %d0, (%a3)          /* write pixel block */
+    
+    add.l   (%a0)+, %d2         /* add 4 pixel values to the phases */
+    move.l  %d2, (%a1)+         /* store new phases, advance pointer */
+
+    cmp.l   %a1, %d3            /* go up to first line bound */
+    bhi.s   .g_hloop
+    
+.g_lloop:
+    movem.l (%a1), %d2-%d5
+    
+    bclr.l  #31, %d2
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #23, %d2
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #15, %d2
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #7, %d2
+    seq.b   %d0
+    lsr.l   #6, %d0
+    move.w  %d0, (%a3)
+
+    bclr.l  #31, %d3
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #23, %d3
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #15, %d3
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #7, %d3
+    seq.b   %d0
+    lsr.l   #6, %d0
+    move.w  %d0, (%a3)
     
-    add.l   %d4, %d3            /* p0 += v0; p1 += v1; */
-    move.b  %d3, (2, %a0)       /* store p1 */
-    swap    %d3
-    move.b  %d3, (%a0)          /* store p0 */
-
-    move.l  %d2, %d3            /* copy mask */
-    and.l   %d5, %d3            /* %d3 = p2--p3-- */
-    eor.l   %d3, %d5            /* %d5 = --v2--v3 */
-    lsr.l   #8, %d3             /* %d3 = --p2--p3 */
-
-    bclr.l  #23, %d3            /* Z = !(p2 & 0x80); p2 &= ~0x80; */
-    seq.b   %d1                 /* %d1 = ....................001122222222 */
-    lsl.l   #2, %d1             /* %d1 = ..................001122222222.. */
-    bclr.l  #7, %d3             /* Z = !(p3 & 0x80); p3 &= ~0x80; */
-    seq.b   %d1                 /* %d1 = ..................00112233333333 */
-    lsr.l   #6, %d1             /* %d1 = ........................00112233 */
-
-    add.l   %d5, %d3            /* p2 += v2; p3 += v3; */
-    move.b  %d3, (6, %a0)       /* store p3 */
-    swap    %d3
-    move.b  %d3, (4, %a0)       /* store p2 */
-
-    move.w  %d1, (%a1)          /* write pixel block */
-    addq.l  #8, %a0             /* advance address pointer */
-    subq.l  #1, %d0             /* any blocks left? */
-    bne.b   .greyloop
-
-    movem.l (%sp), %d2-%d5
-    lea.l   (4*4, %sp), %sp
+    bclr.l  #31, %d4
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #23, %d4
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #15, %d4
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #7, %d4
+    seq.b   %d0
+    lsr.l   #6, %d0
+    move.w  %d0, (%a3)
+    
+    bclr.l  #31, %d5
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #23, %d5
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #15, %d5
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #7, %d5
+    seq.b   %d0
+    lsr.l   #6, %d0
+    move.w  %d0, (%a3)
+    
+    movem.l (%a0), %d0/%a4-%a6
+    lea.l   (16, %a0), %a0
+    add.l   %d0, %d2
+    add.l   %a4, %d3
+    add.l   %a5, %d4
+    add.l   %a6, %d5
+    movem.l %d2-%d5, (%a1)
+    lea.l   (16, %a1), %a1
+
+    cmp.l   %a1, %d1            /* go up to last line bound */
+    bhi.w   .g_lloop
+    
+    cmp.l   %a1, %a2
+    bls.s   .g_no_tail
+
+.g_tloop:
+    move.l  (%a1), %d2
+
+    bclr.l  #31, %d2
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #23, %d2
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #15, %d2
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #7, %d2
+    seq.b   %d0
+    lsr.l   #6, %d0
+    move.w  %d0, (%a3)
+
+    add.l   (%a0)+, %d2         /* go up to end address */
+    move.l  %d2, (%a1)+
+
+    cmp.l   %a1, %a2
+    bhi.s   .g_tloop
+
+.g_no_tail:
+    movem.l (%sp), %d2-%d5/%a2-%a6  /* restore registers */
+    lea.l   (9*4, %sp), %sp
     rts
+
 .gd_end:
     .size   lcd_grey_data,.gd_end-lcd_grey_data
diff --git a/firmware/target/coldfire/iaudio/m5/lcd-m5.c b/firmware/target/coldfire/iaudio/m5/lcd-m5.c
index 4f96379..be9d5a3 100644
--- a/firmware/target/coldfire/iaudio/m5/lcd-m5.c
+++ b/firmware/target/coldfire/iaudio/m5/lcd-m5.c
@@ -171,10 +171,13 @@ void lcd_blit(const unsigned char* data, int x, int by, int width,
     }
 }
 
+/* Helper function for lcd_grey_phase_blit(). */
+void lcd_grey_data(unsigned char *values, unsigned char *phases, int count);
+
 /* Performance function that works with an external buffer
    note that by and bheight are in 4-pixel units! */
-void lcd_grey_phase_blit(const struct grey_data *data, int x, int by, 
-                         int width, int bheight, int stride)
+void lcd_grey_phase_blit(unsigned char *values, unsigned char *phases,
+                         int x, int by, int width, int bheight, int stride)
 {
     stride <<= 2; /* 4 pixels per block */
     while (bheight--)
@@ -182,8 +185,9 @@ void lcd_grey_phase_blit(const struct grey_data *data, int x, int by,
         lcd_write_command_ex(LCD_CNTL_PAGE, by++, -1);
         lcd_write_command_ex(LCD_CNTL_COLUMN, x, -1);
         lcd_write_command(LCD_CNTL_DATA_WRITE);
-        lcd_grey_data(data, width);
-        data += stride;
+        lcd_grey_data(values, phases, width);
+        values += stride;
+        phases += stride;
     }
 }
 
diff --git a/firmware/target/coldfire/iriver/h100/lcd-as-h100.S b/firmware/target/coldfire/iriver/h100/lcd-as-h100.S
index df410fa..776e22a 100644
--- a/firmware/target/coldfire/iriver/h100/lcd-as-h100.S
+++ b/firmware/target/coldfire/iriver/h100/lcd-as-h100.S
@@ -100,59 +100,148 @@ lcd_write_data:
     .type       lcd_grey_data,@function
 
 lcd_grey_data:
-    lea.l   (-4*4, %sp), %sp
-    movem.l %d2-%d5, (%sp)
-    movem.l (4*4+4, %sp), %a0-%a1 /* Data pointer */
-    move.l  %a1, %d0            /* Length */
+    lea.l   (-9*4, %sp), %sp
+    movem.l %d2-%d5/%a2-%a6, (%sp)  /* free some registers */
+    movem.l (9*4+4, %sp), %a0-%a2   /* values, phases, length */
+    lea.l   (%a1, %a2.l*4), %a2     /* end address */
     moveq   #8, %d1
     or.l    %d1, (MBAR2+0xb4)   /* A0 = 1 (data) */
-    lea     0xf0000000, %a1     /* LCD data port */
-    move.l  #0xff00ff00, %d2    /* mask for splitting value/phase pairs */
-
-.greyloop:
-    movem.l (%a0), %d4-%d5      /* fetch 4 pixel phase/value pairs at once */
-                                /* %d4 = p0v0p1v1, %d5 = p2v2p3v3 */
-    move.l  %d2, %d3            /* copy mask */
-    and.l   %d4, %d3            /* %d3 = p0--p1-- */
-    eor.l   %d3, %d4            /* %d4 = --v0--v1 */
-    lsr.l   #8, %d3             /* %d3 = --p0--p1 */
-
-    bclr.l  #23, %d3            /* Z = !(p0 & 0x80); p0 &= ~0x80; */
-    seq.b   %d1                 /* %d1 = ........................00000000 */
-    lsl.l   #2, %d1             /* %d1 = ......................00000000.. */
-    bclr.l  #7, %d3             /* Z = !(p1 & 0x80); p1 &= ~0x80; */
-    seq.b   %d1                 /* %d1 = ......................0011111111 */
-    lsl.l   #2, %d1             /* %d1 = ....................0011111111.. */
+    lea     0xf0000000, %a3     /* LCD data port */
+    
+    moveq.l #15, %d3
+    add.l   %a1, %d3
+    and.l   #0xfffffff0, %d3    /* first line bound */
+    move.l  %a2, %d1
+    and.l   #0xfffffff0, %d1    /* last line bound */
+    cmp.l   %d3, %d1
+    bls.w   .g_tloop            /* no lines to copy - jump to tail loop */
+    cmp.l   %a1, %d0
+    bls.s   .g_lloop            /* no head blocks - jump to line loop */
+
+.g_hloop:
+    move.l  (%a1), %d2          /* fetch 4 pixel phases */
+
+    bclr.l  #31, %d2            /* Z = !(p0 & 0x80); p0 &= ~0x80; */
+    seq.b   %d0                 /* %d0 = ........................00000000 */
+    lsl.l   #2, %d0             /* %d0 = ......................00000000.. */
+    bclr.l  #23, %d2            /* Z = !(p1 & 0x80); p1 &= ~0x80; */
+    seq.b   %d0                 /* %d0 = ......................0011111111 */
+    lsl.l   #2, %d0             /* %d0 = ....................0011111111.. */
+    bclr.l  #15, %d2            /* Z = !(p2 & 0x80); p2 &= ~0x80; */
+    seq.b   %d0                 /* %d0 = ....................001122222222 */
+    lsl.l   #2, %d0             /* %d0 = ..................001122222222.. */
+    bclr.l  #7, %d2             /* Z = !(p3 & 0x80); p3 &= ~0x80; */
+    seq.b   %d0                 /* %d0 = ..................00112233333333 */
+    lsr.l   #6, %d0             /* %d0 = ........................00112233 */
+    move.w  %d0, (%a3)          /* write pixel block */
+    
+    add.l   (%a0)+, %d2         /* add 4 pixel values to the phases */
+    move.l  %d2, (%a1)+         /* store new phases, advance pointer */
+
+    cmp.l   %a1, %d3            /* go up to first line bound */
+    bhi.s   .g_hloop
+    
+.g_lloop:
+    movem.l (%a1), %d2-%d5
     
-    add.l   %d4, %d3            /* p0 += v0; p1 += v1; */
-    move.b  %d3, (2, %a0)       /* store p1 */
-    swap    %d3
-    move.b  %d3, (%a0)          /* store p0 */
-
-    move.l  %d2, %d3            /* copy mask */
-    and.l   %d5, %d3            /* %d3 = p2--p3-- */
-    eor.l   %d3, %d5            /* %d5 = --v2--v3 */
-    lsr.l   #8, %d3             /* %d3 = --p2--p3 */
-
-    bclr.l  #23, %d3            /* Z = !(p2 & 0x80); p2 &= ~0x80; */
-    seq.b   %d1                 /* %d1 = ....................001122222222 */
-    lsl.l   #2, %d1             /* %d1 = ..................001122222222.. */
-    bclr.l  #7, %d3             /* Z = !(p3 & 0x80); p3 &= ~0x80; */
-    seq.b   %d1                 /* %d1 = ..................00112233333333 */
-    lsr.l   #6, %d1             /* %d1 = ........................00112233 */
-
-    add.l   %d5, %d3            /* p2 += v2; p3 += v3; */
-    move.b  %d3, (6, %a0)       /* store p3 */
-    swap    %d3
-    move.b  %d3, (4, %a0)       /* store p2 */
-
-    move.w  %d1, (%a1)          /* write pixel block */
-    addq.l  #8, %a0             /* advance address pointer */
-    subq.l  #1, %d0             /* any blocks left? */
-    bne.b   .greyloop
-
-    movem.l (%sp), %d2-%d5
-    lea.l   (4*4, %sp), %sp
+    bclr.l  #31, %d2
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #23, %d2
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #15, %d2
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #7, %d2
+    seq.b   %d0
+    lsr.l   #6, %d0
+    move.w  %d0, (%a3)
+
+    bclr.l  #31, %d3
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #23, %d3
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #15, %d3
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #7, %d3
+    seq.b   %d0
+    lsr.l   #6, %d0
+    move.w  %d0, (%a3)
+    
+    bclr.l  #31, %d4
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #23, %d4
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #15, %d4
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #7, %d4
+    seq.b   %d0
+    lsr.l   #6, %d0
+    move.w  %d0, (%a3)
+    
+    bclr.l  #31, %d5
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #23, %d5
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #15, %d5
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #7, %d5
+    seq.b   %d0
+    lsr.l   #6, %d0
+    move.w  %d0, (%a3)
+    
+    movem.l (%a0), %d0/%a4-%a6
+    lea.l   (16, %a0), %a0
+    add.l   %d0, %d2
+    add.l   %a4, %d3
+    add.l   %a5, %d4
+    add.l   %a6, %d5
+    movem.l %d2-%d5, (%a1)
+    lea.l   (16, %a1), %a1
+
+    cmp.l   %a1, %d1            /* go up to last line bound */
+    bhi.w   .g_lloop
+    
+    cmp.l   %a1, %a2
+    bls.s   .g_no_tail
+
+.g_tloop:
+    move.l  (%a1), %d2
+
+    bclr.l  #31, %d2
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #23, %d2
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #15, %d2
+    seq.b   %d0
+    lsl.l   #2, %d0
+    bclr.l  #7, %d2
+    seq.b   %d0
+    lsr.l   #6, %d0
+    move.w  %d0, (%a3)
+
+    add.l   (%a0)+, %d2
+    move.l  %d2, (%a1)+
+
+    cmp.l   %a1, %a2            /* go up to end address */
+    bhi.s   .g_tloop
+
+.g_no_tail:
+    movem.l (%sp), %d2-%d5/%a2-%a6  /* restore registers */
+    lea.l   (9*4, %sp), %sp
     rts
+
 .gd_end:
     .size   lcd_grey_data,.gd_end-lcd_grey_data
diff --git a/firmware/target/coldfire/iriver/h100/lcd-h100.c b/firmware/target/coldfire/iriver/h100/lcd-h100.c
index c17de95..2ba1925 100644
--- a/firmware/target/coldfire/iriver/h100/lcd-h100.c
+++ b/firmware/target/coldfire/iriver/h100/lcd-h100.c
@@ -180,10 +180,13 @@ void lcd_blit(const unsigned char* data, int x, int by, int width,
     }
 }
 
+/* Helper function for lcd_grey_phase_blit(). */
+void lcd_grey_data(unsigned char *values, unsigned char *phases, int count);
+
 /* Performance function that works with an external buffer
    note that by and bheight are in 4-pixel units! */
-void lcd_grey_phase_blit(const struct grey_data *data, int x, int by, 
-                         int width, int bheight, int stride)
+void lcd_grey_phase_blit(unsigned char *values, unsigned char *phases,
+                         int x, int by, int width, int bheight, int stride)
 {
     stride <<= 2; /* 4 pixels per block */
     while (bheight--)
@@ -191,8 +194,9 @@ void lcd_grey_phase_blit(const struct grey_data *data, int x, int by,
         lcd_write_command_ex(LCD_CNTL_PAGE, by++, -1);
         lcd_write_command_ex(LCD_CNTL_COLUMN, x, -1);
         lcd_write_command(LCD_CNTL_DATA_WRITE);
-        lcd_grey_data(data, width);
-        data += stride;
+        lcd_grey_data(values, phases, width);
+        values += stride;
+        phases += stride;
     }
 }
 
diff --git a/firmware/target/sh/archos/lcd-archos-bitmap.c b/firmware/target/sh/archos/lcd-archos-bitmap.c
index 17c4d76..18d48f4 100644
--- a/firmware/target/sh/archos/lcd-archos-bitmap.c
+++ b/firmware/target/sh/archos/lcd-archos-bitmap.c
@@ -155,10 +155,13 @@ void lcd_blit(const unsigned char* data, int x, int by, int width,
     }
 }
 
+/* Helper function for lcd_grey_phase_blit(). */
+void lcd_grey_data(unsigned char *values, unsigned char *phases, int count);
+
 /* Performance function that works with an external buffer
    note that by and bheight are in 8-pixel units! */
-void lcd_grey_phase_blit(const struct grey_data *data, int x, int by,
-                         int width, int bheight, int stride)
+void lcd_grey_phase_blit(unsigned char *values, unsigned char *phases,
+                         int x, int by, int width, int bheight, int stride)
 {
     stride <<= 3; /* 8 pixels per block */
     while (bheight--)
@@ -167,8 +170,9 @@ void lcd_grey_phase_blit(const struct grey_data *data, int x, int by,
         lcd_write_command (LCD_CNTL_HIGHCOL | (((x+xoffset)>>4) & 0xf));
         lcd_write_command (LCD_CNTL_LOWCOL | ((x+xoffset) & 0xf));
 
-        lcd_grey_data(data, width);
-        data += stride;
+        lcd_grey_data(values, phases, width);
+        values += stride;
+        phases += stride;
     }
 }
 
diff --git a/firmware/target/sh/archos/lcd-as-archos-bitmap.S b/firmware/target/sh/archos/lcd-as-archos-bitmap.S
index 492413f..a60ea9a 100644
--- a/firmware/target/sh/archos/lcd-as-archos-bitmap.S
+++ b/firmware/target/sh/archos/lcd-as-archos-bitmap.S
@@ -210,28 +210,35 @@ _lcd_write_data:
  * one or multiple pixels.
  *
  * Arguments:
- *   r4 - data address, (phase,value)-pairs
- *   r5 - pixel block count
+ *   r4 - pixel value data address
+ *   r5 - pixel phase data address
+ *   r6 - pixel block count
  *
  * Register usage:
- *   r0 - current pixel value
+ *   r0 - scratch / phase signs mask
  *   r1 - scratch
  *   r2 - precalculated port value (CS and SC low, DS and SD high),
  *        negated (neg)!
  *   r3 - lcd port address
- *   r5 - end address
- *   r6/r7 - current/next pixel phase
- *   r8 - current block address (for writing back phase)
- *   r9 - 0x80 (for phase modification)
+ *   r4 - current value address
+ *   r5 - current phase address
+ *   r6 - end address
+ *   r7/r8 - current/next pixel phase
+ *   r9 - current pixel value
+ *   r10  - 0x00000080 \
+ *   r11 - 0x00008000  > for phase sign check
+ *   r12 - 0x00800000 /
  */
 
 _lcd_grey_data:
     mov.l   r8, @-r15       /* save r8 */
-    shll2   r5              /*    v */
+    shll2   r6              /*    v */
     mov.l   r9, @-r15       /* save r9 */
-    shll2   r5              /* r5 *= 16; (8 pixel per block * 2 bytes/pixel) */
+    shll    r6              /* r6 *= 8; (8 pixels per block) */
+    mov.l   r10, @-r15      /* save r10 */
+    add     r4, r6          /* end address */
     mov.l   .lcdr, r3       /* put lcd data port address in r3 */
-    add     r4, r5          /* end address */
+    nop                     /* keep alignment */
 
     /* This code will fail if an interrupt changes the contents of PBDRL.
      * If so, we must disable the interrupt here. If disabling interrupts
@@ -240,110 +247,90 @@ _lcd_grey_data:
      * this would significantly decrease performance. */
 
     mov.b   @r3, r0         /* r0 = PBDRL */
-    mov     #0x80, r9       /* for phase modification - "or #imm,xx" only allows r0 */
-    mov.b   @r4+, r6        /* fetch first pixel phase */
     or      #(LCD_DS|LCD_SD), r0    /* r0 |= LCD_DS|LCD_SD */
+    mov.l   r11, @-r15      /* save r11 */
     and     #(~(LCD_CS|LCD_SC)), r0 /* r0 &= ~(LCD_CS|LCD_SC) */
+    mov.l   r12, @-r15      /* save r12 */
     neg     r0, r2          /* r2 = 0 - r0 */
-    mov     #-3, r0         /* offset for storing phase */
 
     /* loop exploits that SD is on bit 0 for recorders and Ondios */
 
-.greyloop:
-    cmp/pz  r6              /* phase non-negative? */
-    mov.b   @r4+, r8        /* fetch pixel value */
-    negc    r2, r1          /* T -> SD, SC low */
-    mov.b   r1, @r3         /* set port */
-    or      r9, r6          /* r6 -= (r6 >= 0) ? 128 : 0; */
-    mov.b   @r4+, r7        /* fetch next pixel phase */
-    add     #(LCD_SC), r1   /* rise SC */
-    mov.b   r1, @r3         /* set port */
-    add     r8, r6          /* calculate new phase */
-    mov.b   r6, @(r0,r4)    /* store phase */
+    mov.w   .ptest, r10
+    swap.b  r10, r11
+    mov.l   @r5, r7
+    swap.w  r10, r12
+    mov.l   .pmask, r0
 
+.greyloop:
     cmp/pz  r7
-    mov.b   @r4+, r8
+    mov.l   @r4+, r9
     negc    r2, r1
     mov.b   r1, @r3
-    or      r9, r7
-    mov.b   @r4+, r6
     add     #(LCD_SC), r1
     mov.b   r1, @r3
-    add     r8, r7
-    mov.b   r7, @(r0,r4)
 
-    cmp/pz  r6
-    mov.b   @r4+, r8
+    tst     r12, r7
+    mov.l   @(4,r5), r8
     negc    r2, r1
     mov.b   r1, @r3
-    or      r9, r6
-    mov.b   @r4+, r7
     add     #(LCD_SC), r1
     mov.b   r1, @r3
-    add     r8, r6
-    mov.b   r6, @(r0,r4)
 
-    cmp/pz  r7
-    mov.b   @r4+, r8
+    tst     r11, r7
     negc    r2, r1
+    tst     r10, r7
     mov.b   r1, @r3
-    or      r9, r7
-    mov.b   @r4+, r6
     add     #(LCD_SC), r1
     mov.b   r1, @r3
-    add     r8, r7
-    mov.b   r7, @(r0,r4)
 
-    cmp/pz  r6
-    mov.b   @r4+, r8
+    or      r0, r7
+    sub     r9, r7
     negc    r2, r1
     mov.b   r1, @r3
-    or      r9, r6
-    mov.b   @r4+, r7
     add     #(LCD_SC), r1
     mov.b   r1, @r3
-    add     r8, r6
-    mov.b   r6, @(r0,r4)
 
-    cmp/pz  r7
-    mov.b   @r4+, r8
+    cmp/pz  r8
+    mov.l   r7, @r5
     negc    r2, r1
     mov.b   r1, @r3
-    or      r9, r7
-    mov.b   @r4+, r6
     add     #(LCD_SC), r1
     mov.b   r1, @r3
-    add     r8, r7
-    mov.b   r7, @(r0,r4)
 
-    cmp/pz  r6
-    mov.b   @r4+, r8
+    tst     r12, r8
+    mov.l   @r4+, r9
     negc    r2, r1
     mov.b   r1, @r3
-    or      r9, r6
-    mov.b   @r4+, r7
     add     #(LCD_SC), r1
     mov.b   r1, @r3
-    add     r8, r6
-    mov.b   r6, @(r0,r4)
 
-    cmp/pz  r7
-    mov.b   @r4+, r8
+    tst     r11, r8
+    mov.l   @(8,r5), r7
     negc    r2, r1
     mov.b   r1, @r3
-    or      r9, r7
-    mov.b   @r4+, r6
     add     #(LCD_SC), r1
     mov.b   r1, @r3
-    add     r8, r7
-    mov.b   r7, @(r0,r4)
 
-    cmp/hi  r4, r5          /* some blocks left? */
+    tst     r10, r8
+    or      r0, r8
+    negc    r2, r1
+    mov.b   r1, @r3
+    add     #(LCD_SC), r1
+    mov.b   r1, @r3
+
+    sub     r9, r8
+    mov.l   r8, @(4,r5)
+    
+    add     #8, r5
+    cmp/hi  r4, r6
     bt      .greyloop
 
+    mov.l   @r15+, r12      /* restore r12 */
     mov     #(LCD_CS|LCD_DS|LCD_SD|LCD_SC), r0
-    mov.l   @r15+, r9       /* restore r9 */
+    mov.l   @r15+, r11      /* restore r11 */
     or      r0, r1          /* restore port */
+    mov.l   @r15+, r10      /* restore r10 */
+    mov.l   @r15+, r9       /* restore r9 */
     mov.l   @r15+, r8       /* restore r8 */
     rts
     mov.b   r1, @r3
@@ -351,7 +338,11 @@ _lcd_grey_data:
     /* This is the place to reenable the interrupts, if we have disabled
      * them. See above. */
 
+.ptest:
+    .short  0x0080
 
     .align  2
 .lcdr:
     .long   LCDR
+.pmask:
+    .long   0x80808080