qcam: format_converter: Add fully-planar YUV formats support

Add support for the YUV420, YVU420 and YUV422 formats supported by
libcamera. YUV420 can be produced by the Raspberry Pi pipeline handler,
being able to display it is useful for testing.

Signed-off-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
Reviewed-by: Kieran Bingham <kieran.bingham@ideasonboard.com>
Reviewed-by: Paul Elder <paul.elder@ideasonboard.com>

2 files changed, 68 insertions, 0 deletions

diff --git a/src/qcam/format_converter.cpp b/src/qcam/format_converter.cpp
index 7979ea8a..d4d3223b 100644
--- a/src/qcam/format_converter.cpp
+++ b/src/qcam/format_converter.cpp
@@ -8,6 +8,7 @@
 #include "format_converter.h"
 
 #include <errno.h>
+#include <utility>
 
 #include <QImage>
 
@@ -141,6 +142,25 @@ int FormatConverter::configure(const libcamera::PixelFormat &format,
 		cb_pos_ = 1;
 		break;
 
+	case libcamera::formats::YUV420:
+		formatFamily_ = YUVPlanar;
+		horzSubSample_ = 2;
+		vertSubSample_ = 2;
+		nvSwap_ = false;
+		break;
+	case libcamera::formats::YVU420:
+		formatFamily_ = YUVPlanar;
+		horzSubSample_ = 2;
+		vertSubSample_ = 2;
+		nvSwap_ = true;
+		break;
+	case libcamera::formats::YUV422:
+		formatFamily_ = YUVPlanar;
+		horzSubSample_ = 2;
+		vertSubSample_ = 1;
+		nvSwap_ = false;
+		break;
+
 	case libcamera::formats::MJPEG:
 		formatFamily_ = MJPEG;
 		break;
@@ -172,6 +192,9 @@ void FormatConverter::convert(const Image *src, size_t size, QImage *dst)
 	case YUVSemiPlanar:
 		convertYUVSemiPlanar(src, dst->bits());
 		break;
+	case YUVPlanar:
+		convertYUVPlanar(src, dst->bits());
+		break;
 	};
 }
 
@@ -247,6 +270,49 @@ void FormatConverter::convertYUVPacked(const Image *srcImage, unsigned char *dst
 	}
 }
 
+void FormatConverter::convertYUVPlanar(const Image *srcImage, unsigned char *dst)
+{
+	unsigned int c_stride = stride_ / horzSubSample_;
+	unsigned int c_inc = horzSubSample_ == 1 ? 1 : 0;
+	const unsigned char *src_y = srcImage->data(0).data();
+	const unsigned char *src_cb = srcImage->data(1).data();
+	const unsigned char *src_cr = srcImage->data(2).data();
+	int r, g, b;
+
+	if (nvSwap_)
+		std::swap(src_cb, src_cr);
+
+	for (unsigned int y = 0; y < height_; y++) {
+		const unsigned char *line_y = src_y + y * stride_;
+		const unsigned char *line_cb = src_cb + (y / vertSubSample_) *
+					       c_stride;
+		const unsigned char *line_cr = src_cr + (y / vertSubSample_) *
+					       c_stride;
+
+		for (unsigned int x = 0; x < width_; x += 2) {
+			yuv_to_rgb(*line_y, *line_cb, *line_cr, &r, &g, &b);
+			dst[0] = b;
+			dst[1] = g;
+			dst[2] = r;
+			dst[3] = 0xff;
+			line_y++;
+			line_cb += c_inc;
+			line_cr += c_inc;
+			dst += 4;
+
+			yuv_to_rgb(*line_y, *line_cb, *line_cr, &r, &g, &b);
+			dst[0] = b;
+			dst[1] = g;
+			dst[2] = r;
+			dst[3] = 0xff;
+			line_y++;
+			line_cb += 1;
+			line_cr += 1;
+			dst += 4;
+		}
+	}
+}
+
 void FormatConverter::convertYUVSemiPlanar(const Image *srcImage, unsigned char *dst)
 {
 	unsigned int c_stride = stride_ * (2 / horzSubSample_);
diff --git a/src/qcam/format_converter.h b/src/qcam/format_converter.h
index 9ce2cc6d..9da2df5d 100644
--- a/src/qcam/format_converter.h
+++ b/src/qcam/format_converter.h
@@ -29,11 +29,13 @@ private:
 		MJPEG,
 		RGB,
 		YUVPacked,
+		YUVPlanar,
 		YUVSemiPlanar,
 	};
 
 	void convertRGB(const Image *src, unsigned char *dst);
 	void convertYUVPacked(const Image *src, unsigned char *dst);
+	void convertYUVPlanar(const Image *src, unsigned char *dst);
 	void convertYUVSemiPlanar(const Image *src, unsigned char *dst);
 
 	libcamera::PixelFormat format_;