3 files changed, 120 insertions, 19 deletions

diff --git a/apps/plugins/test_scanrate.c b/apps/plugins/test_scanrate.c
index fb59193272..1aa9febf30 100644
--- a/apps/plugins/test_scanrate.c
+++ b/apps/plugins/test_scanrate.c
@@ -89,6 +89,9 @@ PLUGIN_HEADER
 #define DEFAULT_SCAN_RATE 1500
 #define HORIZ_SCAN /* LCD controller updates the panel sideways */
 #define NEED_BOOST
+#elif defined MPIO_HD200
+#define DEFAULT_SCAN_RATE 1460
+#define NEED_BOOST
 #elif defined IAUDIO_M5
 #define DEFAULT_SCAN_RATE 730
 #elif defined IPOD_1G2G
diff --git a/firmware/target/coldfire/mpio/hd200/lcd-as-hd200.S b/firmware/target/coldfire/mpio/hd200/lcd-as-hd200.S
index add9f694de..9d23d18e2d 100644
--- a/firmware/target/coldfire/mpio/hd200/lcd-as-hd200.S
+++ b/firmware/target/coldfire/mpio/hd200/lcd-as-hd200.S
@@ -8,6 +8,7 @@
  * $Id:$
  *
  * Copyright (C) 2010 Marcin Bukat
+ * based on lcd-as-m3.S by Jens Arnold
  *
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License
@@ -99,5 +100,80 @@ lcd_write_data:
 
 .write_end:
     rts
+.wd_end:
     .size   lcd_write_data,.wd_end-lcd_write_data
 
+    .global lcd_mono_data
+    .type   lcd_mono_data, @function
+
+lcd_mono_data:
+    move.l  (4, %sp), %a0       /* p_bytes */
+    move.l  (8, %sp), %d0       /* count */
+    lea.l   LCD_BASE_ADDRESS+2, %a1  /* LCD data port address */
+
+.md_loop:
+    move.b  (%a0)+, %d1
+    move.w  %d1, (%a1)    /* byte transfers actually */
+    move.w  %d1, (%a1)
+    subq.l  #1, %d0
+    bne.s   .md_loop
+    rts
+.md_end:
+    .size   lcd_mono_data,.md_end-lcd_mono_data
+
+    .global     lcd_grey_data
+    .type       lcd_grey_data,@function
+
+lcd_grey_data:
+    lea.l   (-2*4, %sp), %sp
+    movem.l  %a2-%a3, (%sp)
+    movem.l (2*4+4, %sp), %a0-%a2  /* values, phases, length */
+    add.l   %a2, %a2
+    lea.l   (%a1, %a2.l*4), %a2    /* end address */
+    lea.l   LCD_BASE_ADDRESS+2, %a3 /* LCD data port address */
+.ph_loop:
+    clr.l   %d1
+    move.l  (%a1), %d0          /* fetch 4 pixel phases */
+    bclr.l  #31, %d0            /* Z = !(p0 & 0x80); p0 &= ~0x80; */
+    seq.b   %d1                 /* %d1 = ........................00000000 */
+    lsl.l   #1, %d1             /* %d1 = .......................00000000. */
+    bclr.l  #23, %d0            /* Z = !(p1 & 0x80); p1 &= ~0x80; */
+    seq.b   %d1                 /* %d1 = .......................011111111 */
+    lsl.l   #1, %d1             /* %d1 = ......................011111111. */
+    bclr.l  #15, %d0            /* Z = !(p2 & 0x80); p2 &= ~0x80; */
+    seq.b   %d1                 /* %d1 = ......................0122222222 */
+    lsl.l   #1, %d1             /* %d1 = .....................0122222222. */
+    bclr.l  #7, %d0             /* Z = !(p3 & 0x80); p3 &= ~0x80; */
+    seq.b   %d1                 /* %d1 = .....................01233333333 */
+    lsl.l   #1, %d1             /* %d1 = ....................01233333333. */
+    add.l   (%a0)+, %d0         /* add 4 pixel values to the phases */
+    move.l  %d0, (%a1)+         /* store new phases, advance pointer */
+
+    clr.l   %d1
+    move.l  (%a1), %d0          /* fetch 4 pixel phases */
+    bclr.l  #31, %d0            /* Z = !(p0 & 0x80); p0 &= ~0x80; */
+    seq.b   %d1                 /* %d1 = ....................012344444444 */
+    lsl.l   #1, %d1             /* %d1 = ...................012344444444. */
+    bclr.l  #23, %d0            /* Z = !(p1 & 0x80); p1 &= ~0x80; */
+    seq.b   %d1                 /* %d1 = ...................0123455555555 */
+    lsl.l   #1, %d1             /* %d1 = ..................0123455555555. */
+    bclr.l  #15, %d0            /* Z = !(p2 & 0x80); p2 &= ~0x80; */
+    seq.b   %d1                 /* %d1 = ..................01234566666666 */
+    lsl.l   #1, %d1             /* %d1 = .................01234566666666. */
+    bclr.l  #7, %d0             /* Z = !(p3 & 0x80); p3 &= ~0x80; */
+    seq.b   %d1                 /* %d1 = .................012345677777777 */
+    lsr.l   #7, %d1             /* %d1 = ........................01234567 */
+    add.l   (%a0)+, %d0         /* add 4 pixel values to the phases */
+    move.l  %d0, (%a1)+         /* store new phases, advance pointer */
+
+    move.w  %d1, (%a3)          /* transfer to lcd */
+    move.w  %d1, (%a3)          /* transfer to lcd */
+
+    cmp.l   %a2, %a1
+    bls    .ph_loop
+
+    movem.l (%sp), %a2-%a3
+    lea.l   (2*4, %sp), %sp
+    rts
+.grey_end:
+    .size    lcd_grey_data,.grey_end-lcd_grey_data
diff --git a/firmware/target/coldfire/mpio/hd200/lcd-hd200.c b/firmware/target/coldfire/mpio/hd200/lcd-hd200.c
index 8cb9e8e087..f3e9eef03b 100644
--- a/firmware/target/coldfire/mpio/hd200/lcd-hd200.c
+++ b/firmware/target/coldfire/mpio/hd200/lcd-hd200.c
@@ -215,27 +215,49 @@ void lcd_update_rect(int x, int y, int width, int height)
     
 }
 
-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;
-    /* empty stub */
-}
+/* 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,
                    int bheight, int stride)
 {
-    (void)data;
-    (void)x;
-    (void)by;
-    (void)width;
-    (void)bheight;
-    (void)stride;
-    /* empty stub */
+    if (lcd_initialized)
+    {
+        while (bheight--)
+        {
+            lcd_write_command(LCD_SET_PAGE | (by & 0xf));
+            lcd_write_command_e(LCD_SET_COLUMN | ((x >> 4) & 0xf), x & 0xf);
+
+            lcd_mono_data(data, width);
+            data += stride;
+            by++;
+        }
+    }
 }
+
+/* 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)
+{
+    if (lcd_initialized)
+    {
+        stride <<= 3; /* 8 pixels per block */
+        while (bheight--)
+        {
+            lcd_write_command(LCD_SET_PAGE | (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++;
+        }
+    }
+}
+