1 files changed, 63 insertions, 0 deletions

diff --git a/firmware/target/coldfire/iaudio/x5/lcd-x5.c b/firmware/target/coldfire/iaudio/x5/lcd-x5.c
index a6a4fc0176..266a381c40 100644
--- a/firmware/target/coldfire/iaudio/x5/lcd-x5.c
+++ b/firmware/target/coldfire/iaudio/x5/lcd-x5.c
@@ -414,6 +414,69 @@ bool lcd_active(void)
 #endif
 /*** update functions ***/
 
+/* Line write helper function for lcd_yuv_blit. Write two lines of yuv420.
+ * y should have two lines of Y back to back, 2nd line first.
+ * c should contain the Cb and Cr data for the two lines of Y back to back.
+ * Needs EMAC set to saturated, signed integer mode.
+ */
+extern void lcd_write_yuv420_lines(const unsigned char *y,
+                                   const unsigned char *c, int width);
+
+/* Performance function to blit a YUV bitmap directly to the LCD
+ * src_x, src_y, width and height should be even and within the LCD's
+ * boundaries.
+ */
+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)
+{
+    /* IRAM Y, Cb/bu, guv and Cb/rv buffers. */
+    unsigned char y_ibuf[LCD_WIDTH*2];
+    unsigned char c_ibuf[LCD_WIDTH];
+    const unsigned char *ysrc, *usrc, *vsrc;
+    const unsigned char *ysrc_max;
+
+    if (!display_on)
+        return;
+
+    width &= ~1;  /* stay on the safe side */
+    height &= ~1;
+
+    lcd_write_reg(R_ENTRY_MODE, R_ENTRY_MODE_DIT_HORZ);
+    /* Set start position and window */
+    lcd_write_reg(R_VERT_RAM_ADDR_POS, (LCD_WIDTH-1) << 8);
+
+    ysrc = src[0] + src_y * stride + src_x;
+    usrc = src[1] + (src_y * stride >> 2) + (src_x >> 1);
+    vsrc = src[2] + (src_y * stride >> 2) + (src_x >> 1);
+    ysrc_max = ysrc + height * stride;
+
+    unsigned long macsr = coldfire_get_macsr();
+    coldfire_set_macsr(EMAC_SATURATE);
+
+    do
+    {
+        lcd_write_reg(R_HORIZ_RAM_ADDR_POS, ((y + y_offset + 1) << 8) | (y + y_offset));
+        lcd_write_reg(R_RAM_ADDR_SET, (x << 8) | (y + y_offset));
+        lcd_begin_write_gram();
+
+        memcpy(y_ibuf + width, ysrc, width);
+        memcpy(y_ibuf, ysrc + stride, width);
+        memcpy(c_ibuf, usrc, width >> 1);
+        memcpy(c_ibuf + (width >> 1), vsrc, width >> 1);
+        lcd_write_yuv420_lines(y_ibuf, c_ibuf, width >> 1);
+        
+        y += 2;
+        ysrc += 2 * stride;
+        usrc += stride >> 1;
+        vsrc += stride >> 1;
+    }
+    while (ysrc < ysrc_max);
+
+    coldfire_set_macsr(macsr);
+} /* lcd_yuv_blit */
+
+
 /* Update the display.
    This must be called after all other LCD functions that change the
    lcd frame buffer. */