kms++: Add support for the planar YUV formats

Add support for the 6 planar YUV formats (YUV and YVU, combined with
420, 422 or 444 subsampling) to the PixelFormat class, the Python API,
and the drawing utilities.

Signed-off-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com>

2 files changed, 168 insertions, 8 deletions

diff --git a/kms++util/src/drawing.cpp b/kms++util/src/drawing.cpp
index a329b83..c1c639a 100644
--- a/kms++util/src/drawing.cpp
+++ b/kms++util/src/drawing.cpp
@@ -115,6 +115,33 @@ void draw_rgb_pixel(IFramebuffer& buf, unsigned x, unsigned y, RGB color)
 	}
 }
 
+void draw_yuv444_pixel(IFramebuffer& buf, unsigned x, unsigned y, YUV yuv)
+{
+	if (x >= buf.width() || y >= buf.height())
+		throw runtime_error("attempt to draw outside the buffer");
+
+	uint8_t *py = (uint8_t*)(buf.map(0) + buf.stride(0) * y + x);
+	uint8_t *pu = (uint8_t*)(buf.map(1) + buf.stride(1) * y + x);
+	uint8_t *pv = (uint8_t*)(buf.map(2) + buf.stride(2) * y + x);
+
+	switch (buf.format()) {
+	case PixelFormat::YUV444:
+		py[0] = yuv.y;
+		pu[0] = yuv.u;
+		pv[0] = yuv.v;
+		break;
+
+	case PixelFormat::YVU444:
+		py[0] = yuv.y;
+		pu[0] = yuv.v;
+		pv[0] = yuv.u;
+		break;
+
+	default:
+		throw std::invalid_argument("invalid pixelformat");
+	}
+}
+
 static void draw_yuv422_packed_macropixel(IFramebuffer& buf, unsigned x, unsigned y,
 					  YUV yuv1, YUV yuv2)
 {
@@ -190,6 +217,38 @@ static void draw_yuv422_semiplanar_macropixel(IFramebuffer& buf, unsigned x, uns
 	}
 }
 
+static void draw_yuv422_planar_macropixel(IFramebuffer& buf, unsigned x, unsigned y,
+					  YUV yuv1, YUV yuv2)
+{
+	uint8_t *py = (uint8_t*)(buf.map(0) + buf.stride(0) * y + x);
+	uint8_t *pu = (uint8_t*)(buf.map(1) + buf.stride(1) * y + x / 2);
+	uint8_t *pv = (uint8_t*)(buf.map(2) + buf.stride(2) * y + x / 2);
+
+	uint8_t y0 = yuv1.y;
+	uint8_t y1 = yuv2.y;
+	uint8_t u = (yuv1.u + yuv2.u) / 2;
+	uint8_t v = (yuv1.v + yuv2.v) / 2;
+
+	switch (buf.format()) {
+	case PixelFormat::YUV422:
+		py[0] = y0;
+		py[1] = y1;
+		pu[0] = u;
+		pv[0] = v;
+		break;
+
+	case PixelFormat::YVU422:
+		py[0] = y0;
+		py[1] = y1;
+		pu[0] = v;
+		pv[0] = u;
+		break;
+
+	default:
+		throw std::invalid_argument("invalid pixelformat");
+	}
+}
+
 void draw_yuv422_macropixel(IFramebuffer& buf, unsigned x, unsigned y, YUV yuv1, YUV yuv2)
 {
 	if ((x + 1) >= buf.width() || y >= buf.height())
@@ -210,20 +269,19 @@ void draw_yuv422_macropixel(IFramebuffer& buf, unsigned x, unsigned y, YUV yuv1,
 		draw_yuv422_semiplanar_macropixel(buf, x, y, yuv1, yuv2);
 		break;
 
+	case PixelFormat::YUV422:
+	case PixelFormat::YVU422:
+		draw_yuv422_planar_macropixel(buf, x, y, yuv1, yuv2);
+		break;
+
 	default:
 		throw std::invalid_argument("invalid pixelformat");
 	}
 }
 
-void draw_yuv420_macropixel(IFramebuffer& buf, unsigned x, unsigned y,
-			    YUV yuv1, YUV yuv2, YUV yuv3, YUV yuv4)
+static void draw_yuv420_semiplanar_macropixel(IFramebuffer& buf, unsigned x, unsigned y,
+					      YUV yuv1, YUV yuv2, YUV yuv3, YUV yuv4)
 {
-	if ((x + 1) >= buf.width() || (y + 1) >= buf.height())
-		throw runtime_error("attempt to draw outside the buffer");
-
-	ASSERT((x & 1) == 0);
-	ASSERT((y & 1) == 0);
-
 	uint8_t *py1 = (uint8_t*)(buf.map(0) + buf.stride(0) * (y + 0) + x);
 	uint8_t *py2 = (uint8_t*)(buf.map(0) + buf.stride(0) * (y + 1) + x);
 
@@ -260,6 +318,71 @@ void draw_yuv420_macropixel(IFramebuffer& buf, unsigned x, unsigned y,
 	}
 }
 
+static void draw_yuv420_planar_macropixel(IFramebuffer& buf, unsigned x, unsigned y,
+					  YUV yuv1, YUV yuv2, YUV yuv3, YUV yuv4)
+{
+	uint8_t *py1 = (uint8_t*)(buf.map(0) + buf.stride(0) * (y + 0) + x);
+	uint8_t *py2 = (uint8_t*)(buf.map(0) + buf.stride(0) * (y + 1) + x);
+
+	uint8_t *pu = (uint8_t*)(buf.map(1) + buf.stride(1) * (y / 2) + x / 2);
+	uint8_t *pv = (uint8_t*)(buf.map(2) + buf.stride(2) * (y / 2) + x / 2);
+
+	uint8_t y0 = yuv1.y;
+	uint8_t y1 = yuv2.y;
+	uint8_t y2 = yuv3.y;
+	uint8_t y3 = yuv4.y;
+	uint8_t u = (yuv1.u + yuv2.u + yuv3.u + yuv4.u) / 4;
+	uint8_t v = (yuv1.v + yuv2.v + yuv3.v + yuv4.v) / 4;
+
+	switch (buf.format()) {
+	case PixelFormat::YUV420:
+		py1[0] = y0;
+		py1[1] = y1;
+		py2[0] = y2;
+		py2[1] = y3;
+		pu[0] = u;
+		pv[0] = v;
+		break;
+
+	case PixelFormat::YVU420:
+		py1[0] = y0;
+		py1[1] = y1;
+		py2[0] = y2;
+		py2[1] = y3;
+		pu[0] = v;
+		pv[0] = u;
+		break;
+
+	default:
+		throw std::invalid_argument("invalid pixelformat");
+	}
+}
+
+void draw_yuv420_macropixel(IFramebuffer& buf, unsigned x, unsigned y,
+			    YUV yuv1, YUV yuv2, YUV yuv3, YUV yuv4)
+{
+	if ((x + 1) >= buf.width() || (y + 1) >= buf.height())
+		throw runtime_error("attempt to draw outside the buffer");
+
+	ASSERT((x & 1) == 0);
+	ASSERT((y & 1) == 0);
+
+	switch (buf.format()) {
+	case PixelFormat::NV12:
+	case PixelFormat::NV21:
+		draw_yuv420_semiplanar_macropixel(buf, x, y, yuv1, yuv2, yuv3, yuv4);
+		break;
+
+	case PixelFormat::YUV420:
+	case PixelFormat::YVU420:
+		draw_yuv420_planar_macropixel(buf, x, y, yuv1, yuv2, yuv3, yuv4);
+		break;
+
+	default:
+		throw std::invalid_argument("invalid pixelformat");
+	}
+}
+
 void draw_rect(IFramebuffer &fb, uint32_t x, uint32_t y, uint32_t w, uint32_t h, RGB color)
 {
 	unsigned i, j;
@@ -283,12 +406,23 @@ void draw_rect(IFramebuffer &fb, uint32_t x, uint32_t y, uint32_t w, uint32_t h,
 		}
 		break;
 
+	case PixelFormat::YUV444:
+	case PixelFormat::YVU444:
+		for (j = 0; j < h; j++) {
+			for (i = 0; i < w; i++) {
+				draw_yuv444_pixel(fb, x + i, y + j, yuvcolor);
+			}
+		}
+		break;
+
 	case PixelFormat::UYVY:
 	case PixelFormat::YUYV:
 	case PixelFormat::YVYU:
 	case PixelFormat::VYUY:
 	case PixelFormat::NV16:
 	case PixelFormat::NV61:
+	case PixelFormat::YUV422:
+	case PixelFormat::YVU422:
 		for (j = 0; j < h; j++) {
 			for (i = 0; i < w; i += 2) {
 				draw_yuv422_macropixel(fb, x + i, y + j, yuvcolor, yuvcolor);
@@ -298,6 +432,8 @@ void draw_rect(IFramebuffer &fb, uint32_t x, uint32_t y, uint32_t w, uint32_t h,
 
 	case PixelFormat::NV12:
 	case PixelFormat::NV21:
+	case PixelFormat::YUV420:
+	case PixelFormat::YVU420:
 		for (j = 0; j < h; j += 2) {
 			for (i = 0; i < w; i += 2) {
 				draw_yuv420_macropixel(fb, x + i, y + j,
@@ -361,12 +497,25 @@ static void draw_char(IFramebuffer& buf, uint32_t xpos, uint32_t ypos, char c, R
 		}
 		break;
 
+	case PixelFormat::YUV444:
+	case PixelFormat::YVU444:
+		for (y = 0; y < 8; y++) {
+			for (x = 0; x < 8; x++) {
+				bool b = get_char_pixel(c, x, y);
+
+				draw_yuv444_pixel(buf, xpos + x, ypos + y, b ? yuvcolor : YUV(RGB()));
+			}
+		}
+		break;
+
 	case PixelFormat::UYVY:
 	case PixelFormat::YUYV:
 	case PixelFormat::YVYU:
 	case PixelFormat::VYUY:
 	case PixelFormat::NV16:
 	case PixelFormat::NV61:
+	case PixelFormat::YUV422:
+	case PixelFormat::YVU422:
 		for (y = 0; y < 8; y++) {
 			for (x = 0; x < 8; x += 2) {
 				bool b0 = get_char_pixel(c, x, y);
@@ -380,6 +529,8 @@ static void draw_char(IFramebuffer& buf, uint32_t xpos, uint32_t ypos, char c, R
 
 	case PixelFormat::NV12:
 	case PixelFormat::NV21:
+	case PixelFormat::YUV420:
+	case PixelFormat::YVU420:
 		for (y = 0; y < 8; y += 2) {
 			for (x = 0; x < 8; x += 2) {
 				bool b00 = get_char_pixel(c, x, y);
diff --git a/kms++util/src/testpat.cpp b/kms++util/src/testpat.cpp
index c120de3..ac38673 100644
--- a/kms++util/src/testpat.cpp
+++ b/kms++util/src/testpat.cpp
@@ -120,6 +120,15 @@ static void draw_test_pattern_part(IFramebuffer& fb, unsigned start_y, unsigned
 
 	case PixelColorType::YUV:
 		switch (plane_info.xsub + plane_info.ysub) {
+		case 2:
+			for (y = start_y; y < end_y; y++) {
+				for (x = 0; x < w; x++) {
+					RGB pixel = get_test_pattern_pixel(fb, x, y);
+					draw_yuv444_pixel(fb, x, y, pixel.yuv(yuvt));
+				}
+			}
+			break;
+
 		case 3:
 			for (y = start_y; y < end_y; y++) {
 				for (x = 0; x < w; x += 2) {