[mirror_ubuntu-jammy-kernel.git] / drivers / media / platform / 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 <linux/component.h>
#include <linux/module.h>
#include <linux/mod_devicetable.h>
#include <linux/platform_device.h>
#include <media/v4l2-ioctl.h>
#include <media/videobuf2-core.h>
#include <media/videobuf2-vmalloc.h>

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

#define VIMC_CAP_DRV_NAME "vimc-capture"

static const u32 vimc_cap_supported_pixfmt[] = {
	V4L2_PIX_FMT_BGR24,
	V4L2_PIX_FMT_RGB24,
	V4L2_PIX_FMT_ARGB32,
	V4L2_PIX_FMT_SBGGR8,
	V4L2_PIX_FMT_SGBRG8,
	V4L2_PIX_FMT_SGRBG8,
	V4L2_PIX_FMT_SRGGB8,
	V4L2_PIX_FMT_SBGGR10,
	V4L2_PIX_FMT_SGBRG10,
	V4L2_PIX_FMT_SGRBG10,
	V4L2_PIX_FMT_SRGGB10,
	V4L2_PIX_FMT_SBGGR10ALAW8,
	V4L2_PIX_FMT_SGBRG10ALAW8,
	V4L2_PIX_FMT_SGRBG10ALAW8,
	V4L2_PIX_FMT_SRGGB10ALAW8,
	V4L2_PIX_FMT_SBGGR10DPCM8,
	V4L2_PIX_FMT_SGBRG10DPCM8,
	V4L2_PIX_FMT_SGRBG10DPCM8,
	V4L2_PIX_FMT_SRGGB10DPCM8,
	V4L2_PIX_FMT_SBGGR12,
	V4L2_PIX_FMT_SGBRG12,
	V4L2_PIX_FMT_SGRBG12,
	V4L2_PIX_FMT_SRGGB12,
};

struct vimc_cap_device {
	struct vimc_ent_device ved;
	struct video_device vdev;
	struct device *dev;
	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;
};

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

struct vimc_cap_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_cap_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_cap_get_format(struct vimc_ent_device *ved,
				struct v4l2_pix_format *fmt)
{
	struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
						    ved);

	*fmt = vcap->format;
}

static int vimc_cap_g_fmt_vid_cap(struct file *file, void *priv,
				  struct v4l2_format *f)
{
	struct vimc_cap_device *vcap = video_drvdata(file);

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

	return 0;
}

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

	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;

	vimc_colorimetry_clamp(format);

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

	/* TODO: Add support for custom bytesperline values */

	/* Don't accept a pixelformat that is not on the table */
	if (!v4l2_format_info(format->pixelformat))
		format->pixelformat = fmt_default.pixelformat;

	return v4l2_fill_pixfmt(format, format->pixelformat,
				format->width, format->height);
}

static int vimc_cap_s_fmt_vid_cap(struct file *file, void *priv,
				  struct v4l2_format *f)
{
	struct vimc_cap_device *vcap = video_drvdata(file);

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

	vimc_cap_try_fmt_vid_cap(file, priv, f);

	dev_dbg(vcap->dev, "%s: format update: "
		"old:%dx%d (0x%x, %d, %d, %d, %d) "
		"new:%dx%d (0x%x, %d, %d, %d, %d)\n", vcap->vdev.name,
		/* old */
		vcap->format.width, vcap->format.height,
		vcap->format.pixelformat, vcap->format.colorspace,
		vcap->format.quantization, vcap->format.xfer_func,
		vcap->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);

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

	return 0;
}

static int vimc_cap_enum_fmt_vid_cap(struct file *file, void *priv,
				     struct v4l2_fmtdesc *f)
{
	if (f->index >= ARRAY_SIZE(vimc_cap_supported_pixfmt))
		return -EINVAL;

	f->pixelformat = vimc_cap_supported_pixfmt[f->index];

	return 0;
}

static bool vimc_cap_is_pixfmt_supported(u32 pixelformat)
{
	unsigned int i;

	for (i = 0; i < ARRAY_SIZE(vimc_cap_supported_pixfmt); i++)
		if (vimc_cap_supported_pixfmt[i] == pixelformat)
			return true;
	return false;
}

static int vimc_cap_enum_framesizes(struct file *file, void *fh,
				    struct v4l2_frmsizeenum *fsize)
{
	if (fsize->index)
		return -EINVAL;

	if (!vimc_cap_is_pixfmt_supported(fsize->pixel_format))
		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_cap_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_cap_ioctl_ops = {
	.vidioc_querycap = vimc_cap_querycap,

	.vidioc_g_fmt_vid_cap = vimc_cap_g_fmt_vid_cap,
	.vidioc_s_fmt_vid_cap = vimc_cap_s_fmt_vid_cap,
	.vidioc_try_fmt_vid_cap = vimc_cap_try_fmt_vid_cap,
	.vidioc_enum_fmt_vid_cap = vimc_cap_enum_fmt_vid_cap,
	.vidioc_enum_framesizes = vimc_cap_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_cap_return_all_buffers(struct vimc_cap_device *vcap,
					enum vb2_buffer_state state)
{
	struct vimc_cap_buffer *vbuf, *node;

	spin_lock(&vcap->qlock);

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

	spin_unlock(&vcap->qlock);
}

static int vimc_cap_start_streaming(struct vb2_queue *vq, unsigned int count)
{
	struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
	struct media_entity *entity = &vcap->vdev.entity;
	int ret;

	vcap->sequence = 0;

	/* Start the media pipeline */
	ret = media_pipeline_start(entity, &vcap->stream.pipe);
	if (ret) {
		vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_QUEUED);
		return ret;
	}

	vcap->stream.producer_pixfmt = vcap->format.pixelformat;
	ret = vimc_streamer_s_stream(&vcap->stream, &vcap->ved, 1);
	if (ret) {
		media_pipeline_stop(entity);
		vimc_cap_return_all_buffers(vcap, 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_cap_stop_streaming(struct vb2_queue *vq)
{
	struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);

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

	/* Stop the media pipeline */
	media_pipeline_stop(&vcap->vdev.entity);

	/* Release all active buffers */
	vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_ERROR);
}

static void vimc_cap_buf_queue(struct vb2_buffer *vb2_buf)
{
	struct vimc_cap_device *vcap = vb2_get_drv_priv(vb2_buf->vb2_queue);
	struct vimc_cap_buffer *buf = container_of(vb2_buf,
						   struct vimc_cap_buffer,
						   vb2.vb2_buf);

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

static int vimc_cap_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
				unsigned int *nplanes, unsigned int sizes[],
				struct device *alloc_devs[])
{
	struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);

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

	return 0;
}

static int vimc_cap_buffer_prepare(struct vb2_buffer *vb)
{
	struct vimc_cap_device *vcap = vb2_get_drv_priv(vb->vb2_queue);
	unsigned long size = vcap->format.sizeimage;

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

static const struct vb2_ops vimc_cap_qops = {
	.start_streaming	= vimc_cap_start_streaming,
	.stop_streaming		= vimc_cap_stop_streaming,
	.buf_queue		= vimc_cap_buf_queue,
	.queue_setup		= vimc_cap_queue_setup,
	.buf_prepare		= vimc_cap_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_cap_mops = {
	.link_validate		= vimc_link_validate,
};

static void vimc_cap_release(struct video_device *vdev)
{
	struct vimc_cap_device *vcap =
		container_of(vdev, struct vimc_cap_device, vdev);

	vimc_pads_cleanup(vcap->ved.pads);
	kfree(vcap);
}

static void vimc_cap_comp_unbind(struct device *comp, struct device *master,
				 void *master_data)
{
	struct vimc_ent_device *ved = dev_get_drvdata(comp);
	struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
						    ved);

	vb2_queue_release(&vcap->queue);
	media_entity_cleanup(ved->ent);
	video_unregister_device(&vcap->vdev);
}

static void *vimc_cap_process_frame(struct vimc_ent_device *ved,
				    const void *frame)
{
	struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
						    ved);
	struct vimc_cap_buffer *vimc_buf;
	void *vbuf;

	spin_lock(&vcap->qlock);

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

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

	spin_unlock(&vcap->qlock);

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

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

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

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

static int vimc_cap_comp_bind(struct device *comp, struct device *master,
			      void *master_data)
{
	struct v4l2_device *v4l2_dev = master_data;
	struct vimc_platform_data *pdata = comp->platform_data;
	struct vimc_cap_device *vcap;
	struct video_device *vdev;
	struct vb2_queue *q;
	int ret;

	/* Allocate the vimc_cap_device struct */
	vcap = kzalloc(sizeof(*vcap), GFP_KERNEL);
	if (!vcap)
		return -ENOMEM;

	/* Allocate the pads */
	vcap->ved.pads =
		vimc_pads_init(1, (const unsigned long[1]) {MEDIA_PAD_FL_SINK});
	if (IS_ERR(vcap->ved.pads)) {
		ret = PTR_ERR(vcap->ved.pads);
		goto err_free_vcap;
	}

	/* Initialize the media entity */
	vcap->vdev.entity.name = pdata->entity_name;
	vcap->vdev.entity.function = MEDIA_ENT_F_IO_V4L;
	ret = media_entity_pads_init(&vcap->vdev.entity,
				     1, vcap->ved.pads);
	if (ret)
		goto err_clean_pads;

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

	/* Initialize the vb2 queue */
	q = &vcap->queue;
	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	q->io_modes = VB2_MMAP | VB2_DMABUF | VB2_USERPTR;
	q->drv_priv = vcap;
	q->buf_struct_size = sizeof(struct vimc_cap_buffer);
	q->ops = &vimc_cap_qops;
	q->mem_ops = &vb2_vmalloc_memops;
	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
	q->min_buffers_needed = 2;
	q->lock = &vcap->lock;

	ret = vb2_queue_init(q);
	if (ret) {
		dev_err(comp, "%s: vb2 queue init failed (err=%d)\n",
			pdata->entity_name, ret);
		goto err_clean_m_ent;
	}

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

	/* Set default frame format */
	vcap->format = fmt_default;
	v4l2_fill_pixfmt(&vcap->format, vcap->format.pixelformat,
			 vcap->format.width, vcap->format.height);

	/* Fill the vimc_ent_device struct */
	vcap->ved.ent = &vcap->vdev.entity;
	vcap->ved.process_frame = vimc_cap_process_frame;
	vcap->ved.vdev_get_format = vimc_cap_get_format;
	dev_set_drvdata(comp, &vcap->ved);
	vcap->dev = comp;

	/* Initialize the video_device struct */
	vdev = &vcap->vdev;
	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
	vdev->entity.ops = &vimc_cap_mops;
	vdev->release = vimc_cap_release;
	vdev->fops = &vimc_cap_fops;
	vdev->ioctl_ops = &vimc_cap_ioctl_ops;
	vdev->lock = &vcap->lock;
	vdev->queue = q;
	vdev->v4l2_dev = v4l2_dev;
	vdev->vfl_dir = VFL_DIR_RX;
	strscpy(vdev->name, pdata->entity_name, sizeof(vdev->name));
	video_set_drvdata(vdev, &vcap->ved);

	/* Register the video_device with the v4l2 and the media framework */
	ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
	if (ret) {
		dev_err(comp, "%s: video register failed (err=%d)\n",
			vcap->vdev.name, ret);
		goto err_release_queue;
	}

	return 0;

err_release_queue:
	vb2_queue_release(q);
err_clean_m_ent:
	media_entity_cleanup(&vcap->vdev.entity);
err_clean_pads:
	vimc_pads_cleanup(vcap->ved.pads);
err_free_vcap:
	kfree(vcap);

	return ret;
}

static const struct component_ops vimc_cap_comp_ops = {
	.bind = vimc_cap_comp_bind,
	.unbind = vimc_cap_comp_unbind,
};

static int vimc_cap_probe(struct platform_device *pdev)
{
	return component_add(&pdev->dev, &vimc_cap_comp_ops);
}

static int vimc_cap_remove(struct platform_device *pdev)
{
	component_del(&pdev->dev, &vimc_cap_comp_ops);

	return 0;
}

static const struct platform_device_id vimc_cap_driver_ids[] = {
	{
		.name           = VIMC_CAP_DRV_NAME,
	},
	{ }
};

static struct platform_driver vimc_cap_pdrv = {
	.probe		= vimc_cap_probe,
	.remove		= vimc_cap_remove,
	.id_table	= vimc_cap_driver_ids,
	.driver		= {
		.name	= VIMC_CAP_DRV_NAME,
	},
};

module_platform_driver(vimc_cap_pdrv);

MODULE_DEVICE_TABLE(platform, vimc_cap_driver_ids);

MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Capture");
MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>");
MODULE_LICENSE("GPL");
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	*/
f2fe8906 HK	7
4a29b709 HF	8	#include <linux/component.h>
4a29b709 HF	9	#include <linux/module.h>
ac316725	10	#include <linux/mod_devicetable.h>
4a29b709	11	#include <linux/platform_device.h>
f2fe8906 HK	12	#include <media/v4l2-ioctl.h>
	13	#include <media/videobuf2-core.h>
	14	#include <media/videobuf2-vmalloc.h>
	15
4a29b709	16	#include "vimc-common.h"
adc589d2	17	#include "vimc-streamer.h"
4a29b709 HF	18
4a29b709 HF	19	#define VIMC_CAP_DRV_NAME "vimc-capture"
f2fe8906	20
b6c61a6c HF	21	static const u32 vimc_cap_supported_pixfmt[] = {
	22	V4L2_PIX_FMT_BGR24,
	23	V4L2_PIX_FMT_RGB24,
	24	V4L2_PIX_FMT_ARGB32,
	25	V4L2_PIX_FMT_SBGGR8,
	26	V4L2_PIX_FMT_SGBRG8,
	27	V4L2_PIX_FMT_SGRBG8,
	28	V4L2_PIX_FMT_SRGGB8,
	29	V4L2_PIX_FMT_SBGGR10,
	30	V4L2_PIX_FMT_SGBRG10,
	31	V4L2_PIX_FMT_SGRBG10,
	32	V4L2_PIX_FMT_SRGGB10,
	33	V4L2_PIX_FMT_SBGGR10ALAW8,
	34	V4L2_PIX_FMT_SGBRG10ALAW8,
	35	V4L2_PIX_FMT_SGRBG10ALAW8,
	36	V4L2_PIX_FMT_SRGGB10ALAW8,
	37	V4L2_PIX_FMT_SBGGR10DPCM8,
	38	V4L2_PIX_FMT_SGBRG10DPCM8,
	39	V4L2_PIX_FMT_SGRBG10DPCM8,
	40	V4L2_PIX_FMT_SRGGB10DPCM8,
	41	V4L2_PIX_FMT_SBGGR12,
	42	V4L2_PIX_FMT_SGBRG12,
	43	V4L2_PIX_FMT_SGRBG12,
	44	V4L2_PIX_FMT_SRGGB12,
	45	};
	46
f2fe8906 HK	47	struct vimc_cap_device {
	48	struct vimc_ent_device ved;
	49	struct video_device vdev;
4a29b709	50	struct device *dev;
f2fe8906 HK	51	struct v4l2_pix_format format;
	52	struct vb2_queue queue;
	53	struct list_head buf_list;
	54	/*
	55	* NOTE: in a real driver, a spin lock must be used to access the
	56	* queue because the frames are generated from a hardware interruption
	57	* and the isr is not allowed to sleep.
	58	* Even if it is not necessary a spinlock in the vimc driver, we
	59	* use it here as a code reference
	60	*/
	61	spinlock_t qlock;
	62	struct mutex lock;
	63	u32 sequence;
adc589d2	64	struct vimc_stream stream;
f2fe8906 HK	65	};
f2fe8906 HK	66
535d296f HF	67	static const struct v4l2_pix_format fmt_default = {
	68	.width = 640,
	69	.height = 480,
	70	.pixelformat = V4L2_PIX_FMT_RGB24,
	71	.field = V4L2_FIELD_NONE,
	72	.colorspace = V4L2_COLORSPACE_DEFAULT,
	73	};
	74
f2fe8906 HK	75	struct vimc_cap_buffer {
	76	/*
	77	* struct vb2_v4l2_buffer must be the first element
	78	* the videobuf2 framework will allocate this struct based on
	79	* buf_struct_size and use the first sizeof(struct vb2_buffer) bytes of
	80	* memory as a vb2_buffer
	81	*/
	82	struct vb2_v4l2_buffer vb2;
	83	struct list_head list;
	84	};
	85
	86	static int vimc_cap_querycap(struct file file, void priv,
	87	struct v4l2_capability *cap)
	88	{
04ee6d61	89	strscpy(cap->driver, VIMC_PDEV_NAME, sizeof(cap->driver));
c0decac1	90	strscpy(cap->card, KBUILD_MODNAME, sizeof(cap->card));
f2fe8906	91	snprintf(cap->bus_info, sizeof(cap->bus_info),
6fd369dd	92	"platform:%s", VIMC_PDEV_NAME);
f2fe8906 HK	93
	94	return 0;
	95	}
	96
288a22d4 HF	97	static void vimc_cap_get_format(struct vimc_ent_device *ved,
	98	struct v4l2_pix_format *fmt)
	99	{
	100	struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
	101	ved);
	102
	103	*fmt = vcap->format;
	104	}
	105
535d296f	106	static int vimc_cap_g_fmt_vid_cap(struct file file, void priv,
f2fe8906 HK	107	struct v4l2_format *f)
	108	{
	109	struct vimc_cap_device *vcap = video_drvdata(file);
	110
	111	f->fmt.pix = vcap->format;
	112
	113	return 0;
	114	}
	115
535d296f HF	116	static int vimc_cap_try_fmt_vid_cap(struct file file, void priv,
	117	struct v4l2_format *f)
	118	{
	119	struct v4l2_pix_format *format = &f->fmt.pix;
535d296f HF	120
	121	format->width = clamp_t(u32, format->width, VIMC_FRAME_MIN_WIDTH,
	122	VIMC_FRAME_MAX_WIDTH) & ~1;
	123	format->height = clamp_t(u32, format->height, VIMC_FRAME_MIN_HEIGHT,
	124	VIMC_FRAME_MAX_HEIGHT) & ~1;
	125
b6c61a6c	126	vimc_colorimetry_clamp(format);
535d296f HF	127
	128	if (format->field == V4L2_FIELD_ANY)
	129	format->field = fmt_default.field;
	130
b6c61a6c	131	/* TODO: Add support for custom bytesperline values */
535d296f	132
b6c61a6c HF	133	/* Don't accept a pixelformat that is not on the table */
	134	if (!v4l2_format_info(format->pixelformat))
	135	format->pixelformat = fmt_default.pixelformat;
	136
	137	return v4l2_fill_pixfmt(format, format->pixelformat,
	138	format->width, format->height);
535d296f HF	139	}
	140
	141	static int vimc_cap_s_fmt_vid_cap(struct file file, void priv,
	142	struct v4l2_format *f)
	143	{
	144	struct vimc_cap_device *vcap = video_drvdata(file);
	145
	146	/* Do not change the format while stream is on */
	147	if (vb2_is_busy(&vcap->queue))
	148	return -EBUSY;
	149
	150	vimc_cap_try_fmt_vid_cap(file, priv, f);
	151
4a29b709	152	dev_dbg(vcap->dev, "%s: format update: "
535d296f HF	153	"old:%dx%d (0x%x, %d, %d, %d, %d) "
	154	"new:%dx%d (0x%x, %d, %d, %d, %d)\n", vcap->vdev.name,
	155	/* old */
	156	vcap->format.width, vcap->format.height,
	157	vcap->format.pixelformat, vcap->format.colorspace,
	158	vcap->format.quantization, vcap->format.xfer_func,
	159	vcap->format.ycbcr_enc,
	160	/* new */
	161	f->fmt.pix.width, f->fmt.pix.height,
	162	f->fmt.pix.pixelformat, f->fmt.pix.colorspace,
	163	f->fmt.pix.quantization, f->fmt.pix.xfer_func,
	164	f->fmt.pix.ycbcr_enc);
	165
	166	vcap->format = f->fmt.pix;
	167
	168	return 0;
	169	}
	170
f2fe8906 HK	171	static int vimc_cap_enum_fmt_vid_cap(struct file file, void priv,
	172	struct v4l2_fmtdesc *f)
	173	{
b6c61a6c	174	if (f->index >= ARRAY_SIZE(vimc_cap_supported_pixfmt))
f2fe8906 HK	175	return -EINVAL;
f2fe8906 HK	176
b6c61a6c	177	f->pixelformat = vimc_cap_supported_pixfmt[f->index];
535d296f HF	178
	179	return 0;
	180	}
	181
b6c61a6c HF	182	static bool vimc_cap_is_pixfmt_supported(u32 pixelformat)
	183	{
	184	unsigned int i;
	185
	186	for (i = 0; i < ARRAY_SIZE(vimc_cap_supported_pixfmt); i++)
	187	if (vimc_cap_supported_pixfmt[i] == pixelformat)
	188	return true;
	189	return false;
	190	}
	191
535d296f HF	192	static int vimc_cap_enum_framesizes(struct file file, void fh,
	193	struct v4l2_frmsizeenum *fsize)
	194	{
535d296f HF	195	if (fsize->index)
	196	return -EINVAL;
	197
b6c61a6c	198	if (!vimc_cap_is_pixfmt_supported(fsize->pixel_format))
535d296f HF	199	return -EINVAL;
	200
	201	fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
	202	fsize->stepwise.min_width = VIMC_FRAME_MIN_WIDTH;
	203	fsize->stepwise.max_width = VIMC_FRAME_MAX_WIDTH;
	204	fsize->stepwise.min_height = VIMC_FRAME_MIN_HEIGHT;
	205	fsize->stepwise.max_height = VIMC_FRAME_MAX_HEIGHT;
5efbc65f HF	206	fsize->stepwise.step_width = 1;
5efbc65f HF	207	fsize->stepwise.step_height = 1;
f2fe8906 HK	208
	209	return 0;
	210	}
	211
	212	static const struct v4l2_file_operations vimc_cap_fops = {
	213	.owner = THIS_MODULE,
	214	.open = v4l2_fh_open,
	215	.release = vb2_fop_release,
	216	.read = vb2_fop_read,
	217	.poll = vb2_fop_poll,
	218	.unlocked_ioctl = video_ioctl2,
	219	.mmap = vb2_fop_mmap,
	220	};
	221
	222	static const struct v4l2_ioctl_ops vimc_cap_ioctl_ops = {
	223	.vidioc_querycap = vimc_cap_querycap,
	224
535d296f HF	225	.vidioc_g_fmt_vid_cap = vimc_cap_g_fmt_vid_cap,
	226	.vidioc_s_fmt_vid_cap = vimc_cap_s_fmt_vid_cap,
	227	.vidioc_try_fmt_vid_cap = vimc_cap_try_fmt_vid_cap,
f2fe8906	228	.vidioc_enum_fmt_vid_cap = vimc_cap_enum_fmt_vid_cap,
535d296f	229	.vidioc_enum_framesizes = vimc_cap_enum_framesizes,
f2fe8906 HK	230
	231	.vidioc_reqbufs = vb2_ioctl_reqbufs,
	232	.vidioc_create_bufs = vb2_ioctl_create_bufs,
	233	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
	234	.vidioc_querybuf = vb2_ioctl_querybuf,
	235	.vidioc_qbuf = vb2_ioctl_qbuf,
	236	.vidioc_dqbuf = vb2_ioctl_dqbuf,
	237	.vidioc_expbuf = vb2_ioctl_expbuf,
	238	.vidioc_streamon = vb2_ioctl_streamon,
	239	.vidioc_streamoff = vb2_ioctl_streamoff,
	240	};
	241
	242	static void vimc_cap_return_all_buffers(struct vimc_cap_device *vcap,
	243	enum vb2_buffer_state state)
	244	{
	245	struct vimc_cap_buffer vbuf, node;
	246
	247	spin_lock(&vcap->qlock);
	248
	249	list_for_each_entry_safe(vbuf, node, &vcap->buf_list, list) {
	250	list_del(&vbuf->list);
	251	vb2_buffer_done(&vbuf->vb2.vb2_buf, state);
	252	}
	253
	254	spin_unlock(&vcap->qlock);
	255	}
	256
f2fe8906 HK	257	static int vimc_cap_start_streaming(struct vb2_queue *vq, unsigned int count)
	258	{
	259	struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
	260	struct media_entity *entity = &vcap->vdev.entity;
	261	int ret;
	262
	263	vcap->sequence = 0;
	264
	265	/* Start the media pipeline */
adc589d2	266	ret = media_pipeline_start(entity, &vcap->stream.pipe);
f2fe8906 HK	267	if (ret) {
	268	vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_QUEUED);
	269	return ret;
	270	}
	271
b6c61a6c	272	vcap->stream.producer_pixfmt = vcap->format.pixelformat;
adc589d2	273	ret = vimc_streamer_s_stream(&vcap->stream, &vcap->ved, 1);
f2fe8906 HK	274	if (ret) {
	275	media_pipeline_stop(entity);
	276	vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_QUEUED);
	277	return ret;
	278	}
	279
	280	return 0;
	281	}
	282
	283	/*
	284	* Stop the stream engine. Any remaining buffers in the stream queue are
	285	* dequeued and passed on to the vb2 framework marked as STATE_ERROR.
	286	*/
	287	static void vimc_cap_stop_streaming(struct vb2_queue *vq)
	288	{
	289	struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
	290
adc589d2	291	vimc_streamer_s_stream(&vcap->stream, &vcap->ved, 0);
f2fe8906 HK	292
	293	/* Stop the media pipeline */
	294	media_pipeline_stop(&vcap->vdev.entity);
	295
	296	/* Release all active buffers */
	297	vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_ERROR);
	298	}
	299
	300	static void vimc_cap_buf_queue(struct vb2_buffer *vb2_buf)
	301	{
	302	struct vimc_cap_device *vcap = vb2_get_drv_priv(vb2_buf->vb2_queue);
	303	struct vimc_cap_buffer *buf = container_of(vb2_buf,
	304	struct vimc_cap_buffer,
	305	vb2.vb2_buf);
	306
	307	spin_lock(&vcap->qlock);
	308	list_add_tail(&buf->list, &vcap->buf_list);
	309	spin_unlock(&vcap->qlock);
	310	}
	311
	312	static int vimc_cap_queue_setup(struct vb2_queue vq, unsigned int nbuffers,
	313	unsigned int *nplanes, unsigned int sizes[],
	314	struct device *alloc_devs[])
	315	{
	316	struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
	317
	318	if (*nplanes)
	319	return sizes[0] < vcap->format.sizeimage ? -EINVAL : 0;
	320	/* We don't support multiplanes for now */
	321	*nplanes = 1;
	322	sizes[0] = vcap->format.sizeimage;
	323
	324	return 0;
	325	}
	326
	327	static int vimc_cap_buffer_prepare(struct vb2_buffer *vb)
	328	{
	329	struct vimc_cap_device *vcap = vb2_get_drv_priv(vb->vb2_queue);
	330	unsigned long size = vcap->format.sizeimage;
	331
	332	if (vb2_plane_size(vb, 0) < size) {
4a29b709	333	dev_err(vcap->dev, "%s: buffer too small (%lu < %lu)\n",
f2fe8906 HK	334	vcap->vdev.name, vb2_plane_size(vb, 0), size);
	335	return -EINVAL;
	336	}
	337	return 0;
	338	}
	339
	340	static const struct vb2_ops vimc_cap_qops = {
	341	.start_streaming = vimc_cap_start_streaming,
	342	.stop_streaming = vimc_cap_stop_streaming,
	343	.buf_queue = vimc_cap_buf_queue,
	344	.queue_setup = vimc_cap_queue_setup,
	345	.buf_prepare = vimc_cap_buffer_prepare,
	346	/*
	347	* Since q->lock is set we can use the standard
	348	* vb2_ops_wait_prepare/finish helper functions.
	349	*/
	350	.wait_prepare = vb2_ops_wait_prepare,
	351	.wait_finish = vb2_ops_wait_finish,
	352	};
	353
f2fe8906	354	static const struct media_entity_operations vimc_cap_mops = {
288a22d4	355	.link_validate = vimc_link_validate,
f2fe8906 HK	356	};
f2fe8906 HK	357
3650a23e HV	358	static void vimc_cap_release(struct video_device *vdev)
	359	{
	360	struct vimc_cap_device *vcap =
	361	container_of(vdev, struct vimc_cap_device, vdev);
	362
	363	vimc_pads_cleanup(vcap->ved.pads);
	364	kfree(vcap);
	365	}
	366
4a29b709 HF	367	static void vimc_cap_comp_unbind(struct device comp, struct device master,
4a29b709 HF	368	void *master_data)
f2fe8906	369	{
4a29b709	370	struct vimc_ent_device *ved = dev_get_drvdata(comp);
f2fe8906 HK	371	struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
	372	ved);
	373
	374	vb2_queue_release(&vcap->queue);
	375	media_entity_cleanup(ved->ent);
	376	video_unregister_device(&vcap->vdev);
f2fe8906 HK	377	}
f2fe8906 HK	378
adc589d2 LM	379	static void vimc_cap_process_frame(struct vimc_ent_device ved,
adc589d2 LM	380	const void *frame)
f2fe8906 HK	381	{
	382	struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
	383	ved);
	384	struct vimc_cap_buffer *vimc_buf;
	385	void *vbuf;
	386
	387	spin_lock(&vcap->qlock);
	388
	389	/* Get the first entry of the list */
	390	vimc_buf = list_first_entry_or_null(&vcap->buf_list,
	391	typeof(*vimc_buf), list);
	392	if (!vimc_buf) {
	393	spin_unlock(&vcap->qlock);
adc589d2	394	return ERR_PTR(-EAGAIN);
f2fe8906 HK	395	}
	396
	397	/* Remove this entry from the list */
	398	list_del(&vimc_buf->list);
	399
	400	spin_unlock(&vcap->qlock);
	401
	402	/* Fill the buffer */
	403	vimc_buf->vb2.vb2_buf.timestamp = ktime_get_ns();
	404	vimc_buf->vb2.sequence = vcap->sequence++;
	405	vimc_buf->vb2.field = vcap->format.field;
	406
	407	vbuf = vb2_plane_vaddr(&vimc_buf->vb2.vb2_buf, 0);
	408
	409	memcpy(vbuf, frame, vcap->format.sizeimage);
	410
	411	/* Set it as ready */
	412	vb2_set_plane_payload(&vimc_buf->vb2.vb2_buf, 0,
	413	vcap->format.sizeimage);
	414	vb2_buffer_done(&vimc_buf->vb2.vb2_buf, VB2_BUF_STATE_DONE);
adc589d2	415	return NULL;
f2fe8906 HK	416	}
f2fe8906 HK	417
4a29b709 HF	418	static int vimc_cap_comp_bind(struct device comp, struct device master,
4a29b709 HF	419	void *master_data)
f2fe8906	420	{
4a29b709 HF	421	struct v4l2_device *v4l2_dev = master_data;
4a29b709 HF	422	struct vimc_platform_data *pdata = comp->platform_data;
f2fe8906 HK	423	struct vimc_cap_device *vcap;
	424	struct video_device *vdev;
	425	struct vb2_queue *q;
	426	int ret;
	427
f2fe8906 HK	428	/* Allocate the vimc_cap_device struct */
	429	vcap = kzalloc(sizeof(*vcap), GFP_KERNEL);
	430	if (!vcap)
4a29b709	431	return -ENOMEM;
f2fe8906 HK	432
f2fe8906 HK	433	/* Allocate the pads */
4a29b709 HF	434	vcap->ved.pads =
4a29b709 HF	435	vimc_pads_init(1, (const unsigned long[1]) {MEDIA_PAD_FL_SINK});
f2fe8906 HK	436	if (IS_ERR(vcap->ved.pads)) {
	437	ret = PTR_ERR(vcap->ved.pads);
	438	goto err_free_vcap;
	439	}
	440
	441	/* Initialize the media entity */
4a29b709	442	vcap->vdev.entity.name = pdata->entity_name;
f2fe8906 HK	443	vcap->vdev.entity.function = MEDIA_ENT_F_IO_V4L;
f2fe8906 HK	444	ret = media_entity_pads_init(&vcap->vdev.entity,
4a29b709	445	1, vcap->ved.pads);
f2fe8906 HK	446	if (ret)
	447	goto err_clean_pads;
	448
	449	/* Initialize the lock */
	450	mutex_init(&vcap->lock);
	451
	452	/* Initialize the vb2 queue */
	453	q = &vcap->queue;
	454	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
09714569	455	q->io_modes = VB2_MMAP \| VB2_DMABUF \| VB2_USERPTR;
f2fe8906 HK	456	q->drv_priv = vcap;
	457	q->buf_struct_size = sizeof(struct vimc_cap_buffer);
	458	q->ops = &vimc_cap_qops;
	459	q->mem_ops = &vb2_vmalloc_memops;
	460	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
	461	q->min_buffers_needed = 2;
	462	q->lock = &vcap->lock;
	463
	464	ret = vb2_queue_init(q);
	465	if (ret) {
4a29b709 HF	466	dev_err(comp, "%s: vb2 queue init failed (err=%d)\n",
4a29b709 HF	467	pdata->entity_name, ret);
f2fe8906 HK	468	goto err_clean_m_ent;
	469	}
	470
	471	/* Initialize buffer list and its lock */
	472	INIT_LIST_HEAD(&vcap->buf_list);
	473	spin_lock_init(&vcap->qlock);
	474
535d296f HF	475	/* Set default frame format */
535d296f HF	476	vcap->format = fmt_default;
b6c61a6c HF	477	v4l2_fill_pixfmt(&vcap->format, vcap->format.pixelformat,
b6c61a6c HF	478	vcap->format.width, vcap->format.height);
f2fe8906 HK	479
f2fe8906 HK	480	/* Fill the vimc_ent_device struct */
f2fe8906 HK	481	vcap->ved.ent = &vcap->vdev.entity;
f2fe8906 HK	482	vcap->ved.process_frame = vimc_cap_process_frame;
288a22d4	483	vcap->ved.vdev_get_format = vimc_cap_get_format;
4a29b709 HF	484	dev_set_drvdata(comp, &vcap->ved);
4a29b709 HF	485	vcap->dev = comp;
f2fe8906 HK	486
	487	/* Initialize the video_device struct */
	488	vdev = &vcap->vdev;
	489	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE \| V4L2_CAP_STREAMING;
	490	vdev->entity.ops = &vimc_cap_mops;
3650a23e	491	vdev->release = vimc_cap_release;
f2fe8906 HK	492	vdev->fops = &vimc_cap_fops;
	493	vdev->ioctl_ops = &vimc_cap_ioctl_ops;
	494	vdev->lock = &vcap->lock;
	495	vdev->queue = q;
	496	vdev->v4l2_dev = v4l2_dev;
	497	vdev->vfl_dir = VFL_DIR_RX;
c0decac1	498	strscpy(vdev->name, pdata->entity_name, sizeof(vdev->name));
f2fe8906 HK	499	video_set_drvdata(vdev, &vcap->ved);
	500
	501	/* Register the video_device with the v4l2 and the media framework */
	502	ret = video_register_device(vdev, VFL_TYPE_GRABBER, -1);
	503	if (ret) {
4a29b709	504	dev_err(comp, "%s: video register failed (err=%d)\n",
f2fe8906 HK	505	vcap->vdev.name, ret);
	506	goto err_release_queue;
	507	}
	508
4a29b709	509	return 0;
f2fe8906 HK	510
	511	err_release_queue:
	512	vb2_queue_release(q);
	513	err_clean_m_ent:
	514	media_entity_cleanup(&vcap->vdev.entity);
	515	err_clean_pads:
	516	vimc_pads_cleanup(vcap->ved.pads);
	517	err_free_vcap:
	518	kfree(vcap);
	519
4a29b709	520	return ret;
f2fe8906	521	}
4a29b709 HF	522
	523	static const struct component_ops vimc_cap_comp_ops = {
	524	.bind = vimc_cap_comp_bind,
	525	.unbind = vimc_cap_comp_unbind,
	526	};
	527
	528	static int vimc_cap_probe(struct platform_device *pdev)
	529	{
	530	return component_add(&pdev->dev, &vimc_cap_comp_ops);
	531	}
	532
	533	static int vimc_cap_remove(struct platform_device *pdev)
	534	{
	535	component_del(&pdev->dev, &vimc_cap_comp_ops);
	536
	537	return 0;
	538	}
	539
bf183e0f JMC	540	static const struct platform_device_id vimc_cap_driver_ids[] = {
	541	{
	542	.name = VIMC_CAP_DRV_NAME,
	543	},
	544	{ }
	545	};
	546
4a29b709 HF	547	static struct platform_driver vimc_cap_pdrv = {
	548	.probe = vimc_cap_probe,
	549	.remove = vimc_cap_remove,
bf183e0f	550	.id_table = vimc_cap_driver_ids,
4a29b709 HF	551	.driver = {
	552	.name = VIMC_CAP_DRV_NAME,
	553	},
	554	};
	555
4a29b709 HF	556	module_platform_driver(vimc_cap_pdrv);
	557
	558	MODULE_DEVICE_TABLE(platform, vimc_cap_driver_ids);
	559
	560	MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC) Capture");
	561	MODULE_AUTHOR("Helen Mae Koike Fornazier <helen.fornazier@gmail.com>");
	562	MODULE_LICENSE("GPL");