Revert accidental tree commit. /me needs a smaller enter key...

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

9 files changed, 401 insertions, 152 deletions

diff --git a/firmware/SOURCES b/firmware/SOURCES
index 1d60ea1..0a8ac2a 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -513,7 +513,6 @@ 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/backlight.c b/firmware/backlight.c
index 4c4471a..0f66fbf 100644
--- a/firmware/backlight.c
+++ b/firmware/backlight.c
@@ -468,7 +468,7 @@ void backlight_thread(void)
                 lcd_remote_off();
                 break;
 #elif defined HAVE_REMOTE_LCD_AS_MAIN
-            case SYS_REMOTE_PLUGGED:        
+            case SYS_REMOTE_PLUGGED:
                 lcd_on();
                 lcd_update();
                 break;
diff --git a/firmware/export/config-player.h b/firmware/export/config-player.h
index c69fe8c..b9e3035 100644
--- a/firmware/export/config-player.h
+++ b/firmware/export/config-player.h
@@ -46,7 +46,7 @@
 
 /* Uncomment this if you want to enable ATA power-off control.
  * Attention, some players crash when ATA power-off is enabled! */
-#define HAVE_ATA_POWER_OFF
+//#define HAVE_ATA_POWER_OFF
 
 /* Define this if you control ata power player style
    (with PB4, new player only) */
diff --git a/firmware/export/id3.h b/firmware/export/id3.h
index 267f1b4..bb3b6a6 100644
--- a/firmware/export/id3.h
+++ b/firmware/export/id3.h
@@ -49,7 +49,6 @@ enum
     AFMT_WAVPACK,      /* WavPack */
     AFMT_ALAC,         /* Apple Lossless Audio Codec */
     AFMT_AAC,          /* Advanced Audio Coding (AAC) in M4A container */
-    AFMT_MOD,          /* MOD File Format */
     AFMT_SHN,          /* Shorten */
     AFMT_SID,          /* SID File Format */
     AFMT_ADX,          /* ADX File Format */
diff --git a/firmware/id3.c b/firmware/id3.c
index 87b4ce9..f594528 100644
--- a/firmware/id3.c
+++ b/firmware/id3.c
@@ -90,9 +90,6 @@ const struct afmt_entry audio_formats[AFMT_NUM_CODECS] =
     [AFMT_AAC] =
         AFMT_ENTRY("AAC",  "aac",     NULL,          "mp4\0"      ),
     /* Shorten */
-    [AFMT_MOD] =
-        AFMT_ENTRY("MOD",  "mod",     NULL,          "mod\0"      ),
-    /* Shorten */
     [AFMT_SHN] =
         AFMT_ENTRY("SHN",  "shorten", NULL,          "shn\0"      ),
     /* SID File Format */
diff --git a/firmware/target/arm/iriver/h10/lcd-h10_5gb.c b/firmware/target/arm/iriver/h10/lcd-h10_5gb.c
index 669654f..5b022e0 100644
--- a/firmware/target/arm/iriver/h10/lcd-h10_5gb.c
+++ b/firmware/target/arm/iriver/h10/lcd-h10_5gb.c
@@ -108,7 +108,6 @@ void lcd_init_device(void)
 {  
     CLCD_CLOCK_SRC |= 0xc0000000; /* Set LCD interface clock to PLL */
     /* H10 LCD is initialised by the bootloader */
-    lcd_write_reg(R_ENTRY_MODE, 0x1030); /* BGR =1, ID1 = 1, ID0 = 1 */
 }
 
 /*** update functions ***/
@@ -276,51 +275,88 @@ void lcd_blit_yuv(unsigned char * const src[3],
 
 
 /* Update a fraction of the display. */
-void lcd_update_rect(int x, int y, int width, int height)
+void lcd_update_rect(int x0, int y0, int width, int height)
 {
-    const fb_data *addr;
-    int bytes_to_write;
-    
-    if (x + width >= LCD_WIDTH)
-        width = LCD_WIDTH - x;
-    if (y + height >= LCD_HEIGHT)
-        height = LCD_HEIGHT - y;
-        
-    if ((width <= 0) || (height <= 0))
-        return; /* Nothing left to do. 0 would hang the transfer. */
-        
-    /* Ensure x and width are both even, so we can read
-     * 32-bit aligned data from the framebuffer */
-    width = (width + (x & 1) + 1) & ~1;
-    x &= ~1;
-
-    lcd_write_reg(R_VERT_RAM_ADDR_POS, (LCD_HEIGHT-1) << 8);
-    lcd_write_reg(R_HORIZ_RAM_ADDR_POS, ((x + width - 1) << 8) | x);
-    lcd_write_reg(R_RAM_ADDR_SET, (y << 8) | x);
+    int x1, y1;
+    int newx,newwidth;
+    unsigned long *addr;
 
+    /* Ensure x and width are both even - so we can read 32-bit aligned 
+       data from lcd_framebuffer */
+    newx=x0&~1;
+    newwidth=width&~1;
+    if (newx+newwidth < x0+width) { newwidth+=2; }
+    x0=newx; width=newwidth;
+
+    /* calculate the drawing region */
+    y1 = (y0 + height) - 1;           /* max vert */
+    x1 = (x0 + width) - 1;          /* max horiz */
+
+
+    /* swap max horiz < start horiz */
+    if (y1 < y0) {
+        int t;
+        t = y0;
+        y0 = y1;
+        y1 = t;
+    }
+
+    /* swap max vert < start vert */
+    if (x1 < x0) {
+        int t;
+        t = x0;
+        x0 = x1;
+        x1 = t;
+    }
+
+    /* max horiz << 8 | start horiz */
+    lcd_write_reg(R_HORIZ_RAM_ADDR_POS, (x1 << 8) | x0);
+
+    /* max vert << 8 | start vert */
+    lcd_write_reg(R_VERT_RAM_ADDR_POS, (y1 << 8) | y0);
+
+    /* start vert << 8 | start horiz */
+    lcd_write_reg(R_RAM_ADDR_SET, (y0 << 8) | x0);
+
+    /* start drawing */
     lcd_send_cmd(R_WRITE_DATA_2_GRAM);
 
-    addr = &lcd_framebuffer[y][x];
-    bytes_to_write = width * height * sizeof(fb_data);  
-    /* must be <= 0x10000, but that's guaranteed on H10. */
+    addr = (unsigned long*)&lcd_framebuffer[y0][x0];
 
-    LCD2_BLOCK_CTRL   = 0x10000080;
-    LCD2_BLOCK_CONFIG = 0xc0010000 | (bytes_to_write - 1);
-    LCD2_BLOCK_CTRL   = 0x34000000;
-    
-    do
-    {
-        int w = width >> 1;
-        do
-        {
-            while (!(LCD2_BLOCK_CTRL & LCD2_BLOCK_TXOK));
-            LCD2_BLOCK_DATA = *(unsigned long*)addr;  /* output 2 pixels */
-            addr += 2;
+    while (height > 0) {
+        int c, r;
+        int h, pixels_to_write;
+
+        pixels_to_write = (width * height) * 2;
+        h = height;
+
+        /* calculate how much we can do in one go */
+        if (pixels_to_write > 0x10000) {
+            h = (0x10000/2) / width;
+            pixels_to_write = (width * h) * 2;
         }
-        while (--w > 0);
-        addr += LCD_WIDTH - width;
+
+        LCD2_BLOCK_CTRL = 0x10000080;
+        LCD2_BLOCK_CONFIG = 0xc0010000 | (pixels_to_write - 1);
+        LCD2_BLOCK_CTRL = 0x34000000;
+
+        /* for each row */
+        for (r = 0; r < h; r++) {
+            /* for each column */
+            for (c = 0; c < width; c += 2) {
+                while (!(LCD2_BLOCK_CTRL & LCD2_BLOCK_TXOK));
+
+                /* output 2 pixels */
+                LCD2_BLOCK_DATA = *addr++;
+            }
+            addr += (LCD_WIDTH - width)/2;
+        }
+
+        while (!(LCD2_BLOCK_CTRL & LCD2_BLOCK_READY));
+        LCD2_BLOCK_CONFIG = 0;
+
+        height -= h;
     }
-    while (--height > 0);
 }
 
 /* Update the display.
diff --git a/firmware/target/coldfire/iaudio/lcd-remote-iaudio.c b/firmware/target/coldfire/iaudio/lcd-remote-iaudio.c
index 6565d30..5a03dc6 100644
--- a/firmware/target/coldfire/iaudio/lcd-remote-iaudio.c
+++ b/firmware/target/coldfire/iaudio/lcd-remote-iaudio.c
@@ -64,106 +64,83 @@ bool remote_initialized = false;
 static inline void _write_byte(unsigned data)
 {
     asm volatile (
-        "move.w  %%sr,%%d2           \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 */
+        "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"
+        "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 */
+        "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"
         
-        "eor.l   %[cbit], %%d0       \n"  /* precalculate opposite state of clock line */
-        
+        "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   %[dbit], %%d0       \n"  /* 1: Flip data bit */
+        "eor.l   %%d1, (%[gpo0])     \n"  /* 1: Flip both CLK and DATA */
+        ".word   0x51fa              \n"  /* (trapf.w - shadow next insn) */
     "1:                              \n"
-        "move.l  %%d0, %%d1          \n"
-        "move.l  %%d0, (%[gpo0])     \n"  /* Output new state and set CLK = 0*/
-        "eor.l   %[cbit], %%d1       \n"
-        "nop                         \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   %[dbit], %%d0       \n"
+        "eor.l   %%d1, (%[gpo0])     \n"
+        ".word   0x51fa              \n"
     "1:                              \n"
-        "move.l  %%d1, (%[gpo0])     \n"  /* set CLK = 1 (delayed) */
-        "move.l  %%d0, %%d1          \n"
-        "move.l  %%d0, (%[gpo0])     \n"
-        "eor.l   %[cbit], %%d1       \n"
-        "nop                         \n"
+        "eor.l   %[cbit], (%[gpo0])  \n"
+        "eor.l   %[cbit], (%[gpo0])  \n"
 
-        "lsl.l   #1,%[data]          \n"  /* ..unrolled.. */
+        "lsl.l   #1,%[data]          \n"
         "bcc.s   1f                  \n"
-        "eor.l   %[dbit], %%d0       \n"
+        "eor.l   %%d1, (%[gpo0])     \n"
+        ".word   0x51fa              \n"
     "1:                              \n"
-        "move.l  %%d1, (%[gpo0])     \n"
-        "move.l  %%d0, %%d1          \n"
-        "move.l  %%d0, (%[gpo0])     \n"
-        "eor.l   %[cbit], %%d1       \n"
-        "nop                         \n"
+        "eor.l   %[cbit], (%[gpo0])  \n"
+        "eor.l   %[cbit], (%[gpo0])  \n"
 
-        "lsl.l   #1,%[data]          \n"  /* ..unrolled.. */
+        "lsl.l   #1,%[data]          \n"
         "bcc.s   1f                  \n"
-        "eor.l   %[dbit], %%d0       \n"
+        "eor.l   %%d1, (%[gpo0])     \n"
+        ".word   0x51fa              \n"
     "1:                              \n"
-        "move.l  %%d1, (%[gpo0])     \n"
-        "move.l  %%d0, %%d1          \n"
-        "move.l  %%d0, (%[gpo0])     \n"
-        "eor.l   %[cbit], %%d1       \n"
-        "nop                         \n"
+        "eor.l   %[cbit], (%[gpo0])  \n"
+        "eor.l   %[cbit], (%[gpo0])  \n"
 
-        "lsl.l   #1,%[data]          \n"  /* ..unrolled.. */
+        "lsl.l   #1,%[data]          \n"
         "bcc.s   1f                  \n"
-        "eor.l   %[dbit], %%d0       \n"
+        "eor.l   %%d1, (%[gpo0])     \n"
+        ".word   0x51fa              \n"
     "1:                              \n"
-        "move.l  %%d1, (%[gpo0])     \n"
-        "move.l  %%d0, %%d1          \n"
-        "move.l  %%d0, (%[gpo0])     \n"
-        "eor.l   %[cbit], %%d1       \n"
-        "nop                         \n"
+        "eor.l   %[cbit], (%[gpo0])  \n"
+        "eor.l   %[cbit], (%[gpo0])  \n"
 
-        "lsl.l   #1,%[data]          \n"  /* ..unrolled.. */
+        "lsl.l   #1,%[data]          \n"
         "bcc.s   1f                  \n"
-        "eor.l   %[dbit], %%d0       \n"
+        "eor.l   %%d1, (%[gpo0])     \n"
+        ".word   0x51fa              \n"
     "1:                              \n"
-        "move.l  %%d1, (%[gpo0])     \n"
-        "move.l  %%d0, %%d1          \n"
-        "move.l  %%d0, (%[gpo0])     \n"
-        "eor.l   %[cbit], %%d1       \n"
-        "nop                         \n"
+        "eor.l   %[cbit], (%[gpo0])  \n"
+        "eor.l   %[cbit], (%[gpo0])  \n"
 
-        "lsl.l   #1,%[data]          \n"  /* ..unrolled.. */
+        "lsl.l   #1,%[data]          \n"
         "bcc.s   1f                  \n"
-        "eor.l   %[dbit], %%d0       \n"
+        "eor.l   %%d1, (%[gpo0])     \n"
+        ".word   0x51fa              \n"
     "1:                              \n"
-        "move.l  %%d1, (%[gpo0])     \n"
-        "move.l  %%d0, %%d1          \n"
-        "move.l  %%d0, (%[gpo0])     \n"
-        "eor.l   %[cbit], %%d1       \n"
-        "nop                         \n"
+        "eor.l   %[cbit], (%[gpo0])  \n"
+        "eor.l   %[cbit], (%[gpo0])  \n"
 
-        "lsl.l   #1,%[data]          \n"  /* ..unrolled.. */
+        "lsl.l   #1,%[data]          \n"
         "bcc.s   1f                  \n"
-        "eor.l   %[dbit], %%d0       \n"
+        "eor.l   %%d1, (%[gpo0])     \n"
+        ".word   0x51fa              \n"
     "1:                              \n"
-        "move.l  %%d1, (%[gpo0])     \n"
-        "move.l  %%d0, %%d1          \n"
-        "move.l  %%d0, (%[gpo0])     \n"
-        "eor.l   %[cbit], %%d1       \n"
-        "nop                         \n"
-
-        "nop                         \n"
-        "move.l  %%d1, (%[gpo0])     \n" /* set CLK = 1 (delayed) */
-
-        "move.w  %%d2, %%sr          \n" /* Restore interrupt level */
+        "eor.l   %[cbit], (%[gpo0])  \n"
+        "eor.l   %[cbit], (%[gpo0])  \n"
         : /* outputs */
         [data]"+d"(data)
         : /* inputs */
@@ -171,7 +148,7 @@ static inline void _write_byte(unsigned data)
         [cbit]"d"(0x00004000),
         [dbit]"d"(0x00002000)
         : /* clobbers */
-        "d0", "d1", "d2"
+        "d0", "d1"
     );
 }
 
@@ -180,7 +157,7 @@ static inline void _write_byte(unsigned data)
 static inline void _write_fast(unsigned data)
 {
     asm volatile (
-        "move.w  %%sr,%%d2           \n"  /* Get current interrupt level */
+        "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 */
@@ -262,7 +239,7 @@ static inline void _write_fast(unsigned data)
         "move.l  %%d1, (%[gpo0])     \n"
         "move.l  %%d0, (%[gpo0])     \n"
 
-        "move.w  %%d2, %%sr          \n" /* Restore interrupt level */
+        "move.w  %%d3, %%sr          \n" /* Restore interrupt level */
         : /* outputs */
         [data]"+d"(data)
         : /* inputs */
@@ -270,7 +247,7 @@ static inline void _write_fast(unsigned data)
         [cbit]"d"(0x00004000),
         [dbit]"d"(0x00002000)
         : /* clobbers */
-        "d0", "d1", "d2"
+        "d0", "d1", "d2", "d3"
     );
 }
 
diff --git a/firmware/target/coldfire/iaudio/m3/button-m3.c b/firmware/target/coldfire/iaudio/m3/button-m3.c
index f5d4b59..ecea221 100644
--- a/firmware/target/coldfire/iaudio/m3/button-m3.c
+++ b/firmware/target/coldfire/iaudio/m3/button-m3.c
@@ -116,7 +116,7 @@ int button_read_device(void)
                     else
                         btn |= BUTTON_RC_VOL_DOWN;
         }
-        if ((GPIO_READ & 0x80000000) == 0)    
+        if ((GPIO_READ & 0x80000000) == 0)
             btn |= BUTTON_RC_PLAY;
     }
 
diff --git a/firmware/target/coldfire/iaudio/m3/lcd-m3.c b/firmware/target/coldfire/iaudio/m3/lcd-m3.c
index 4bc2238..ae72832 100644
--- a/firmware/target/coldfire/iaudio/m3/lcd-m3.c
+++ b/firmware/target/coldfire/iaudio/m3/lcd-m3.c
@@ -61,6 +61,263 @@ 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;
@@ -200,9 +457,6 @@ 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,
@@ -223,28 +477,20 @@ void lcd_blit_mono(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);
+/* TODO: implement grey blit function */
 
 /* 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)
 {
-    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++;
-        }
-    }
+    (void)values;
+    (void)phases;
+    (void)x;
+    (void)by;
+    (void)width;
+    (void)bheight;
+    (void)stride;
 }
 
 /* Update the display.
@@ -255,13 +501,12 @@ 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);
         }
@@ -291,7 +536,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);
@@ -302,23 +547,19 @@ 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);