[PATCH kms++ 4/4] kms++util: Add Y210 drawing support

[Tomi Valkeinen <tomi.valkeinen+renesas@xxxxxxxxxxxxxxxx>]

Add support for drawing Y210 pixels.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen+renesas@xxxxxxxxxxxxxxxx>
---
 kms++util/src/drawing.cpp | 31 +++++++++++++++++++++++++++++++
 1 file changed, 31 insertions(+)

diff --git a/kms++util/src/drawing.cpp b/kms++util/src/drawing.cpp
index 79e0d90..7e1b40b 100644
--- a/kms++util/src/drawing.cpp
+++ b/kms++util/src/drawing.cpp
@@ -3,6 +3,7 @@
 
 #include <kms++/kms++.h>
 #include <kms++util/kms++util.h>
+#include <kms++util/endian.h>
 
 using namespace std;
 
@@ -179,6 +180,32 @@ static void draw_yuv422_packed_macropixel(IFramebuffer& buf, unsigned x, unsigne
 	}
 }
 
+static void draw_y2xx_packed_macropixel(IFramebuffer& buf, unsigned x, unsigned y,
+					  YUV yuv1, YUV yuv2)
+{
+	const uint32_t macro_size = 4;
+	uint16_t* p = (uint16_t*)(buf.map(0) + buf.stride(0) * y + x * macro_size);
+
+	switch (buf.format()) {
+	case PixelFormat::Y210: {
+		// XXX naive shift left, similar to 10-bit funcs in class RGB
+		uint16_t y0 = yuv1.y << 2;
+		uint16_t y1 = yuv2.y << 2;
+		uint16_t cb = ((yuv1.u  << 2) + (yuv2.u << 2)) / 2;
+		uint16_t cr = ((yuv1.v  << 2) + (yuv2.v << 2)) / 2;
+
+		write16le(y0, &p[0]);
+		write16le(cb, &p[1]);
+		write16le(y1, &p[2]);
+		write16le(cr, &p[3]);
+		break;
+	}
+
+	default:
+		throw std::invalid_argument("invalid pixelformat");
+	}
+}
+
 static void draw_yuv422_semiplanar_macropixel(IFramebuffer& buf, unsigned x, unsigned y,
 					      YUV yuv1, YUV yuv2)
 {
@@ -257,6 +284,10 @@ void draw_yuv422_macropixel(IFramebuffer& buf, unsigned x, unsigned y, YUV yuv1,
 		draw_yuv422_packed_macropixel(buf, x, y, yuv1, yuv2);
 		break;
 
+	case PixelFormat::Y210:
+		draw_y2xx_packed_macropixel(buf, x, y, yuv1, yuv2);
+		break;
+
 	case PixelFormat::NV16:
 	case PixelFormat::NV61:
 		draw_yuv422_semiplanar_macropixel(buf, x, y, yuv1, yuv2);
-- 
2.34.1