1 files changed, 204 insertions, 1 deletions

diff --git a/firmware/drivers/lcd-16bit.c b/firmware/drivers/lcd-16bit.c
index 28c3285a9c..300dbba9ac 100644
--- a/firmware/drivers/lcd-16bit.c
+++ b/firmware/drivers/lcd-16bit.c
@@ -913,10 +913,213 @@ void lcd_bitmap_transparent(const fb_data *src, int x, int y,
     lcd_bitmap_transparent_part(src, 0, 0, width, x, y, width, height);
 }
 
+/**
+ * |R|   |1.000000 -0.000001  1.402000| |Y'|
+ * |G| = |1.000000 -0.334136 -0.714136| |Pb|
+ * |B|   |1.000000  1.772000  0.000000| |Pr|
+ * Scaled, normalized, rounded and tweaked to yield RGB 565:
+ * |R|   |74   0 101| |Y' -  16| >> 9
+ * |G| = |74 -24 -51| |Cb - 128| >> 8
+ * |B|   |74 128   0| |Cr - 128| >> 9
+ */
+#define YFAC    (74)
+#define RVFAC   (101)
+#define GUFAC   (-24)
+#define GVFAC   (-51)
+#define BUFAC   (128)
+
+static inline int clamp(int val, int min, int max)
+{
+    if (val < min)
+        val = min;
+    else if (val > max)
+        val = max;
+    return val;
+}
+
+__attribute__((weak)) void lcd_yuv_set_options(unsigned options)
+{
+    (void)options;
+}
+
+/* Draw a partial YUV colour bitmap - similiar behavior to lcd_blit_yuv
+   in the core */
+
+__attribute__((weak)) 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)
+{
+    const unsigned char *ysrc, *usrc, *vsrc;
+    int linecounter;
+    fb_data *dst, *row_end;
+    long z;
+
+    /* width and height must be >= 2 and an even number */
+    width &= ~1;
+    linecounter = height >> 1;
+
+#if LCD_WIDTH >= LCD_HEIGHT
+    dst     = &lcd_framebuffer[y][x];
+    row_end = dst + width;
+#else
+    dst     = &lcd_framebuffer[x][LCD_WIDTH - y - 1];
+    row_end = dst + LCD_WIDTH * width;
+#endif
+
+    z    = stride * src_y;
+    ysrc = src[0] + z + src_x;
+    usrc = src[1] + (z >> 2) + (src_x >> 1);
+    vsrc = src[2] + (usrc - src[1]);
+
+    /* stride => amount to jump from end of last row to start of next */
+    stride -= width;
+
+    /* upsampling, YUV->RGB conversion and reduction to RGB565 in one go */
+
+    do
+    {
+        do
+        {
+            int y, cb, cr, rv, guv, bu, r, g, b;
+
+            y  = YFAC*(*ysrc++ - 16);
+            cb = *usrc++ - 128;
+            cr = *vsrc++ - 128;
+
+            rv  =            RVFAC*cr;
+            guv = GUFAC*cb + GVFAC*cr;
+            bu  = BUFAC*cb;
+
+            r = y + rv;
+            g = y + guv;
+            b = y + bu;
+
+            if ((unsigned)(r | g | b) > 64*256-1)
+            {
+                r = clamp(r, 0, 64*256-1);
+                g = clamp(g, 0, 64*256-1);
+                b = clamp(b, 0, 64*256-1);
+            }
+
+            *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
+
+#if LCD_WIDTH >= LCD_HEIGHT
+            dst++;
+#else
+            dst += LCD_WIDTH;
+#endif
+
+            y = YFAC*(*ysrc++ - 16);
+            r = y + rv;
+            g = y + guv;
+            b = y + bu;
+
+            if ((unsigned)(r | g | b) > 64*256-1)
+            {
+                r = clamp(r, 0, 64*256-1);
+                g = clamp(g, 0, 64*256-1);
+                b = clamp(b, 0, 64*256-1);
+            }
+
+            *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
+
+#if LCD_WIDTH >= LCD_HEIGHT
+            dst++;
+#else
+            dst += LCD_WIDTH;
+#endif
+        }
+        while (dst < row_end);
+
+        ysrc    += stride;
+        usrc    -= width >> 1;
+        vsrc    -= width >> 1;
+
+#if LCD_WIDTH >= LCD_HEIGHT
+        row_end += LCD_WIDTH;
+        dst     += LCD_WIDTH - width;
+#else
+        row_end -= 1;
+        dst     -= LCD_WIDTH*width + 1;
+#endif
+
+        do
+        {
+            int y, cb, cr, rv, guv, bu, r, g, b;
+
+            y  = YFAC*(*ysrc++ - 16);
+            cb = *usrc++ - 128;
+            cr = *vsrc++ - 128;
+
+            rv  =            RVFAC*cr;
+            guv = GUFAC*cb + GVFAC*cr;
+            bu  = BUFAC*cb;
+
+            r = y + rv;
+            g = y + guv;
+            b = y + bu;
+
+            if ((unsigned)(r | g | b) > 64*256-1)
+            {
+                r = clamp(r, 0, 64*256-1);
+                g = clamp(g, 0, 64*256-1);
+                b = clamp(b, 0, 64*256-1);
+            }
+
+            *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
+
+#if LCD_WIDTH >= LCD_HEIGHT
+            dst++;
+#else
+            dst += LCD_WIDTH;
+#endif
+
+            y = YFAC*(*ysrc++ - 16);
+            r = y + rv;
+            g = y + guv;
+            b = y + bu;
+
+            if ((unsigned)(r | g | b) > 64*256-1)
+            {
+                r = clamp(r, 0, 64*256-1);
+                g = clamp(g, 0, 64*256-1);
+                b = clamp(b, 0, 64*256-1);
+            }
+
+            *dst = LCD_RGBPACK_LCD(r >> 9, g >> 8, b >> 9);
+
+#if LCD_WIDTH >= LCD_HEIGHT
+            dst++;
+#else
+            dst += LCD_WIDTH;
+#endif
+        }
+        while (dst < row_end);
+
+        ysrc    += stride;
+        usrc    += stride >> 1;
+        vsrc    += stride >> 1;
+
+#if LCD_WIDTH >= LCD_HEIGHT
+        row_end += LCD_WIDTH;
+        dst     += LCD_WIDTH - width;
+#else
+        row_end -= 1;
+        dst     -= LCD_WIDTH*width + 1;
+#endif
+    }
+    while (--linecounter > 0);
+
+#if LCD_WIDTH >= LCD_HEIGHT
+    lcd_update_rect(x, y, width, height);
+#else
+    lcd_update_rect(LCD_WIDTH - y - height, x, height, width);
+#endif
+}
+
 #define ROW_INC LCD_WIDTH
 #define COL_INC 1
 
 #include "lcd-16bit-common.c"
 
 #include "lcd-bitmap-common.c"
-