1 files changed, 43 insertions, 11 deletions

diff --git a/firmware/target/arm/sandisk/sansa-c200/lcd-c200.c b/firmware/target/arm/sandisk/sansa-c200/lcd-c200.c
index 5975ab159d..fa8581f286 100644
--- a/firmware/target/arm/sandisk/sansa-c200/lcd-c200.c
+++ b/firmware/target/arm/sandisk/sansa-c200/lcd-c200.c
@@ -22,6 +22,9 @@
 #include "kernel.h"
 #include "system.h"
 
+/* Display status */
+static unsigned lcd_yuv_options NOCACHEBSS_ATTR = 0;
+
 /* LCD command set for Samsung S6B33B2 */
 
 #define R_OSCILLATION_MODE     0x02
@@ -206,10 +209,20 @@ 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(unsigned char const * const src[3],
                                    int width,
                                    int stride);
+extern void lcd_write_yuv420_lines_odither(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 */
 void lcd_yuv_blit(unsigned char * const src[3],
                   int src_x, int src_y, int stride,
@@ -236,19 +249,38 @@ void lcd_yuv_blit(unsigned char * const src[3],
     lcd_send_command(x);
     lcd_send_command(x + width - 1);
 
-    do
+    if (lcd_yuv_options & LCD_YUV_DITHER)
     {
-        lcd_send_command(R_Y_ADDR_AREA);
-        lcd_send_command(y);
-        lcd_send_command(y + 1);
-
-        lcd_write_yuv420_lines(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;
-        y += 2;
+        do
+        {
+            lcd_send_command(R_Y_ADDR_AREA);
+            lcd_send_command(y);
+            lcd_send_command(y + 1);
+
+            lcd_write_yuv420_lines_odither(yuv_src, width, stride, x, y);
+            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;
+            y += 2;
+        }
+         while (--height > 0);
+    }
+    else
+    {
+        do
+        {
+            lcd_send_command(R_Y_ADDR_AREA);
+            lcd_send_command(y);
+            lcd_send_command(y + 1);
+
+            lcd_write_yuv420_lines(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;
+            y += 2;
+        }
+         while (--height > 0);
     }
-    while (--height > 0);
 }
 
 /* Update the display.