1 files changed, 46 insertions, 0 deletions

diff --git a/firmware/target/arm/s5l8702/ipod6g/lcd-6g.c b/firmware/target/arm/s5l8702/ipod6g/lcd-6g.c
index e1406549f4..14647a5697 100644
--- a/firmware/target/arm/s5l8702/ipod6g/lcd-6g.c
+++ b/firmware/target/arm/s5l8702/ipod6g/lcd-6g.c
@@ -530,3 +530,49 @@ void lcd_update_rect(int x, int y, int width, int height)
 
     displaylcd_dma(pixels);
 }
+
+/* Line write helper function for lcd_yuv_blit. Writes two lines of yuv420. */
+extern void lcd_write_yuv420_lines(unsigned char const * const src[3],
+                                   uint16_t* outbuf,
+                                   int width,
+                                   int stride);
+
+/* Blit a YUV bitmap directly to the LCD */
+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) ICODE_ATTR;
+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 int z;
+    unsigned char const * yuv_src[3];
+
+#ifdef HAVE_LCD_SLEEP
+    if (!lcd_active()) return;
+#endif
+
+    width = (width + 1) & ~1;       /* ensure width is even */
+
+    int pixels = width * height;
+    uint16_t* out = lcd_dblbuf[0];
+
+    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]);
+
+    displaylcd_setup(x, y, width, height);
+
+    height >>= 1;
+
+    do {
+        lcd_write_yuv420_lines(yuv_src, out, width, stride);
+        yuv_src[0] += stride << 1;
+        yuv_src[1] += stride >> 1; /* Skip down one chroma line */
+        yuv_src[2] += stride >> 1;
+        out += width << 1;
+    } while (--height);
+
+    displaylcd_dma(pixels);
+}