1 files changed, 37 insertions, 8 deletions

diff --git a/firmware/target/arm/sandisk/sansa-e200/lcd-e200.c b/firmware/target/arm/sandisk/sansa-e200/lcd-e200.c
index 3e1f74d7a1..0d3a1a3049 100644
--- a/firmware/target/arm/sandisk/sansa-e200/lcd-e200.c
+++ b/firmware/target/arm/sandisk/sansa-e200/lcd-e200.c
@@ -29,6 +29,7 @@
 /* Power and display status */
 static bool power_on   = false; /* Is the power turned on?   */
 static bool display_on NOCACHEBSS_ATTR = false; /* Is the display turned on? */
+static unsigned lcd_yuv_options NOCACHEBSS_ATTR = 0;
 
 /* Reverse Flag */
 #define R_DISP_CONTROL_NORMAL 0x0004
@@ -625,11 +626,22 @@ void lcd_blit(const fb_data* data, int x, int by, int width,
     (void)stride;
 }
 
+void lcd_yuv_set_options(unsigned options)
+{
+    lcd_yuv_options = options;
+}
+
 /* Line write helper function for lcd_yuv_blit. Write two lines of yuv420. */
 extern void lcd_write_yuv420_lines(fb_data *dst,
                                    unsigned char const * const src[3],
                                    int width,
                                    int stride);
+extern void lcd_write_yuv420_lines_odither(fb_data *dst,
+                                           unsigned char const * const src[3],
+                                           int width,
+                                           int stride,
+                                           int x_screen, /* To align dither pattern */
+                                           int y_screen);
 /* Performance function to blit a YUV bitmap directly to the LCD */
 /* For the e200 - show it rotated */
 /* So the LCD_WIDTH is now the height */
@@ -647,21 +659,38 @@ void lcd_yuv_blit(unsigned char * const src[3],
     width &= ~1;
     height >>= 1;
 
+    y = LCD_WIDTH - 1 - y;
     fb_data *dst = (fb_data*)lcd_driver_framebuffer + 
-                   x * LCD_WIDTH + (LCD_WIDTH - y) - 1;
+                   x * LCD_WIDTH + y;
 
     z = stride*src_y;
     yuv_src[0] = src[0] + z + src_x;
     yuv_src[1] = src[1] + (z >> 2) + (src_x >> 1);
     yuv_src[2] = src[2] + (yuv_src[1] - src[1]);
 
-    do
+    if (lcd_yuv_options & LCD_YUV_DITHER)
+    {
+        do
+        {
+            lcd_write_yuv420_lines_odither(dst, yuv_src, width, stride, y, x);
+            yuv_src[0] += stride << 1; /* Skip down two luma lines */
+            yuv_src[1] += stride >> 1; /* Skip down one chroma line */
+            yuv_src[2] += stride >> 1;
+            dst -= 2;
+            y -= 2;
+        }
+        while (--height > 0);
+    }
+    else
     {
-        lcd_write_yuv420_lines(dst, yuv_src, width, stride);
-        yuv_src[0] += stride << 1; /* Skip down two luma lines */
-        yuv_src[1] += stride >> 1; /* Skip down one chroma line */
-        yuv_src[2] += stride >> 1;
-        dst -= 2;
+        do
+        {
+            lcd_write_yuv420_lines(dst, yuv_src, width, stride);
+            yuv_src[0] += stride << 1; /* Skip down two luma lines */
+            yuv_src[1] += stride >> 1; /* Skip down one chroma line */
+            yuv_src[2] += stride >> 1;
+            dst -= 2;
+        }
+        while (--height > 0);
     }
-    while (--height > 0);
 }