// SPDX-License-Identifier: (GPL-2.0-only OR MIT)
/*
 * Copyright (C) 2025 Amlogic, Inc. All rights reserved
 */

#include <linux/cleanup.h>
#include <linux/pm_runtime.h>

#include <media/v4l2-ctrls.h>
#include <media/v4l2-event.h>
#include <media/v4l2-ioctl.h>
#include <media/v4l2-mc.h>
#include <media/videobuf2-dma-contig.h>

#include "c4-isp-common.h"
#include "hw/c4-isp-hw.h"
#include "hw/c4-isp-regs.h"

static const struct c4_isp_cap_format_info cap_formats[] = {
	/* YUV formats */
	{
		.code = MEDIA_BUS_FMT_YUV10_1X30,
		.fourcc = V4L2_PIX_FMT_NV12M,
		.plane0_bits = IDWMIF_CFG_0_BITS_MODE_8BITS,
		.plane1_bits = IDWMIF_CFG_0_BITS_MODE_16BITS,
		.plane0_pack = IDWMIF_CFG_0_PACK_MODE_8X,
		.plane1_pack = IDWMIF_CFG_0_PACK_MODE_4X,
	}, {
		.code = MEDIA_BUS_FMT_YUV10_1X30,
		.fourcc = V4L2_PIX_FMT_NV21M,
		.plane0_bits = IDWMIF_CFG_0_BITS_MODE_8BITS,
		.plane1_bits = IDWMIF_CFG_0_BITS_MODE_16BITS,
		.plane0_pack = IDWMIF_CFG_0_PACK_MODE_8X,
		.plane1_pack = IDWMIF_CFG_0_PACK_MODE_4X,
	}, {
		.code = MEDIA_BUS_FMT_YUV10_1X30,
		.fourcc = V4L2_PIX_FMT_NV12,
		.plane0_bits = IDWMIF_CFG_0_BITS_MODE_8BITS,
		.plane1_bits = IDWMIF_CFG_0_BITS_MODE_16BITS,
		.plane0_pack = IDWMIF_CFG_0_PACK_MODE_8X,
		.plane1_pack = IDWMIF_CFG_0_PACK_MODE_4X,
	}, {
		.code = MEDIA_BUS_FMT_YUV10_1X30,
		.fourcc = V4L2_PIX_FMT_NV21,
		.plane0_bits = IDWMIF_CFG_0_BITS_MODE_8BITS,
		.plane1_bits = IDWMIF_CFG_0_BITS_MODE_16BITS,
		.plane0_pack = IDWMIF_CFG_0_PACK_MODE_8X,
		.plane1_pack = IDWMIF_CFG_0_PACK_MODE_4X,
	},
};

static int c4_isp_cap_dummy_buff_create(struct c4_isp_capture *cap)
{
	struct c4_isp_data_buffer *dummy_buff = &cap->dummy_buff;
	struct v4l2_pix_format_mplane *pix_mp = &cap->format.pix_mp;

	if (pix_mp->num_planes == 1)
		dummy_buff->size = pix_mp->plane_fmt[C4_ISP_PLANE_Y].sizeimage;
	else
		dummy_buff->size =
			MAX(pix_mp->plane_fmt[C4_ISP_PLANE_Y].sizeimage,
			    pix_mp->plane_fmt[C4_ISP_PLANE_UV].sizeimage);

	/* The driver never access vaddr, no mapping is required */
	dummy_buff->vaddr = dma_alloc_attrs(cap->isp->dev, dummy_buff->size,
					    &dummy_buff->dma_addr, GFP_KERNEL,
					    DMA_ATTR_NO_KERNEL_MAPPING);
	if (!dummy_buff->vaddr)
		return -ENOMEM;

	return 0;
}

static void c4_isp_cap_dummy_buff_destroy(struct c4_isp_capture *cap)
{
	dma_free_attrs(cap->isp->dev, cap->dummy_buff.size,
		       cap->dummy_buff.vaddr, cap->dummy_buff.dma_addr,
		       DMA_ATTR_NO_KERNEL_MAPPING);
}

static void c4_isp_cap_cfg_buff(struct c4_isp_capture *cap)
{
	cap->buff = list_first_entry_or_null(&cap->pending,
					     struct c4_isp_cap_buffer, list);

	c4_isp_scwmif_cfg_buff(cap);

	if (cap->buff)
		list_del(&cap->buff->list);
}

static void c4_isp_cap_start(struct c4_isp_capture *cap)
{
	struct v4l2_pix_format_mplane *pix_mp = &cap->format.pix_mp;
	u32 buf_vsize;

	scoped_guard(spinlock_irqsave, &cap->buff_lock) {
		c4_isp_cap_cfg_buff(cap);
	}

	c4_isp_scwmif_cfg_format(cap);

	c4_isp_hw_link_venc(cap->isp, pix_mp->height, &buf_vsize);

	if (cap->id == C4_ISP_CAP_DEV_0)
		c4_isp_scwmif_cfg_loop(cap, buf_vsize);

	c4_isp_scwmif_enable(cap, false);
}

static void c4_isp_cap_stop(struct c4_isp_capture *cap)
{
	c4_isp_scwmif_disable(cap, false);
}

static void c4_isp_cap_cfg_drop_frame(struct c4_isp_capture *cap)
{
	if (cap->id == C4_ISP_CAP_DEV_0)
		return;
	if (cap->fps_output == 0) {
		c4_isp_disp_disable(cap->rsz, true);
		c4_isp_scwmif_disable(cap, true);
	} else if (cap->fps_output < cap->isp->core.cfg_info.out_fps) {
		// video output fps < isp output fps,enable drop frame.
		u32 pass_num = 1;
		u32 drop_num = cap->isp->core.cfg_info.out_fps /
						cap->fps_output - 1;

		if (cap->drop_frame_num < drop_num) {
			// drop next frame.
			cap->drop_frame_num++;
			c4_isp_disp_disable(cap->rsz, true);
			c4_isp_scwmif_disable(cap, true);
		} else if (cap->pass_frame_num < pass_num) {
			// pass next frame.
			cap->pass_frame_num++;
			c4_isp_disp_enable(cap->rsz, true);
			c4_isp_scwmif_enable(cap, true);
		}

		// one round over, reset drop frame num.
		if (cap->drop_frame_num >= drop_num && cap->pass_frame_num >= pass_num) {
			cap->drop_frame_num = 0;
			cap->pass_frame_num = 0;
		}
	} else {
		c4_isp_disp_enable(cap->rsz, true);
		c4_isp_scwmif_enable(cap, true);
	}
}

static void c4_isp_cap_done(struct c4_isp_capture *cap)
{
	struct c4_isp_cap_buffer *buff = cap->buff;
	bool swap_buff = false;

	// if wrmif is enabled, buffer is written, swap new buffer to hw.
	if (c4_isp_scwmif_hw_enable_bit(cap))
		swap_buff = true;

	c4_isp_cap_cfg_drop_frame(cap);

	if (!swap_buff)
		return;

	guard(spinlock_irqsave)(&cap->buff_lock);

	if (buff)
	{
		buff->vb.sequence = cap->isp->be_frm_seq;
		buff->vb.vb2_buf.timestamp = ktime_get();
		buff->vb.field = V4L2_FIELD_NONE;
		vb2_buffer_done(&buff->vb.vb2_buf, VB2_BUF_STATE_DONE);
	}

	c4_isp_cap_cfg_buff(cap);
}

/* V4L2 video operations */

static const struct c4_isp_cap_format_info *c4_cap_find_fmt(u32 fourcc)
{
	for (unsigned int i = 0; i < ARRAY_SIZE(cap_formats); i++) {
		if (cap_formats[i].fourcc == fourcc)
			return &cap_formats[i];
	}

	return NULL;
}

static void c4_cap_try_fmt(struct v4l2_pix_format_mplane *pix_mp)
{
	const struct c4_isp_cap_format_info *fmt;
	const struct v4l2_format_info *info;
	struct v4l2_plane_pix_format *plane;

	fmt = c4_cap_find_fmt(pix_mp->pixelformat);
	if (!fmt)
		fmt = &cap_formats[0];

	pix_mp->width = clamp(pix_mp->width, C4_ISP_MIN_WIDTH,
			      C4_ISP_MAX_WIDTH);
	pix_mp->height = clamp(pix_mp->height, C4_ISP_MIN_HEIGHT,
			       C4_ISP_MAX_HEIGHT);
	pix_mp->pixelformat = fmt->fourcc;
	pix_mp->field = V4L2_FIELD_NONE;
	pix_mp->colorspace = V4L2_COLORSPACE_SRGB;
	pix_mp->ycbcr_enc = V4L2_YCBCR_ENC_601;
	pix_mp->quantization = V4L2_QUANTIZATION_LIM_RANGE;

	info = v4l2_format_info(fmt->fourcc);
	pix_mp->num_planes = info->mem_planes;
	memset(pix_mp->plane_fmt, 0, sizeof(pix_mp->plane_fmt));

	for (unsigned int i = 0; i < info->comp_planes; i++) {
		unsigned int hdiv = (i == 0) ? 1 : info->hdiv;
		unsigned int vdiv = (i == 0) ? 1 : info->vdiv;

		plane = &pix_mp->plane_fmt[i];

		plane->bytesperline = DIV_ROUND_UP(pix_mp->width, hdiv) *
				      info->bpp[i] / info->bpp_div[i];
		plane->bytesperline = ALIGN(plane->bytesperline,
					    C4_ISP_WMIF_STRIDE_ALIGN_BYTES);
		plane->sizeimage = plane->bytesperline *
				   DIV_ROUND_UP(pix_mp->height, vdiv);
	}
	if (pix_mp->num_planes == 1) {
		if (pix_mp->pixelformat == V4L2_PIX_FMT_NV12 ||
		    pix_mp->pixelformat == V4L2_PIX_FMT_NV21) {
			plane = &pix_mp->plane_fmt[0];

			plane->bytesperline = ALIGN(pix_mp->width,
							C4_ISP_WMIF_STRIDE_ALIGN_BYTES);
			plane->sizeimage = plane->bytesperline * pix_mp->height * 3 / 2;
		}
	}
}

static void c4_isp_cap_return_buffers(struct c4_isp_capture *cap,
				      enum vb2_buffer_state state)
{
	struct c4_isp_cap_buffer *buff;

	guard(spinlock_irqsave)(&cap->buff_lock);

	if (cap->buff) {
		vb2_buffer_done(&cap->buff->vb.vb2_buf, state);
		cap->buff = NULL;
	}

	while (!list_empty(&cap->pending)) {
		buff = list_first_entry(&cap->pending,
					struct c4_isp_cap_buffer, list);
		list_del(&buff->list);
		vb2_buffer_done(&buff->vb.vb2_buf, state);
	}
}

static int c4_isp_cap_querycap(struct file *file, void *fh,
			       struct v4l2_capability *cap)
{
	strscpy(cap->driver, C4_ISP_DRIVER_NAME, sizeof(cap->driver));
	strscpy(cap->card, "AML C4 ISP5", sizeof(cap->card));

	return 0;
}

static int c4_isp_cap_enum_fmt(struct file *file, void *fh,
			       struct v4l2_fmtdesc *f)
{
	const struct c4_isp_cap_format_info *fmt;
	unsigned int index = 0;
	unsigned int i;

	if (!f->mbus_code) {
		if (f->index >= ARRAY_SIZE(cap_formats))
			return -EINVAL;

		fmt = &cap_formats[f->index];
		f->pixelformat = fmt->fourcc;
		return 0;
	}

	for (i = 0; i < ARRAY_SIZE(cap_formats); i++) {
		fmt = &cap_formats[i];
		if (f->mbus_code != fmt->code)
			continue;

		if (index++ == f->index) {
			f->pixelformat = cap_formats[i].fourcc;
			return 0;
		}
	}

	return -EINVAL;
}

static int c4_isp_cap_g_fmt_mplane(struct file *file, void *fh,
				   struct v4l2_format *f)
{
	struct c4_isp_capture *cap = video_drvdata(file);

	f->fmt.pix_mp = cap->format.pix_mp;

	return 0;
}

static int c4_isp_cap_s_fmt_mplane(struct file *file, void *fh,
				   struct v4l2_format *f)
{
	struct c4_isp_capture *cap = video_drvdata(file);

	c4_cap_try_fmt(&f->fmt.pix_mp);

	cap->format.pix_mp = f->fmt.pix_mp;
	cap->format.info = c4_cap_find_fmt(f->fmt.pix_mp.pixelformat);

	return 0;
}

static int c4_isp_cap_try_fmt_mplane(struct file *file, void *fh,
				struct v4l2_format *f)
{
	c4_cap_try_fmt(&f->fmt.pix_mp);

	return 0;
}

static int c4_isp_cap_enum_frmsize(struct file *file, void *fh,
				struct v4l2_frmsizeenum *fsize)
{
	const struct c4_isp_cap_format_info *fmt;

	if (fsize->index)
		return -EINVAL;

	fmt = c4_cap_find_fmt(fsize->pixel_format);
	if (!fmt)
		return -EINVAL;

	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
	fsize->stepwise.min_width = C4_ISP_MIN_WIDTH;
	fsize->stepwise.min_height = C4_ISP_MIN_HEIGHT;
	fsize->stepwise.max_width = C4_ISP_MAX_WIDTH;
	fsize->stepwise.max_height = C4_ISP_MAX_HEIGHT;
	fsize->stepwise.step_width = 2;
	fsize->stepwise.step_height = 2;

	return 0;
}

static int c4_isp_cap_subs_evt(struct v4l2_fh *fh,
			       const struct v4l2_event_subscription *sub)
{
	struct c4_isp_capture *cap = video_get_drvdata(fh->vdev);

	if (sub->id != 0)
		return -EINVAL;

	switch (sub->type) {
	case AML_ISP_V4L2_EVENT_STREAM_START:
	case AML_ISP_V4L2_EVENT_STREAM_STOP:
		cap->is_subs_evt = true;
		return v4l2_event_subscribe(fh, sub, 0, NULL);
	default:
		return -EINVAL;
	}
}

static int c4_isp_cap_unsubs_evt(struct v4l2_fh *fh,
				 const struct v4l2_event_subscription *sub)
{
	struct c4_isp_capture *cap = video_get_drvdata(fh->vdev);

	cap->is_subs_evt = false;
	return v4l2_event_unsubscribe(fh, sub);
}

static const struct v4l2_ioctl_ops isp_cap_v4l2_ioctl_ops = {
	.vidioc_querycap		= c4_isp_cap_querycap,
	.vidioc_enum_fmt_vid_cap	= c4_isp_cap_enum_fmt,
	.vidioc_g_fmt_vid_cap_mplane	= c4_isp_cap_g_fmt_mplane,
	.vidioc_s_fmt_vid_cap_mplane	= c4_isp_cap_s_fmt_mplane,
	.vidioc_try_fmt_vid_cap_mplane	= c4_isp_cap_try_fmt_mplane,
	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
	.vidioc_querybuf		= vb2_ioctl_querybuf,
	.vidioc_qbuf			= vb2_ioctl_qbuf,
	.vidioc_expbuf			= vb2_ioctl_expbuf,
	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
	.vidioc_streamon		= vb2_ioctl_streamon,
	.vidioc_streamoff		= vb2_ioctl_streamoff,
	.vidioc_enum_framesizes         = c4_isp_cap_enum_frmsize,
	.vidioc_subscribe_event		= c4_isp_cap_subs_evt,
	.vidioc_unsubscribe_event	= c4_isp_cap_unsubs_evt,
};

static const struct v4l2_file_operations isp_cap_v4l2_fops = {
	.open = v4l2_fh_open,
	.release = vb2_fop_release,
	.poll = vb2_fop_poll,
	.unlocked_ioctl = video_ioctl2,
	.mmap = vb2_fop_mmap,
};

static int c4_isp_cap_link_validate(struct media_link *link)
{
	struct video_device *vdev =
		media_entity_to_video_device(link->sink->entity);
	struct v4l2_subdev *sd =
		media_entity_to_v4l2_subdev(link->source->entity);
	struct c4_isp_capture *cap = video_get_drvdata(vdev);
	struct v4l2_subdev_format src_fmt = {
		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
		.pad = link->source->index,
	};
	int ret;

	ret = v4l2_subdev_call_state_active(sd, pad, get_fmt, &src_fmt);
	if (ret)
		return ret;

	if (src_fmt.format.width != cap->format.pix_mp.width ||
	    src_fmt.format.height != cap->format.pix_mp.height ||
	    src_fmt.format.code != cap->format.info->code) {
		dev_err(cap->isp->dev,
			"link %s: %u -> %s: %u not valid: 0x%04x/%ux%u not match 0x%04x/%ux%u\n",
			link->source->entity->name, link->source->index,
			link->sink->entity->name, link->sink->index,
			src_fmt.format.code, src_fmt.format.width,
			src_fmt.format.height, cap->format.info->code,
			cap->format.pix_mp.width, cap->format.pix_mp.height);

		return -EPIPE;
	}

	return 0;
}

static const struct media_entity_operations isp_cap_entity_ops = {
	.link_validate = c4_isp_cap_link_validate,
};

static int c4_isp_vb2_queue_setup(struct vb2_queue *q,
				  unsigned int *num_buffers,
				  unsigned int *num_planes,
				  unsigned int sizes[],
				  struct device *alloc_devs[])
{
	struct c4_isp_capture *cap = vb2_get_drv_priv(q);
	const struct v4l2_pix_format_mplane *pix_mp = &cap->format.pix_mp;
	unsigned int i;

	if (*num_planes) {
		if (*num_planes != pix_mp->num_planes)
			return -EINVAL;

		for (i = 0; i < pix_mp->num_planes; i++)
			if (sizes[i] < pix_mp->plane_fmt[i].sizeimage)
				return -EINVAL;

		return 0;
	}

	*num_planes = pix_mp->num_planes;
	for (i = 0; i < pix_mp->num_planes; i++)
		sizes[i] = pix_mp->plane_fmt[i].sizeimage;

	return 0;
}

static void c4_isp_vb2_buf_queue(struct vb2_buffer *vb)
{
	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
	struct c4_isp_cap_buffer *buf =
			container_of(v4l2_buf, struct c4_isp_cap_buffer, vb);
	struct c4_isp_capture *cap = vb2_get_drv_priv(vb->vb2_queue);

	guard(spinlock_irqsave)(&cap->buff_lock);

	list_add_tail(&buf->list, &cap->pending);
}

static int c4_isp_vb2_buf_prepare(struct vb2_buffer *vb)
{
	struct c4_isp_capture *cap = vb2_get_drv_priv(vb->vb2_queue);
	unsigned long size;

	for (unsigned int i = 0; i < cap->format.pix_mp.num_planes; i++) {
		size = cap->format.pix_mp.plane_fmt[i].sizeimage;
		if (vb2_plane_size(vb, i) < size) {
			dev_err(cap->isp->dev,
				"User buffer too small (%ld < %lu)\n",
				vb2_plane_size(vb, i), size);
			return -EINVAL;
		}

		vb2_set_plane_payload(vb, i, size);
	}

	return 0;
}

static int c4_isp_cal_isp_plane_num(struct c4_isp_capture *cap)
{
	return 2; // fixed 2 for isp5 hw.
}

static dma_addr_t c4_isp_cal_plane_dma_addr(struct c4_isp_capture *cap, struct vb2_buffer *vb, int plane)
{
	if (plane == C4_ISP_PLANE_UV) {
		if (cap->format.info->fourcc == V4L2_PIX_FMT_NV12 ||
		    cap->format.info->fourcc == V4L2_PIX_FMT_NV21) {
			dma_addr_t y_addr = vb2_dma_contig_plane_dma_addr(vb, 0);
			return y_addr + cap->format.pix_mp.plane_fmt[0].bytesperline * cap->format.pix_mp.height;
		}
	}
	// cap->format.pix_mp.num_planes is 2
	return vb2_dma_contig_plane_dma_addr(vb, plane);
}

static int c4_isp_vb2_buf_init(struct vb2_buffer *vb)
{
	struct c4_isp_capture *cap = vb2_get_drv_priv(vb->vb2_queue);
	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
	struct c4_isp_cap_buffer *buf =
		container_of(v4l2_buf, struct c4_isp_cap_buffer, vb);

	// init isp_plane_num for each plane
	buf->isp_plane_num = c4_isp_cal_isp_plane_num(cap);

	for (unsigned int i = 0; i < buf->isp_plane_num; i++)
		buf->dma_addr[i] = c4_isp_cal_plane_dma_addr(cap, vb, i);

	return 0;
}

static int c4_isp_vb2_start_streaming(struct vb2_queue *q,
				      unsigned int count)
{
	struct c4_isp_capture *cap = vb2_get_drv_priv(q);
	int ret;

	cap->drop_frame_num = 0;
	cap->pass_frame_num = 0;
	ret = video_device_pipeline_start(&cap->vdev, &cap->isp->sc_pipe);
	if (ret) {
		dev_err(cap->isp->dev,
			"Failed to start cap%u pipeline: %d\n", cap->id, ret);
		goto err_return_buffers;
	}

	ret = c4_isp_cap_dummy_buff_create(cap);
	if (ret)
		goto err_pipeline_stop;

	ret = pm_runtime_resume_and_get(cap->isp->dev);
	if (ret)
		goto err_dummy_destroy;

	c4_isp_rdma_init_config(cap->isp);
	c4_isp_cap_start(cap);

	ret = v4l2_subdev_enable_streams(&cap->rsz->sd, C4_ISP_RSZ_PAD_SOURCE,
					 BIT(0));
	if (ret)
		goto err_isp_cap_stop;

	struct v4l2_event ev = {
		.type = AML_ISP_V4L2_EVENT_STREAM_START,
	};
	if (cap->is_subs_evt) {
		dev_info(cap->isp->dev, "cap%u: queue STREAM_START v4l2 event\n",
			 cap->id);
		v4l2_event_queue(&cap->vdev, &ev);
	} else {
		dev_err(cap->isp->dev,
			 "cap%u: STREAM_START event skipped (no evt subscription)\n",
			 cap->id);
	}

	return 0;

err_isp_cap_stop:
	c4_isp_cap_stop(cap);
	c4_isp_rdma_deinit_config(cap->isp);
	pm_runtime_put(cap->isp->dev);
err_dummy_destroy:
	c4_isp_cap_dummy_buff_destroy(cap);
err_pipeline_stop:
	video_device_pipeline_stop(&cap->vdev);
err_return_buffers:
	c4_isp_cap_return_buffers(cap, VB2_BUF_STATE_QUEUED);
	return ret;
}

static void c4_isp_vb2_stop_streaming(struct vb2_queue *q)
{
	struct c4_isp_capture *cap = vb2_get_drv_priv(q);
	struct v4l2_event ev = {
		.type = AML_ISP_V4L2_EVENT_STREAM_STOP,
	};
	if (cap->is_subs_evt) {
		dev_info(cap->isp->dev, "cap%u: queue STREAM_STOP v4l2 event\n",
			 cap->id);
		v4l2_event_queue(&cap->vdev, &ev);
	} else {
		dev_info(cap->isp->dev,
			 "cap%u: STREAM_STOP event skipped (no evt subscription)\n",
			 cap->id);
	}

	c4_isp_cap_stop(cap);
	c4_isp_rdma_deinit_config(cap->isp);

	c4_isp_cap_return_buffers(cap, VB2_BUF_STATE_ERROR);

	v4l2_subdev_disable_streams(&cap->rsz->sd, C4_ISP_RSZ_PAD_SOURCE,
				    BIT(0));

	pm_runtime_put(cap->isp->dev);

	c4_isp_cap_dummy_buff_destroy(cap);

	video_device_pipeline_stop(&cap->vdev);
}

static const struct vb2_ops isp_video_vb2_ops = {
	.queue_setup = c4_isp_vb2_queue_setup,
	.buf_queue = c4_isp_vb2_buf_queue,
	.buf_prepare = c4_isp_vb2_buf_prepare,
	.buf_init = c4_isp_vb2_buf_init,
	.wait_prepare = vb2_ops_wait_prepare,
	.wait_finish = vb2_ops_wait_finish,
	.start_streaming = c4_isp_vb2_start_streaming,
	.stop_streaming = c4_isp_vb2_stop_streaming,
};

static int c4_isp_v4l2_s_std_ctrl(struct v4l2_ctrl *ctrl)
{
	int ret = 0;
	struct c4_isp_capture *cap = container_of(ctrl->handler,
						struct c4_isp_capture, ctrl_handler);

	switch (ctrl->id)
	{
	case V4L2_CID_C4_VIDEO_OUTPUT_FPS:
		cap->fps_output = ctrl->val;
		dev_info(cap->isp->dev, "ISP cap %d output fps: %d\n", cap->id, cap->fps_output);
		break;
	default:
		dev_err(cap->isp->dev, "Error ctrl->id %u, flag 0x%lx\n",
				ctrl->id, ctrl->flags);
		ret = -EINVAL;
		break;
	}

	return ret;
}

static const struct v4l2_ctrl_ops v4l2_ctrl_std_ops = {
	.s_ctrl = c4_isp_v4l2_s_std_ctrl,
};

static const struct v4l2_ctrl_config v4l2_ctrl_output_fps = {
	.ops = &v4l2_ctrl_std_ops,
	.id = V4L2_CID_C4_VIDEO_OUTPUT_FPS,
	.name = "ISP output fps",
	.type = V4L2_CTRL_TYPE_INTEGER,
	.flags = V4L2_CTRL_FLAG_EXECUTE_ON_WRITE,
	.min = 0,
	.max = 360,
	.step = 1,
	.def = 30,
};

static int c4_isp_v4l2_ctrl_init(struct c4_isp_capture *cap)
{
	int rtn = 0;
	struct v4l2_ctrl_handler *hdl = &cap->ctrl_handler;

	v4l2_ctrl_handler_init(hdl, 2);

	v4l2_ctrl_new_custom(hdl, &v4l2_ctrl_output_fps, NULL);

	cap->vdev.ctrl_handler = hdl;
	rtn = v4l2_ctrl_handler_setup(cap->vdev.ctrl_handler);
	if (rtn)
	{
		dev_err(cap->isp->dev, "Could not sync v4l2 controls\n");
		return rtn;
	}

	if (hdl->error)
	{
		dev_err(cap->isp->dev, "Control initialization error %d\n",
				hdl->error);
		rtn = hdl->error;
	}

	return rtn;
}

static int c4_isp_register_capture(struct c4_isp_capture *cap)
{
	struct video_device *vdev = &cap->vdev;
	struct vb2_queue *vb2_q = &cap->vb2_q;
	int ret;

	snprintf(vdev->name, sizeof(vdev->name), "c4-isp-cap%u", cap->id);
	vdev->fops = &isp_cap_v4l2_fops;
	vdev->ioctl_ops = &isp_cap_v4l2_ioctl_ops;
	vdev->v4l2_dev = &cap->isp->v4l2_dev;
	vdev->entity.ops = &isp_cap_entity_ops;
	vdev->lock = &cap->lock;
	vdev->minor = -1;
	vdev->queue = vb2_q;
	vdev->release = video_device_release_empty;
	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE |
			    V4L2_CAP_STREAMING | V4L2_CAP_IO_MC;
	vdev->vfl_dir = VFL_DIR_RX;
	video_set_drvdata(vdev, cap);

	vb2_q->drv_priv = cap;
	vb2_q->mem_ops = &vb2_dma_contig_memops;
	vb2_q->ops = &isp_video_vb2_ops;
	vb2_q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	vb2_q->io_modes = VB2_DMABUF | VB2_MMAP;
	vb2_q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
	vb2_q->buf_struct_size = sizeof(struct c4_isp_cap_buffer);
	vb2_q->dev = cap->isp->dev;
	vb2_q->lock = &cap->lock;

	ret = vb2_queue_init(vb2_q);
	if (ret)
		goto err_destroy;

	cap->pad.flags = MEDIA_PAD_FL_SINK;
	ret = media_entity_pads_init(&vdev->entity, 1, &cap->pad);
	if (ret)
		goto err_queue_release;

	ret = c4_isp_v4l2_ctrl_init(cap);
	if (ret)
		goto err_entity_cleanup;

	ret = video_register_device(vdev, VFL_TYPE_VIDEO, C4_V4L2_VIDEO_BEGIN_NUM);
	if (ret) {
		dev_err(cap->isp->dev,
			"Failed to register %s: %d\n", vdev->name, ret);
		goto err_ctrl_cleanup;
	}

	return 0;
err_ctrl_cleanup:
	v4l2_ctrl_handler_free(&cap->ctrl_handler);
err_entity_cleanup:
	media_entity_cleanup(&vdev->entity);
err_queue_release:
	vb2_queue_release(vb2_q);
err_destroy:
	mutex_destroy(&cap->lock);
	return ret;
}

int c4_isp_captures_register(struct c4_isp_device *isp)
{
	int ret;
	unsigned int i;
	struct c4_isp_capture *cap;

	for (i = C4_ISP_CAP_DEV_0; i < C4_ISP_NUM_CAP_DEVS; i++) {
		cap = &isp->caps[i];
		memset(cap, 0, sizeof(*cap));

		cap->format.pix_mp.width = C4_ISP_DEFAULT_WIDTH;
		cap->format.pix_mp.height = C4_ISP_DEFAULT_HEIGHT;
		cap->format.pix_mp.field = V4L2_FIELD_NONE;
		cap->format.pix_mp.pixelformat = V4L2_PIX_FMT_NV12M;
		cap->format.pix_mp.colorspace = V4L2_COLORSPACE_SRGB;
		cap->format.info =
			c4_cap_find_fmt(cap->format.pix_mp.pixelformat);

		c4_cap_try_fmt(&cap->format.pix_mp);

		cap->id = i;
		cap->rsz = &isp->resizers[i];
		cap->isp = isp;
		INIT_LIST_HEAD(&cap->pending);
		spin_lock_init(&cap->buff_lock);
		mutex_init(&cap->lock);

		ret = c4_isp_register_capture(cap);
		if (ret) {
			cap->isp = NULL;
			mutex_destroy(&cap->lock);
			c4_isp_captures_unregister(isp);
			return ret;
		}
	}

	return 0;
}

void c4_isp_captures_unregister(struct c4_isp_device *isp)
{
	unsigned int i;
	struct c4_isp_capture *cap;

	for (i = C4_ISP_CAP_DEV_0; i < C4_ISP_NUM_CAP_DEVS; i++) {
		cap = &isp->caps[i];

		if (!cap->isp)
			continue;
		vb2_queue_release(&cap->vb2_q);
		media_entity_cleanup(&cap->vdev.entity);
		video_unregister_device(&cap->vdev);
		mutex_destroy(&cap->lock);
	}
}

void c4_isp_captures_isr(struct c4_isp_device *isp, enum c4_isp_cap_devs id)
{
	struct c4_isp_capture *cap = &isp->caps[id];

	if (!cap->vb2_q.streaming)
		return;

	c4_isp_cap_done(cap);
}