iAudio M3: Optimised LCD driver, with more/better assembly code. Speedup is ~80% when boosted, ~15% when unboosted. Also implemented grey phase blitting. * Adapted the greyscale library, and the plugins using it. * Fixed a bug in greyscale scroll down for vertically packed pixels.

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

20 files changed, 744 insertions, 315 deletions

diff --git a/apps/plugins/SOURCES b/apps/plugins/SOURCES
index 23c22f9826..b6603a44c0 100644
--- a/apps/plugins/SOURCES
+++ b/apps/plugins/SOURCES
@@ -46,18 +46,16 @@ flipit.c
 #ifdef HAVE_LCD_BITMAP             /* Not for the Player */
 brickmania.c
 maze.c
-mazezam.c
+mazezam.c      
 text_editor.c
 wavview.c
 robotfindskitten.c
 
 /* Plugins needing the grayscale lib on low-depth LCDs */
-#ifndef IAUDIO_M3 /* FIXME: no greyscale library yet */
 fire.c
 jpeg.c
 mandelbrot.c
 plasma.c
-#endif
 
 blackjack.c
 bounce.c
diff --git a/apps/plugins/SUBDIRS b/apps/plugins/SUBDIRS
index 9bda7c2715..b0bffc73eb 100644
--- a/apps/plugins/SUBDIRS
+++ b/apps/plugins/SUBDIRS
@@ -34,7 +34,7 @@ pacbox
 
 /* For all the color targets */
 #if defined(HAVE_LCD_COLOR) || \
-    (LCD_DEPTH == 2) && !defined(ARCHOS_AV300) && !defined(IAUDIO_M3)
+    (LCD_DEPTH == 2) && !defined(ARCHOS_AV300)
 doom
 #endif
 
@@ -42,9 +42,7 @@ doom
 #if CONFIG_CODEC == SWCODEC
 midi
 /* beatbox */
-#ifndef IAUDIO_M3 /* TODO: no greyscale lib yet */
 mpegplayer
 #endif
-#endif
 
 #endif /* IRIVER_IFP7XX_SERIES */
diff --git a/apps/plugins/doom/i_video.c b/apps/plugins/doom/i_video.c
index bc0f204d3b..ce4b670583 100644
--- a/apps/plugins/doom/i_video.c
+++ b/apps/plugins/doom/i_video.c
@@ -235,6 +235,16 @@ void I_ShutdownGraphics(void)
 #define DOOMBUTTON_ESC     BUTTON_OFF
 #define DOOMBUTTON_ENTER   BUTTON_SELECT
 #define DOOMBUTTON_WEAPON  BUTTON_ON
+#elif CONFIG_KEYPAD == IAUDIO_M3_PAD
+#define DOOMBUTTON_ESC        BUTTON_RC_REC
+#define DOOMBUTTON_UP         BUTTON_RC_VOL_UP
+#define DOOMBUTTON_DOWN       BUTTON_RC_VOL_DOWN
+#define DOOMBUTTON_LEFT       BUTTON_RC_REW
+#define DOOMBUTTON_RIGHT      BUTTON_RC_FF
+#define DOOMBUTTON_OPEN       BUTTON_RC_PLAY
+#define DOOMBUTTON_SHOOT      BUTTON_RC_MODE
+#define DOOMBUTTON_ENTER      BUTTON_RC_PLAY
+#define DOOMBUTTON_WEAPON     BUTTON_RC_MENU
 #elif CONFIG_KEYPAD == COWOND2_PAD
 #define DOOMBUTTON_ESC        BUTTON_POWER
 #define DOOMBUTTON_UP         BUTTON_UP
diff --git a/apps/plugins/greyscale.c b/apps/plugins/greyscale.c
index 522a81fc28..42ba08f22e 100644
--- a/apps/plugins/greyscale.c
+++ b/apps/plugins/greyscale.c
@@ -78,6 +78,17 @@ PLUGIN_HEADER
 #define GREYSCALE_LEFT BUTTON_LEFT
 #define GREYSCALE_RIGHT BUTTON_RIGHT
 #define GREYSCALE_OFF BUTTON_EQ
+
+#elif CONFIG_KEYPAD == IAUDIO_M3_PAD
+#define GREYSCALE_SHIFT BUTTON_RC_PLAY /* somewhat dangerous... */
+#define GREYSCALE_UP BUTTON_RC_VOL_UP
+#define GREYSCALE_DOWN BUTTON_RC_VOL_DOWN
+#define GREYSCALE_LEFT BUTTON_RC_REW
+#define GREYSCALE_RIGHT BUTTON_RC_FF
+#define GREYSCALE_OFF BUTTON_RC_REC
+
+#define GREYSCALE_RC_OFF BUTTON_REC
+
 #endif
 
 #define GFX_HEIGHT (LCD_HEIGHT-8)
diff --git a/apps/plugins/jpeg.c b/apps/plugins/jpeg.c
index e2c1b4861b..f128558c5a 100644
--- a/apps/plugins/jpeg.c
+++ b/apps/plugins/jpeg.c
@@ -198,6 +198,18 @@ PLUGIN_HEADER
 #define JPEG_NEXT (BUTTON_DISPLAY | BUTTON_RIGHT)
 #define JPEG_PREVIOUS (BUTTON_DISPLAY | BUTTON_LEFT)
 
+#elif CONFIG_KEYPAD == IAUDIO_M3_PAD
+#define JPEG_ZOOM_PRE    BUTTON_RC_PLAY
+#define JPEG_ZOOM_IN     (BUTTON_RC_PLAY|BUTTON_REL)
+#define JPEG_ZOOM_OUT    (BUTTON_RC_PLAY|BUTTON_REPEAT)
+#define JPEG_UP          BUTTON_RC_VOL_UP
+#define JPEG_DOWN        BUTTON_RC_VOL_DOWN
+#define JPEG_LEFT        BUTTON_RC_REW
+#define JPEG_RIGHT       BUTTON_RC_FF
+#define JPEG_MENU        BUTTON_RC_REC
+#define JPEG_NEXT        BUTTON_RC_MODE
+#define JPEG_PREVIOUS    BUTTON_RC_MENU
+
 #elif CONFIG_KEYPAD == COWOND2_PAD
 #define JPEG_ZOOM_IN    BUTTON_PLUS
 #define JPEG_ZOOM_OUT   BUTTON_MINUS
diff --git a/apps/plugins/lib/SOURCES b/apps/plugins/lib/SOURCES
index f1aded6122..6a44c80ffa 100644
--- a/apps/plugins/lib/SOURCES
+++ b/apps/plugins/lib/SOURCES
@@ -3,8 +3,7 @@ configfile.c
 fixedpoint.c
 playback_control.c
 rgb_hsv.c
-#if defined(HAVE_LCD_BITMAP) && (LCD_DEPTH < 4) \
- && !defined(IAUDIO_M3) /* TODO: Test whether it can be implemented */
+#if defined(HAVE_LCD_BITMAP) && (LCD_DEPTH < 4)
 grey_core.c
 grey_draw.c
 grey_parm.c
diff --git a/apps/plugins/lib/grey.h b/apps/plugins/lib/grey.h
index ce37e17829..4298ae3ada 100644
--- a/apps/plugins/lib/grey.h
+++ b/apps/plugins/lib/grey.h
@@ -134,8 +134,8 @@ void grey_ub_scroll_down(int count);
 
 #if LCD_PIXELFORMAT == HORIZONTAL_PACKING
 #define _GREY_BSHIFT 0
-#else
-#if LCD_DEPTH == 1
+#else /* vertical packing or vertical interleaved */
+#if (LCD_DEPTH == 1) || (LCD_PIXELFORMAT == VERTICAL_INTERLEAVED)
 #define _GREY_BSHIFT 3
 #elif LCD_DEPTH == 2
 #define _GREY_BSHIFT 2
@@ -155,7 +155,7 @@ struct _grey_info
 #if LCD_PIXELFORMAT == HORIZONTAL_PACKING
     int bx;         /* 8-pixel units */
     int bwidth;     /* 8-pixel units */
-#else /* vertical packing */
+#else /* vertical packing or vertical interleaved */
     int by;         /* 4-pixel or 8-pixel units */
     int bheight;    /* 4-pixel or 8-pixel units */
 #endif
diff --git a/apps/plugins/lib/grey_coldfire.S b/apps/plugins/lib/grey_coldfire.S
index 39df087a85..a040193956 100644
--- a/apps/plugins/lib/grey_coldfire.S
+++ b/apps/plugins/lib/grey_coldfire.S
@@ -31,7 +31,14 @@
     .global     _grey_line1
     .type       _grey_line1, @function
     
-#if (LCD_PIXELFORMAT == VERTICAL_PACKING) && (LCD_DEPTH == 2)
+#if (LCD_PIXELFORMAT == VERTICAL_PACKING) \
+ || (LCD_PIXELFORMAT == VERTICAL_INTERLEAVED)
+ 
+#if (LCD_DEPTH == 1) || (LCD_PIXELFORMAT == VERTICAL_INTERLEAVED)
+#define GREY_BSIZE 8
+#elif LCD_DEPTH == 2
+#define GREY_BSIZE 4
+#endif
     
 /****************************************************************************
  * void _grey_line1(int width,
@@ -52,7 +59,7 @@ _grey_line1:
 
     move.b  (%a1)+, %d0
     move.b  (%d0.l, %a2), (%a0)
-    addq.l  #4, %a0
+    addq.l  #GREY_BSIZE, %a0
     subq.l  #1, %d2
 .p1_h_end:
 
@@ -66,10 +73,10 @@ _grey_line1:
     move.w  %d1, %d0
     lsr.l   #8, %d0
     move.b  (%d0.l, %a2), (%a0)
-    addq.l  #4, %a0
+    addq.l  #GREY_BSIZE, %a0
     move.b  %d1, %d0
     move.b  (%d0.l, %a2), (%a0)
-    addq.l  #4, %a0
+    addq.l  #GREY_BSIZE, %a0
     subq.l  #2, %d2
 .p2_h_end:
 
@@ -82,18 +89,18 @@ _grey_line1:
     move.w  %d1, %d0
     lsr.l   #8, %d0
     move.b  (%d0.l, %a2), (%a0)
-    addq.l  #4, %a0
+    addq.l  #GREY_BSIZE, %a0
     move.b  %d1, %d0
     move.b  (%d0.l, %a2), (%a0)
-    addq.l  #4, %a0
+    addq.l  #GREY_BSIZE, %a0
     swap    %d1
     move.w  %d1, %d0
     lsr.l   #8, %d0
     move.b  (%d0.l, %a2), (%a0)
-    addq.l  #4, %a0
+    addq.l  #GREY_BSIZE, %a0
     move.b  %d1, %d0
     move.b  (%d0.l, %a2), (%a0)
-    addq.l  #4, %a0
+    addq.l  #GREY_BSIZE, %a0
     subq.l  #4, %d2
     bhs.s   .p4_loop
     
@@ -107,10 +114,10 @@ _grey_line1:
     move.w  %d1, %d0
     lsr.l   #8, %d0
     move.b  (%d0.l, %a2), (%a0)
-    addq.l  #4, %a0
+    addq.l  #GREY_BSIZE, %a0
     move.b  %d1, %d0
     move.b  (%d0.l, %a2), (%a0)
-    addq.l  #4, %a0
+    addq.l  #GREY_BSIZE, %a0
 .p2_t_end:
 
     btst.l  #0, %d2
diff --git a/apps/plugins/lib/grey_core.c b/apps/plugins/lib/grey_core.c
index caa7af2f25..8a30faa5f8 100644
--- a/apps/plugins/lib/grey_core.c
+++ b/apps/plugins/lib/grey_core.c
@@ -26,7 +26,8 @@
 #include "plugin.h"
 #include "grey.h"
 
-#if defined(CPU_PP) && defined(HAVE_ADJUSTABLE_CPU_FREQ)
+#if defined(HAVE_ADJUSTABLE_CPU_FREQ) && \
+    (defined(CPU_PP) || (CONFIG_LCD == LCD_TL0350A))
 #define NEED_BOOST
 #endif
 
@@ -143,6 +144,42 @@ static const unsigned char lcdlinear[256] = {
     203, 206, 209, 212, 215, 219, 222, 226,
     229, 233, 236, 240, 244, 248, 251, 255
 };
+#elif CONFIG_LCD == LCD_TL0350A
+/* measured and interpolated curve for iaudio remote */
+static const unsigned char lcdlinear[256] = {
+      5,   9,  13,  17,  21,  25,  29,  33,
+     36,  39,  42,  45,  48,  51,  54,  57,
+     60,  62,  65,  67,  70,  72,  75,  77,
+     80,  82,  84,  86,  87,  89,  91,  93,
+     94,  95,  96,  97,  97,  98,  99,  99,
+    100, 100, 101, 102, 103, 103, 104, 105,
+    106, 106, 107, 108, 108, 109, 110, 111,
+    112, 112, 113, 113, 114, 114, 115, 115,
+    116, 116, 117, 117, 118, 118, 119, 119,
+    120, 120, 121, 121, 122, 122, 123, 123,
+    124, 124, 124, 125, 125, 126, 126, 126,
+    127, 127, 127, 128, 128, 129, 129, 129,
+    130, 130, 131, 131, 132, 132, 133, 133,
+    134, 134, 135, 135, 136, 136, 137, 137,
+    138, 138, 139, 139, 140, 140, 141, 141,
+    142, 142, 143, 143, 144, 144, 145, 145,
+    146, 146, 147, 147, 148, 149, 149, 150,
+    151, 151, 152, 152, 153, 154, 154, 155,
+    156, 156, 157, 157, 158, 159, 159, 160,
+    161, 161, 162, 163, 164, 164, 165, 166,
+    167, 167, 168, 169, 170, 170, 171, 172,
+    173, 173, 174, 175, 176, 176, 177, 178,
+    179, 179, 180, 181, 182, 182, 183, 184,
+    185, 186, 187, 188, 188, 189, 191, 191,
+    192, 193, 194, 195, 195, 196, 197, 198,
+    199, 200, 201, 202, 203, 204, 205, 206,
+    207, 208, 209, 210, 211, 212, 213, 214,
+    215, 216, 217, 218, 219, 220, 222, 223,
+    224, 225, 226, 227, 228, 229, 230, 231,
+    232, 233, 234, 235, 236, 237, 238, 239,
+    240, 240, 241, 242, 243, 243, 244, 245,
+    246, 246, 247, 248, 249, 250, 251, 252
+};
 #endif
 #else /* SIMULATOR */
 /* undo a (generic) PC display gamma of 2.0 to simulate target behaviour */
@@ -225,7 +262,7 @@ static unsigned long _grey_get_pixel(int x, int y)
     int yg = y - _grey_info.y;
 #if LCD_PIXELFORMAT == HORIZONTAL_PACKING
     int idx = _grey_info.width * yg + xg;
-#else
+#else /* vertical packing or vertical interleaved */
     int idx = _grey_info.width * (yg & ~_GREY_BMASK) 
             + (xg << _GREY_BSHIFT) + (~yg & _GREY_BMASK);
 #endif
@@ -243,7 +280,7 @@ static void _timer_isr(void)
                                        _grey_info.bx, _grey_info.y,
                                        _grey_info.bwidth, _grey_info.height,
                                        _grey_info.width);
-#else
+#else /* vertical packing or vertical interleaved */
     _grey_info.rb->lcd_blit_grey_phase(_grey_info.values, _grey_info.phases,
                                        _grey_info.x, _grey_info.by,
                                        _grey_info.width, _grey_info.bheight,
@@ -355,8 +392,8 @@ bool grey_init(struct plugin_api* newrb, unsigned char *gbuf, long gbuf_size,
 #if LCD_PIXELFORMAT == HORIZONTAL_PACKING
     bdim = (width + 7) >> 3;
     width = bdim << 3;
-#else /* vertical packing */
-#if LCD_DEPTH == 1
+#else /* vertical packing or vertical interleaved */
+#if (LCD_DEPTH == 1) || (LCD_PIXELFORMAT == VERTICAL_INTERLEAVED)
     bdim = (height + 7) >> 3;
     height = bdim << 3;
 #elif LCD_DEPTH == 2
@@ -408,7 +445,7 @@ bool grey_init(struct plugin_api* newrb, unsigned char *gbuf, long gbuf_size,
 #if LCD_PIXELFORMAT == HORIZONTAL_PACKING
     _grey_info.bx = 0;
     _grey_info.bwidth = bdim;
-#else
+#else /* vertical packing or vertical interleaved */
     _grey_info.by = 0;
     _grey_info.bheight = bdim;
 #endif
@@ -491,6 +528,9 @@ void grey_show(bool enable)
         _grey_info.rb->timer_register(1, NULL, TIMER_FREQ / 83, 1, _timer_isr);
 #elif CONFIG_LCD == LCD_MROBE100
         _grey_info.rb->timer_register(1, NULL, TIMER_FREQ / 83, 1, _timer_isr); /* not calibrated/tested */
+#elif CONFIG_LCD == LCD_TL0350A
+        _grey_info.rb->timer_register(1, NULL, TIMER_FREQ / 75, 1, _timer_isr); /* verified */
+        /* This is half of the actual frame frequency, but 150Hz is too much */
 #endif /* CONFIG_LCD */
 #endif /* !SIMULATOR */
         _grey_info.rb->screen_dump_set_hook(grey_screendump_hook);
@@ -607,8 +647,11 @@ static void grey_screendump_hook(int fd)
 #elif LCD_DEPTH == 2
     int shift;
 #endif
-#endif /* LCD_PIXELFORMAT == VERTICAL_PACKING */
-    unsigned char *lcdptr;
+#elif LCD_PIXELFORMAT == VERTICAL_INTERLEAVED
+    unsigned data;
+    int shift;
+#endif /* LCD_PIXELFORMAT */
+    fb_data *lcdptr;
     unsigned char *clut_entry;
     unsigned char linebuf[MAX(4*BMP_VARCOLORS,BMP_LINESIZE)];
 
@@ -660,7 +703,7 @@ static void grey_screendump_hook(int fd)
             lcdptr++;
         }
 #endif /* LCD_DEPTH */
-#else /* LCD_PIXELFORMAT == VERTICAL_PACKING */
+#elif LCD_PIXELFORMAT == VERTICAL_PACKING
 #if LCD_DEPTH == 1
         mask = 1 << (y & 7);
         lcdptr = _grey_info.rb->lcd_framebuffer + _GREY_MULUQ(LCD_WIDTH, y >> 3);
@@ -708,6 +751,32 @@ static void grey_screendump_hook(int fd)
             lcdptr++;
         }
 #endif /* LCD_DEPTH */
+#elif LCD_PIXELFORMAT == VERTICAL_INTERLEAVED
+#if LCD_DEPTH == 2
+        shift = y & 7;
+        lcdptr = _grey_info.rb->lcd_framebuffer + _GREY_MULUQ(LCD_WIDTH, y >> 3);
+
+        for (x = 0; x < LCD_WIDTH; x++)
+        {
+            gx = x - _grey_info.x;
+
+            if (((unsigned)gy < (unsigned)_grey_info.height)
+                && ((unsigned)gx < (unsigned)_grey_info.width))
+            {
+                linebuf[x] = BMP_FIXEDCOLORS
+                           + _grey_info.values[_GREY_MULUQ(_grey_info.width,
+                                                           gy & ~_GREY_BMASK)
+                                               + (gx << _GREY_BSHIFT)
+                                               + (~gy & _GREY_BMASK)];
+            }
+            else
+            {
+                data = (*lcdptr >> shift) & 0x0101;
+                linebuf[x] = ((data >> 7) | data) & 3;
+            }
+            lcdptr++;
+        }
+#endif /* LCD_DEPTH */
 #endif /* LCD_PIXELFORMAT */
 
         _grey_info.rb->write(fd, linebuf, BMP_LINESIZE);
diff --git a/apps/plugins/lib/grey_draw.c b/apps/plugins/lib/grey_draw.c
index 7b24ba4c70..6c2508327b 100644
--- a/apps/plugins/lib/grey_draw.c
+++ b/apps/plugins/lib/grey_draw.c
@@ -630,14 +630,14 @@ void grey_ub_gray_bitmap_part(const unsigned char *src, int src_x, int src_y,
     {
 #if LCD_PIXELFORMAT == HORIZONTAL_PACKING
         int idx = _GREY_MULUQ(_grey_info.width, yc);
-#else
+#else /* vertical packing or vertical interleaved */
         int idx = _GREY_MULUQ(_grey_info.width, yc & ~_GREY_BMASK)
                 + (~yc & _GREY_BMASK);
 #endif /* LCD_PIXELFORMAT */
 
-#if (LCD_PIXELFORMAT == VERTICAL_PACKING) && \
-     ((LCD_DEPTH == 2) && defined(CPU_COLDFIRE) \
-      || (LCD_DEPTH == 1) && (CONFIG_CPU == SH7034))
+#if ((LCD_PIXELFORMAT == VERTICAL_PACKING) && (LCD_DEPTH == 1) && (CONFIG_CPU == SH7034)) \
+ || ((LCD_PIXELFORMAT == VERTICAL_PACKING) && (LCD_DEPTH == 2) && defined(CPU_COLDFIRE)) \
+ || ((LCD_PIXELFORMAT == VERTICAL_INTERLEAVED) && defined(CPU_COLDFIRE))
         _grey_line1(width, dst + idx, src, _grey_info.gvalue);
 #else
         unsigned char *dst_row = dst + idx;
diff --git a/apps/plugins/lib/grey_parm.c b/apps/plugins/lib/grey_parm.c
index 63d09dfd72..e4aaec3efd 100644
--- a/apps/plugins/lib/grey_parm.c
+++ b/apps/plugins/lib/grey_parm.c
@@ -34,8 +34,8 @@ void grey_set_position(int x, int y)
 #if LCD_PIXELFORMAT == HORIZONTAL_PACKING
     _grey_info.bx = (x + 4) >> 3;
     x = 8 * _grey_info.bx;
-#else
-#if LCD_DEPTH == 1
+#else /* vertical packing or vertical interleaved */
+#if (LCD_DEPTH == 1) || (LCD_PIXELFORMAT == VERTICAL_INTERLEAVED)
     _grey_info.by = (y + 4) >> 3;
     y = 8 * _grey_info.by;
 #elif LCD_DEPTH == 2
diff --git a/apps/plugins/lib/grey_scroll.c b/apps/plugins/lib/grey_scroll.c
index 12a27daf23..56021d149a 100644
--- a/apps/plugins/lib/grey_scroll.c
+++ b/apps/plugins/lib/grey_scroll.c
@@ -191,7 +191,8 @@ void grey_ub_scroll_up(int count)
                               _grey_info.fg_brightness :
                               _grey_info.bg_brightness];
 
-#if LCD_PIXELFORMAT == VERTICAL_PACKING
+#if (LCD_PIXELFORMAT == VERTICAL_PACKING) \
+ || (LCD_PIXELFORMAT == VERTICAL_INTERLEAVED)
     if (count & _GREY_BMASK)
     {
         /* Scrolling by fractional blocks - move pixel wise. */
@@ -262,7 +263,8 @@ void grey_ub_scroll_down(int count)
                               _grey_info.fg_brightness :
                               _grey_info.bg_brightness];
 
-#if LCD_PIXELFORMAT == VERTICAL_PACKING
+#if (LCD_PIXELFORMAT == VERTICAL_PACKING) \
+ || (LCD_PIXELFORMAT == VERTICAL_INTERLEAVED)
     if (count & _GREY_BMASK)
     {
         /* Scrolling by fractional blocks - move pixel wise. */
@@ -302,6 +304,9 @@ void grey_ub_scroll_down(int count)
             }
             while (dst < line_end);
         }
+        /* Top pixel in a block has the highest address, but dst must point
+         * to the lowest address in that block for the subsequent fill. */
+        dst -= _GREY_BMASK;
     }
     else
 #endif
@@ -311,7 +316,7 @@ void grey_ub_scroll_down(int count)
         dst -= blen;
         _grey_info.rb->memmove(dst, start, blen);
     }
-    _grey_info.rb->memset(start, blank, dst - start); 
+    _grey_info.rb->memset(start, blank, dst - start);
     /* Fill remainder at once. */
 #ifdef SIMULATOR
     _grey_info.rb->sim_lcd_ex_update_rect(_grey_info.x, _grey_info.y,
diff --git a/apps/plugins/mandelbrot.c b/apps/plugins/mandelbrot.c
index 26ec3e4ab9..4a4872b827 100644
--- a/apps/plugins/mandelbrot.c
+++ b/apps/plugins/mandelbrot.c
@@ -202,6 +202,22 @@ PLUGIN_HEADER
 #define MANDELBROT_MAXITER_DEC  BUTTON_PLAY
 #define MANDELBROT_RESET        BUTTON_DISPLAY
 
+#elif CONFIG_KEYPAD == IAUDIO_M3_PAD
+#define MANDELBROT_QUIT            BUTTON_RC_REC
+#define MANDELBROT_UP              BUTTON_RC_VOL_UP
+#define MANDELBROT_DOWN            BUTTON_RC_VOL_DOWN
+#define MANDELBROT_LEFT            BUTTON_RC_REW
+#define MANDELBROT_RIGHT           BUTTON_RC_FF
+#define MANDELBROT_ZOOM_IN_PRE     BUTTON_RC_PLAY
+#define MANDELBROT_ZOOM_IN         (BUTTON_RC_PLAY | BUTTON_REL)
+#define MANDELBROT_ZOOM_OUT_PRE    BUTTON_RC_PLAY
+#define MANDELBROT_ZOOM_OUT        (BUTTON_RC_PLAY | BUTTON_REPEAT)
+#define MANDELBROT_MAXITER_INC_PRE BUTTON_RC_MODE
+#define MANDELBROT_MAXITER_INC     (BUTTON_RC_MODE|BUTTON_REL)
+#define MANDELBROT_MAXITER_DEC_PRE BUTTON_RC_MODE
+#define MANDELBROT_MAXITER_DEC     (BUTTON_RC_MODE|BUTTON_REPEAT)
+#define MANDELBROT_RESET           BUTTON_RC_MENU
+
 #elif CONFIG_KEYPAD == COWOND2_PAD
 #define MANDELBROT_QUIT          BUTTON_POWER
 #define MANDELBROT_UP            BUTTON_UP
diff --git a/apps/plugins/mpegplayer/mpeg_settings.c b/apps/plugins/mpegplayer/mpeg_settings.c
index 7dfdd36927..b7b7def729 100644
--- a/apps/plugins/mpegplayer/mpeg_settings.c
+++ b/apps/plugins/mpegplayer/mpeg_settings.c
@@ -102,6 +102,14 @@ struct mpeg_settings settings;
 #define MPEG_START_TIME_SCROLL_UP   BUTTON_PLAY
 #define MPEG_START_TIME_EXIT        BUTTON_POWER
 
+#elif CONFIG_KEYPAD == IAUDIO_M3_PAD
+#define MPEG_START_TIME_SELECT      BUTTON_RC_PLAY
+#define MPEG_START_TIME_LEFT        BUTTON_RC_REW
+#define MPEG_START_TIME_RIGHT       BUTTON_RC_FF
+#define MPEG_START_TIME_UP          BUTTON_RC_VOL_UP
+#define MPEG_START_TIME_DOWN        BUTTON_RC_VOL_DOWN
+#define MPEG_START_TIME_EXIT        BUTTON_RC_REC
+
 #elif CONFIG_KEYPAD == COWOND2_PAD
 #define MPEG_START_TIME_SELECT      BUTTON_SELECT
 #define MPEG_START_TIME_SCROLL_UP   BUTTON_PLUS
diff --git a/apps/plugins/mpegplayer/mpegplayer.c b/apps/plugins/mpegplayer/mpegplayer.c
index d068fd8254..8a947904a3 100644
--- a/apps/plugins/mpegplayer/mpegplayer.c
+++ b/apps/plugins/mpegplayer/mpegplayer.c
@@ -202,6 +202,15 @@ PLUGIN_IRAM_DECLARE
 #define MPEG_RW         BUTTON_LEFT
 #define MPEG_FF         BUTTON_RIGHT
 
+#elif CONFIG_KEYPAD == IAUDIO_M3_PAD
+#define MPEG_MENU       BUTTON_RC_MENU
+#define MPEG_STOP       BUTTON_RC_REC
+#define MPEG_PAUSE      BUTTON_RC_PLAY
+#define MPEG_VOLDOWN    BUTTON_RC_VOL_DOWN
+#define MPEG_VOLUP      BUTTON_RC_VOL_UP
+#define MPEG_RW         BUTTON_RC_REW
+#define MPEG_FF         BUTTON_RC_FF
+
 #elif CONFIG_KEYPAD == COWOND2_PAD
 #define MPEG_MENU       BUTTON_MENU
 #define MPEG_STOP       BUTTON_POWER
diff --git a/apps/plugins/plasma.c b/apps/plugins/plasma.c
index d5c1ef427f..2e04bd59b5 100644
--- a/apps/plugins/plasma.c
+++ b/apps/plugins/plasma.c
@@ -59,6 +59,7 @@ static int plasma_frequency;
 #define PLASMA_QUIT BUTTON_MENU
 #define PLASMA_INCREASE_FREQUENCY BUTTON_SCROLL_FWD
 #define PLASMA_DECREASE_FREQUENCY BUTTON_SCROLL_BACK
+
 #elif (CONFIG_KEYPAD == GIGABEAT_PAD)
 #define PLASMA_QUIT BUTTON_A
 #define PLASMA_INCREASE_FREQUENCY BUTTON_UP
@@ -74,26 +75,38 @@ static int plasma_frequency;
 #define PLASMA_QUIT BUTTON_POWER
 #define PLASMA_INCREASE_FREQUENCY BUTTON_UP
 #define PLASMA_DECREASE_FREQUENCY BUTTON_DOWN
+
 #elif (CONFIG_KEYPAD == IRIVER_H10_PAD)
 #define PLASMA_QUIT BUTTON_POWER
 #define PLASMA_INCREASE_FREQUENCY BUTTON_SCROLL_UP
 #define PLASMA_DECREASE_FREQUENCY BUTTON_SCROLL_DOWN
+
 #elif (CONFIG_KEYPAD == GIGABEAT_S_PAD)
 #define PLASMA_QUIT BUTTON_BACK
 #define PLASMA_INCREASE_FREQUENCY BUTTON_UP
 #define PLASMA_DECREASE_FREQUENCY BUTTON_DOWN
+
 #elif (CONFIG_KEYPAD == MROBE100_PAD)
 #define PLASMA_QUIT BUTTON_POWER
 #define PLASMA_INCREASE_FREQUENCY BUTTON_UP
 #define PLASMA_DECREASE_FREQUENCY BUTTON_DOWN
+
+#elif (CONFIG_KEYPAD == IAUDIO_M3_PAD)
+#define PLASMA_QUIT BUTTON_RC_REC
+#define PLASMA_INCREASE_FREQUENCY BUTTON_RC_VOL_UP
+#define PLASMA_DECREASE_FREQUENCY BUTTON_RC_VOL_DOWN
+#define PLASMA_RC_QUIT            BUTTON_REC
+
 #elif (CONFIG_KEYPAD == COWOND2_PAD)
 #define PLASMA_QUIT BUTTON_POWER
 #define PLASMA_INCREASE_FREQUENCY BUTTON_UP
 #define PLASMA_DECREASE_FREQUENCY BUTTON_DOWN
+
 #else
 #define PLASMA_QUIT BUTTON_OFF
 #define PLASMA_INCREASE_FREQUENCY BUTTON_UP
 #define PLASMA_DECREASE_FREQUENCY BUTTON_DOWN
+
 #if (CONFIG_KEYPAD == IRIVER_H100_PAD) || (CONFIG_KEYPAD == IRIVER_H300_PAD)
 #define PLASMA_RC_QUIT            BUTTON_RC_STOP
 #endif
diff --git a/apps/plugins/test_fps.c b/apps/plugins/test_fps.c
index 7c86fabdc0..a312c13662 100644
--- a/apps/plugins/test_fps.c
+++ b/apps/plugins/test_fps.c
@@ -260,7 +260,7 @@ static void time_remote_update(void)
 }
 #endif
 
-#if (LCD_DEPTH < 4) && !defined(IAUDIO_M3)
+#if LCD_DEPTH < 4
 
 GREY_INFO_STRUCT_IRAM
 static unsigned char greydata[LCD_HEIGHT][LCD_WIDTH];
@@ -357,7 +357,7 @@ enum plugin_status plugin_start(struct plugin_api* api, void* parameter)
     log_text("Main LCD YUV");
     time_main_yuv();
 #endif
-#if (LCD_DEPTH < 4) && !defined(IAUDIO_M3)
+#if LCD_DEPTH < 4
     log_text("Greyscale library");
     time_greyscale();
 #endif
diff --git a/firmware/SOURCES b/firmware/SOURCES
index 0a8ac2a8e3..1d60ea1016 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -513,6 +513,7 @@ target/coldfire/iaudio/m3/backlight-m3.c
 target/coldfire/iaudio/m3/button-m3.c
 target/coldfire/iaudio/m3/fmradio_i2c-m3.c
 target/coldfire/iaudio/m3/lcd-m3.c
+target/coldfire/iaudio/m3/lcd-as-m3.S
 target/coldfire/iaudio/m3/power-m3.c
 target/coldfire/iaudio/m3/powermgmt-m3.c
 target/coldfire/iaudio/m3/system-m3.c
diff --git a/firmware/target/coldfire/iaudio/m3/lcd-as-m3.S b/firmware/target/coldfire/iaudio/m3/lcd-as-m3.S
new file mode 100644
index 0000000000..5f77e01e86
--- /dev/null
+++ b/firmware/target/coldfire/iaudio/m3/lcd-as-m3.S
@@ -0,0 +1,516 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2008 by Jens Arnold
+ *
+ * All files in this archive are subject to the GNU General Public License.
+ * See the file COPYING in the source tree root for full license agreement.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+
+#define CLOCK_MASK     0x20000000
+#define DATA_MASK      0x04000000
+#define GPIO_OUT_ADDR  0x80000004
+
+#define CS_MASK        0x00010000
+#define RS_MASK        0x00001000
+#define GPIO1_OUT_ADDR 0x800000b4
+
+    .extern cpu_frequency       /* Global variable from system.c */
+
+    .section    .icode,"ax",@progbits
+
+    /* Output 8 bits to the LCD. Instruction order is devised to maximize the
+     * delay between changing the data line and the CLK L->H transition, which
+     * makes the LCD controller sample DATA.
+     *
+     * Custom calling convention:
+     *   %a0 - GPIO_OUT_ADDR
+     *   %d3 - data byte
+     *   %d6 - DATA_MASK
+     *   %d7 - CLOCK_MASK
+     * Clobbers:
+     *   %d0..%d3 
+     */
+.write_byte:
+    move.w  %sr, %d2
+    move.w  #0x2700, %sr
+
+    move.l  (%a0), %d0          /* Get current state of data port */
+    move.l  %d0, %d1
+    and.l   %d6, %d1            /* Check current state of data line */
+    beq.s   1f                  /*   and set it as previous-state bit */
+    bset    #8, %d3
+1:
+    move.l  %d3, %d1            /* Compute the 'bit derivative', i.e. a value */
+    lsr.l   #1, %d1             /*   with 1's where the data changes from the */
+    eor.l   %d1, %d3            /*   previous state, and 0's where it doesn't */
+    swap    %d3                 /* Shift data to upper byte */
+    lsl.l   #8, %d3
+
+    eor.l   %d7, %d0            /* precalculate opposite state of clock line */
+
+    lsl.l   #1,%d3              /* Shift out MSB */
+    bcc.s   1f
+    eor.l   %d6, %d0            /* 1: Flip data bit */
+1:
+    move.l  %d0, %d1
+    move.l  %d0, (%a0)          /* Output new state and set CLK = 0*/
+    eor.l   %d7, %d1
+    bra.w   .wr_bit7
+
+
+    /* Output 16 bits to the LCD. Instruction order is devised to maximize the
+     * delay between changing the data line and the CLK L->H transition, which
+     * makes the LCD controller sample DATA.
+     *
+     * Custom calling convention:
+     *   %a0 - GPIO_OUT_ADDR
+     *   %d3 - data word
+     *   %d6 - DATA_MASK
+     *   %d7 - CLOCK_MASK
+     * Clobbers:
+     *   %d0..%d3 
+     */
+.write_word:
+    move.w  %sr, %d2
+    move.w  #0x2700, %sr
+
+    move.l  (%a0), %d0          /* Get current state of data port */
+    move.l  %d0, %d1
+    and.l   %d6, %d1            /* Check current state of data line */
+    beq.s   1f                  /*   and set it as previous-state bit */
+    bset    #16, %d3
+1:
+    move.l  %d3, %d1            /* Compute the 'bit derivative', i.e. a value */
+    lsr.l   #1, %d1             /*   with 1's where the data changes from the */
+    eor.l   %d1, %d3            /*   previous state, and 0's where it doesn't */
+    swap    %d3                 /* Shift data to upper word */
+
+    eor.l   %d7, %d0            /* precalculate opposite state of clock line */
+
+    lsl.l   #1,%d3              /* Shift out MSB */
+    bcc.s   1f
+    eor.l   %d6, %d0            /* 1: Flip data bit */
+1:
+    move.l  %d0, %d1
+    move.l  %d0, (%a0)          /* Output new state and set CLK = 0*/
+    eor.l   %d7, %d1
+    nop
+
+.macro bit_out
+    lsl.l   #1,%d3
+    bcc.s   1f
+    eor.l   %d6, %d0
+1:
+    move.l  %d1, (%a0)          /* Set CLK = 1 (delayed) */
+    move.l  %d0, %d1
+    move.l  %d0, (%a0)
+    eor.l   %d7, %d1
+.endm
+    bit_out
+    nop
+    bit_out
+    nop
+    bit_out
+    nop
+    bit_out
+    nop
+    bit_out
+    nop
+    bit_out
+    nop
+    bit_out
+    nop
+    bit_out
+    nop
+
+.wr_bit7:
+    bit_out
+    nop
+    bit_out
+    nop
+    bit_out
+    nop
+    bit_out
+    nop
+    bit_out
+    nop
+    bit_out
+    nop
+    bit_out
+    nop
+
+    nop
+    move.l  %d1, (%a0)          /* Set CLK = 1 (delayed) */
+    move.w  %d2, %sr
+    rts
+
+
+    /* Output 16 bits to the LCD as fast as possible. Use only at < 60MHz.
+     *
+     * Custom calling convention:
+     *   %a0 - GPIO_OUT_ADDR
+     *   %d3 - data word
+     *   %d6 - DATA_MASK
+     *   %d7 - CLOCK_MASK
+     * Clobbers:
+     *   %d0..%d3 
+     */
+.write_word_fast:
+    move.w  %sr, %d2            /* Get current interrupt level */
+    move.w  #0x2700, %sr        /* Disable interrupts */
+
+    move.l  (%a0), %d0          /* Get current state of data port */
+    move.l  %d0, %d1
+    and.l   %d6, %d1            /* Check current state of data line */
+    beq.s   1f                  /*   and set it as previous-state bit */
+    bset    #16, %d3
+1:
+    move.l  %d3, %d1            /* Compute the 'bit derivative', i.e. a value */
+    lsr.l   #1, %d1             /*   with 1's where the data changes from the */
+    eor.l   %d1, %d3            /*   previous state, and 0's where it doesn't */
+    swap    %d3                 /* Shift data to upper byte */
+
+    move.l  %d0, %d1            /* precalculate opposite state of clock line */
+    eor.l   %d7, %d1
+    
+.macro bit_out_fast
+    lsl.l   #1,%d3              /* Shift out MSB */
+    bcc.s   1f                  
+    eor.l   %d6, %d0            /* 1: Flip data bit */
+    eor.l   %d6, %d1            /*   for both clock states */
+1:
+    move.l  %d1, (%a0)          /* Output new state and set CLK = 0*/
+    move.l  %d0, (%a0)          /* set CLK = 1 */
+.endm
+    bit_out_fast
+    bit_out_fast
+    bit_out_fast
+    bit_out_fast
+    bit_out_fast
+    bit_out_fast
+    bit_out_fast
+    bit_out_fast
+    bit_out_fast
+    bit_out_fast
+    bit_out_fast
+    bit_out_fast
+    bit_out_fast
+    bit_out_fast
+    bit_out_fast
+    bit_out_fast
+
+    move.w  %d2, %sr            /* Restore interrupt level */
+    rts
+
+
+    .global lcd_write_command
+    .type   lcd_write_command, @function
+
+lcd_write_command:
+    lea.l   (-4*4, %sp), %sp
+    movem.l %d2-%d3/%d6-%d7, (%sp)
+    
+    move.l  (4*4+4, %sp), %d3   /* cmd */
+
+    lea.l   GPIO_OUT_ADDR, %a0
+    lea.l   GPIO1_OUT_ADDR, %a1
+    move.l  #DATA_MASK, %d6
+    move.l  #CLOCK_MASK, %d7
+
+    move.l  #~(RS_MASK+CS_MASK), %d0
+    and.l   %d0, (%a1)
+
+    bsr.w   .write_byte
+
+    move.l  #CS_MASK, %d0
+    or.l    %d0, (%a1)
+    
+    movem.l (%sp), %d2-%d3/%d6-%d7
+    lea.l   (4*4, %sp), %sp
+    rts
+
+
+    .global lcd_write_command_e
+    .type   lcd_write_command_e, @function
+
+lcd_write_command_e:
+    lea.l   (-4*4, %sp), %sp
+    movem.l %d2-%d3/%d6-%d7, (%sp)
+    
+    movem.l (4*4+4, %sp), %d2-%d3   /* cmd, data */
+
+    lea.l   GPIO_OUT_ADDR, %a0
+    lea.l   GPIO1_OUT_ADDR, %a1
+    move.l  #DATA_MASK, %d6
+    move.l  #CLOCK_MASK, %d7
+
+    move.l  #~(RS_MASK+CS_MASK), %d0
+    and.l   %d0, (%a1)
+
+    lsl.l   #8, %d2
+    or.l    %d2, %d3
+    bsr.w   .write_word
+
+    move.l  #CS_MASK, %d0
+    or.l    %d0, (%a1)
+    
+    movem.l (%sp), %d2-%d3/%d6-%d7
+    lea.l   (4*4, %sp), %sp
+    rts
+
+
+    .global lcd_write_data
+    .type   lcd_write_data, @function
+
+lcd_write_data:
+    lea.l   (-7*4, %sp), %sp
+    movem.l %d2-%d4/%d6-%d7/%a2-%a3, (%sp)
+    
+    move.l  (7*4+4, %sp), %a2   /* p_words */
+    move.l  (7*4+8, %sp), %d4   /* count */
+
+    lea.l   GPIO_OUT_ADDR, %a0
+    lea.l   GPIO1_OUT_ADDR, %a1
+    move.l  #DATA_MASK, %d6
+    move.l  #CLOCK_MASK, %d7
+
+    lea.l   .write_word, %a3
+    move.l  cpu_frequency, %d0
+    cmp.l   #60000000, %d0
+    bhi.b   1f
+    lea.l   .write_word_fast, %a3
+1:
+
+    move.l  #RS_MASK, %d0
+    or.l    %d0, (%a1)
+    move.l  #~CS_MASK, %d0
+    and.l   %d0, (%a1)
+
+.wd_loop:                
+    clr.l   %d3
+    move.w  (%a2)+, %d3
+    jsr     (%a3)
+    subq.l  #1, %d4
+    bne.s   .wd_loop
+
+    move.l  #CS_MASK, %d0
+    or.l    %d0, (%a1)
+    
+    movem.l (%sp), %d2-%d4/%d6-%d7/%a2-%a3
+    lea.l   (7*4, %sp), %sp
+    rts
+    
+    
+/*** The following functions are only needed for main LCDs ***/
+
+
+    .global lcd_mono_data
+    .type   lcd_mono_data, @function
+
+lcd_mono_data:
+    lea.l   (-7*4, %sp), %sp
+    movem.l %d2-%d4/%d6-%d7/%a2-%a3, (%sp)
+    
+    move.l  (7*4+4, %sp), %a2   /* p_bytes */
+    move.l  (7*4+8, %sp), %d4   /* count */
+
+    lea.l   GPIO_OUT_ADDR, %a0
+    lea.l   GPIO1_OUT_ADDR, %a1
+    move.l  #DATA_MASK, %d6
+    move.l  #CLOCK_MASK, %d7
+
+    lea.l   .write_word, %a3
+    move.l  cpu_frequency, %d0
+    cmp.l   #60000000, %d0
+    bhi.b   1f
+    lea.l   .write_word_fast, %a3
+1:
+
+    move.l  #RS_MASK, %d0
+    or.l    %d0, (%a1)
+    move.l  #~CS_MASK, %d0
+    and.l   %d0, (%a1)
+
+.md_loop:
+    clr.l   %d3
+    move.b  (%a2)+, %d3
+    move.l  %d3, %d2
+    lsl.l   #8, %d2
+    or.l    %d2, %d3
+    jsr     (%a3)
+    subq.l  #1, %d4
+    bne.s   .md_loop
+
+    move.l  #CS_MASK, %d0
+    or.l    %d0, (%a1)
+
+    movem.l (%sp), %d2-%d4/%d6-%d7/%a2-%a3
+    lea.l   (7*4, %sp), %sp
+    rts
+
+
+    .global     lcd_grey_data
+    .type       lcd_grey_data,@function
+
+lcd_grey_data:
+    lea.l   (-9*4, %sp), %sp
+    movem.l %d2-%d7/%a2-%a4, (%sp)
+    
+    movem.l (9*4+4, %sp), %a2-%a4   /* values, phases, length */
+    add.l   %a4, %a4
+    lea.l   (%a3, %a4.l*4), %a4     /* end address */
+    
+    lea.l   GPIO_OUT_ADDR, %a0
+    lea.l   GPIO1_OUT_ADDR, %a1
+    move.l  #DATA_MASK, %d6
+    move.l  #CLOCK_MASK, %d7
+
+    move.l  #RS_MASK, %d0
+    or.l    %d0, (%a1)
+    move.l  #~CS_MASK, %d0
+    and.l   %d0, (%a1)
+    
+    clr.l   %d5
+    move.l  (%a3), %d4          /* fetch 4 pixel phases */
+    bclr.l  #31, %d4            /* Z = !(p0 & 0x80); p0 &= ~0x80; */
+    seq.b   %d5                 /* %d5 = ........................00000000 */
+    lsl.l   #1, %d5             /* %d5 = .......................00000000. */
+    bclr.l  #23, %d4            /* Z = !(p1 & 0x80); p1 &= ~0x80; */
+    seq.b   %d5                 /* %d5 = .......................011111111 */
+    lsl.l   #1, %d5             /* %d5 = ......................011111111. */
+    bclr.l  #15, %d4            /* Z = !(p2 & 0x80); p2 &= ~0x80; */
+    seq.b   %d5                 /* %d5 = ......................0122222222 */
+    lsl.l   #1, %d5             /* %d5 = .....................0122222222. */
+    bclr.l  #7, %d4             /* Z = !(p3 & 0x80); p3 &= ~0x80; */
+    seq.b   %d5                 /* %d5 = .....................01233333333 */
+    lsl.l   #1, %d5             /* %d5 = ....................01233333333. */
+    add.l   (%a2)+, %d4         /* add 4 pixel values to the phases */
+    move.l  %d4, (%a3)+         /* store new phases, advance pointer */
+
+    move.l  (%a3), %d4          /* fetch 4 pixel phases */
+    bclr.l  #31, %d4            /* Z = !(p0 & 0x80); p0 &= ~0x80; */
+    seq.b   %d5                 /* %d5 = ....................012344444444 */
+    lsl.l   #1, %d5             /* %d5 = ...................012344444444. */
+    bclr.l  #23, %d4            /* Z = !(p1 & 0x80); p1 &= ~0x80; */
+    seq.b   %d5                 /* %d5 = ...................0123455555555 */
+    lsl.l   #1, %d5             /* %d5 = ..................0123455555555. */
+    bclr.l  #15, %d4            /* Z = !(p2 & 0x80); p2 &= ~0x80; */
+    seq.b   %d5                 /* %d5 = ..................01234566666666 */
+    lsl.l   #1, %d5             /* %d5 = .................01234566666666. */
+    bclr.l  #7, %d4             /* Z = !(p3 & 0x80); p3 &= ~0x80; */
+    seq.b   %d5                 /* %d5 = .................012345677777777 */
+    lsr.l   #7, %d5             /* %d5 = ........................01234567 */
+    add.l   (%a2)+, %d4         /* add 4 pixel values to the phases */
+    move.l  %d4, (%a3)+         /* store new phases, advance pointer */
+    
+    move.l  %d5, %d3
+    lsl.l   #8, %d3
+    or.l    %d5, %d3
+    
+    cmp.l   %a3, %a4
+    bls.w   .gd_last
+
+.gd_loop:
+    move.w  %sr, %d2
+    move.w  #0x2700, %sr
+
+    move.l  (%a0), %d0          /* Get current state of data port */
+    move.l  %d0, %d1
+    and.l   %d6, %d1            /* Check current state of data line */
+    beq.s   1f                  /*   and set it as previous-state bit */
+    bset    #16, %d3
+1:
+    move.l  %d3, %d1            /* Compute the 'bit derivative', i.e. a value */
+    lsr.l   #1, %d1             /*   with 1's where the data changes from the */
+    eor.l   %d1, %d3            /*   previous state, and 0's where it doesn't */
+    swap    %d3                 /* Shift data to upper word */
+
+    eor.l   %d7, %d0            /* precalculate opposite state of clock line */
+
+    lsl.l   #1,%d3              /* Shift out MSB */
+    bcc.s   1f
+    eor.l   %d6, %d0            /* 1: Flip data bit */
+1:
+    move.l  %d0, %d1
+    move.l  %d0, (%a0)          /* Output new state and set CLK = 0*/
+    eor.l   %d7, %d1
+
+    move.l  (%a3), %d4          /* fetch 4 pixel phases */
+    bit_out
+    bclr.l  #31, %d4            /* Z = !(p0 & 0x80); p0 &= ~0x80; */
+    seq.b   %d5                 /* %d5 = ........................00000000 */
+    lsl.l   #1, %d5             /* %d5 = .......................00000000. */
+    bit_out
+    bclr.l  #23, %d4            /* Z = !(p1 & 0x80); p1 &= ~0x80; */
+    seq.b   %d5                 /* %d5 = .......................011111111 */
+    lsl.l   #1, %d5             /* %d5 = ......................011111111. */
+    bit_out
+    bclr.l  #15, %d4            /* Z = !(p2 & 0x80); p2 &= ~0x80; */
+    seq.b   %d5                 /* %d5 = ......................0122222222 */
+    lsl.l   #1, %d5             /* %d5 = .....................0122222222. */
+    bit_out
+    bclr.l  #7, %d4             /* Z = !(p3 & 0x80); p3 &= ~0x80; */
+    seq.b   %d5                 /* %d5 = .....................01233333333 */
+    lsl.l   #1, %d5             /* %d5 = ....................01233333333. */
+    bit_out
+    add.l   (%a2)+, %d4         /* add 4 pixel values to the phases */
+    bit_out
+    move.l  %d4, (%a3)+         /* store new phases, advance pointer */
+
+    bit_out
+    move.l  (%a3), %d4          /* fetch 4 pixel phases */
+    bit_out
+    bclr.l  #31, %d4            /* Z = !(p0 & 0x80); p0 &= ~0x80; */
+    seq.b   %d5                 /* %d5 = ....................012344444444 */
+    lsl.l   #1, %d5             /* %d5 = ...................012344444444. */
+    bit_out
+    bclr.l  #23, %d4            /* Z = !(p1 & 0x80); p1 &= ~0x80; */
+    seq.b   %d5                 /* %d5 = ...................0123455555555 */
+    lsl.l   #1, %d5             /* %d5 = ..................0123455555555. */
+    bit_out
+    bclr.l  #15, %d4            /* Z = !(p2 & 0x80); p2 &= ~0x80; */
+    seq.b   %d5                 /* %d5 = ..................01234566666666 */
+    lsl.l   #1, %d5             /* %d5 = .................01234566666666. */
+    bit_out
+    bclr.l  #7, %d4             /* Z = !(p3 & 0x80); p3 &= ~0x80; */
+    seq.b   %d5                 /* %d5 = .................012345677777777 */
+    lsr.l   #7, %d5             /* %d5 = ........................01234567 */
+    bit_out
+    add.l   (%a2)+, %d4         /* add 4 pixel values to the phases */
+    bit_out
+    move.l  %d4, (%a3)+         /* store new phases, advance pointer */
+
+    bit_out
+    nop
+    bit_out
+    move.l  %d5, %d3
+    lsl.l   #8, %d3
+    or.l    %d5, %d3
+    
+    nop
+    move.l  %d1, (%a0)          /* Set CLK = 1 (delayed) */
+    move.w  %d2, %sr
+
+    cmp.l   %a3, %a4
+    bhi.w   .gd_loop
+
+.gd_last:
+    bsr.w   .write_word
+
+    move.l  #CS_MASK, %d0
+    or.l    %d0, (%a1)
+
+    movem.l (%sp), %d2-%d7/%a2-%a4
+    lea.l   (9*4, %sp), %sp
+    rts
diff --git a/firmware/target/coldfire/iaudio/m3/lcd-m3.c b/firmware/target/coldfire/iaudio/m3/lcd-m3.c
index ae72832a82..81f19bd73f 100644
--- a/firmware/target/coldfire/iaudio/m3/lcd-m3.c
+++ b/firmware/target/coldfire/iaudio/m3/lcd-m3.c
@@ -61,263 +61,6 @@ static int cached_contrast = DEFAULT_CONTRAST_SETTING;
 bool initialized = false;
 
 
-/* Standard low-level byte writer. Requires CLK high on entry */
-static inline void _write_byte(unsigned data)
-{
-    asm volatile (
-        "move.l  (%[gpo0]), %%d0     \n"  /* Get current state of data line */
-        "and.l   %[dbit], %%d0       \n"
-        "beq.s   1f                  \n"  /*   and set it as previous-state bit */
-        "bset    #8, %[data]         \n"  
-    "1:                              \n"
-        "move.l  %[data], %%d0       \n"  /* Compute the 'bit derivative', i.e. a value */
-        "lsr.l   #1, %%d0            \n"  /*   with 1's where the data changes from the */
-        "eor.l   %%d0, %[data]       \n"  /*   previous state, and 0's where it doesn't */
-        "swap    %[data]             \n"  /* Shift data to upper byte */
-        "lsl.l   #8, %[data]         \n"
-        
-        "move.l  %[cbit], %%d1       \n"  /* Prepare mask for flipping CLK */
-        "or.l    %[dbit], %%d1       \n"  /*   and DATA at once */
-
-        "lsl.l   #1,%[data]          \n"  /* Shift out MSB */
-        "bcc.s   1f                  \n"
-        "eor.l   %%d1, (%[gpo0])     \n"  /* 1: Flip both CLK and DATA */
-        ".word   0x51fa              \n"  /* (trapf.w - shadow next insn) */
-    "1:                              \n"
-        "eor.l   %[cbit], (%[gpo0])  \n"  /*   else flip CLK only */
-        "eor.l   %[cbit], (%[gpo0])  \n"  /* Flip CLK again */
-
-        "lsl.l   #1,%[data]          \n"  /* ..unrolled.. */
-        "bcc.s   1f                  \n"
-        "eor.l   %%d1, (%[gpo0])     \n"
-        ".word   0x51fa              \n"
-    "1:                              \n"
-        "eor.l   %[cbit], (%[gpo0])  \n"
-        "eor.l   %[cbit], (%[gpo0])  \n"
-
-        "lsl.l   #1,%[data]          \n"
-        "bcc.s   1f                  \n"
-        "eor.l   %%d1, (%[gpo0])     \n"
-        ".word   0x51fa              \n"
-    "1:                              \n"
-        "eor.l   %[cbit], (%[gpo0])  \n"
-        "eor.l   %[cbit], (%[gpo0])  \n"
-
-        "lsl.l   #1,%[data]          \n"
-        "bcc.s   1f                  \n"
-        "eor.l   %%d1, (%[gpo0])     \n"
-        ".word   0x51fa              \n"
-    "1:                              \n"
-        "eor.l   %[cbit], (%[gpo0])  \n"
-        "eor.l   %[cbit], (%[gpo0])  \n"
-
-        "lsl.l   #1,%[data]          \n"
-        "bcc.s   1f                  \n"
-        "eor.l   %%d1, (%[gpo0])     \n"
-        ".word   0x51fa              \n"
-    "1:                              \n"
-        "eor.l   %[cbit], (%[gpo0])  \n"
-        "eor.l   %[cbit], (%[gpo0])  \n"
-
-        "lsl.l   #1,%[data]          \n"
-        "bcc.s   1f                  \n"
-        "eor.l   %%d1, (%[gpo0])     \n"
-        ".word   0x51fa              \n"
-    "1:                              \n"
-        "eor.l   %[cbit], (%[gpo0])  \n"
-        "eor.l   %[cbit], (%[gpo0])  \n"
-
-        "lsl.l   #1,%[data]          \n"
-        "bcc.s   1f                  \n"
-        "eor.l   %%d1, (%[gpo0])     \n"
-        ".word   0x51fa              \n"
-    "1:                              \n"
-        "eor.l   %[cbit], (%[gpo0])  \n"
-        "eor.l   %[cbit], (%[gpo0])  \n"
-
-        "lsl.l   #1,%[data]          \n"
-        "bcc.s   1f                  \n"
-        "eor.l   %%d1, (%[gpo0])     \n"
-        ".word   0x51fa              \n"
-    "1:                              \n"
-        "eor.l   %[cbit], (%[gpo0])  \n"
-        "eor.l   %[cbit], (%[gpo0])  \n"
-        : /* outputs */
-        [data]"+d"(data)
-        : /* inputs */
-        [gpo0]"a"(&GPIO_OUT),
-        [cbit]"d"(0x20000000),
-        [dbit]"d"(0x04000000)
-        : /* clobbers */
-        "d0", "d1"
-    );
-}
-
-/* Fast low-level byte writer. Don't use with high CPU clock.
- * Requires CLK high on entry */
-static inline void _write_fast(unsigned data)
-{
-    asm volatile (
-        "move.w  %%sr,%%d3           \n"  /* Get current interrupt level */
-        "move.w  #0x2700,%%sr        \n"  /* Disable interrupts */
-
-        "move.l  (%[gpo0]), %%d0     \n"  /* Get current state of data port */
-        "move.l  %%d0, %%d1          \n"
-        "and.l   %[dbit], %%d1       \n"  /* Check current state of data line */
-        "beq.s   1f                  \n"  /*   and set it as previous-state bit */
-        "bset    #8, %[data]         \n"
-    "1:                              \n"
-        "move.l  %[data], %%d1       \n"  /* Compute the 'bit derivative', i.e. a value */
-        "lsr.l   #1, %%d1            \n"  /*   with 1's where the data changes from the */
-        "eor.l   %%d1, %[data]       \n"  /*   previous state, and 0's where it doesn't */
-        "swap    %[data]             \n"  /* Shift data to upper byte */
-        "lsl.l   #8, %[data]         \n"
-        
-        "move.l  %%d0, %%d1          \n"  /* precalculate opposite state of clock line */
-        "eor.l   %[cbit], %%d1       \n"
-        
-        "lsl.l   #1,%[data]          \n"  /* Shift out MSB */
-        "bcc.s   1f                  \n"
-        "eor.l   %[dbit], %%d0       \n"  /* 1: Flip data bit */
-        "eor.l   %[dbit], %%d1       \n"  /*   for both clock states */
-    "1:                              \n"
-        "move.l  %%d1, (%[gpo0])     \n"  /* Output new state and set CLK */
-        "move.l  %%d0, (%[gpo0])     \n"  /* reset CLK */
-
-        "lsl.l   #1,%[data]          \n"  /* ..unrolled.. */
-        "bcc.s   1f                  \n"
-        "eor.l   %[dbit], %%d0       \n"
-        "eor.l   %[dbit], %%d1       \n"
-    "1:                              \n"
-        "move.l  %%d1, (%[gpo0])     \n"
-        "move.l  %%d0, (%[gpo0])     \n"
-
-        "lsl.l   #1,%[data]          \n"
-        "bcc.s   1f                  \n"
-        "eor.l   %[dbit], %%d0       \n"
-        "eor.l   %[dbit], %%d1       \n"
-    "1:                              \n"
-        "move.l  %%d1, (%[gpo0])     \n"
-        "move.l  %%d0, (%[gpo0])     \n"
-
-        "lsl.l   #1,%[data]          \n"
-        "bcc.s   1f                  \n"
-        "eor.l   %[dbit], %%d0       \n"
-        "eor.l   %[dbit], %%d1       \n"
-    "1:                              \n"
-        "move.l  %%d1, (%[gpo0])     \n"
-        "move.l  %%d0, (%[gpo0])     \n"
-
-        "lsl.l   #1,%[data]          \n"
-        "bcc.s   1f                  \n"
-        "eor.l   %[dbit], %%d0       \n"
-        "eor.l   %[dbit], %%d1       \n"
-    "1:                              \n"
-        "move.l  %%d1, (%[gpo0])     \n"
-        "move.l  %%d0, (%[gpo0])     \n"
-
-        "lsl.l   #1,%[data]          \n"
-        "bcc.s   1f                  \n"
-        "eor.l   %[dbit], %%d0       \n"
-        "eor.l   %[dbit], %%d1       \n"
-    "1:                              \n"
-        "move.l  %%d1, (%[gpo0])     \n"
-        "move.l  %%d0, (%[gpo0])     \n"
-
-        "lsl.l   #1,%[data]          \n"
-        "bcc.s   1f                  \n"
-        "eor.l   %[dbit], %%d0       \n"
-        "eor.l   %[dbit], %%d1       \n"
-    "1:                              \n"
-        "move.l  %%d1, (%[gpo0])     \n"
-        "move.l  %%d0, (%[gpo0])     \n"
-
-        "lsl.l   #1,%[data]          \n"
-        "bcc.s   1f                  \n"
-        "eor.l   %[dbit], %%d0       \n"
-        "eor.l   %[dbit], %%d1       \n"
-    "1:                              \n"
-        "move.l  %%d1, (%[gpo0])     \n"
-        "move.l  %%d0, (%[gpo0])     \n"
-
-        "move.w  %%d3, %%sr          \n" /* Restore interrupt level */
-        : /* outputs */
-        [data]"+d"(data)
-        : /* inputs */
-        [gpo0]"a"(&GPIO_OUT),
-        [cbit]"d"(0x20000000),
-        [dbit]"d"(0x04000000)
-        : /* clobbers */
-        "d0", "d1", "d2", "d3"
-    );
-}
-
-void lcd_write_command(int cmd)
-{
-    RS_LO;
-    CS_LO;
-    _write_byte(cmd);
-    CS_HI;
-}
-
-void lcd_write_command_e(int cmd, int data)
-{
-    RS_LO;
-    CS_LO;
-    _write_byte(cmd);
-    _write_byte(data);
-    CS_HI;
-}
-
-void lcd_write_data(const fb_data *p_words, int count)
-{
-    const unsigned char *p_bytes = (const unsigned char *)p_words;
-    const unsigned char *p_end = (const unsigned char *)(p_words + count);
-
-    RS_HI;
-    CS_LO;
-    if (cpu_frequency < 50000000)
-    {
-        while (p_bytes < p_end)
-            _write_fast(*p_bytes++);
-    }
-    else
-    {
-        while (p_bytes < p_end)
-            _write_byte(*p_bytes++);
-    }
-    CS_HI;
-}
-
-static void lcd_mono_data(const unsigned char *p_words, int count)
-{
-    unsigned data;
-    const unsigned char *p_bytes = p_words;
-    const unsigned char *p_end = p_words + count;
-
-    RS_HI;
-    CS_LO;
-    if (cpu_frequency < 50000000)
-    {
-        while (p_bytes < p_end)
-        {
-            data = *p_bytes++;
-            _write_fast(data);
-            _write_fast(data);
-        }
-    }
-    else
-    {
-        while (p_bytes < p_end)
-        {
-            data = *p_bytes++;
-            _write_byte(data);
-            _write_byte(data);
-        }
-    }
-    CS_HI;
-}
-
 int lcd_default_contrast(void)
 {
     return DEFAULT_CONTRAST_SETTING;
@@ -457,6 +200,9 @@ void lcd_init_device(void)
 #endif
 }
 
+/* Helper function. */
+void lcd_mono_data(const unsigned char *data, int count);
+
 /* Performance function that works with an external buffer
    note that by and bheight are in 8-pixel units! */
 void lcd_blit_mono(const unsigned char *data, int x, int by, int width,
@@ -477,36 +223,44 @@ void lcd_blit_mono(const unsigned char *data, int x, int by, int width,
     }
 }
 
-/* TODO: implement grey blit function */
+/* 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_blit_grey_phase(unsigned char *values, unsigned char *phases,
                          int x, int by, int width, int bheight, int stride)
 {
-    (void)values;
-    (void)phases;
-    (void)x;
-    (void)by;
-    (void)width;
-    (void)bheight;
-    (void)stride;
+    if (initialized)
+    {
+        stride <<= 3; /* 8 pixels per block */
+        while (bheight--)
+        {
+            lcd_write_command(LCD_SET_PAGE | ((by > 5 ? by + 2 : by) & 0xf));
+            lcd_write_command_e(LCD_SET_COLUMN | ((x >> 4) & 0xf), x & 0xf);
+
+            lcd_grey_data(values, phases, width);
+            values += stride;
+            phases += stride;
+            by++;
+        }
+    }
 }
 
 /* Update the display.
    This must be called after all other LCD functions that change the display. */
-void lcd_update(void) ICODE_ATTR;
 void lcd_update(void)
 {
     int y;
     if (initialized)
     {
-        for(y = 0;y < LCD_FBHEIGHT;y++) {
+        for(y = 0;y < LCD_FBHEIGHT;y++) 
+        {
             /* Copy display bitmap to hardware.
                The COM48-COM63 lines are not connected so we have to skip
                them. Further, the column address doesn't wrap, so we
                have to update one page at a time. */
-            lcd_write_command(LCD_SET_PAGE | (y>5?y+2:y));
+            lcd_write_command(LCD_SET_PAGE | (y > 5 ? y + 2 : y));
             lcd_write_command_e(LCD_SET_COLUMN | 0, 0);
             lcd_write_data(lcd_framebuffer[y], LCD_WIDTH);
         }
@@ -514,7 +268,6 @@ void lcd_update(void)
 }
 
 /* Update a fraction of the display. */
-void lcd_update_rect(int, int, int, int) ICODE_ATTR;
 void lcd_update_rect(int x, int y, int width, int height)
 {
     if (initialized)
@@ -536,7 +289,7 @@ void lcd_update_rect(int x, int y, int width, int height)
            COM48-COM63 are not connected, so we need to skip those */
         for (; y <= ymax; y++) 
         {
-            lcd_write_command(LCD_SET_PAGE | ((y > 5?y + 2:y) & 0xf));
+            lcd_write_command(LCD_SET_PAGE | ((y > 5 ? y + 2 : y) & 0xf));
             lcd_write_command_e(LCD_SET_COLUMN | ((x >> 4) & 0xf), x & 0xf);
 
             lcd_write_data(&lcd_framebuffer[y][x], width);
@@ -547,19 +300,23 @@ void lcd_update_rect(int x, int y, int width, int height)
 void lcd_set_invert_display(bool yesno)
 {
     cached_invert = yesno;
-    if(initialized)
+    if (initialized)
         lcd_write_command(LCD_REVERSE | yesno);
 }
 
 void lcd_set_flip(bool yesno)
 {
     cached_flip = yesno;
-    if(initialized) {
-        if(yesno) {
+    if (initialized)
+    {
+        if(yesno) 
+        {
             lcd_write_command(LCD_SELECT_ADC | 0);
             lcd_write_command(LCD_SELECT_SHL | 0);
             lcd_write_command_e(LCD_SET_COM0, 16);
-        } else {
+        }
+        else 
+        {
             lcd_write_command(LCD_SELECT_ADC | 1);
             lcd_write_command(LCD_SELECT_SHL | 8);
             lcd_write_command_e(LCD_SET_COM0, 0);