qcam: format_converter: Add NV formats support

Add support for some NV formats:
- V4L2_PIX_FMT_NV12, V4L2_PIX_FMT_NV21
- V4L2_PIX_FMT_NV16, V4L2_PIX_FMT_NV61
- V4L2_PIX_FMT_NV24, V4L2_PIX_FMT_NV42

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

2 files changed, 96 insertions, 10 deletions

diff --git a/src/qcam/format_converter.cpp b/src/qcam/format_converter.cpp
index 192767c2..86b03b2c 100644
--- a/src/qcam/format_converter.cpp
+++ b/src/qcam/format_converter.cpp
@@ -31,6 +31,42 @@ int FormatConverter::configure(unsigned int format, unsigned int width,
 			       unsigned int height)
 {
 	switch (format) {
+	case V4L2_PIX_FMT_NV12:
+		formatFamily_ = NV;
+		horzSubSample_ = 2;
+		vertSubSample_ = 2;
+		nvSwap_ = false;
+		break;
+	case V4L2_PIX_FMT_NV21:
+		formatFamily_ = NV;
+		horzSubSample_ = 2;
+		vertSubSample_ = 2;
+		nvSwap_ = true;
+		break;
+	case V4L2_PIX_FMT_NV16:
+		formatFamily_ = NV;
+		horzSubSample_ = 2;
+		vertSubSample_ = 1;
+		nvSwap_ = false;
+		break;
+	case V4L2_PIX_FMT_NV61:
+		formatFamily_ = NV;
+		horzSubSample_ = 2;
+		vertSubSample_ = 1;
+		nvSwap_ = true;
+		break;
+	case V4L2_PIX_FMT_NV24:
+		formatFamily_ = NV;
+		horzSubSample_ = 1;
+		vertSubSample_ = 1;
+		nvSwap_ = false;
+		break;
+	case V4L2_PIX_FMT_NV42:
+		formatFamily_ = NV;
+		horzSubSample_ = 1;
+		vertSubSample_ = 1;
+		nvSwap_ = true;
+		break;
 	case V4L2_PIX_FMT_BGR24:
 		formatFamily_ = RGB;
 		r_pos_ = 2;
@@ -99,9 +135,62 @@ void FormatConverter::convert(const unsigned char *src, size_t size,
 	case RGB:
 		convertRGB(src, dst->bits());
 		break;
+	case NV:
+		convertNV(src, dst->bits());
+		break;
 	};
 }
 
+static void yuv_to_rgb(int y, int u, int v, int *r, int *g, int *b)
+{
+	int c = y - 16;
+	int d = u - 128;
+	int e = v - 128;
+	*r = CLIP(( 298 * c           + 409 * e + 128) >> RGBSHIFT);
+	*g = CLIP(( 298 * c - 100 * d - 208 * e + 128) >> RGBSHIFT);
+	*b = CLIP(( 298 * c + 516 * d           + 128) >> RGBSHIFT);
+}
+
+void FormatConverter::convertNV(const unsigned char *src, unsigned char *dst)
+{
+	unsigned int c_stride = width_ * (2 / horzSubSample_);
+	unsigned int c_inc = horzSubSample_ == 1 ? 2 : 0;
+	unsigned int cb_pos = nvSwap_ ? 1 : 0;
+	unsigned int cr_pos = nvSwap_ ? 0 : 1;
+	const unsigned char *src_c = src + width_ * height_;
+	int r, g, b;
+
+	for (unsigned int y = 0; y < height_; y++) {
+		const unsigned char *src_y = src + y * width_;
+		const unsigned char *src_cb = src_c + (y / vertSubSample_) *
+					      c_stride + cb_pos;
+		const unsigned char *src_cr = src_c + (y / vertSubSample_) *
+					      c_stride + cr_pos;
+
+		for (unsigned int x = 0; x < width_; x += 2) {
+			yuv_to_rgb(*src_y, *src_cb, *src_cr, &r, &g, &b);
+			dst[0] = b;
+			dst[1] = g;
+			dst[2] = r;
+			dst[3] = 0xff;
+			src_y++;
+			src_cb += c_inc;
+			src_cr += c_inc;
+			dst += 4;
+
+			yuv_to_rgb(*src_y, *src_cb, *src_cr, &r, &g, &b);
+			dst[0] = b;
+			dst[1] = g;
+			dst[2] = r;
+			dst[3] = 0xff;
+			src_y++;
+			src_cb += 2;
+			src_cr += 2;
+			dst += 4;
+		}
+	}
+}
+
 void FormatConverter::convertRGB(const unsigned char *src, unsigned char *dst)
 {
 	unsigned int x, y;
@@ -124,16 +213,6 @@ void FormatConverter::convertRGB(const unsigned char *src, unsigned char *dst)
 	}
 }
 
-static void yuv_to_rgb(int y, int u, int v, int *r, int *g, int *b)
-{
-	int c = y - 16;
-	int d = u - 128;
-	int e = v - 128;
-	*r = CLIP(( 298 * c           + 409 * e + 128) >> RGBSHIFT);
-	*g = CLIP(( 298 * c - 100 * d - 208 * e + 128) >> RGBSHIFT);
-	*b = CLIP(( 298 * c + 516 * d           + 128) >> RGBSHIFT);
-}
-
 void FormatConverter::convertYUV(const unsigned char *src, unsigned char *dst)
 {
 	unsigned int src_x, src_y, dst_x, dst_y;
diff --git a/src/qcam/format_converter.h b/src/qcam/format_converter.h
index bca44aa3..391e6a44 100644
--- a/src/qcam/format_converter.h
+++ b/src/qcam/format_converter.h
@@ -22,10 +22,12 @@ public:
 private:
 	enum FormatFamily {
 		MJPEG,
+		NV,
 		RGB,
 		YUV,
 	};
 
+	void convertNV(const unsigned char *src, unsigned char *dst);
 	void convertRGB(const unsigned char *src, unsigned char *dst);
 	void convertYUV(const unsigned char *src, unsigned char *dst);
 
@@ -35,6 +37,11 @@ private:
 
 	enum FormatFamily formatFamily_;
 
+	/* NV parameters */
+	unsigned int horzSubSample_;
+	unsigned int vertSubSample_;
+	bool nvSwap_;
+
 	/* RGB parameters */
 	unsigned int bpp_;
 	unsigned int r_pos_;