1 files changed, 98 insertions, 0 deletions

diff --git a/firmware/drivers/lcd-memframe.c b/firmware/drivers/lcd-memframe.c
index 357b4af32a..bb1682b074 100644
--- a/firmware/drivers/lcd-memframe.c
+++ b/firmware/drivers/lcd-memframe.c
@@ -110,3 +110,101 @@ void lcd_update_rect(int x, int y, int width, int height)
     }
 }
 #endif /* LCD_OPTIMIZED_UPDATE_RECT */
+
+
+/*** YUV functions ***/
+static unsigned lcd_yuv_options SHAREDBSS_ATTR = 0;
+
+
+/* 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);
+
+void lcd_yuv_set_options(unsigned options)
+{
+    lcd_yuv_options = options;
+}
+
+#ifndef LCD_OPTIMIZED_BLIT_YUV
+/* 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.
+ *
+ * For portrait LCDs, show it rotated counterclockwise by 90 degrees
+ */
+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)
+{
+    /* Macrofy the bits that change between orientations */
+#if CONFIG_ORIENTATION == SCREEN_PORTRAIT
+    #define LCD_FRAMEBUF_ADDR_ORIENTED(col, row) \
+        LCD_FRAMEBUF_ADDR(row, col)
+    #define lcd_write_yuv420_lines_odither_oriented(dst, src, w, s, col, row) \
+        lcd_write_yuv420_lines_odither(dst, src, w, s, row, col)
+    #define YUV_NEXTLINE() dst -= 2
+    #define YUV_DITHER_NEXTLINE() dst -= 2, y -= 2
+#else
+    #define LCD_FRAMEBUF_ADDR_ORIENTED(col, row) \
+        LCD_FRAMEBUF_ADDR(col, row)
+    #define lcd_write_yuv420_lines_odither_oriented(dst, src, w, s, col, row) \
+        lcd_write_yuv420_lines_odither(dst, src, w, s, col, row)
+    #define YUV_NEXTLINE() dst += 2*LCD_FBWIDTH
+    #define YUV_DITHER_NEXTLINE() dst += 2*LCD_FBWIDTH, y += 2
+#endif
+
+    if (!lcd_write_enabled())
+        return;
+
+    /* Sorry, but width and height must be >= 2 or else */
+    width &= ~1;
+    height >>= 1;
+
+#if CONFIG_ORIENTATION == SCREEN_PORTRAIT
+    /* Adjust portrait coordinates to make (0, 0) the upper right corner */
+    y = LCD_WIDTH - 1 - y;
+#endif
+
+    fb_data *dst = LCD_FRAMEBUF_ADDR_ORIENTED(x, y);
+    int z = stride*src_y;
+
+    unsigned char const * yuv_src[3];
+    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]);
+
+    if (lcd_yuv_options & LCD_YUV_DITHER)
+    {
+        do
+        {
+            lcd_write_yuv420_lines_odither_oriented(dst, 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;
+            YUV_DITHER_NEXTLINE();
+        }
+        while (--height > 0);
+    }
+    else
+    {
+        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;
+            YUV_NEXTLINE();
+        }
+        while (--height > 0);
+    }
+}
+#endif /* LCD_OPTIMIZED_BLIT_YUV */