/* 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)
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,
},
};
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;
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;
}
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;
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;