From b780905b23300534554421264bb43df15881ac32 Mon Sep 17 00:00:00 2001
From: Jeffy Chen <jeffy.chen@rock-chips.com>
Date: Thu, 13 Jan 2022 09:15:54 +0800
Subject: [PATCH 4/6] Support PIXMAN_nv16 format

Signed-off-by: Jeffy Chen <jeffy.chen@rock-chips.com>
---
 pixman/pixman-access.c | 75 ++++++++++++++++++++++++++++++++++++++++++
 pixman/pixman.c        |  3 +-
 pixman/pixman.h        |  2 ++
 3 files changed, 79 insertions(+), 1 deletion(-)

diff --git a/pixman/pixman-access.c b/pixman/pixman-access.c
index e7f97ea..05c1a18 100644
--- a/pixman/pixman-access.c
+++ b/pixman/pixman-access.c
@@ -220,6 +220,14 @@
     ((uint8_t *) ((bits) + offset0 +                                    \
                   (stride) * ((line) >> 1)))
 
+/*
+ * NV16 setup and access macros
+ */
+
+#define NV16_SETUP(image) NV12_SETUP(image)
+#define NV16_Y(line) NV12_Y(line)
+#define NV16_UV(line) ((uint8_t *) ((bits) + offset0 + (stride) * (line)))
+
 /* Misc. helpers */
 
 static force_inline void
@@ -946,6 +954,42 @@ fetch_scanline_nv12 (bits_image_t   *image,
     }
 }
 
+static void
+fetch_scanline_nv16 (bits_image_t   *image,
+                     int             x,
+                     int             line,
+                     int             width,
+                     uint32_t *      buffer,
+                     const uint32_t *mask)
+{
+    NV16_SETUP (image);
+    uint8_t *y_line = NV16_Y (line);
+    uint8_t *uv_line = NV16_UV (line);
+    int i;
+    
+    for (i = 0; i < width; i++)
+    {
+	int16_t y, u, v;
+	int32_t r, g, b;
+
+	y = y_line[x + i] - 16;
+	u = uv_line[(x + i) & -2] - 128;
+	v = uv_line[((x + i) & -2) + 1] - 128;
+
+	/* R = 1.164(Y - 16) + 1.596(V - 128) */
+	r = 0x012b27 * y + 0x019a2e * v;
+	/* G = 1.164(Y - 16) - 0.813(V - 128) - 0.391(U - 128) */
+	g = 0x012b27 * y - 0x00d0f2 * v - 0x00647e * u;
+	/* B = 1.164(Y - 16) + 2.018(U - 128) */
+	b = 0x012b27 * y + 0x0206a2 * u;
+
+	*buffer++ = 0xff000000 |
+	    (r >= 0 ? r < 0x1000000 ? r         & 0xff0000 : 0xff0000 : 0) |
+	    (g >= 0 ? g < 0x1000000 ? (g >> 8)  & 0x00ff00 : 0x00ff00 : 0) |
+	    (b >= 0 ? b < 0x1000000 ? (b >> 16) & 0x0000ff : 0x0000ff : 0);
+    }
+}
+
 /**************************** Pixel wise fetching *****************************/
 
 #ifndef PIXMAN_FB_ACCESSORS
@@ -1189,6 +1233,32 @@ fetch_pixel_nv12 (bits_image_t *image,
 	(b >= 0 ? b < 0x1000000 ? (b >> 16) & 0x0000ff : 0x0000ff : 0);
 }
 
+static uint32_t
+fetch_pixel_nv16 (bits_image_t *image,
+		  int           offset,
+		  int           line)
+{
+    NV16_SETUP (image);
+    int16_t y = NV16_Y (line)[offset] - 16;
+    int16_t u = NV16_UV (line)[offset & -2] - 128;
+    int16_t v = NV16_UV (line)[(offset & -2) + 1] - 128;
+    int32_t r, g, b;
+    
+    /* R = 1.164(Y - 16) + 1.596(V - 128) */
+    r = 0x012b27 * y + 0x019a2e * v;
+    
+    /* G = 1.164(Y - 16) - 0.813(V - 128) - 0.391(U - 128) */
+    g = 0x012b27 * y - 0x00d0f2 * v - 0x00647e * u;
+    
+    /* B = 1.164(Y - 16) + 2.018(U - 128) */
+    b = 0x012b27 * y + 0x0206a2 * u;
+    
+    return 0xff000000 |
+	(r >= 0 ? r < 0x1000000 ? r         & 0xff0000 : 0xff0000 : 0) |
+	(g >= 0 ? g < 0x1000000 ? (g >> 8)  & 0x00ff00 : 0x00ff00 : 0) |
+	(b >= 0 ? b < 0x1000000 ? (b >> 16) & 0x0000ff : 0x0000ff : 0);
+}
+
 /*********************************** Store ************************************/
 
 #ifndef PIXMAN_FB_ACCESSORS
@@ -1672,6 +1742,11 @@ static const format_info_t accessors[] =
       fetch_pixel_nv12, fetch_pixel_generic_float,
       NULL, NULL },
 
+    { PIXMAN_nv16,
+      fetch_scanline_nv16, fetch_scanline_generic_float,
+      fetch_pixel_nv16, fetch_pixel_generic_float,
+      NULL, NULL },
+
     { PIXMAN_null },
 };
 
diff --git a/pixman/pixman.c b/pixman/pixman.c
index ac06002..cd7479a 100644
--- a/pixman/pixman.c
+++ b/pixman/pixman.c
@@ -1072,6 +1072,7 @@ pixman_format_supported_source (pixman_format_code_t format)
     case PIXMAN_yv12:
     case PIXMAN_i420:
     case PIXMAN_nv12:
+    case PIXMAN_nv16:
 	return TRUE;
 
     default:
@@ -1095,7 +1096,7 @@ pixman_format_supported_destination (pixman_format_code_t format)
 {
     /* YUV formats cannot be written to at the moment */
     if (format == PIXMAN_yuy2 || format == PIXMAN_yv12 ||
-	format == PIXMAN_nv12 || format == PIXMAN_i420)
+	format == PIXMAN_nv12 || format == PIXMAN_i420 || format == PIXMAN_nv16)
 	return FALSE;
 
     return pixman_format_supported_source (format);
diff --git a/pixman/pixman.h b/pixman/pixman.h
index e8b0a78..2ff0d7f 100644
--- a/pixman/pixman.h
+++ b/pixman/pixman.h
@@ -864,6 +864,7 @@ struct pixman_indexed
 /* HACK: Use maximum value to avoid conflict */
 #define PIXMAN_TYPE_NV12	0x3f
 #define PIXMAN_TYPE_I420	0x3e
+#define PIXMAN_TYPE_NV16	0x3d
 
 #define PIXMAN_FORMAT_COLOR(f)				\
 	(PIXMAN_FORMAT_TYPE(f) == PIXMAN_TYPE_ARGB ||	\
@@ -945,6 +946,7 @@ typedef enum {
 
 /* YUV formats */
     PIXMAN_yuy2 =	 PIXMAN_FORMAT(16,PIXMAN_TYPE_YUY2,0,0,0,0),
+    PIXMAN_nv16 =	 PIXMAN_FORMAT(16,PIXMAN_TYPE_NV16,0,0,0,0),
     PIXMAN_yv12 =	 PIXMAN_FORMAT(12,PIXMAN_TYPE_YV12,0,0,0,0),
     PIXMAN_i420 =	 PIXMAN_FORMAT(12,PIXMAN_TYPE_I420,0,0,0,0),
     PIXMAN_nv12 =	 PIXMAN_FORMAT(12,PIXMAN_TYPE_NV12,0,0,0,0),
-- 
2.20.1