4 files changed, 188 insertions, 11 deletions

diff --git a/firmware/SOURCES b/firmware/SOURCES
index 23b5baea3e..d765bb8720 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -854,7 +854,6 @@ target/arm/sandisk/audio-c200_e200.c
 
 #ifdef SANSA_E200
 drivers/lcd-memframe.c
-target/arm/lcd-as-memframe.S
 target/arm/sandisk/sansa-e200/lcd-e200.c
 target/arm/sandisk/sansa-e200/button-e200.c
 target/arm/sandisk/sansa-e200/powermgmt-e200.c
@@ -1034,7 +1033,6 @@ target/arm/iriver/h10/lcd-h10_5gb.c
 
 #ifdef GIGABEAT_F
 drivers/lcd-memframe.c
-target/arm/lcd-as-memframe.S
 target/arm/s3c2440/gigabeat-fx/ata-meg-fx.c
 target/arm/s3c2440/gigabeat-fx/backlight-meg-fx.c
 target/arm/s3c2440/gigabeat-fx/button-meg-fx.c
@@ -1051,7 +1049,6 @@ target/arm/s3c2440/gigabeat-fx/wmcodec-meg-fx.c
 #ifdef GIGABEAT_S
 drivers/lcd-memframe.c
 target/arm/bits-armv6.S
-target/arm/lcd-as-memframe.S
 target/arm/mmu-armv6.S
 target/arm/imx31/ata-imx31.c
 target/arm/imx31/avic-imx31.c
@@ -1103,9 +1100,6 @@ target/arm/tms320dm320/uart-dm320.c
 #endif /* CONFIG_CPU == DM320 */
 
 #ifdef MROBE_500
-#if !defined(LCD_USE_DMA)
-target/arm/lcd-as-memframe.S
-#endif
 target/arm/tms320dm320/mrobe-500/crt0-board.S
 target/arm/tms320dm320/mrobe-500/adc-mr500.c
 target/arm/tms320dm320/mrobe-500/ata-mr500.c
@@ -1124,7 +1118,6 @@ target/arm/tms320dm320/mrobe-500/usb-mr500.c
 
 #ifdef CREATIVE_ZVx
 drivers/lcd-memframe.c
-target/arm/lcd-as-memframe.S
 target/arm/tms320dm320/creative-zvm/adc-creativezvm.c
 target/arm/tms320dm320/creative-zvm/ata-creativezvm.c
 target/arm/tms320dm320/creative-zvm/dma-creativezvm.c
@@ -1138,7 +1131,6 @@ target/arm/tms320dm320/creative-zvm/usb-creativezvm.c
 
 #ifdef SANSA_CONNECT
 drivers/lcd-memframe.c
-target/arm/lcd-as-memframe.S
 target/arm/tms320dm320/sdmmc-dm320.c
 target/arm/tms320dm320/sansa-connect/crt0-board.S
 target/arm/tms320dm320/sansa-connect/lcd-sansaconnect.c
@@ -1347,7 +1339,6 @@ target/arm/as3525/lcd-as-e200v2-fuze-fuzev2.S
 drivers/lcd-memframe.c
 drivers/synaptics-rmi.c
 drivers/generic_i2c.c
-target/arm/lcd-as-memframe.S
 target/arm/imx233/sansa-fuzeplus/fmradio-i2c-fuzeplus.c
 target/arm/imx233/sansa-fuzeplus/backlight-fuzeplus.c
 target/arm/imx233/sansa-fuzeplus/lcd-fuzeplus.c
@@ -1389,7 +1380,6 @@ drivers/nand_id.c
 drivers/pcf50606.c
 drivers/pcf50635.c
 drivers/tsc200x.c
-target/arm/lcd-as-memframe.S
 target/arm/tcc780x/adc-tcc780x.c
 target/arm/tcc780x/system-tcc780x.c
 target/arm/tcc780x/kernel-tcc780x.c
@@ -1558,7 +1548,6 @@ target/arm/at91sam/lyre_proto1/timer-lyre_proto1.c
 
 #if defined(MINI2440)
 drivers/lcd-memframe.c
-target/arm/lcd-as-memframe.S
 target/arm/s3c2440/dma-s3c2440.c
 target/arm/s3c2440/sd-s3c2440.c
 target/arm/s3c2440/uart-s3c2440.c
diff --git a/firmware/asm/SOURCES b/firmware/asm/SOURCES
index 805727ea93..216089f003 100644
--- a/firmware/asm/SOURCES
+++ b/firmware/asm/SOURCES
@@ -5,3 +5,10 @@ memmove.c
 memset.c
 strlen.c
 #endif
+
+#if (defined(SANSA_E200) || defined(GIGABEAT_F) || defined(GIGABEAT_S) || \
+    defined(CREATIVE_ZVx) || defined(SANSA_CONNECT) || defined(SANSA_FUZEPLUS) || \
+    defined(COWON_D2) || defined(MINI2440) || (defined(MROBE_500) && !defined(LCD_USE_DMA))) && \
+    !defined(SIMULATOR)
+lcd-as-memframe.c
+#endif
diff --git a/firmware/target/arm/lcd-as-memframe.S b/firmware/asm/arm/lcd-as-memframe.S
index 52ab0447c2..52ab0447c2 100644
--- a/firmware/target/arm/lcd-as-memframe.S
+++ b/firmware/asm/arm/lcd-as-memframe.S
diff --git a/firmware/asm/lcd-as-memframe.c b/firmware/asm/lcd-as-memframe.c
new file mode 100644
index 0000000000..5f4917b721
--- /dev/null
+++ b/firmware/asm/lcd-as-memframe.c
@@ -0,0 +1,181 @@
+
+#include <string.h>
+#include "lcd.h"
+void lcd_copy_buffer_rect(fb_data *dst, fb_data *src, int width, int height)
+{
+    do {
+        memcpy(dst, src, width * sizeof(fb_data));
+        src += LCD_WIDTH;
+        dst += LCD_WIDTH;
+    } while (--height);
+}
+
+#define YFAC    (74)
+#define RVFAC   (101)
+#define GUFAC   (-24)
+#define GVFAC   (-51)
+#define BUFAC   (128)
+
+static inline int clamp(int val, int min, int max)
+{
+    if (val < min)
+        val = min;
+    else if (val > max)
+        val = max;
+    return val;
+}
+
+extern void lcd_write_yuv420_lines(fb_data *dst,
+                                   unsigned char const * const src[3],
+                                   int width,
+                                   int stride)
+{
+    /* Draw a partial YUV colour bitmap - similiar behavior to lcd_blit_yuv
+       in the core */
+    const unsigned char *ysrc, *usrc, *vsrc;
+    int height = 2, linecounter;
+    fb_data *row_end;
+
+    /* width and height must be >= 2 and an even number */
+    width &= ~1;
+    linecounter = height >> 1;
+
+#if LCD_WIDTH >= LCD_HEIGHT
+    row_end = dst + width;
+#else
+    row_end = dst + LCD_WIDTH * width;
+#endif
+
+    ysrc = src[0];
+    usrc = src[1];
+    vsrc = src[2];
+
+    /* stride => amount to jump from end of last row to start of next */
+    stride -= width;
+
+    /* upsampling, YUV->RGB conversion and reduction to RGB565 in one go */
+
+    do
+    {
+        int y, cb, cr, rv, guv, bu, r, g, b;
+
+        y  = YFAC*(*ysrc++ - 16);
+        cb = *usrc++ - 128;
+        cr = *vsrc++ - 128;
+
+        rv  =            RVFAC*cr;
+        guv = GUFAC*cb + GVFAC*cr;
+        bu  = BUFAC*cb;
+
+        r = y + rv;
+        g = y + guv;
+        b = y + bu;
+
+        if ((unsigned)(r | g | b) > 64*256-1)
+        {
+            r = clamp(r, 0, 64*256-1);
+            g = clamp(g, 0, 64*256-1);
+            b = clamp(b, 0, 64*256-1);
+        }
+
+        *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
+
+#if LCD_WIDTH >= LCD_HEIGHT
+        dst++;
+#else
+        dst += LCD_WIDTH;
+#endif
+
+        y = YFAC*(*ysrc++ - 16);
+        r = y + rv;
+        g = y + guv;
+        b = y + bu;
+
+        if ((unsigned)(r | g | b) > 64*256-1)
+        {
+            r = clamp(r, 0, 64*256-1);
+            g = clamp(g, 0, 64*256-1);
+            b = clamp(b, 0, 64*256-1);
+        }
+
+        *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
+
+#if LCD_WIDTH >= LCD_HEIGHT
+        dst++;
+#else
+        dst += LCD_WIDTH;
+#endif
+    }
+    while (dst < row_end);
+
+    ysrc    += stride;
+    usrc    -= width >> 1;
+    vsrc    -= width >> 1;
+
+#if LCD_WIDTH >= LCD_HEIGHT
+    row_end += LCD_WIDTH;
+    dst     += LCD_WIDTH - width;
+#else
+    row_end -= 1;
+    dst     -= LCD_WIDTH*width + 1;
+#endif
+
+    do
+    {
+        int y, cb, cr, rv, guv, bu, r, g, b;
+
+        y  = YFAC*(*ysrc++ - 16);
+        cb = *usrc++ - 128;
+        cr = *vsrc++ - 128;
+
+        rv  =            RVFAC*cr;
+        guv = GUFAC*cb + GVFAC*cr;
+        bu  = BUFAC*cb;
+
+        r = y + rv;
+        g = y + guv;
+        b = y + bu;
+
+        if ((unsigned)(r | g | b) > 64*256-1)
+        {
+            r = clamp(r, 0, 64*256-1);
+            g = clamp(g, 0, 64*256-1);
+            b = clamp(b, 0, 64*256-1);
+        }
+
+        *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
+
+#if LCD_WIDTH >= LCD_HEIGHT
+        dst++;
+#else
+        dst += LCD_WIDTH;
+#endif
+
+        y = YFAC*(*ysrc++ - 16);
+        r = y + rv;
+        g = y + guv;
+        b = y + bu;
+
+        if ((unsigned)(r | g | b) > 64*256-1)
+        {
+            r = clamp(r, 0, 64*256-1);
+            g = clamp(g, 0, 64*256-1);
+            b = clamp(b, 0, 64*256-1);
+        }
+
+        *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
+
+#if LCD_WIDTH >= LCD_HEIGHT
+        dst++;
+#else
+        dst += LCD_WIDTH;
+#endif
+    }
+    while (dst < row_end);
+}
+
+void lcd_write_yuv420_lines_odither(fb_data *dst,
+                                   unsigned char const * const src[3],
+                                   int width, int stride,
+                                   int x_screen, int y_screen)
+__attribute__((alias("lcd_write_yuv420_lines")));