[linux-block.git] / drivers / media / test-drivers / vimc / vimc-capture.c

// SPDX-License-Identifier: GPL-2.0-or-later
/*
 * vimc-capture.c Virtual Media Controller Driver
 *
 * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
 */

#include <media/v4l2-ioctl.h>
#include <media/videobuf2-core.h>
#include <media/videobuf2-dma-contig.h>
#include <media/videobuf2-vmalloc.h>

#include "vimc-common.h"
#include "vimc-streamer.h"

struct vimc_capture_device {
	struct vimc_ent_device ved;
	struct video_device vdev;
	struct v4l2_pix_format format;
	struct vb2_queue queue;
	struct list_head buf_list;
	/*
	 * NOTE: in a real driver, a spin lock must be used to access the
	 * queue because the frames are generated from a hardware interruption
	 * and the isr is not allowed to sleep.
	 * Even if it is not necessary a spinlock in the vimc driver, we
	 * use it here as a code reference
	 */
	spinlock_t qlock;
	struct mutex lock;
	u32 sequence;
	struct vimc_stream stream;
	struct media_pad pad;
};

static const struct v4l2_pix_format fmt_default = {
	.width = 640,
	.height = 480,
	.pixelformat = V4L2_PIX_FMT_RGB24,
	.field = V4L2_FIELD_NONE,
	.colorspace = V4L2_COLORSPACE_SRGB,
};

struct vimc_capture_buffer {
	/*
	 * struct vb2_v4l2_buffer must be the first element
	 * the videobuf2 framework will allocate this struct based on
	 * buf_struct_size and use the first sizeof(struct vb2_buffer) bytes of
	 * memory as a vb2_buffer
	 */
	struct vb2_v4l2_buffer vb2;
	struct list_head list;
};

static int vimc_capture_querycap(struct file *file, void *priv,
			     struct v4l2_capability *cap)
{
	strscpy(cap->driver, VIMC_PDEV_NAME, sizeof(cap->driver));
	strscpy(cap->card, KBUILD_MODNAME, sizeof(cap->card));
	snprintf(cap->bus_info, sizeof(cap->bus_info),
		 "platform:%s", VIMC_PDEV_NAME);

	return 0;
}

static void vimc_capture_get_format(struct vimc_ent_device *ved,
				struct v4l2_pix_format *fmt)
{
	struct vimc_capture_device *vcapture = container_of(ved, struct vimc_capture_device,
						    ved);

	*fmt = vcapture->format;
}

static int vimc_capture_g_fmt_vid_cap(struct file *file, void *priv,
				  struct v4l2_format *f)
{
	struct vimc_capture_device *vcapture = video_drvdata(file);

	f->fmt.pix = vcapture->format;

	return 0;
}

static int vimc_capture_try_fmt_vid_cap(struct file *file, void *priv,
				    struct v4l2_format *f)
{
	struct v4l2_pix_format *format = &f->fmt.pix;
	const struct vimc_pix_map *vpix;

	format->width = clamp_t(u32, format->width, VIMC_FRAME_MIN_WIDTH,
				VIMC_FRAME_MAX_WIDTH) & ~1;
	format->height = clamp_t(u32, format->height, VIMC_FRAME_MIN_HEIGHT,
				 VIMC_FRAME_MAX_HEIGHT) & ~1;

	/* Don't accept a pixelformat that is not on the table */
	vpix = vimc_pix_map_by_pixelformat(format->pixelformat);
	if (!vpix) {
		format->pixelformat = fmt_default.pixelformat;
		vpix = vimc_pix_map_by_pixelformat(format->pixelformat);
	}
	/* TODO: Add support for custom bytesperline values */
	format->bytesperline = format->width * vpix->bpp;
	format->sizeimage = format->bytesperline * format->height;

	if (format->field == V4L2_FIELD_ANY)
		format->field = fmt_default.field;

	vimc_colorimetry_clamp(format);

	if (format->colorspace == V4L2_COLORSPACE_DEFAULT)
		format->colorspace = fmt_default.colorspace;

	return 0;
}

static int vimc_capture_s_fmt_vid_cap(struct file *file, void *priv,
				  struct v4l2_format *f)
{
	struct vimc_capture_device *vcapture = video_drvdata(file);
	int ret;

	/* Do not change the format while stream is on */
	if (vb2_is_busy(&vcapture->queue))
		return -EBUSY;

	ret = vimc_capture_try_fmt_vid_cap(file, priv, f);
	if (ret)
		return ret;

	dev_dbg(vcapture->ved.dev, "%s: format update: "
		"old:%dx%d (0x%x, %d, %d, %d, %d) "
		"new:%dx%d (0x%x, %d, %d, %d, %d)\n", vcapture->vdev.name,
		/* old */
		vcapture->format.width, vcapture->format.height,
		vcapture->format.pixelformat, vcapture->format.colorspace,
		vcapture->format.quantization, vcapture->format.xfer_func,
		vcapture->format.ycbcr_enc,
		/* new */
		f->fmt.pix.width, f->fmt.pix.height,
		f->fmt.pix.pixelformat,	f->fmt.pix.colorspace,
		f->fmt.pix.quantization, f->fmt.pix.xfer_func,
		f->fmt.pix.ycbcr_enc);

	vcapture->format = f->fmt.pix;

	return 0;
}

static int vimc_capture_enum_fmt_vid_cap(struct file *file, void *priv,
				     struct v4l2_fmtdesc *f)
{
	const struct vimc_pix_map *vpix;

	if (f->mbus_code) {
		if (f->index > 0)
			return -EINVAL;

		vpix = vimc_pix_map_by_code(f->mbus_code);
	} else {
		vpix = vimc_pix_map_by_index(f->index);
	}

	if (!vpix)
		return -EINVAL;

	f->pixelformat = vpix->pixelformat;

	return 0;
}

static int vimc_capture_enum_framesizes(struct file *file, void *fh,
				    struct v4l2_frmsizeenum *fsize)
{
	const struct vimc_pix_map *vpix;

	if (fsize->index)
		return -EINVAL;

	/* Only accept code in the pix map table */
	vpix = vimc_pix_map_by_code(fsize->pixel_format);
	if (!vpix)
		return -EINVAL;

	fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
	fsize->stepwise.min_width = VIMC_FRAME_MIN_WIDTH;
	fsize->stepwise.max_width = VIMC_FRAME_MAX_WIDTH;
	fsize->stepwise.min_height = VIMC_FRAME_MIN_HEIGHT;
	fsize->stepwise.max_height = VIMC_FRAME_MAX_HEIGHT;
	fsize->stepwise.step_width = 1;
	fsize->stepwise.step_height = 1;

	return 0;
}

static const struct v4l2_file_operations vimc_capture_fops = {
	.owner		= THIS_MODULE,
	.open		= v4l2_fh_open,
	.release	= vb2_fop_release,
	.read           = vb2_fop_read,
	.poll		= vb2_fop_poll,
	.unlocked_ioctl = video_ioctl2,
	.mmap           = vb2_fop_mmap,
};

static const struct v4l2_ioctl_ops vimc_capture_ioctl_ops = {
	.vidioc_querycap = vimc_capture_querycap,

	.vidioc_g_fmt_vid_cap = vimc_capture_g_fmt_vid_cap,
	.vidioc_s_fmt_vid_cap = vimc_capture_s_fmt_vid_cap,
	.vidioc_try_fmt_vid_cap = vimc_capture_try_fmt_vid_cap,
	.vidioc_enum_fmt_vid_cap = vimc_capture_enum_fmt_vid_cap,
	.vidioc_enum_framesizes = vimc_capture_enum_framesizes,

	.vidioc_reqbufs = vb2_ioctl_reqbufs,
	.vidioc_create_bufs = vb2_ioctl_create_bufs,
	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
	.vidioc_querybuf = vb2_ioctl_querybuf,
	.vidioc_qbuf = vb2_ioctl_qbuf,
	.vidioc_dqbuf = vb2_ioctl_dqbuf,
	.vidioc_expbuf = vb2_ioctl_expbuf,
	.vidioc_streamon = vb2_ioctl_streamon,
	.vidioc_streamoff = vb2_ioctl_streamoff,
};

static void vimc_capture_return_all_buffers(struct vimc_capture_device *vcapture,
					enum vb2_buffer_state state)
{
	struct vimc_capture_buffer *vbuf, *node;

	spin_lock(&vcapture->qlock);

	list_for_each_entry_safe(vbuf, node, &vcapture->buf_list, list) {
		list_del(&vbuf->list);
		vb2_buffer_done(&vbuf->vb2.vb2_buf, state);
	}

	spin_unlock(&vcapture->qlock);
}

static int vimc_capture_start_streaming(struct vb2_queue *vq, unsigned int count)
{
	struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq);
	int ret;

	vcapture->sequence = 0;

	/* Start the media pipeline */
	ret = video_device_pipeline_start(&vcapture->vdev, &vcapture->stream.pipe);
	if (ret) {
		vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_QUEUED);
		return ret;
	}

	ret = vimc_streamer_s_stream(&vcapture->stream, &vcapture->ved, 1);
	if (ret) {
		video_device_pipeline_stop(&vcapture->vdev);
		vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_QUEUED);
		return ret;
	}

	return 0;
}

/*
 * Stop the stream engine. Any remaining buffers in the stream queue are
 * dequeued and passed on to the vb2 framework marked as STATE_ERROR.
 */
static void vimc_capture_stop_streaming(struct vb2_queue *vq)
{
	struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq);

	vimc_streamer_s_stream(&vcapture->stream, &vcapture->ved, 0);

	/* Stop the media pipeline */
	video_device_pipeline_stop(&vcapture->vdev);

	/* Release all active buffers */
	vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_ERROR);
}

static void vimc_capture_buf_queue(struct vb2_buffer *vb2_buf)
{
	struct vimc_capture_device *vcapture = vb2_get_drv_priv(vb2_buf->vb2_queue);
	struct vimc_capture_buffer *buf = container_of(vb2_buf,
						   struct vimc_capture_buffer,
						   vb2.vb2_buf);

	spin_lock(&vcapture->qlock);
	list_add_tail(&buf->list, &vcapture->buf_list);
	spin_unlock(&vcapture->qlock);
}

static int vimc_capture_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
				unsigned int *nplanes, unsigned int sizes[],
				struct device *alloc_devs[])
{
	struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq);

	if (*nplanes)
		return sizes[0] < vcapture->format.sizeimage ? -EINVAL : 0;
	/* We don't support multiplanes for now */
	*nplanes = 1;
	sizes[0] = vcapture->format.sizeimage;

	return 0;
}

static int vimc_capture_buffer_prepare(struct vb2_buffer *vb)
{
	struct vimc_capture_device *vcapture = vb2_get_drv_priv(vb->vb2_queue);
	unsigned long size = vcapture->format.sizeimage;

	if (vb2_plane_size(vb, 0) < size) {
		dev_err(vcapture->ved.dev, "%s: buffer too small (%lu < %lu)\n",
			vcapture->vdev.name, vb2_plane_size(vb, 0), size);
		return -EINVAL;
	}
	return 0;
}

static const struct vb2_ops vimc_capture_qops = {
	.start_streaming	= vimc_capture_start_streaming,
	.stop_streaming		= vimc_capture_stop_streaming,
	.buf_queue		= vimc_capture_buf_queue,
	.queue_setup		= vimc_capture_queue_setup,
	.buf_prepare		= vimc_capture_buffer_prepare,
	/*
	 * Since q->lock is set we can use the standard
	 * vb2_ops_wait_prepare/finish helper functions.
	 */
	.wait_prepare		= vb2_ops_wait_prepare,
	.wait_finish		= vb2_ops_wait_finish,
};

static const struct media_entity_operations vimc_capture_mops = {
	.link_validate		= vimc_vdev_link_validate,
};

static void vimc_capture_release(struct vimc_ent_device *ved)
{
	struct vimc_capture_device *vcapture =
		container_of(ved, struct vimc_capture_device, ved);

	media_entity_cleanup(vcapture->ved.ent);
	kfree(vcapture);
}

static void vimc_capture_unregister(struct vimc_ent_device *ved)
{
	struct vimc_capture_device *vcapture =
		container_of(ved, struct vimc_capture_device, ved);

	vb2_video_unregister_device(&vcapture->vdev);
}

static void *vimc_capture_process_frame(struct vimc_ent_device *ved,
				    const void *frame)
{
	struct vimc_capture_device *vcapture = container_of(ved, struct vimc_capture_device,
						    ved);
	struct vimc_capture_buffer *vimc_buf;
	void *vbuf;

	spin_lock(&vcapture->qlock);

	/* Get the first entry of the list */
	vimc_buf = list_first_entry_or_null(&vcapture->buf_list,
					    typeof(*vimc_buf), list);
	if (!vimc_buf) {
		spin_unlock(&vcapture->qlock);
		return ERR_PTR(-EAGAIN);
	}

	/* Remove this entry from the list */
	list_del(&vimc_buf->list);

	spin_unlock(&vcapture->qlock);

	/* Fill the buffer */
	vimc_buf->vb2.vb2_buf.timestamp = ktime_get_ns();
	vimc_buf->vb2.sequence = vcapture->sequence++;
	vimc_buf->vb2.field = vcapture->format.field;

	vbuf = vb2_plane_vaddr(&vimc_buf->vb2.vb2_buf, 0);

	memcpy(vbuf, frame, vcapture->format.sizeimage);

	/* Set it as ready */
	vb2_set_plane_payload(&vimc_buf->vb2.vb2_buf, 0,
			      vcapture->format.sizeimage);
	vb2_buffer_done(&vimc_buf->vb2.vb2_buf, VB2_BUF_STATE_DONE);
	return NULL;
}

static struct vimc_ent_device *vimc_capture_add(struct vimc_device *vimc,
					    const char *vcfg_name)
{
	struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
	const struct vimc_pix_map *vpix;
	struct vimc_capture_device *vcapture;
	struct video_device *vdev;
	struct vb2_queue *q;
	int ret;

	/* Allocate the vimc_capture_device struct */
	vcapture = kzalloc(sizeof(*vcapture), GFP_KERNEL);
	if (!vcapture)
		return ERR_PTR(-ENOMEM);

	/* Initialize the media entity */
	vcapture->vdev.entity.name = vcfg_name;
	vcapture->vdev.entity.function = MEDIA_ENT_F_IO_V4L;
	vcapture->pad.flags = MEDIA_PAD_FL_SINK;
	ret = media_entity_pads_init(&vcapture->vdev.entity,
				     1, &vcapture->pad);
	if (ret)
		goto err_free_vcapture;

	/* Initialize the lock */
	mutex_init(&vcapture->lock);

	/* Initialize the vb2 queue */
	q = &vcapture->queue;
	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	q->io_modes = VB2_MMAP | VB2_DMABUF;
	if (vimc_allocator == VIMC_ALLOCATOR_VMALLOC)
		q->io_modes |= VB2_USERPTR;
	q->drv_priv = vcapture;
	q->buf_struct_size = sizeof(struct vimc_capture_buffer);
	q->ops = &vimc_capture_qops;
	q->mem_ops = vimc_allocator == VIMC_ALLOCATOR_DMA_CONTIG
		   ? &vb2_dma_contig_memops : &vb2_vmalloc_memops;
	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
	q->min_buffers_needed = 2;
	q->lock = &vcapture->lock;
	q->dev = v4l2_dev->dev;

	ret = vb2_queue_init(q);
	if (ret) {
		dev_err(vimc->mdev.dev, "%s: vb2 queue init failed (err=%d)\n",
			vcfg_name, ret);
		goto err_clean_m_ent;
	}

	/* Initialize buffer list and its lock */
	INIT_LIST_HEAD(&vcapture->buf_list);
	spin_lock_init(&vcapture->qlock);

	/* Set default frame format */
	vcapture->format = fmt_default;
	vpix = vimc_pix_map_by_pixelformat(vcapture->format.pixelformat);
	vcapture->format.bytesperline = vcapture->format.width * vpix->bpp;
	vcapture->format.sizeimage = vcapture->format.bytesperline *
				 vcapture->format.height;

	/* Fill the vimc_ent_device struct */
	vcapture->ved.ent = &vcapture->vdev.entity;
	vcapture->ved.process_frame = vimc_capture_process_frame;
	vcapture->ved.vdev_get_format = vimc_capture_get_format;
	vcapture->ved.dev = vimc->mdev.dev;

	/* Initialize the video_device struct */
	vdev = &vcapture->vdev;
	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING
			  | V4L2_CAP_IO_MC;
	vdev->entity.ops = &vimc_capture_mops;
	vdev->release = video_device_release_empty;
	vdev->fops = &vimc_capture_fops;
	vdev->ioctl_ops = &vimc_capture_ioctl_ops;
	vdev->lock = &vcapture->lock;
	vdev->queue = q;
	vdev->v4l2_dev = v4l2_dev;
	vdev->vfl_dir = VFL_DIR_RX;
	strscpy(vdev->name, vcfg_name, sizeof(vdev->name));
	video_set_drvdata(vdev, &vcapture->ved);

	/* Register the video_device with the v4l2 and the media framework */
	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
	if (ret) {
		dev_err(vimc->mdev.dev, "%s: video register failed (err=%d)\n",
			vcapture->vdev.name, ret);
		goto err_clean_m_ent;
	}

	return &vcapture->ved;

err_clean_m_ent:
	media_entity_cleanup(&vcapture->vdev.entity);
err_free_vcapture:
	kfree(vcapture);

	return ERR_PTR(ret);
}

struct vimc_ent_type vimc_capture_type = {
	.add = vimc_capture_add,
	.unregister = vimc_capture_unregister,
	.release = vimc_capture_release
};
Commit	Line	Data
c942fddf	1	// SPDX-License-Identifier: GPL-2.0-or-later
f2fe8906 HK	2	/*
	3	* vimc-capture.c Virtual Media Controller Driver
	4	*
	5	* Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
f2fe8906 HK	6	*/
	7
	8	#include <media/v4l2-ioctl.h>
	9	#include <media/videobuf2-core.h>
4a2e0a80	10	#include <media/videobuf2-dma-contig.h>
f2fe8906 HK	11	#include <media/videobuf2-vmalloc.h>
f2fe8906 HK	12
4a29b709	13	#include "vimc-common.h"
adc589d2	14	#include "vimc-streamer.h"
4a29b709	15
ec917d77	16	struct vimc_capture_device {
f2fe8906 HK	17	struct vimc_ent_device ved;
	18	struct video_device vdev;
	19	struct v4l2_pix_format format;
	20	struct vb2_queue queue;
	21	struct list_head buf_list;
	22	/*
	23	* NOTE: in a real driver, a spin lock must be used to access the
	24	* queue because the frames are generated from a hardware interruption
	25	* and the isr is not allowed to sleep.
	26	* Even if it is not necessary a spinlock in the vimc driver, we
	27	* use it here as a code reference
	28	*/
	29	spinlock_t qlock;
	30	struct mutex lock;
	31	u32 sequence;
adc589d2	32	struct vimc_stream stream;
23df45d0	33	struct media_pad pad;
f2fe8906 HK	34	};
f2fe8906 HK	35
535d296f HF	36	static const struct v4l2_pix_format fmt_default = {
	37	.width = 640,
	38	.height = 480,
	39	.pixelformat = V4L2_PIX_FMT_RGB24,
	40	.field = V4L2_FIELD_NONE,
9a42a5ff	41	.colorspace = V4L2_COLORSPACE_SRGB,
535d296f HF	42	};
535d296f HF	43
ec917d77	44	struct vimc_capture_buffer {
f2fe8906 HK	45	/*
	46	* struct vb2_v4l2_buffer must be the first element
	47	* the videobuf2 framework will allocate this struct based on
	48	* buf_struct_size and use the first sizeof(struct vb2_buffer) bytes of
	49	* memory as a vb2_buffer
	50	*/
	51	struct vb2_v4l2_buffer vb2;
	52	struct list_head list;
	53	};
	54
ec917d77	55	static int vimc_capture_querycap(struct file file, void priv,
f2fe8906 HK	56	struct v4l2_capability *cap)
f2fe8906 HK	57	{
04ee6d61	58	strscpy(cap->driver, VIMC_PDEV_NAME, sizeof(cap->driver));
c0decac1	59	strscpy(cap->card, KBUILD_MODNAME, sizeof(cap->card));
f2fe8906	60	snprintf(cap->bus_info, sizeof(cap->bus_info),
6fd369dd	61	"platform:%s", VIMC_PDEV_NAME);
f2fe8906 HK	62
	63	return 0;
	64	}
	65
ec917d77	66	static void vimc_capture_get_format(struct vimc_ent_device *ved,
288a22d4 HF	67	struct v4l2_pix_format *fmt)
288a22d4 HF	68	{
ec917d77	69	struct vimc_capture_device *vcapture = container_of(ved, struct vimc_capture_device,
288a22d4 HF	70	ved);
288a22d4 HF	71
ec917d77	72	*fmt = vcapture->format;
288a22d4 HF	73	}
288a22d4 HF	74
ec917d77	75	static int vimc_capture_g_fmt_vid_cap(struct file file, void priv,
f2fe8906 HK	76	struct v4l2_format *f)
f2fe8906 HK	77	{
ec917d77	78	struct vimc_capture_device *vcapture = video_drvdata(file);
f2fe8906	79
ec917d77	80	f->fmt.pix = vcapture->format;
f2fe8906 HK	81
	82	return 0;
	83	}
	84
ec917d77	85	static int vimc_capture_try_fmt_vid_cap(struct file file, void priv,
535d296f HF	86	struct v4l2_format *f)
	87	{
	88	struct v4l2_pix_format *format = &f->fmt.pix;
09c41a23	89	const struct vimc_pix_map *vpix;
535d296f HF	90
	91	format->width = clamp_t(u32, format->width, VIMC_FRAME_MIN_WIDTH,
	92	VIMC_FRAME_MAX_WIDTH) & ~1;
	93	format->height = clamp_t(u32, format->height, VIMC_FRAME_MIN_HEIGHT,
	94	VIMC_FRAME_MAX_HEIGHT) & ~1;
	95
09c41a23 HK	96	/* Don't accept a pixelformat that is not on the table */
	97	vpix = vimc_pix_map_by_pixelformat(format->pixelformat);
	98	if (!vpix) {
	99	format->pixelformat = fmt_default.pixelformat;
	100	vpix = vimc_pix_map_by_pixelformat(format->pixelformat);
	101	}
	102	/* TODO: Add support for custom bytesperline values */
	103	format->bytesperline = format->width * vpix->bpp;
	104	format->sizeimage = format->bytesperline * format->height;
535d296f HF	105
	106	if (format->field == V4L2_FIELD_ANY)
	107	format->field = fmt_default.field;
	108
09c41a23	109	vimc_colorimetry_clamp(format);
b6c61a6c	110
9a42a5ff NS	111	if (format->colorspace == V4L2_COLORSPACE_DEFAULT)
	112	format->colorspace = fmt_default.colorspace;
	113
09c41a23	114	return 0;
535d296f HF	115	}
535d296f HF	116
ec917d77	117	static int vimc_capture_s_fmt_vid_cap(struct file file, void priv,
535d296f HF	118	struct v4l2_format *f)
535d296f HF	119	{
ec917d77	120	struct vimc_capture_device *vcapture = video_drvdata(file);
77ae46e1	121	int ret;
535d296f HF	122
535d296f HF	123	/* Do not change the format while stream is on */
ec917d77	124	if (vb2_is_busy(&vcapture->queue))
535d296f HF	125	return -EBUSY;
535d296f HF	126
ec917d77	127	ret = vimc_capture_try_fmt_vid_cap(file, priv, f);
77ae46e1 AA	128	if (ret)
77ae46e1 AA	129	return ret;
535d296f	130
ec917d77	131	dev_dbg(vcapture->ved.dev, "%s: format update: "
535d296f	132	"old:%dx%d (0x%x, %d, %d, %d, %d) "
ec917d77	133	"new:%dx%d (0x%x, %d, %d, %d, %d)\n", vcapture->vdev.name,
535d296f	134	/* old */
ec917d77 DO	135	vcapture->format.width, vcapture->format.height,
	136	vcapture->format.pixelformat, vcapture->format.colorspace,
	137	vcapture->format.quantization, vcapture->format.xfer_func,
	138	vcapture->format.ycbcr_enc,
535d296f HF	139	/* new */
	140	f->fmt.pix.width, f->fmt.pix.height,
	141	f->fmt.pix.pixelformat, f->fmt.pix.colorspace,
	142	f->fmt.pix.quantization, f->fmt.pix.xfer_func,
	143	f->fmt.pix.ycbcr_enc);
	144
ec917d77	145	vcapture->format = f->fmt.pix;
535d296f HF	146
	147	return 0;
	148	}
	149
ec917d77	150	static int vimc_capture_enum_fmt_vid_cap(struct file file, void priv,
f2fe8906 HK	151	struct v4l2_fmtdesc *f)
f2fe8906 HK	152	{
c7ff09f6 NS	153	const struct vimc_pix_map *vpix;
	154
	155	if (f->mbus_code) {
	156	if (f->index > 0)
	157	return -EINVAL;
	158
	159	vpix = vimc_pix_map_by_code(f->mbus_code);
	160	} else {
	161	vpix = vimc_pix_map_by_index(f->index);
	162	}
09c41a23 HK	163
09c41a23 HK	164	if (!vpix)
f2fe8906 HK	165	return -EINVAL;
f2fe8906 HK	166
09c41a23	167	f->pixelformat = vpix->pixelformat;
535d296f HF	168
	169	return 0;
	170	}
	171
ec917d77	172	static int vimc_capture_enum_framesizes(struct file file, void fh,
535d296f HF	173	struct v4l2_frmsizeenum *fsize)
535d296f HF	174	{
09c41a23 HK	175	const struct vimc_pix_map *vpix;
09c41a23 HK	176
535d296f HF	177	if (fsize->index)
	178	return -EINVAL;
	179
09c41a23 HK	180	/* Only accept code in the pix map table */
	181	vpix = vimc_pix_map_by_code(fsize->pixel_format);
	182	if (!vpix)
535d296f HF	183	return -EINVAL;
	184
	185	fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
	186	fsize->stepwise.min_width = VIMC_FRAME_MIN_WIDTH;
	187	fsize->stepwise.max_width = VIMC_FRAME_MAX_WIDTH;
	188	fsize->stepwise.min_height = VIMC_FRAME_MIN_HEIGHT;
	189	fsize->stepwise.max_height = VIMC_FRAME_MAX_HEIGHT;
5efbc65f HF	190	fsize->stepwise.step_width = 1;
5efbc65f HF	191	fsize->stepwise.step_height = 1;
f2fe8906 HK	192
	193	return 0;
	194	}
	195
ec917d77	196	static const struct v4l2_file_operations vimc_capture_fops = {
f2fe8906 HK	197	.owner = THIS_MODULE,
	198	.open = v4l2_fh_open,
	199	.release = vb2_fop_release,
	200	.read = vb2_fop_read,
	201	.poll = vb2_fop_poll,
	202	.unlocked_ioctl = video_ioctl2,
	203	.mmap = vb2_fop_mmap,
	204	};
	205
ec917d77 DO	206	static const struct v4l2_ioctl_ops vimc_capture_ioctl_ops = {
ec917d77 DO	207	.vidioc_querycap = vimc_capture_querycap,
f2fe8906	208
ec917d77 DO	209	.vidioc_g_fmt_vid_cap = vimc_capture_g_fmt_vid_cap,
	210	.vidioc_s_fmt_vid_cap = vimc_capture_s_fmt_vid_cap,
	211	.vidioc_try_fmt_vid_cap = vimc_capture_try_fmt_vid_cap,
	212	.vidioc_enum_fmt_vid_cap = vimc_capture_enum_fmt_vid_cap,
	213	.vidioc_enum_framesizes = vimc_capture_enum_framesizes,
f2fe8906 HK	214
	215	.vidioc_reqbufs = vb2_ioctl_reqbufs,
	216	.vidioc_create_bufs = vb2_ioctl_create_bufs,
	217	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
	218	.vidioc_querybuf = vb2_ioctl_querybuf,
	219	.vidioc_qbuf = vb2_ioctl_qbuf,
	220	.vidioc_dqbuf = vb2_ioctl_dqbuf,
	221	.vidioc_expbuf = vb2_ioctl_expbuf,
	222	.vidioc_streamon = vb2_ioctl_streamon,
	223	.vidioc_streamoff = vb2_ioctl_streamoff,
	224	};
	225
ec917d77	226	static void vimc_capture_return_all_buffers(struct vimc_capture_device *vcapture,
f2fe8906 HK	227	enum vb2_buffer_state state)
f2fe8906 HK	228	{
ec917d77	229	struct vimc_capture_buffer vbuf, node;
f2fe8906	230
ec917d77	231	spin_lock(&vcapture->qlock);
f2fe8906	232
ec917d77	233	list_for_each_entry_safe(vbuf, node, &vcapture->buf_list, list) {
f2fe8906 HK	234	list_del(&vbuf->list);
	235	vb2_buffer_done(&vbuf->vb2.vb2_buf, state);
	236	}
	237
ec917d77	238	spin_unlock(&vcapture->qlock);
f2fe8906 HK	239	}
f2fe8906 HK	240
ec917d77	241	static int vimc_capture_start_streaming(struct vb2_queue *vq, unsigned int count)
f2fe8906	242	{
ec917d77	243	struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq);
f2fe8906 HK	244	int ret;
f2fe8906 HK	245
ec917d77	246	vcapture->sequence = 0;
f2fe8906 HK	247
f2fe8906 HK	248	/* Start the media pipeline */
12cecbf9	249	ret = video_device_pipeline_start(&vcapture->vdev, &vcapture->stream.pipe);
f2fe8906	250	if (ret) {
ec917d77	251	vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_QUEUED);
f2fe8906 HK	252	return ret;
	253	}
	254
ec917d77	255	ret = vimc_streamer_s_stream(&vcapture->stream, &vcapture->ved, 1);
f2fe8906	256	if (ret) {
12cecbf9	257	video_device_pipeline_stop(&vcapture->vdev);
ec917d77	258	vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_QUEUED);
f2fe8906 HK	259	return ret;
	260	}
	261
	262	return 0;
	263	}
	264
	265	/*
	266	* Stop the stream engine. Any remaining buffers in the stream queue are
	267	* dequeued and passed on to the vb2 framework marked as STATE_ERROR.
	268	*/
ec917d77	269	static void vimc_capture_stop_streaming(struct vb2_queue *vq)
f2fe8906	270	{
ec917d77	271	struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq);
f2fe8906	272
ec917d77	273	vimc_streamer_s_stream(&vcapture->stream, &vcapture->ved, 0);
f2fe8906 HK	274
f2fe8906 HK	275	/* Stop the media pipeline */
12cecbf9	276	video_device_pipeline_stop(&vcapture->vdev);
f2fe8906 HK	277
f2fe8906 HK	278	/* Release all active buffers */
ec917d77	279	vimc_capture_return_all_buffers(vcapture, VB2_BUF_STATE_ERROR);
f2fe8906 HK	280	}
f2fe8906 HK	281
ec917d77	282	static void vimc_capture_buf_queue(struct vb2_buffer *vb2_buf)
f2fe8906	283	{
ec917d77 DO	284	struct vimc_capture_device *vcapture = vb2_get_drv_priv(vb2_buf->vb2_queue);
	285	struct vimc_capture_buffer *buf = container_of(vb2_buf,
	286	struct vimc_capture_buffer,
f2fe8906 HK	287	vb2.vb2_buf);
f2fe8906 HK	288
ec917d77 DO	289	spin_lock(&vcapture->qlock);
	290	list_add_tail(&buf->list, &vcapture->buf_list);
	291	spin_unlock(&vcapture->qlock);
f2fe8906 HK	292	}
f2fe8906 HK	293
ec917d77	294	static int vimc_capture_queue_setup(struct vb2_queue vq, unsigned int nbuffers,
f2fe8906 HK	295	unsigned int *nplanes, unsigned int sizes[],
	296	struct device *alloc_devs[])
	297	{
ec917d77	298	struct vimc_capture_device *vcapture = vb2_get_drv_priv(vq);
f2fe8906 HK	299
f2fe8906 HK	300	if (*nplanes)
ec917d77	301	return sizes[0] < vcapture->format.sizeimage ? -EINVAL : 0;
f2fe8906 HK	302	/* We don't support multiplanes for now */
f2fe8906 HK	303	*nplanes = 1;
ec917d77	304	sizes[0] = vcapture->format.sizeimage;
f2fe8906 HK	305
	306	return 0;
	307	}
	308
ec917d77	309	static int vimc_capture_buffer_prepare(struct vb2_buffer *vb)
f2fe8906	310	{
ec917d77 DO	311	struct vimc_capture_device *vcapture = vb2_get_drv_priv(vb->vb2_queue);
ec917d77 DO	312	unsigned long size = vcapture->format.sizeimage;
f2fe8906 HK	313
f2fe8906 HK	314	if (vb2_plane_size(vb, 0) < size) {
ec917d77 DO	315	dev_err(vcapture->ved.dev, "%s: buffer too small (%lu < %lu)\n",
ec917d77 DO	316	vcapture->vdev.name, vb2_plane_size(vb, 0), size);
f2fe8906 HK	317	return -EINVAL;
	318	}
	319	return 0;
	320	}
	321
ec917d77 DO	322	static const struct vb2_ops vimc_capture_qops = {
	323	.start_streaming = vimc_capture_start_streaming,
	324	.stop_streaming = vimc_capture_stop_streaming,
	325	.buf_queue = vimc_capture_buf_queue,
	326	.queue_setup = vimc_capture_queue_setup,
	327	.buf_prepare = vimc_capture_buffer_prepare,
f2fe8906 HK	328	/*
	329	* Since q->lock is set we can use the standard
	330	* vb2_ops_wait_prepare/finish helper functions.
	331	*/
	332	.wait_prepare = vb2_ops_wait_prepare,
	333	.wait_finish = vb2_ops_wait_finish,
	334	};
	335
ec917d77	336	static const struct media_entity_operations vimc_capture_mops = {
31172e52	337	.link_validate = vimc_vdev_link_validate,
f2fe8906 HK	338	};
f2fe8906 HK	339
ec917d77	340	static void vimc_capture_release(struct vimc_ent_device *ved)
3650a23e	341	{
ec917d77 DO	342	struct vimc_capture_device *vcapture =
ec917d77 DO	343	container_of(ved, struct vimc_capture_device, ved);
3650a23e	344
ec917d77 DO	345	media_entity_cleanup(vcapture->ved.ent);
ec917d77 DO	346	kfree(vcapture);
3650a23e HV	347	}
3650a23e HV	348
ec917d77	349	static void vimc_capture_unregister(struct vimc_ent_device *ved)
f2fe8906	350	{
ec917d77 DO	351	struct vimc_capture_device *vcapture =
ec917d77 DO	352	container_of(ved, struct vimc_capture_device, ved);
f2fe8906	353
ec917d77	354	vb2_video_unregister_device(&vcapture->vdev);
f2fe8906 HK	355	}
f2fe8906 HK	356
ec917d77	357	static void vimc_capture_process_frame(struct vimc_ent_device ved,
adc589d2	358	const void *frame)
f2fe8906	359	{
ec917d77	360	struct vimc_capture_device *vcapture = container_of(ved, struct vimc_capture_device,
f2fe8906	361	ved);
ec917d77	362	struct vimc_capture_buffer *vimc_buf;
f2fe8906 HK	363	void *vbuf;
f2fe8906 HK	364
ec917d77	365	spin_lock(&vcapture->qlock);
f2fe8906 HK	366
f2fe8906 HK	367	/* Get the first entry of the list */
ec917d77	368	vimc_buf = list_first_entry_or_null(&vcapture->buf_list,
f2fe8906 HK	369	typeof(*vimc_buf), list);
f2fe8906 HK	370	if (!vimc_buf) {
ec917d77	371	spin_unlock(&vcapture->qlock);
adc589d2	372	return ERR_PTR(-EAGAIN);
f2fe8906 HK	373	}
	374
	375	/* Remove this entry from the list */
	376	list_del(&vimc_buf->list);
	377
ec917d77	378	spin_unlock(&vcapture->qlock);
f2fe8906 HK	379
	380	/* Fill the buffer */
	381	vimc_buf->vb2.vb2_buf.timestamp = ktime_get_ns();
ec917d77 DO	382	vimc_buf->vb2.sequence = vcapture->sequence++;
ec917d77 DO	383	vimc_buf->vb2.field = vcapture->format.field;
f2fe8906 HK	384
	385	vbuf = vb2_plane_vaddr(&vimc_buf->vb2.vb2_buf, 0);
	386
ec917d77	387	memcpy(vbuf, frame, vcapture->format.sizeimage);
f2fe8906 HK	388
	389	/* Set it as ready */
	390	vb2_set_plane_payload(&vimc_buf->vb2.vb2_buf, 0,
ec917d77	391	vcapture->format.sizeimage);
f2fe8906	392	vb2_buffer_done(&vimc_buf->vb2.vb2_buf, VB2_BUF_STATE_DONE);
adc589d2	393	return NULL;
f2fe8906 HK	394	}
f2fe8906 HK	395
ec917d77	396	static struct vimc_ent_device vimc_capture_add(struct vimc_device vimc,
e472d6a2	397	const char *vcfg_name)
f2fe8906	398	{
f13d5f36	399	struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
09c41a23	400	const struct vimc_pix_map *vpix;
ec917d77	401	struct vimc_capture_device *vcapture;
f2fe8906 HK	402	struct video_device *vdev;
	403	struct vb2_queue *q;
	404	int ret;
	405
ec917d77 DO	406	/* Allocate the vimc_capture_device struct */
	407	vcapture = kzalloc(sizeof(*vcapture), GFP_KERNEL);
	408	if (!vcapture)
967534cb	409	return ERR_PTR(-ENOMEM);
f2fe8906	410
f2fe8906	411	/* Initialize the media entity */
ec917d77 DO	412	vcapture->vdev.entity.name = vcfg_name;
	413	vcapture->vdev.entity.function = MEDIA_ENT_F_IO_V4L;
	414	vcapture->pad.flags = MEDIA_PAD_FL_SINK;
	415	ret = media_entity_pads_init(&vcapture->vdev.entity,
	416	1, &vcapture->pad);
f2fe8906	417	if (ret)
ec917d77	418	goto err_free_vcapture;
f2fe8906 HK	419
f2fe8906 HK	420	/* Initialize the lock */
ec917d77	421	mutex_init(&vcapture->lock);
f2fe8906 HK	422
f2fe8906 HK	423	/* Initialize the vb2 queue */
ec917d77	424	q = &vcapture->queue;
f2fe8906	425	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
4a2e0a80 LP	426	q->io_modes = VB2_MMAP \| VB2_DMABUF;
	427	if (vimc_allocator == VIMC_ALLOCATOR_VMALLOC)
	428	q->io_modes \|= VB2_USERPTR;
ec917d77 DO	429	q->drv_priv = vcapture;
	430	q->buf_struct_size = sizeof(struct vimc_capture_buffer);
	431	q->ops = &vimc_capture_qops;
4a2e0a80 LP	432	q->mem_ops = vimc_allocator == VIMC_ALLOCATOR_DMA_CONTIG
4a2e0a80 LP	433	? &vb2_dma_contig_memops : &vb2_vmalloc_memops;
f2fe8906 HK	434	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
f2fe8906 HK	435	q->min_buffers_needed = 2;
ec917d77	436	q->lock = &vcapture->lock;
4a2e0a80	437	q->dev = v4l2_dev->dev;
f2fe8906 HK	438
	439	ret = vb2_queue_init(q);
	440	if (ret) {
2362f53d	441	dev_err(vimc->mdev.dev, "%s: vb2 queue init failed (err=%d)\n",
f13d5f36	442	vcfg_name, ret);
f2fe8906 HK	443	goto err_clean_m_ent;
	444	}
	445
	446	/* Initialize buffer list and its lock */
ec917d77 DO	447	INIT_LIST_HEAD(&vcapture->buf_list);
ec917d77 DO	448	spin_lock_init(&vcapture->qlock);
f2fe8906	449
535d296f	450	/* Set default frame format */
ec917d77 DO	451	vcapture->format = fmt_default;
	452	vpix = vimc_pix_map_by_pixelformat(vcapture->format.pixelformat);
	453	vcapture->format.bytesperline = vcapture->format.width * vpix->bpp;
	454	vcapture->format.sizeimage = vcapture->format.bytesperline *
	455	vcapture->format.height;
f2fe8906 HK	456
f2fe8906 HK	457	/* Fill the vimc_ent_device struct */
ec917d77 DO	458	vcapture->ved.ent = &vcapture->vdev.entity;
	459	vcapture->ved.process_frame = vimc_capture_process_frame;
	460	vcapture->ved.vdev_get_format = vimc_capture_get_format;
	461	vcapture->ved.dev = vimc->mdev.dev;
f2fe8906 HK	462
f2fe8906 HK	463	/* Initialize the video_device struct */
ec917d77	464	vdev = &vcapture->vdev;
c7ff09f6 NS	465	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE \| V4L2_CAP_STREAMING
c7ff09f6 NS	466	\| V4L2_CAP_IO_MC;
ec917d77	467	vdev->entity.ops = &vimc_capture_mops;
40326513	468	vdev->release = video_device_release_empty;
ec917d77 DO	469	vdev->fops = &vimc_capture_fops;
	470	vdev->ioctl_ops = &vimc_capture_ioctl_ops;
	471	vdev->lock = &vcapture->lock;
f2fe8906 HK	472	vdev->queue = q;
	473	vdev->v4l2_dev = v4l2_dev;
	474	vdev->vfl_dir = VFL_DIR_RX;
f13d5f36	475	strscpy(vdev->name, vcfg_name, sizeof(vdev->name));
ec917d77	476	video_set_drvdata(vdev, &vcapture->ved);
f2fe8906 HK	477
f2fe8906 HK	478	/* Register the video_device with the v4l2 and the media framework */
70cad449	479	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
f2fe8906	480	if (ret) {
2362f53d	481	dev_err(vimc->mdev.dev, "%s: video register failed (err=%d)\n",
ec917d77	482	vcapture->vdev.name, ret);
bda24f43	483	goto err_clean_m_ent;
f2fe8906 HK	484	}
f2fe8906 HK	485
ec917d77	486	return &vcapture->ved;
f2fe8906	487
f2fe8906	488	err_clean_m_ent:
ec917d77 DO	489	media_entity_cleanup(&vcapture->vdev.entity);
	490	err_free_vcapture:
	491	kfree(vcapture);
f2fe8906	492
967534cb	493	return ERR_PTR(ret);
4a29b709	494	}
e472d6a2	495
ec917d77 DO	496	struct vimc_ent_type vimc_capture_type = {
	497	.add = vimc_capture_add,
	498	.unregister = vimc_capture_unregister,
	499	.release = vimc_capture_release
e472d6a2	500	};