media: vim2m: fix driver for it to handle different fourcc formats

Despite vim2m is reporting that it supports RGB565BE and YUYV,
that's not true.

Right now, it just says that it supports both format, but it
doesn't actually support them.

Also, horizontal flip is not properly implemented. It sounds
that it was designed to do a pseudo-horizontal flip using 8
tiles. Yet, as it doesn't do format conversion, the result
is a mess.

I suspect that it was done this way in order to save CPU time,
at the time of OMAP2 days.

That's messy and doesn't really help if someone wants to
use vim2m to test a pipeline.

Worse than that, the unique RGB format it says it supports is
RGB565BE, with is not supported by Gstreamer. That prevents
practical usage of it, even for tests.

So, instead, properly implement fourcc format conversions,
adding a few more RGB formats:

	- RGB and BGR with 24 bits
	- RGB565LE (known as RGB16 at gstreamer)

Also allows using any of the 5 supported formats as either
capture or output.

Note: The YUYV conversion routines are based on the conversion code
written by Hans de Goede inside libv4lconvert (part of v4l-utils),
released under LGPGL 2.1 (GPL 2.0 compatible).

Tested all possible format combinations except for RGB565BE,
as Gstreamer currently doesn't support it.
Signed-off-by: Mauro Carvalho Chehab <mchehab+samsung@kernel.org>

drivers/media/platform/vim2m.c

@@ -60,8 +60,6 @@ MODULE_PARM_DESC(debug, "activates debug info");
 
 /* Default transaction time in msec */
 #define MEM2MEM_DEF_TRANSTIME	40
-#define MEM2MEM_COLOR_STEP	(0xff >> 4)
-#define MEM2MEM_NUM_TILES	8
 
 /* Flags that indicate processing mode */
 #define MEM2MEM_HFLIP	(1 << 0)
@@ -82,22 +80,24 @@ static struct platform_device vim2m_pdev = {
 struct vim2m_fmt {
 	u32	fourcc;
 	int	depth;
-	/* Types the format can be used for */
-	u32	types;
 };
 
 static struct vim2m_fmt formats[] = {
 	{
-		.fourcc	= V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */
+		.fourcc	= V4L2_PIX_FMT_RGB565,  /* rrrrrggg gggbbbbb */
 		.depth	= 16,
-		/* Both capture and output format */
-		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
-	},
-	{
+	}, {
+		.fourcc	= V4L2_PIX_FMT_RGB565X, /* gggbbbbb rrrrrggg */
+		.depth	= 16,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_RGB24,
+		.depth	= 24,
+	}, {
+		.fourcc	= V4L2_PIX_FMT_BGR24,
+		.depth	= 24,
+	}, {
 		.fourcc	= V4L2_PIX_FMT_YUYV,
 		.depth	= 16,
-		/* Output-only format */
-		.types	= MEM2MEM_OUTPUT,
 	},
 };
 
@@ -201,126 +201,253 @@ static struct vim2m_q_data *get_q_data(struct vim2m_ctx *ctx,
 	return NULL;
 }
 
+#define CLIP(__color) \
+	(u8)(((__color) > 0xff) ? 0xff : (((__color) < 0) ? 0 : (__color)))
 
-static int device_process(struct vim2m_ctx *ctx,
-			  struct vb2_v4l2_buffer *in_vb,
-			  struct vb2_v4l2_buffer *out_vb)
+static void copy_two_pixels(struct vim2m_fmt *in, struct vim2m_fmt *out,
+			    u8 **src, u8 **dst, bool reverse)
 {
-	struct vim2m_dev *dev = ctx->dev;
-	struct vim2m_q_data *q_data;
-	u8 *p_in, *p_out;
-	int x, y, t, w;
-	int tile_w, bytes_left;
-	int width, height, bytesperline;
+	u8 _r[2], _g[2], _b[2], *r, *g, *b;
+	int i, step;
 
-	q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	// If format is the same just copy the data, respecting the width
+	if (in->fourcc == out->fourcc) {
+		int depth = out->depth >> 3;
 
-	width	= q_data->width;
-	height	= q_data->height;
-	bytesperline	= (q_data->width * q_data->fmt->depth) >> 3;
+		if (reverse) {
+			if (in->fourcc == V4L2_PIX_FMT_YUYV) {
+				int u, v, y, y1;
 
-	p_in = vb2_plane_vaddr(&in_vb->vb2_buf, 0);
-	p_out = vb2_plane_vaddr(&out_vb->vb2_buf, 0);
-	if (!p_in || !p_out) {
-		v4l2_err(&dev->v4l2_dev,
-			 "Acquiring kernel pointers to buffers failed\n");
-		return -EFAULT;
-	}
+				*src -= 2;
 
-	if (vb2_plane_size(&in_vb->vb2_buf, 0) >
-			vb2_plane_size(&out_vb->vb2_buf, 0)) {
-		v4l2_err(&dev->v4l2_dev, "Output buffer is too small\n");
-		return -EINVAL;
+				y1 = (*src)[0]; /* copy as second point */
+				u  = (*src)[1];
+				y  = (*src)[2]; /* copy as first point */
+				v  = (*src)[3];
+
+				*src -= 2;
+
+				*(*dst)++ = y;
+				*(*dst)++ = u;
+				*(*dst)++ = y1;
+				*(*dst)++ = v;
+				return;
+			}
+
+			memcpy(*dst, *src, depth);
+			memcpy(*dst + depth, *src - depth, depth);
+			*src -= depth << 1;
+		} else {
+			memcpy(*dst, *src, depth << 1);
+			*src += depth << 1;
+		}
+		*dst += depth << 1;
+		return;
 	}
 
-	tile_w = (width * (q_data[V4L2_M2M_DST].fmt->depth >> 3))
-		/ MEM2MEM_NUM_TILES;
-	bytes_left = bytesperline - tile_w * MEM2MEM_NUM_TILES;
-	w = 0;
+	/* Step 1: read two consecutive pixels from src pointer */
 
-	out_vb->sequence =
-		get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE)->sequence++;
-	in_vb->sequence = q_data->sequence++;
-	v4l2_m2m_buf_copy_data(in_vb, out_vb, true);
+	r = _r;
+	g = _g;
+	b = _b;
 
-	switch (ctx->mode) {
-	case MEM2MEM_HFLIP | MEM2MEM_VFLIP:
-		p_out += bytesperline * height - bytes_left;
-		for (y = 0; y < height; ++y) {
-			for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
-				if (w & 0x1) {
-					for (x = 0; x < tile_w; ++x)
-						*--p_out = *p_in++ +
-							MEM2MEM_COLOR_STEP;
-				} else {
-					for (x = 0; x < tile_w; ++x)
-						*--p_out = *p_in++ -
-							MEM2MEM_COLOR_STEP;
-				}
-				++w;
-			}
-			p_in += bytes_left;
-			p_out -= bytes_left;
+	if (reverse)
+		step = -1;
+	else
+		step = 1;
+
+	switch (in->fourcc) {
+	case V4L2_PIX_FMT_RGB565: /* rrrrrggg gggbbbbb */
+		for (i = 0; i < 2; i++) {
+			u16 pix = *(u16 *)*src;
+
+			*r++ = (u8)(((pix & 0xf800) >> 11) << 3) | 0x07;
+			*g++ = (u8)((((pix & 0x07e0) >> 5)) << 2) | 0x03;
+			*b++ = (u8)((pix & 0x1f) << 3) | 0x07;
+
+			*src += step << 1;
 		}
 		break;
+	case V4L2_PIX_FMT_RGB565X: /* gggbbbbb rrrrrggg */
+		for (i = 0; i < 2; i++) {
+			u16 pix = *(u16 *)*src;
 
-	case MEM2MEM_HFLIP:
-		for (y = 0; y < height; ++y) {
-			p_out += MEM2MEM_NUM_TILES * tile_w;
-			for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
-				if (w & 0x01) {
-					for (x = 0; x < tile_w; ++x)
-						*--p_out = *p_in++ +
-							MEM2MEM_COLOR_STEP;
-				} else {
-					for (x = 0; x < tile_w; ++x)
-						*--p_out = *p_in++ -
-							MEM2MEM_COLOR_STEP;
-				}
-				++w;
-			}
-			p_in += bytes_left;
-			p_out += bytesperline;
+			*r++ = (u8)(((0x00f8 & pix) >> 3) << 3) | 0x07;
+			*g++ = (u8)(((pix & 0x7) << 2) |
+				    ((pix & 0xe000) >> 5)) | 0x03;
+			*b++ = (u8)(((pix & 0x1f00) >> 8) << 3) | 0x07;
+
+			*src += step << 1;
 		}
 		break;
+	case V4L2_PIX_FMT_RGB24:
+		for (i = 0; i < 2; i++) {
+			*r++ = (*src)[0];
+			*g++ = (*src)[1];
+			*b++ = (*src)[2];
 
-	case MEM2MEM_VFLIP:
-		p_out += bytesperline * (height - 1);
-		for (y = 0; y < height; ++y) {
-			for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
-				if (w & 0x1) {
-					for (x = 0; x < tile_w; ++x)
-						*p_out++ = *p_in++ +
-							MEM2MEM_COLOR_STEP;
-				} else {
-					for (x = 0; x < tile_w; ++x)
-						*p_out++ = *p_in++ -
-							MEM2MEM_COLOR_STEP;
-				}
-				++w;
-			}
-			p_in += bytes_left;
-			p_out += bytes_left - 2 * bytesperline;
+			*src += step * 3;
 		}
 		break;
+	case V4L2_PIX_FMT_BGR24:
+		for (i = 0; i < 2; i++) {
+			*b++ = (*src)[0];
+			*g++ = (*src)[1];
+			*r++ = (*src)[2];
 
-	default:
-		for (y = 0; y < height; ++y) {
-			for (t = 0; t < MEM2MEM_NUM_TILES; ++t) {
-				if (w & 0x1) {
-					for (x = 0; x < tile_w; ++x)
-						*p_out++ = *p_in++ +
-							MEM2MEM_COLOR_STEP;
-				} else {
-					for (x = 0; x < tile_w; ++x)
-						*p_out++ = *p_in++ -
-							MEM2MEM_COLOR_STEP;
-				}
-				++w;
-			}
-			p_in += bytes_left;
-			p_out += bytes_left;
+			*src += step * 3;
+		}
+		break;
+	default: /* V4L2_PIX_FMT_YUYV */
+	{
+		int u, v, y, y1, u1, v1, tmp;
+
+		if (reverse) {
+			*src -= 2;
+
+			y1 = (*src)[0]; /* copy as second point */
+			u  = (*src)[1];
+			y  = (*src)[2]; /* copy as first point */
+			v  = (*src)[3];
+
+			*src -= 2;
+		} else {
+			y  = *(*src)++;
+			u  = *(*src)++;
+			y1 = *(*src)++;
+			v  = *(*src)++;
 		}
+
+		u1 = (((u - 128) << 7) +  (u - 128)) >> 6;
+		tmp = (((u - 128) << 1) + (u - 128) +
+		       ((v - 128) << 2) + ((v - 128) << 1)) >> 3;
+		v1 = (((v - 128) << 1) +  (v - 128)) >> 1;
+
+		*r++ = CLIP(y + v1);
+		*g++ = CLIP(y - tmp);
+		*b++ = CLIP(y + u1);
+
+		*r = CLIP(y1 + v1);
+		*g = CLIP(y1 - tmp);
+		*b = CLIP(y1 + u1);
+		break;
+	}
+	}
+
+	/* Step 2: store two consecutive points, reversing them if needed */
+
+	r = _r;
+	g = _g;
+	b = _b;
+
+	switch (out->fourcc) {
+	case V4L2_PIX_FMT_RGB565: /* rrrrrggg gggbbbbb */
+		for (i = 0; i < 2; i++) {
+			u16 *pix = (u16 *)*dst;
+
+			*pix = ((*r << 8) & 0xf800) | ((*g << 3) & 0x07e0) |
+			       (*b >> 3);
+
+			*dst += 2;
+		}
+		return;
+	case V4L2_PIX_FMT_RGB565X: /* gggbbbbb rrrrrggg */
+		for (i = 0; i < 2; i++) {
+			u16 *pix = (u16 *)*dst;
+			u8 green = *g++ >> 2;
+
+			*pix = ((green << 8) & 0xe000) | (green & 0x07) |
+			       ((*b++ << 5) & 0x1f00) | ((*r++ & 0xf8));
+
+			*dst += 2;
+		}
+		return;
+	case V4L2_PIX_FMT_RGB24:
+		for (i = 0; i < 2; i++) {
+			*(*dst)++ = *r++;
+			*(*dst)++ = *g++;
+			*(*dst)++ = *b++;
+		}
+		return;
+	case V4L2_PIX_FMT_BGR24:
+		for (i = 0; i < 2; i++) {
+			*(*dst)++ = *b++;
+			*(*dst)++ = *g++;
+			*(*dst)++ = *r++;
+		}
+		return;
+	default: /* V4L2_PIX_FMT_YUYV */
+	{
+		u8 y, y1, u, v;
+
+		y = ((8453  * (*r) + 16594 * (*g) +  3223 * (*b)
+		     + 524288) >> 15);
+		u = ((-4878 * (*r) - 9578  * (*g) + 14456 * (*b)
+		     + 4210688) >> 15);
+		v = ((14456 * (*r++) - 12105 * (*g++) - 2351 * (*b++)
+		     + 4210688) >> 15);
+		y1 = ((8453 * (*r) + 16594 * (*g) +  3223 * (*b)
+		     + 524288) >> 15);
+
+		*(*dst)++ = y;
+		*(*dst)++ = u;
+
+		*(*dst)++ = y1;
+		*(*dst)++ = v;
+		return;
+	}
+	}
+}
+
+static int device_process(struct vim2m_ctx *ctx,
+			  struct vb2_v4l2_buffer *in_vb,
+			  struct vb2_v4l2_buffer *out_vb)
+{
+	struct vim2m_dev *dev = ctx->dev;
+	struct vim2m_q_data *q_data_in, *q_data_out;
+	u8 *p_in, *p, *p_out;
+	int width, height, bytesperline, x, y, start, end, step;
+	struct vim2m_fmt *in, *out;
+
+	q_data_in = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	in = q_data_in->fmt;
+	width = q_data_in->width;
+	height = q_data_in->height;
+	bytesperline = (q_data_in->width * q_data_in->fmt->depth) >> 3;
+
+	q_data_out = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	out = q_data_out->fmt;
+
+	p_in = vb2_plane_vaddr(&in_vb->vb2_buf, 0);
+	p_out = vb2_plane_vaddr(&out_vb->vb2_buf, 0);
+	if (!p_in || !p_out) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Acquiring kernel pointers to buffers failed\n");
+		return -EFAULT;
+	}
+
+	out_vb->sequence = get_q_data(ctx,
+				      V4L2_BUF_TYPE_VIDEO_CAPTURE)->sequence++;
+	in_vb->sequence = q_data_in->sequence++;
+	v4l2_m2m_buf_copy_data(in_vb, out_vb, true);
+
+	if (ctx->mode & MEM2MEM_VFLIP) {
+		start = height - 1;
+		end = -1;
+		step = -1;
+	} else {
+		start = 0;
+		end = height;
+		step = 1;
+	}
+	for (y = start; y != end; y += step) {
+		p = p_in + (y * bytesperline);
+		if (ctx->mode & MEM2MEM_HFLIP)
+			p += bytesperline - (q_data_in->fmt->depth >> 3);
+
+		for (x = 0; x < width >> 1; x++)
+			copy_two_pixels(in, out, &p, &p_out,
+					ctx->mode & MEM2MEM_HFLIP);
 	}
 
 	return 0;
@@ -433,25 +560,11 @@ static int vidioc_querycap(struct file *file, void *priv,
 
 static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
 {
-	int i, num;
 	struct vim2m_fmt *fmt;
 
-	num = 0;
-
-	for (i = 0; i < NUM_FORMATS; ++i) {
-		if (formats[i].types & type) {
-			/* index-th format of type type found ? */
-			if (num == f->index)
-				break;
-			/* Correct type but haven't reached our index yet,
-			 * just increment per-type index */
-			++num;
-		}
-	}
-
-	if (i < NUM_FORMATS) {
+	if (f->index < NUM_FORMATS) {
 		/* Format found */
-		fmt = &formats[i];
+		fmt = &formats[f->index];
 		f->pixelformat = fmt->fourcc;
 		return 0;
 	}
@@ -542,12 +655,6 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 		f->fmt.pix.pixelformat = formats[0].fourcc;
 		fmt = find_format(f);
 	}
-	if (!(fmt->types & MEM2MEM_CAPTURE)) {
-		v4l2_err(&ctx->dev->v4l2_dev,
-			 "Fourcc format (0x%08x) invalid.\n",
-			 f->fmt.pix.pixelformat);
-		return -EINVAL;
-	}
 	f->fmt.pix.colorspace = ctx->colorspace;
 	f->fmt.pix.xfer_func = ctx->xfer_func;
 	f->fmt.pix.ycbcr_enc = ctx->ycbcr_enc;
@@ -560,19 +667,12 @@ static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
 				  struct v4l2_format *f)
 {
 	struct vim2m_fmt *fmt;
-	struct vim2m_ctx *ctx = file2ctx(file);
 
 	fmt = find_format(f);
 	if (!fmt) {
 		f->fmt.pix.pixelformat = formats[0].fourcc;
 		fmt = find_format(f);
 	}
-	if (!(fmt->types & MEM2MEM_OUTPUT)) {
-		v4l2_err(&ctx->dev->v4l2_dev,
-			 "Fourcc format (0x%08x) invalid.\n",
-			 f->fmt.pix.pixelformat);
-		return -EINVAL;
-	}
 	if (!f->fmt.pix.colorspace)
 		f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;