1 files changed, 98 insertions, 0 deletions

diff --git a/firmware/target/arm/as3525/sansa-e200v2/lcd-e200v2.c b/firmware/target/arm/as3525/sansa-e200v2/lcd-e200v2.c
index 141340c003..f69ad48793 100644
--- a/firmware/target/arm/as3525/sansa-e200v2/lcd-e200v2.c
+++ b/firmware/target/arm/as3525/sansa-e200v2/lcd-e200v2.c
@@ -336,6 +336,104 @@ bool lcd_active(void)
 
 /*** update functions ***/
 
+static unsigned lcd_yuv_options = 0;
+
+void lcd_yuv_set_options(unsigned options)
+{
+    lcd_yuv_options = options;
+}
+
+
+#ifndef BOOTLOADER
+static void lcd_window_blit(int xmin, int ymin, int xmax, int ymax)
+{
+    if (!display_flipped)
+    {
+        lcd_write_reg(R_HORIZ_RAM_ADDR_POS,
+            ((LCD_WIDTH-1 - xmin) << 8) | (LCD_WIDTH-1 - xmax));
+        lcd_write_reg(R_VERT_RAM_ADDR_POS,  (ymax << 8) | ymin);
+        lcd_write_reg(R_RAM_ADDR_SET,
+            (ymin << 8) | (LCD_WIDTH-1 - xmin));
+        }
+        else
+    {
+        lcd_write_reg(R_HORIZ_RAM_ADDR_POS, (xmax << 8) | xmin);
+        lcd_write_reg(R_VERT_RAM_ADDR_POS,  (ymax << 8) | ymin);
+        lcd_write_reg(R_RAM_ADDR_SET,       (ymax << 8) | xmin);
+    }
+}
+
+/* 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
+ * src_x, src_y, width and height should be even
+ * x, y, width and height have to be within LCD bounds
+ */
+void lcd_blit_yuv(unsigned char * const src[3],
+                  int src_x, int src_y, int stride,
+                  int x, int y, int width, int height)
+{
+    unsigned char const * yuv_src[3];
+    off_t z;
+
+    /* Sorry, but width and height must be >= 2 or else */
+    width &= ~1;
+    height >>= 1;
+
+    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]);
+
+    lcd_write_reg(R_ENTRY_MODE,
+        display_flipped ? R_ENTRY_MODE_VIDEO_FLIPPED : R_ENTRY_MODE_VIDEO_NORMAL
+    );
+
+    if (lcd_yuv_options & LCD_YUV_DITHER)
+    {
+        do
+        {
+            lcd_window_blit(y, x, y+1, x+width-1);
+
+            lcd_write_cmd(R_WRITE_DATA_2_GRAM);
+
+            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_window_blit(y, x, y+1, x+width-1);
+
+            lcd_write_cmd(R_WRITE_DATA_2_GRAM);
+
+            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);
+    }
+}
+
+#endif
+
+
 /* Update the display.
    This must be called after all other LCD functions that change the display. */
 void lcd_update(void)