/*
   forward-v4l2-io.c - v4l2 driver for SoftLab-NSK Forward video boards

   Copyright (C) 2017 - 2024 SoftLab-NSK <forward@softlab.tv>

   This program is free software; you can redistribute it and/or
   modify it under the terms of the GNU General Public License
   as published by the Free Software Foundation; either version 2
   of the License, or (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

#include "forward-v4l2-io.h"
#include "forward-v4l2-ctrl.h"
#include "forward-v4l2-asi.h"
#include "forward-v4l2-utils.h"
#include "forward-v4l2-genlock.h"
#include "forward-vdma.h"

#include <media/v4l2-ioctl.h>
#include <media/v4l2-event.h>
#include <media/videobuf2-vmalloc.h>
#include <linux/sched.h>

#include "../forward-alsa/forward-alsa-io.h"
#include "forward-hanc.h"

extern bool default_mute;

static void forward_v4l2_io_queue_hw(struct forward_v4l2_io *io, int buffer,
				     struct forward_v4l2_buffer *video,
				     struct forward_v4l2_buffer *vbi)
{
	struct forward_v4l2_dev_ops *ops = io->v4l2->ops;
	struct forward_dev *dev = io->v4l2->dev;
	struct vb2_buffer *vb2_buffer;
	void *vaddress;
	u32 address;
	size_t size;
	bool odd = !(buffer & 0x1);
	struct forward_v4l2_io_buffers *bufs;

	if (buffer < 0 || buffer >= ops->hw_buffers)
		return;

	bufs = &io->mapped_buffers[buffer];
	bufs->vbi = vbi;
	bufs->video = video;
	bufs->mapped = true;

	if (!video)
		return;

#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0)
	vb2_buffer = &video->vb.vb2_buf;
#else
	vb2_buffer = &video->vb;
#endif

	ops->buffer_info(io, buffer, &address, &size);

	vaddress = vb2_plane_vaddr(vb2_buffer, 0) + io->buffer_map_offset;
	size = io->buffer_map_size;

	if (!io->interleave)
		forward_vdma_map_kernel_buf(dev->vdma, io, vaddress, address, size,
					    (io->state == FORWARD_IO_RX));
	else
		forward_vdma_map_kernel_buf(dev->vdma, io, vaddress + (odd ? 0 : size), address,
					    size, (io->state == FORWARD_IO_RX));
}

static void forward_v4l2_io_dequeue_hw(struct forward_v4l2_io *io, int buffer, int video_size,
				       int vbi_size, struct forward_v4l2_buffer **video,
				       struct forward_v4l2_buffer **vbi)
{
	struct forward_v4l2_dev_ops *ops = io->v4l2->ops;
	struct forward_dev *dev = io->v4l2->dev;
	struct forward_v4l2_io_buffers *bufs;
	u32 field = buffer & 0x1;
	u32 address;
	size_t size;

	if (buffer < 0 || buffer >= ops->hw_buffers)
		return;

	*video = NULL;
	*vbi = NULL;

	bufs = &io->mapped_buffers[buffer];

	if (!bufs->mapped)
		return;

	if (bufs->vbi) {
		bufs->vbi->vbi_size[field] = vbi_size;
		bufs->vbi->video_size[field] = video_size;
	}

	if (bufs->video) {
		bufs->video->vbi_size[field] = vbi_size;
		bufs->video->video_size[field] = video_size;
	}

	bufs->mapped = false;
	if (io->interleave && !field) {
		struct forward_v4l2_io_buffers *bufs_next = &io->mapped_buffers[buffer + 1];
		if (bufs_next->mapped && (bufs_next->video == bufs->video))
			*video = NULL;
		else
			*video = bufs->video;
		if (bufs_next->mapped && (bufs_next->vbi == bufs->vbi))
			*vbi = NULL;
		else
			*vbi = bufs->vbi;
	} else {
		*video = bufs->video;
		*vbi = bufs->vbi;
	}
	bufs->vbi = NULL;
	bufs->video = NULL;

	ops->buffer_info(io, buffer, &address, &size);
	size = io->buffer_map_size;

	forward_vdma_unmap_buf(dev->vdma, io, address, size);
}

static void forward_v4l2_io_dequeue_vbi(struct forward_v4l2_io *io, int buffer,
					struct forward_v4l2_buffer **vbi)
{
	struct forward_v4l2_dev_ops *ops = io->v4l2->ops;
	struct forward_v4l2_io_buffers *bufs;
	u32 field = buffer & 0x1;

	if (buffer < 0 || buffer >= ops->hw_buffers)
		return;

	*vbi = NULL;

	bufs = &io->mapped_buffers[buffer];

	if (!bufs->mapped)
		return;

	*vbi = bufs->vbi;
	if (io->interleave && !field) {
		struct forward_v4l2_io_buffers *bufs_next = &io->mapped_buffers[buffer + 1];
		if (bufs_next->mapped && (bufs_next->vbi == bufs->vbi))
			*vbi = NULL;
	}

	bufs->vbi = NULL;
}

static void forward_v4l2_io_extract_vbi(struct forward_v4l2_io *io, bool field,
					struct forward_v4l2_buffer *video,
					struct forward_v4l2_buffer *vbi, bool error)
{
	struct vb2_buffer *in_buffer, *out_buffer;
	u8 *inp, *outp;
	int size[2] = { 0, 0 }, hw_size[2] = { 0, 0 }, buf_size = 0, outs = 0, res_size = 0;
	int f, ifield = field ? 1 : 0;

#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0)
	in_buffer = &video->vb.vb2_buf;
	out_buffer = &vbi->vb.vb2_buf;
#else
	in_buffer = &video->vb;
	out_buffer = &vbi->vb;
#endif
	size[0] = io->timings.sdi.topVBIs[0] * io->timings.sdi.active_width * 2;
	size[1] = io->timings.sdi.topVBIs[1] * io->timings.sdi.active_width * 2;
	hw_size[0] = vbi->vbi_size[0] * 8 / 10;
	hw_size[1] = vbi->vbi_size[1] * 8 / 10;
	buf_size = vb2_plane_size(out_buffer, 0) / 2;
	outs = buf_size;

	inp = vb2_plane_vaddr(in_buffer, 0) + io->buffer_map_offset;
	outp = vb2_plane_vaddr(out_buffer, 0);

	for (f = io->interleave ? 0 : ifield; io->interleave ? (f <= 1) : (f == ifield); f++) {
		int realsize = min(size[f], outs);

		if ((hw_size[f] && (hw_size[f] != size[f])) || error) {
			forward_v4l2_set16b10b(0x0040020000400200ULL, outp, realsize);
		} else {
			forward_v4l2_unpack10b16b(inp, outp, realsize);
		}
		inp += io->buffer_map_size;
		outp += realsize * 2;
		outs -= realsize;
		res_size += realsize;
	}
	vb2_set_plane_payload(out_buffer, 0, res_size * 2);
}

static void forward_v4l2_io_append_vbi(struct forward_v4l2_io *io, bool field,
				       struct forward_v4l2_buffer *video,
				       struct forward_v4l2_buffer *vbi)
{
	struct vb2_buffer *in_buffer, *out_buffer;
	u8 *inp, *outp;
	int size[2] = { 0, 0 }, ins = 0;
	int f, ifield = field ? 1 : 0;

#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0)
	in_buffer = &vbi->vb.vb2_buf;
	out_buffer = &video->vb.vb2_buf;
#else
	in_buffer = &vbi->vb;
	out_buffer = &video->vb;
#endif

	inp = vb2_plane_vaddr(in_buffer, 0);
	outp = vb2_plane_vaddr(out_buffer, 0) + io->buffer_map_offset;

	size[0] = io->timings.sdi.topVBIs[0] * io->timings.sdi.active_width * 2;
	size[1] = io->timings.sdi.topVBIs[1] * io->timings.sdi.active_width * 2;
	ins = vb2_plane_size(out_buffer, 0) / 2;

	for (f = io->interleave ? 0 : ifield; io->interleave ? (f <= 1) : (f == ifield); f++) {
		int realsize = min(size[f], ins);

		if (realsize < size[f])
			forward_v4l2_set16b10b(0x0040020000400200ULL, outp, size[f]);

		if (ins <= 0)
			break;

		forward_v4l2_pack16b10b(inp, outp, realsize);

		inp += realsize * 2;
		ins -= realsize;
		outp += io->buffer_map_size;
	}
}

static void forward_v4l2_io_zero_vbi(struct forward_v4l2_io *io, struct forward_v4l2_buffer *video)
{
	struct vb2_buffer *out_buffer;
	u8 *outp;
	int odd_size, even_size;

#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0)
	out_buffer = &video->vb.vb2_buf;
#else
	out_buffer = &video->vb;
#endif
	outp = vb2_plane_vaddr(out_buffer, 0) + io->buffer_map_offset;

	odd_size = io->timings.sdi.topVBIs[0] * io->timings.sdi.active_width * 2;
	even_size = io->timings.sdi.topVBIs[1] * io->timings.sdi.active_width * 2;

	forward_v4l2_set16b10b(0x0040020000400200ULL, outp, odd_size);
	if (io->interleave) {
		outp += io->buffer_map_size;
		forward_v4l2_set16b10b(0x0040020000400200ULL, outp, even_size);
	}
}

static void forward_v4l2_io_buffer_done(struct forward_v4l2_io *io,
					struct forward_v4l2_buffer *buffer, bool field,
					u64 timestamp, struct v4l2_timecode *timecode, bool error)
{
	struct vb2_buffer *vb2_buffer;

#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0)
	vb2_buffer = &buffer->vb.vb2_buf;
#else
	vb2_buffer = &buffer->vb;
#endif

	if (vb2_buffer->state != VB2_BUF_STATE_ACTIVE)
		return;

	if (io->state == FORWARD_IO_RX) {
		u32 buf_field;
		if (io->format.field != V4L2_FIELD_NONE)
			if (io->interleave)
				buf_field = io->format.field;
			else
				buf_field = field ? V4L2_FIELD_BOTTOM : V4L2_FIELD_TOP;
		else
			buf_field = V4L2_FIELD_NONE;

#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0)
		buffer->vb.field = buf_field;
		if (timecode && timecode->type != 0) {
			buffer->vb.timecode = *timecode;
			buffer->vb.flags |= V4L2_BUF_FLAG_TIMECODE;
		}
#else
		vb2_buffer->v4l2_buf.field = field;
		if (timecode && timecode->type != 0) {
			vb2_buffer->v4l2_buf.timecode = *timecode;
			vb2_buffer->v4l2_buf.flags |= V4L2_BUF_FLAG_TIMECODE;
		}
#endif
	}

#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0)
	buffer->vb.sequence = io->sequence;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 5, 0)
	buffer->vb.vb2_buf.timestamp = timestamp;
#else
	buffer->vb.timestamp.tv_sec = timestamp / NSEC_PER_SEC;
	buffer->vb.timestamp.tv_usec = (timestamp / NSEC_PER_USEC) % USEC_PER_SEC;
#endif
#else
	vb2_buffer->v4l2_buf.sequence = io->sequence;
	vb2_buffer->v4l2_buf.timestamp.tv_sec = timestamp / NSEC_PER_SEC;
	vb2_buffer->v4l2_buf.timestamp.tv_usec = (timestamp / NSEC_PER_USEC) % USEC_PER_SEC;
#endif
	vb2_buffer_done(vb2_buffer, error ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
}

static void forward_v4l2_io_buffer_get(struct forward_v4l2_io_dev *iodev,
				       struct forward_v4l2_buffer **buffer)
{
	unsigned long sflags;
	bool empty;

	*buffer = NULL;
	spin_lock_irqsave(&iodev->q_slock, sflags);
	empty = list_empty(&iodev->buffers);
	if (!empty) {
		*buffer = list_entry(iodev->buffers.next, struct forward_v4l2_buffer, list);
		list_del(&(*buffer)->list);
	}
	spin_unlock_irqrestore(&iodev->q_slock, sflags);
}

static void forward_v4l2_io_buffer_put(struct forward_v4l2_io_dev *iodev,
				       struct forward_v4l2_buffer *buffer)
{
	unsigned long sflags;

	spin_lock_irqsave(&iodev->q_slock, sflags);
	list_add_tail(&buffer->list, &iodev->buffers);
	spin_unlock_irqrestore(&iodev->q_slock, sflags);
}

static bool forward_v4l2_io_empty_queue(struct forward_v4l2_io_dev *iodev,
					enum v4l2_field req_field)
{
	unsigned long sflags;
	bool empty;
	struct forward_v4l2_buffer *buffer;

	spin_lock_irqsave(&iodev->q_slock, sflags);
	empty = list_empty(&iodev->buffers);
	if (!empty) {
		buffer = list_entry(iodev->buffers.next, struct forward_v4l2_buffer, list);
		if ((req_field != V4L2_FIELD_ANY) && (buffer->vb.field != req_field))
			empty = true;
	}
	spin_unlock_irqrestore(&iodev->q_slock, sflags);

	return empty;
}

static void forward_v4l2_io_toogle_streaming(struct forward_v4l2_io *io, bool enable, bool force,
					     u64 timestamp)
{
	struct forward_v4l2_dev_ops *ops = io->v4l2->ops;
	struct forward_v4l2_buffer *video, *vbi;
	int i;

	if (!enable) {
		io->streaming = false;
		io->toggle_streaming = false;

		for (i = 0; i < ops->hw_buffers; i++) {
			forward_v4l2_io_dequeue_hw(io, i, 0, 0, &video, &vbi);

			if ((io->state == FORWARD_IO_RX) && io->vbi_en && video && vbi)
				forward_v4l2_io_extract_vbi(io, io->hw_interlaced && (i & 0x1),
							    video, vbi, true);

			if (vbi)
				forward_v4l2_io_buffer_done(io, vbi, i & 0x1, timestamp, NULL,
							    force ? true : false);
			if (video)
				forward_v4l2_io_buffer_done(io, video, i & 0x1, timestamp, NULL,
							    force ? true : false);
		}

		if (io->v4l2->ops->toggle_mute && !io->manual_mute && default_mute) {
			io->mute = true;
			io->v4l2->ops->toggle_mute(io, true);
		}

	} else if (enable) {
		struct forward_alsa_io *alsa = io->v4l2->dev->io[io->index].alsa;
		if (alsa) {
			struct v4l2_fract fps = forward_v4l2_fps(&io->timings);
			alsa->fps_n = fps.numerator;
			alsa->fps_d = fps.denominator;
		}
		io->streaming = true;
		io->toggle_streaming = false;
		io->sequence = io->hw_interlaced ? -1 : -2;
		io->buf_sequence = 0;
		io->v4l2->ops->start(io);

		if (io->v4l2->ops->toggle_mute && !io->manual_mute && default_mute) {
			io->mute = false;
			io->v4l2->ops->toggle_mute(io, false);
		}
	}
}

static void forward_v4l2_io_toogle_streaming_vbi(struct forward_v4l2_io *io, bool enable,
						 bool force, u64 timestamp)
{
	struct forward_v4l2_dev_ops *ops = io->v4l2->ops;
	struct forward_v4l2_buffer *vbi;
	int i;

	if (!enable) {
		for (i = 0; i < ops->hw_buffers; i++) {
			forward_v4l2_io_dequeue_vbi(io, i, &vbi);

			if (vbi)
				forward_v4l2_io_buffer_done(io, vbi, i & 0x1, timestamp, NULL,
							    force ? true : false);
		}
	}
}

static void __queue_timestamp_event(struct forward_v4l2_io *io,
				    struct v4l2_event_forward_timestamp *evts)
{
	struct v4l2_event evt = { .type = V4L2_EVENT_FORWARD_TIMESTAMP };
	evts->buffer_sequence = io->sequence;
	*((struct v4l2_event_forward_timestamp *)evt.u.data) = *evts;

	v4l2_event_queue(&io->video.vdev, &evt);
}

static void forward_v4l2_io_irq(struct forward_v4l2_io *io, const forward_irq_t *irq, int prev,
				int cur, int next, u64 timestamp)
{
	if (io->stats.hw_last_buf != prev)
		io->stats.hw_noseq_count++;
	io->stats.hw_last_buf = cur;

	if (io->stats.last_irq_time != 0) {
		if (io->stats.max_irq_period < (timestamp - io->stats.last_irq_time))
			io->stats.max_irq_period = timestamp - io->stats.last_irq_time;
	}
	io->stats.last_irq_time = timestamp;
}

static void forward_v4l2_io_irq_func(void *p, const forward_irq_t *irq, u64 timestamp)
{
	struct forward_v4l2_io *io = p;
	int prev = 0, cur = 0, next = 0;

	if (io->v4l2->ops->irq_info(io, irq, &cur, &prev, &next, NULL, NULL))
		forward_v4l2_io_irq(io, irq, prev, cur, next, timestamp);
}

static struct v4l2_timecode forward_v4l2_in_process_hanc(struct forward_v4l2_io *io,
							 const forward_irq_t *irq)
{
	struct forward_alsa_io *alsa = io->v4l2->dev->io[io->index].alsa;
	int hanc_offset;
	size_t hanc_size;
	u16 *hanc_buffer;
	bool field;
	struct forward_hanc_stream hanc;
	struct forward_hanc_smpte_291_header h;
	struct v4l2_timecode timecode = { 0 };

	if (!alsa || !io->hanc_buffer || !io->hanc_buffer_size)
		return timecode;

	alsa->alsa->ops->irq_info(alsa, irq, &hanc_offset, &hanc_size, &field);

	if (!hanc_size || io->hanc_buffer_size <= hanc_offset)
		return timecode;

	hanc_size = min(hanc_size, io->hanc_buffer_size - hanc_offset);
	hanc_buffer = io->hanc_buffer + hanc_offset;

	forward_hanc_open(&hanc, hanc_buffer, hanc_size, 2);

	while (forward_hanc_read_smpte_291_header(&hanc, 1, &h)) {
		if ((h.did == 0x60) && (h.sdid == 0x60)) {
			u64 ltc = 0;
			u16 dbb = 0;
			int i;
			u16 packet[16];
			struct v4l2_fract ti;

			forward_hanc_read_smpte_291_data(&hanc, 1, &h, packet, 16);
			for (i = 0; i < 16; i++) {
				ltc |= (u64)((packet[i] >> 4) & 0xF) << (i * 4);
				dbb |= (u16)((packet[i] >> 3) & 0x1) << i;
			}

			if ((io->atc_type == V4L2_FORWARD_TIMECODE_LTC) && ((dbb & 0xFF) != 0x00))
				continue;
			if ((io->atc_type == V4L2_FORWARD_TIMECODE_VITC) &&
			    (((dbb & 0xFF) != 0x01) && ((dbb & 0xFF) != 0x02)))
				continue;

			timecode.frames = ((ltc >> 0) & 0xF) + ((ltc >> 8) & 0x3) * 10;
			timecode.seconds = ((ltc >> 16) & 0xF) + ((ltc >> 24) & 0x7) * 10;
			timecode.minutes = ((ltc >> 32) & 0xF) + ((ltc >> 40) & 0x7) * 10;
			timecode.hours = ((ltc >> 48) & 0xF) + ((ltc >> 56) & 0x3) * 10;
			timecode.userbits[0] = ((ltc >> 4) & 0xF) | ((ltc >> 8) & 0xF0);
			timecode.userbits[1] = ((ltc >> 20) & 0xF) | ((ltc >> 24) & 0xF0);
			timecode.userbits[2] = ((ltc >> 36) & 0xF) | ((ltc >> 40) & 0xF0);
			timecode.userbits[3] = ((ltc >> 52) & 0xF) | ((ltc >> 56) & 0xF0);
			if (ltc & (1 << 10))
				timecode.flags |= V4L2_TC_FLAG_DROPFRAME;
			if (ltc & (1 << 11))
				timecode.flags |= V4L2_TC_FLAG_COLORFRAME;

			ti = forward_v4l2_frameinterval(&io->hw_timings);
			if (ti.numerator != 0) {
				int fps = ti.denominator / ti.numerator;
				if (fps >= 59)
					timecode.type = V4L2_TC_TYPE_60FPS;
				else if (fps >= 50)
					timecode.type = V4L2_TC_TYPE_50FPS;
				else if (fps >= 29)
					timecode.type = V4L2_TC_TYPE_30FPS;
				else if (fps >= 25)
					timecode.type = V4L2_TC_TYPE_25FPS;
				else if (fps >= 23)
					timecode.type = V4L2_TC_TYPE_24FPS;
			}
			//forward_info(io->v4l2->dev, "IN%d FRAME TIMECODE %02d:%02d:%02d.%02d USER BITS %08x (%016llx)\n",
			//       io->number, timecode.hours, timecode.minutes, timecode.seconds, timecode.frames,
			//       *((uint32_t *)timecode.userbits), ltc);
			break;
		}
	}
	return timecode;
}

static void forward_v4l2_out_set_timecode(struct forward_v4l2_io *io, struct v4l2_timecode *tc,
					  bool field)
{
	struct forward_v4l2_dev_ops *ops = io->v4l2->ops;
	u64 ltc = 0;
	u16 ddb = 0;

	ddb = (io->atc_type == V4L2_FORWARD_TIMECODE_LTC) ? 0x0000 : 0x0001;
	ltc |= ((u64)(tc->frames % 10) & 0xF) | ((u64)((tc->frames / 10) & 0x3) << 8);
	if (tc->flags & V4L2_TC_FLAG_DROPFRAME)
		ltc |= (1 << 9);
	if (tc->flags & V4L2_TC_FLAG_COLORFRAME)
		ltc |= (1 << 10);
	ltc |= ((u64)((tc->seconds % 10) & 0xF) << 16) | ((u64)((tc->seconds / 10) & 0x7) << 24);
	ltc |= ((u64)((tc->minutes % 10) & 0xF) << 32) | ((u64)((tc->minutes / 10) & 0x7) << 40);
	ltc |= ((u64)((tc->hours % 10) & 0xF) << 48) | ((u64)((tc->hours / 10) & 0x3) << 56);
	ltc |= ((u64)(tc->userbits[0] & 0xF) << 4) | ((u64)(tc->userbits[0] & 0xF0) << 8);
	ltc |= ((u64)(tc->userbits[1] & 0xF) << 20) | ((u64)(tc->userbits[0] & 0xF0) << 24);
	ltc |= ((u64)(tc->userbits[2] & 0xF) << 36) | ((u64)(tc->userbits[0] & 0xF0) << 40);
	ltc |= ((u64)(tc->userbits[3] & 0xF) << 52) | ((u64)(tc->userbits[0] & 0xF0) << 56);

	if (io->atc_type != V4L2_FORWARD_TIMECODE_LTC) {
		if ((io->hw_interlaced || (io->atc_freq > 30)) && field) {
			ltc |= ((io->atc_freq == 25) || (io->atc_freq == 50)) ? (1ULL << 59) :
										(1ULL << 27);
			ddb = 0x0002;
		}
	}

	//forward_info(io->v4l2->dev, "OUT%d FRAME TIMECODE %02d:%02d:%02d.%02d USER BITS %08x (%016llx)\n",
	//       io->number, tc->hours, tc->minutes, tc->seconds, tc->frames,
	//       *((uint32_t *)tc->userbits), ltc);

	ops->update_timecode(io, ltc, ddb);
}

static void forward_v4l2_io_process(struct forward_v4l2_io *io, const forward_irq_t *irq, int prev,
				    int cur, int next, int vbi_size, int video_size, u64 timestamp)
{
	bool is_output = (io->state == FORWARD_IO_TX);
	bool is_sdi = (io->hw_timings.type == FORWARD_TIMING_SDI);
	bool is_hdmi = (io->hw_timings.type == FORWARD_TIMING_HDMI);
	struct forward_v4l2_dev_ops *ops = io->v4l2->ops;
	struct forward_v4l2_timings new;
	struct forward_v4l2_buffer *video, *vbi, *new_video = NULL, *new_vbi = NULL;
	bool no_seq, empty_queue, framerep, framedrop, busy, empty;
	int i;
	bool cur_field = cur & 0x1, prev_field = prev & 0x1, next_field = next & 0x1;
	struct v4l2_event vsync = {
		.type = V4L2_EVENT_VSYNC,
	};
	struct v4l2_event_forward_timestamp evts = { 0, 0, 0 };
	u64 cur_time = ktime_to_ns(ktime_get()), end_time;
	struct v4l2_timecode timecode = { 0 };
	enum v4l2_field req_field = V4L2_FIELD_ANY;

	if (ops->io_timestamp)
		evts.eof_timestamp = ops->io_timestamp(io);
	if (ops->hw_timestamp)
		evts.irq_timestamp = ops->hw_timestamp(io->v4l2);
	evts.field = io->hw_interlaced ? (prev_field ? V4L2_FIELD_TOP : V4L2_FIELD_BOTTOM) :
					 V4L2_FIELD_NONE;

	if (!io->hw_interlaced || !cur_field) {
		io->frame_hw_time = evts.eof_timestamp;
	}

	if (io->genlock)
		forward_v4l2_genlock_process_out_irq(io->genlock, cur_field);

	if (io->toggle_streaming && (!io->hw_interlaced || (io->streaming == (!cur_field))))
		forward_v4l2_io_toogle_streaming(io, !io->streaming, false, timestamp);

	if (!io->hw_interlaced || cur_field)
		wake_up_interruptible_all(&io->wait);

	if (!is_output && ops->timings_changed(io, &io->hw_timings, &new)) {
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 16, 0)
		static const struct v4l2_event src_ch = {
			.type = V4L2_EVENT_SOURCE_CHANGE,
			.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
		};

		v4l2_event_queue(&io->video.vdev, &src_ch);
#endif
		io->hw_timings = new;

		if (is_sdi)
			io->hw_interlaced = io->hw_timings.sdi.interlaced;
		else if (is_hdmi)
			io->hw_interlaced = io->hw_timings.hdmi.bt.interlaced;
	}

	no_seq = (io->hw_prev_buffer >= 0) && (prev != io->hw_prev_buffer);
	if (io->streaming && (!io->hw_interlaced || !(io->hw_prev_buffer & 0x1) || io->asi))
		io->sequence++;
	io->buf_sequence = io->sequence;

	if (io->streaming && no_seq) {
		if (is_output)
			forward_info(
				io->v4l2->dev,
				"V4L2 OUT%d hw drop: expected buffer %d, got %d, sequence = %d, period = %lld",
				io->number, io->hw_prev_buffer, prev, io->sequence,
				(s64)cur_time - (s64)io->stats.last_proc_time);
		else
			forward_info(
				io->v4l2->dev,
				"V4L2  IN%d hw drop: expected buffer %d, got %d, sequence = %d, period = %lld",
				io->number, io->hw_prev_buffer, prev, io->sequence,
				(s64)cur_time - (s64)io->stats.last_proc_time);
	}

	io->hw_prev_buffer = cur;

	if (!io->asi) {
		vsync.u.vsync.field = io->hw_interlaced ?
					      (cur_field ? V4L2_FIELD_BOTTOM : V4L2_FIELD_TOP) :
					      V4L2_FIELD_NONE;
		v4l2_event_queue(&io->video.vdev, &vsync);

		if (io->streaming && (!io->hw_interlaced || !cur_field)) {
			struct v4l2_event framesync = {
				.type = V4L2_EVENT_FRAME_SYNC,
			};
			framesync.u.frame_sync.frame_sequence = io->sequence + 1;
			v4l2_event_queue(&io->video.vdev, &framesync);
		}
	}

	__queue_timestamp_event(io, &evts);

	if (io->stats.max_proc_delay < (cur_time - timestamp))
		io->stats.max_proc_delay = cur_time - timestamp;

	if (io->stats.last_proc_time != 0) {
		if (io->stats.max_proc_period < (cur_time - io->stats.last_proc_time))
			io->stats.max_proc_period = cur_time - io->stats.last_proc_time;
	}
	io->stats.last_proc_time = cur_time;

	if (io->stats.sw_last_buf != prev)
		io->stats.sw_noseq_count++;
	io->stats.sw_last_buf = cur;

	if (!io->streaming)
		return;

	if (!is_output && is_sdi && (io->atc_type != V4L2_FORWARD_TIMECODE_DISABLED))
		timecode = forward_v4l2_in_process_hanc(io, irq);

	if (is_output && (io->format.field == V4L2_FIELD_ALTERNATE)) {
		req_field = next_field ? V4L2_FIELD_BOTTOM : V4L2_FIELD_TOP;
	}

	empty_queue = forward_v4l2_io_empty_queue(&io->video, req_field);
	framerep = is_output && empty_queue && (!io->interleave || !next_field);
	framedrop = !is_output && empty_queue && (!io->interleave || !next_field);

	if (framedrop || framerep)
		io->stats.frame_drops++;

	if (framerep)
		forward_info(io->v4l2->dev, "V4L2 OUT%d sw rep: sequence = %d, period = %lld",
			     io->number, io->sequence,
			     (s64)cur_time - (s64)io->stats.last_proc_time);
	if (framedrop)
		forward_info(io->v4l2->dev, "V4L2  IN%d sw drop: sequence = %d, period = %lld",
			     io->number, io->sequence,
			     (s64)cur_time - (s64)io->stats.last_proc_time);

	// Take next buffer from V4L2 or use current if interleaving
	if (io->interleave && next_field) {
		new_vbi = io->mapped_buffers[cur].vbi;
		new_video = io->mapped_buffers[cur].video;
	} else if (!empty_queue) {
		forward_v4l2_io_buffer_get(&io->vbi, &new_vbi);
		forward_v4l2_io_buffer_get(&io->video, &new_video);
	}

	// Unmap previous buffer
	forward_v4l2_io_dequeue_hw(io, prev, video_size, vbi_size, &video, &vbi);
	if (!is_output && io->vbi_en && video && vbi)
		forward_v4l2_io_extract_vbi(io, io->hw_interlaced && prev_field, video, vbi, false);

	// Repeat / overwrite last buffer if there is no new buffer to display / capture
	// FIXME: In field alternating mode last field will be displayed
	if (framerep || framedrop) {
		if (io->mapped_buffers[cur].mapped) {
			new_vbi = io->mapped_buffers[cur].vbi;
			new_video = io->mapped_buffers[cur].video;
		}
	}

	// Unmap all buffers besides current & previous
	for (i = 0; i < ops->hw_buffers; i++) {
		struct forward_v4l2_buffer *bad_video, *bad_vbi;

		if ((i == cur) || (i == prev))
			continue;

		forward_v4l2_io_dequeue_hw(io, i, 0, 0, &bad_video, &bad_vbi);

		if ((bad_vbi && (bad_vbi == new_vbi)) || (bad_video && (bad_video == new_video)) ||
		    (bad_vbi && (bad_vbi == vbi)) || (bad_video && (bad_video == video)))
			continue;

		if (!is_output && io->vbi_en && bad_video && bad_vbi)
			forward_v4l2_io_extract_vbi(io, io->hw_interlaced && (i & 0x1), bad_video,
						    bad_vbi, true);

		if (bad_video && (!io->hw_interlaced || !(i & 0x1)))
			io->sequence++;

		if (bad_video)
			forward_v4l2_io_buffer_done(io, bad_video, i & 0x1, timestamp, NULL, true);
		if (bad_vbi)
			forward_v4l2_io_buffer_done(io, bad_vbi, i & 0x1, timestamp, NULL, true);
	}

	// Check buffer is still used in mapping and return it back to application if not.
	busy = false;
	for (i = 0; i < ops->hw_buffers; i++) {
		if (io->mapped_buffers[i].mapped &&
		    ((video && (io->mapped_buffers[i].video == video)) ||
		     (vbi && (io->mapped_buffers[i].vbi == vbi)))) {
			busy = true;
			break;
		}
	}

	empty = false;

	if (io->asi && !is_output && !busy && video) {
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0)
		struct vb2_buffer *vb2_buffer = &video->vb.vb2_buf;
#else
		struct vb2_buffer *vb2_buffer = &video->vb;
#endif
		if (!io->raw) {
			size_t errors;
			size_t asiSize = forward_v4l2_decode_asi(
				vb2_plane_vaddr(vb2_buffer, 0) + io->buffer_map_offset,
				vb2_plane_vaddr(vb2_buffer, 0), io->format.sizeimage * 10 / 8,
				io->format.sizeimage, &errors);
			vb2_set_plane_payload(vb2_buffer, 0, asiSize);
			io->stats.data_errors += errors;

			if (!asiSize)
				empty = true;
		}
	}

	if (!busy && !empty) {
		if (vbi)
			forward_v4l2_io_buffer_done(io, vbi, prev & 0x1, timestamp, &timecode,
						    false);
		if (video)
			forward_v4l2_io_buffer_done(io, video, prev & 0x1, timestamp, &timecode,
						    false);
	} else if (!busy && empty) {
		// Do not return empty ASI buffers, some crapware becomes dissapointed
		io->sequence--;

		if (video)
			forward_v4l2_io_buffer_put(&io->video, video);
		if (vbi)
			forward_v4l2_io_buffer_put(&io->vbi, vbi);
	} else if (is_output && no_seq && busy &&
		   (!io->hw_interlaced || !(io->hw_prev_buffer & 0x1))) {
		// Hack, keep sequence continous on output genlock switch
		io->sequence--;
	}

	if (is_output && io->vbi_en && (!io->interleave || !next_field)) {
		if (framerep && new_video)
			forward_v4l2_io_zero_vbi(io, new_video);
		else if (new_vbi && new_video)
			forward_v4l2_io_append_vbi(io, next_field, new_video, new_vbi);
		else if (new_video)
			forward_v4l2_io_zero_vbi(io, new_video);
	}
	if (framerep && io->asi)
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0)
		forward_v4l2_clear_asi(vb2_plane_vaddr(&new_video->vb.vb2_buf, 0),
				       io->raw ? (io->format.sizeimage) :
						 (io->format.sizeimage * 10 / 8));
#else
		forward_v4l2_clear_asi(vb2_plane_vaddr(&new_video->vb, 0),
				       io->format.sizeimage * 10 / 8);
#endif

	forward_v4l2_io_queue_hw(io, next, new_video, new_vbi);
	if (is_output && is_sdi && (io->atc_type != V4L2_FORWARD_TIMECODE_DISABLED)) {
		struct v4l2_timecode *tc;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0)
		tc = &new_video->vb.timecode;
#else
		tc = &new_video->vb.v4l2_buf.timecode;
#endif
		//tc->frames = io->sequence % 25;
		//tc->seconds = (io->sequence / 25) % 60;
		//tc->minutes = ((io->sequence / 25) / 60) % 60;
		//tc->hours = (((io->sequence / 25) / 60) / 60) % 24;
		forward_v4l2_out_set_timecode(io, tc, next_field);
	}

	end_time = ktime_to_ns(ktime_get());

	if (io->stats.max_proc_time < (end_time - cur_time))
		io->stats.max_proc_time = end_time - cur_time;
}

static void forward_v4l2_io_irq_wq_func(void *p, const forward_irq_t *irq, u64 timestamp)
{
	struct forward_v4l2_io *io = p;
	int prev = 0, cur = 0, next = 0;
	int video_size = 0, vbi_size = 0;

	mutex_lock(&io->lock);
	if (!io->ready) {
		mutex_unlock(&io->lock);
		return;
	}
	if (!io->v4l2->ops->irq_info(io, irq, &cur, &prev, &next, &vbi_size, &video_size)) {
		mutex_unlock(&io->lock);
		return;
	}
	forward_v4l2_io_process(io, irq, prev, cur, next, vbi_size, video_size, timestamp);
	mutex_unlock(&io->lock);
}

static unsigned int forward_v4l2_io_calculate_buffer_size(struct forward_v4l2_io *io,
							  unsigned int size)
{
	unsigned int hwsize = 0;
	size = (((size - 1) >> PAGE_SHIFT) + 1) << PAGE_SHIFT;

	io->buffer_map_offset = 0;
	io->buffer_map_size = size;

	if (io->format_conv == FORWARD_V4L2_CONV_HDMI420_YUV420) {
		hwsize = size;
	} else if (io->format_conv == FORWARD_V4L2_CONV_10BIT_V210) {
		hwsize = size;
	} else if (io->format_conv == FORWARD_V4L2_CONV_10BIT_16BIT) {
		hwsize = size * 5 / 8;
	}
	if (io->asi && !io->raw) {
		hwsize = size * 5 / 4;
	}
	hwsize = (((hwsize - 1) >> PAGE_SHIFT) + 1) << PAGE_SHIFT;

	// Alloc enough space for two page-aligned fields + weaved result
	if (io->interleave) {
		size *= 2;
		if (hwsize)
			hwsize *= 2;
		else
			hwsize = size;
	}

	if (hwsize) {
		io->buffer_map_offset = size;
		io->buffer_map_size = hwsize / (io->interleave ? 2 : 1);
	}

	return size + hwsize;
}

#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 5, 0)
static int forward_v4l2_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
				    unsigned int *nplanes, unsigned int sizes[],
				    struct device *alloc_devs[])
#elif LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0)
static int forward_v4l2_queue_setup(struct vb2_queue *vq, const void *parg, unsigned *nbuffers,
				    unsigned *nplanes, unsigned sizes[], void *alloc_ctxs[])
#else
static int forward_v4l2_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
				    unsigned *nbuffers, unsigned *nplanes, unsigned sizes[],
				    void *alloc_ctxs[])
#endif

{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)vb2_get_drv_priv(vq);
	struct forward_v4l2_io *io = iodev->io;
	unsigned int size;

	if ((iodev->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
	    (iodev->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)) {
		struct v4l2_fract fi;

		io->asi = (io->format.pixelformat == V4L2_PIX_FMT_MPEG) |
			  (io->format.pixelformat == V4L2_PIX_FMT_SL_RAW_ASI);
		io->raw = (io->format.pixelformat == V4L2_PIX_FMT_SL_RAW) |
			  (io->format.pixelformat == V4L2_PIX_FMT_SL_RAW_ASI);
		io->interleave = !io->asi && !io->raw &&
				 ((io->format.field == V4L2_FIELD_INTERLACED) ||
				  (io->format.field == V4L2_FIELD_INTERLACED_TB) ||
				  (io->format.field == V4L2_FIELD_INTERLACED_BT));
		if (io->asi || io->raw)
			io->vbi_en = false;

		if (io->asi || io->raw || (io->format.pixelformat == V4L2_PIX_FMT_YUV420))
			size = io->format.sizeimage;
		else {
			size = io->format.height * io->format.bytesperline;
		}
		fi = forward_v4l2_frameinterval(&io->timings);
		if (fi.numerator)
			io->atc_freq = fi.denominator / fi.numerator;

		if (*nplanes && (sizes[0] < size))
			return -EINVAL;

		if (io->interleave)
			size = (((io->format.height - 1) / 2) + 1) * io->format.bytesperline;

		if (io->vbi_en) {
			size += 2 * io->timings.sdi.topVBIs[0] * io->timings.sdi.active_width * 20 /
				8;
			if (io->interleave)
				size += 2 * io->timings.sdi.topVBIs[1] *
					io->timings.sdi.active_width * 20 / 8;
		}

		size = forward_v4l2_io_calculate_buffer_size(io, size);

		if (*nbuffers < FORWARD_V4L2_MIN_BUFFERS)
			*nbuffers = FORWARD_V4L2_MIN_BUFFERS;

		*nplanes = 1;
		sizes[0] = size;
	} else if ((iodev->type == V4L2_BUF_TYPE_VBI_CAPTURE) ||
		   (iodev->type == V4L2_BUF_TYPE_VBI_OUTPUT)) {
		size = io->vbi_format.count[0] * io->vbi_format.samples_per_line * 2;
		if (io->format.field != V4L2_FIELD_NONE)
			size += io->vbi_format.count[1] * io->vbi_format.samples_per_line * 2;

		if (*nplanes && (sizes[0] < size))
			return -EINVAL;

		if (*nbuffers < FORWARD_V4L2_MIN_BUFFERS)
			*nbuffers = FORWARD_V4L2_MIN_BUFFERS;

		*nplanes = 1;
		sizes[0] = size;
	}

	return 0;
}

static int forward_v4l2_buf_init(struct vb2_buffer *buf)
{
	struct forward_v4l2_io_dev *iodev =
		(struct forward_v4l2_io_dev *)vb2_get_drv_priv(buf->vb2_queue);

	if (iodev->type == V4L2_BUF_TYPE_VBI_OUTPUT) {
		int i;
		u32 *p = vb2_plane_vaddr(buf, 0);
		for (i = 0; i < (int)vb2_plane_size(buf, 0) / 4; i++)
			p[i] = 0x00400200;
	}

	return 0;
}

static int forward_v4l2_buf_prepare(struct vb2_buffer *buf)
{
	struct forward_v4l2_io_dev *iodev =
		(struct forward_v4l2_io_dev *)vb2_get_drv_priv(buf->vb2_queue);
	struct forward_v4l2_io *io = iodev->io;
	int offsets[2] = { 0, 0 };

	if ((iodev->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
	    (iodev->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)) {
		if (vb2_plane_size(buf, 0) < ((io->asi || io->raw) ?
						      io->format.sizeimage :
						      io->format.height * io->format.bytesperline))
			return -EINVAL;
	}

	if (iodev->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
		unsigned int inbytesperline = io->format.bytesperline;
		unsigned int outbytesperline = io->format.bytesperline;

		if (io->format_conv == FORWARD_V4L2_CONV_10BIT_V210)
			outbytesperline = io->format.width * 20 / 8;
		else if (io->format_conv == FORWARD_V4L2_CONV_10BIT_16BIT)
			outbytesperline = io->format.width * 20 / 8;

		if (!io->asi || io->raw) {
			if (io->vbi_en) {
				offsets[0] = io->timings.sdi.topVBIs[0] *
					     io->timings.sdi.active_width * 20 / 8;
				offsets[1] = io->timings.sdi.topVBIs[1] *
					     io->timings.sdi.active_width * 20 / 8;
			}
			offsets[0] += io->buffer_map_offset;
			offsets[1] += io->buffer_map_offset;
			if (io->interleave) {
				offsets[1] += io->buffer_map_size;
				forward_v4l2_deinterleave_color_convert(
					buf, offsets, inbytesperline, outbytesperline,
					io->format.height, io->format_conv,
					(io->format.field == V4L2_FIELD_INTERLACED_BT));
			} else if (io->vbi_en) {
				forward_v4l2_output_color_convert(buf, offsets, inbytesperline,
								  outbytesperline,
								  io->format.height,
								  io->format_conv);
			}
		} else {
			forward_v4l2_render_asi(vb2_plane_vaddr(buf, 0),
						vb2_plane_vaddr(buf, 0) + io->buffer_map_offset,
						vb2_get_plane_payload(buf, 0),
						io->format.sizeimage * 10 / 8);
		}
	} else if (iodev->type == V4L2_BUF_TYPE_VBI_CAPTURE) {
		int i;
		u32 *p = vb2_plane_vaddr(buf, 0);
		for (i = 0; i < (int)vb2_plane_size(buf, 0) / 4; i++)
			p[i] = 0x00400200;
	}

	return 0;
}

#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 15, 0)
static void forward_v4l2_buf_finish(struct vb2_buffer *buf)
#else
static int forward_v4l2_buf_finish(struct vb2_buffer *buf)
#endif
{
	struct forward_v4l2_io_dev *iodev =
		(struct forward_v4l2_io_dev *)vb2_get_drv_priv(buf->vb2_queue);
	struct forward_v4l2_io *io = iodev->io;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0)
	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(buf);
	struct forward_v4l2_buffer *fdbuf = container_of(vbuf, struct forward_v4l2_buffer, vb);
#else
	struct forward_v4l2_buffer *fdbuf = container_of(buf, struct forward_v4l2_buffer, vb);
#endif

	int offsets[2] = { 0, 0 };

	if (iodev->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
		unsigned int inbytesperline = io->format.bytesperline;
		unsigned int outbytesperline = io->format.bytesperline;

		if (io->format_conv == FORWARD_V4L2_CONV_10BIT_V210)
			inbytesperline = io->format.width * 20 / 8;
		else if (io->format_conv == FORWARD_V4L2_CONV_10BIT_16BIT)
			inbytesperline = io->format.width * 20 / 8;

		if (!io->asi) {
			if (io->vbi_en) {
				offsets[0] = io->timings.sdi.topVBIs[0] *
					     io->timings.sdi.active_width * 20 / 8;

				if (fdbuf->vbi_size[0] && (fdbuf->vbi_size[0] <= 2 * offsets[0]))
					offsets[0] = fdbuf->vbi_size[0];

				offsets[1] = io->timings.sdi.topVBIs[1] *
					     io->timings.sdi.active_width * 20 / 8;

				if (fdbuf->vbi_size[1] && (fdbuf->vbi_size[1] <= 2 * offsets[1]))
					offsets[1] = fdbuf->vbi_size[1];
			}
			offsets[0] += io->buffer_map_offset;
			offsets[1] += io->buffer_map_offset;

			if (io->interleave) {
				offsets[1] += io->buffer_map_size;
				forward_v4l2_interleave_color_conv(
					buf, offsets, inbytesperline, outbytesperline,
					io->format.height, io->format_conv,
					(io->format.field == V4L2_FIELD_INTERLACED_BT));
			} else {
				forward_v4l2_input_color_convert(buf, offsets, inbytesperline,
								 outbytesperline, io->format.height,
								 io->format_conv);
			}
		}

		if (io->raw)
			vb2_set_plane_payload(buf, 0, io->format.sizeimage);
		else if (!io->asi)
			vb2_set_plane_payload(buf, 0, io->format.bytesperline * io->format.height);
	} else if (iodev->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
		if (io->raw) {
			void *p = vb2_plane_vaddr(buf, 0);
			size_t size = vb2_plane_size(buf, 0);
			// TODO: Fill Raw SDI
			if (io->asi) {
				memset(p, 0, size);
			} else {
				memset(p, 0, size);
			}
		}
	} else if (iodev->type == V4L2_BUF_TYPE_VBI_OUTPUT) {
		int i;
		u32 *p = vb2_plane_vaddr(buf, 0);
		for (i = 0; i < (int)vb2_plane_size(buf, 0) / 4; i++)
			p[i] = 0x00400200;
	}
#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 15, 0)
	return 0;
#endif
}

static void forward_v4l2_buf_queue(struct vb2_buffer *buf)
{
	struct forward_v4l2_io_dev *iodev =
		(struct forward_v4l2_io_dev *)vb2_get_drv_priv(buf->vb2_queue);
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0)
	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(buf);
	struct forward_v4l2_buffer *fdbuf = container_of(vbuf, struct forward_v4l2_buffer, vb);
#else
	struct forward_v4l2_buffer *fdbuf = container_of(buf, struct forward_v4l2_buffer, vb);
#endif
	forward_v4l2_io_buffer_put(iodev, fdbuf);
}

static void forward_v4l2_done_all_buffers(struct forward_v4l2_io_dev *io,
					  enum vb2_buffer_state state)
{
	unsigned long sflags;
	struct forward_v4l2_buffer *buf;

	spin_lock_irqsave(&io->q_slock, sflags);
	while (!list_empty(&io->buffers)) {
		buf = list_entry(io->buffers.next, struct forward_v4l2_buffer, list);
		list_del(&buf->list);
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 4, 0)
		vb2_buffer_done(&buf->vb.vb2_buf, state);
#else
		vb2_buffer_done(&buf->vb, state);
#endif
	}
	spin_unlock_irqrestore(&io->q_slock, sflags);
}

static int forward_v4l2_start_streaming(struct vb2_queue *vq, unsigned count)
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)vb2_get_drv_priv(vq);
	struct forward_v4l2_io *io = iodev->io;
	int status;

	if (!io->v4l2->ops->vbi_support)
		io->vbi_en = false;

	if ((iodev->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
	    (iodev->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)) {
		u32 region_address;
		size_t region_size;

		io->v4l2->ops->prepare(io);
		io->v4l2->ops->region_info(io, &region_address, &region_size);
		status = forward_vdma_get_region(io->v4l2->dev->vdma, io, region_address,
						 region_size);
		if (status)
			goto fail;

		mutex_lock(&io->lock);
		if (!io->streaming)
			io->toggle_streaming = true;
		mutex_unlock(&io->lock);

		/*wait_event_interruptible_timeout(io->wait, io->toggle_streaming == false, HZ / 10);

		// If not started after 1/10 sec = no interrupts = no signal = force start
		mutex_lock(&io->lock);
		if (io->toggle_streaming)
			forward_v4l2_io_toogle_streaming(io, true, true, ktime_to_ns(ktime_get()));
		mutex_unlock(&io->lock);*/
	} else {
		if (!io->vbi_en) {
			status = -EOPNOTSUPP;
			goto fail;
		}
	}

	return 0;

fail:
	forward_v4l2_done_all_buffers(iodev, VB2_BUF_STATE_QUEUED);
	return status;
}

#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 16, 0)
static void forward_v4l2_stop_streaming(struct vb2_queue *vq)
#else
static int forward_v4l2_stop_streaming(struct vb2_queue *vq)
#endif
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)vb2_get_drv_priv(vq);
	struct forward_v4l2_io *io = iodev->io;

	if ((iodev->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
	    (iodev->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)) {
		io->v4l2->ops->stop(io);

		mutex_lock(&io->lock);
		if (io->streaming)
			io->toggle_streaming = true;
		else if (!io->streaming && io->toggle_streaming) // Stopped before actual start
			io->toggle_streaming = false; // Abort
		mutex_unlock(&io->lock);

		wait_event_interruptible_timeout(io->wait, io->toggle_streaming == false, HZ / 10);

		// If not stopped after 1/10 sec = no interrupts = no signal = force stop
		mutex_lock(&io->lock);
		if (io->toggle_streaming)
			forward_v4l2_io_toogle_streaming(io, false, true, ktime_to_ns(ktime_get()));
		mutex_unlock(&io->lock);

		forward_vdma_put_all_regions(io->v4l2->dev->vdma, io);
	} else {
		if (!io->vbi_en)
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 16, 0)
			return;
#else
			return -EOPNOTSUPP;
#endif
		mutex_lock(&io->lock);
		if (io->streaming)
			forward_v4l2_io_toogle_streaming_vbi(io, false, true,
							     ktime_to_ns(ktime_get()));
		mutex_unlock(&io->lock);
	}

	forward_v4l2_done_all_buffers(iodev, VB2_BUF_STATE_ERROR);
#if LINUX_VERSION_CODE < KERNEL_VERSION(3, 16, 0)
	return 0;
#endif
}

const struct vb2_ops forward_v4l2_qops = {
	.queue_setup = forward_v4l2_queue_setup,
	.buf_init = forward_v4l2_buf_init,
	.buf_prepare = forward_v4l2_buf_prepare,
	.buf_finish = forward_v4l2_buf_finish,
	.buf_queue = forward_v4l2_buf_queue,
	.start_streaming = forward_v4l2_start_streaming,
	.stop_streaming = forward_v4l2_stop_streaming,
	.wait_prepare = vb2_ops_wait_prepare,
	.wait_finish = vb2_ops_wait_finish,
};

static int forward_v4l2_c_querycap(struct file *file, void *priv, struct v4l2_capability *cap)
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)video_drvdata(file);
	struct forward_v4l2_io *io = iodev->io;

	strncpy((char *)cap->driver, FORWARD_V4L2_MODULE_NAME, sizeof(cap->driver));

	if (io->state == FORWARD_IO_RX) {
		snprintf((char *)cap->card, sizeof(cap->card), "%s IN%d", io->v4l2->dev->name,
			 io->number);
		cap->capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VBI_CAPTURE |
				    V4L2_CAP_DEVICE_CAPS;
	} else if (io->state == FORWARD_IO_TX) {
		snprintf((char *)cap->card, sizeof(cap->card), "%s OUT%d", io->v4l2->dev->name,
			 io->number);
		cap->capabilities = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_VBI_OUTPUT |
				    V4L2_CAP_DEVICE_CAPS;
	}
	cap->capabilities |= V4L2_CAP_STREAMING;

	if (io->v4l2->dev->pci_dev) {
		snprintf((char *)cap->bus_info, sizeof(cap->bus_info), "PCI:%s",
			 pci_name(io->v4l2->dev->pci_dev));
	}
	cap->version = FORWARD_VERSION_CODE;

	switch (iodev->type) {
	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
		cap->device_caps = V4L2_CAP_VIDEO_CAPTURE;
		break;
	case V4L2_BUF_TYPE_VBI_CAPTURE:
		cap->device_caps = V4L2_CAP_VBI_CAPTURE;
		break;
	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
		cap->device_caps = V4L2_CAP_VIDEO_OUTPUT;
		break;
	case V4L2_BUF_TYPE_VBI_OUTPUT:
		cap->device_caps = V4L2_CAP_VBI_OUTPUT;
		break;
	default:
		break;
	}
	cap->device_caps |= V4L2_CAP_STREAMING;

	return 0;
}

static int forward_v4l2_c_g_parm(struct file *file, void *priv, struct v4l2_streamparm *parm)
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)video_drvdata(file);
	struct forward_v4l2_io *io = iodev->io;

	if ((iodev->type != V4L2_CAP_VIDEO_CAPTURE) && (iodev->type != V4L2_CAP_VIDEO_OUTPUT))
		return -EINVAL;

	parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
	parm->parm.capture.timeperframe = forward_v4l2_frameinterval(&io->timings);
	parm->parm.capture.readbuffers = 0;

	return 0;
}

static int forward_v4l2_c_s_parm(struct file *file, void *priv, struct v4l2_streamparm *parm)
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)video_drvdata(file);
	struct forward_v4l2_io *io = iodev->io;

	if ((iodev->type != V4L2_CAP_VIDEO_CAPTURE) && (iodev->type != V4L2_CAP_VIDEO_OUTPUT))
		return -EINVAL;

	parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
	parm->parm.capture.timeperframe = forward_v4l2_frameinterval(&io->timings);
	parm->parm.capture.readbuffers = 0;

	return 0;
}

static int forward_v4l2_c_enum_input(struct file *file, void *priv, struct v4l2_input *i)
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)video_drvdata(file);
	struct forward_v4l2_io *io = iodev->io;

	if (i->index > 0)
		return -EINVAL;

	i->type = V4L2_INPUT_TYPE_CAMERA;
	i->std = 0;
	strncpy((char *)i->name, "Digital", sizeof(i->name));
	i->capabilities = V4L2_IN_CAP_DV_TIMINGS;
	i->status = 0;
	if (!io->cd)
		i->status |= V4L2_IN_ST_NO_SIGNAL;
	if (!io->locked)
		i->status |= V4L2_IN_ST_NO_SYNC;
	return 0;
}

static int forward_v4l2_c_s_input(struct file *file, void *priv, unsigned int i)
{
	if (i > 0)
		return -EINVAL;

	return 0;
}

static int forward_v4l2_c_g_input(struct file *file, void *priv, unsigned int *i)
{
	*i = 0;
	return 0;
}

static int forward_v4l2_c_enum_output(struct file *file, void *priv, struct v4l2_output *o)
{
	if (o->index > 0)
		return -EINVAL;

	o->type = V4L2_INPUT_TYPE_CAMERA;
	o->std = 0;
	strncpy((char *)o->name, "Digital", sizeof(o->name));
	o->capabilities = V4L2_IN_CAP_DV_TIMINGS;

	return 0;
}

static int forward_v4l2_c_s_output(struct file *file, void *priv, unsigned int i)
{
	if (i > 0)
		return -EINVAL;

	return 0;
}

static int forward_v4l2_c_g_output(struct file *file, void *priv, unsigned int *i)
{
	*i = 0;
	return 0;
}

static int forward_v4l2_c_enum_frameintervals(struct file *file, void *priv,
					      struct v4l2_frmivalenum *fival)
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)video_drvdata(file);
	struct forward_v4l2_io *io = iodev->io;

	if (fival->index)
		return -EINVAL;
	if (fival->width != io->format.width || fival->height != io->format.height ||
	    fival->pixel_format != io->format.pixelformat)
		return -EINVAL;

	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
	fival->discrete = forward_v4l2_frameinterval(&io->timings);
	return 0;
}

static int forward_v4l2_c_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)video_drvdata(file);
	struct forward_v4l2_io *io = iodev->io;

	if (f->type != iodev->type)
		return -EINVAL;

	switch (f->type) {
	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
		forward_v4l2_fill_pix_fmt(&io->timings, &f->fmt.pix);
		if (io->v4l2->ops->check_fmt)
			io->v4l2->ops->check_fmt(io, &f->fmt.pix);
		break;
	case V4L2_BUF_TYPE_VBI_CAPTURE:
	case V4L2_BUF_TYPE_VBI_OUTPUT:
		forward_v4l2_fill_vbi_fmt(&io->timings, &f->fmt.vbi);
		break;
	}

	return 0;
}

static int forward_v4l2_c_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)video_drvdata(file);
	struct forward_v4l2_io *io = iodev->io;

	if (io->streaming)
		return -EBUSY;

	if (f->type != iodev->type)
		return -EINVAL;

	if ((f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) && (io->timings.type == FORWARD_TIMING_SDI)) {
		bool need_tim_update = false;
		if ((f->fmt.pix.pixelformat == V4L2_PIX_FMT_MPEG) ||
		    (f->fmt.pix.pixelformat == V4L2_PIX_FMT_SL_RAW_ASI)) {
			need_tim_update =
				(io->timings.sdi.flags & FORWARD_SDI_TIMINGS_F_ASI) ? false : true;
			io->timings.sdi.flags |= FORWARD_SDI_TIMINGS_F_ASI;
		} else {
			need_tim_update =
				(io->timings.sdi.flags & FORWARD_SDI_TIMINGS_F_ASI) ? true : false;
			io->timings.sdi.flags &= ~FORWARD_SDI_TIMINGS_F_ASI;
		}
		if (need_tim_update)
			io->v4l2->ops->timings_set(io, &io->timings);
	}

	switch (f->type) {
	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
		forward_v4l2_fill_pix_fmt(&io->timings, &f->fmt.pix);
		if (io->v4l2->ops->check_fmt)
			io->v4l2->ops->check_fmt(io, &f->fmt.pix);
		io->format = f->fmt.pix;
		if (io->v4l2->ops->set_fmt)
			io->v4l2->ops->set_fmt(io, &io->format);
		break;
	case V4L2_BUF_TYPE_VBI_CAPTURE:
	case V4L2_BUF_TYPE_VBI_OUTPUT:
		forward_v4l2_fill_vbi_fmt(&io->timings, &f->fmt.vbi);
		io->vbi_format = f->fmt.vbi;
		break;
	}

	return 0;
}

static int forward_v4l2_c_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)video_drvdata(file);
	struct forward_v4l2_io *io = iodev->io;

	if (f->type != iodev->type)
		return -EINVAL;

	switch (f->type) {
	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
		f->fmt.pix = io->format;
		break;
	case V4L2_BUF_TYPE_VBI_CAPTURE:
	case V4L2_BUF_TYPE_VBI_OUTPUT:
		f->fmt.vbi = io->vbi_format;
		break;
	}

	return 0;
}

static int forward_v4l2_c_enum_fmt(struct file *file, void *priv, struct v4l2_fmtdesc *f)
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)video_drvdata(file);
	struct forward_v4l2_io *io = iodev->io;
	struct forward_v4l2 *v4l2 = io->v4l2;

	if ((f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) && (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT))
		return -EINVAL;

	return v4l2->ops->enum_fmt(io, f->index, &f->pixelformat, (char *)f->description,
				   sizeof(f->description));
}

static int forward_v4l2_enum_timings(struct forward_v4l2_io *io, int index,
				     struct forward_v4l2_timings *timings)
{
	struct forward_v4l2 *v4l2 = io->v4l2;
	int vld_idx = 0;
	int i;

	for (i = 0; i < forward_v4l2_timings_count; i++) {
		bool valid = false;
		const struct forward_v4l2_timings *cur = NULL;
		cur = &forward_v4l2_timings[i];

		if (v4l2->ops->timings_valid)
			valid = v4l2->ops->timings_valid(io, cur);
		else
			valid = true;

		if (!valid)
			continue;

		if (index == vld_idx) {
			*timings = *cur;
			return 0;
		}

		vld_idx++;
	}
	return -EINVAL;
}

static int forward_v4l2_c_s_dv_timings(struct file *file, void *priv,
				       struct v4l2_dv_timings *timings)
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)video_drvdata(file);
	struct forward_v4l2_io *io = iodev->io;
	int i;
	struct forward_v4l2_timings t;

	if (io->streaming)
		return -EBUSY;

	for (i = 0; !forward_v4l2_enum_timings(io, i, &t); i++) {
		if (!forward_v4l2_dv_match(&t, timings))
			continue;

		io->timings = t;
		io->v4l2->ops->timings_set(io, &io->timings);
		forward_v4l2_fill_pix_fmt(&io->timings, &io->format);
		forward_v4l2_fill_vbi_fmt(&io->timings, &io->vbi_format);
		if (io->v4l2->ops->check_fmt)
			io->v4l2->ops->check_fmt(io, &io->format);
		if (io->v4l2->ops->set_fmt)
			io->v4l2->ops->set_fmt(io, &io->format);

		if (io->state == FORWARD_IO_TX) {
			io->hw_timings = t;
			if (io->hw_timings.type == FORWARD_TIMING_SDI)
				io->hw_interlaced = io->hw_timings.sdi.interlaced;
			else if (io->hw_timings.type == FORWARD_TIMING_HDMI)
				io->hw_interlaced = io->hw_timings.hdmi.bt.interlaced;
		}

		return 0;
	}
	return -EINVAL;
}

static int forward_v4l2_c_enum_dv_timings(struct file *file, void *priv,
					  struct v4l2_enum_dv_timings *timings)
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)video_drvdata(file);
	struct forward_v4l2_io *io = iodev->io;
	int result;
	struct forward_v4l2_timings t;

	result = forward_v4l2_enum_timings(io, timings->index, &t);

	if (result)
		return result;

	forward_v4l2_fill_dv_timings(&t, &timings->timings);

	return 0;
}

static int forward_v4l2_c_query_dv_timings(struct file *file, void *priv,
					   struct v4l2_dv_timings *timings)
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)video_drvdata(file);
	struct forward_v4l2_io *io = iodev->io;
	struct forward_v4l2 *v4l2 = io->v4l2;
	struct forward_v4l2_timings t;
	int result;

	if (v4l2->ops->timings_query) {
		result = v4l2->ops->timings_query(io, &t);
		if (result)
			return result;

		if (!v4l2->ops->timings_valid(io, &t))
			return -ERANGE;

		forward_v4l2_fill_dv_timings(&t, timings);

		return 0;
	}

	return -EOPNOTSUPP;
}

static int forward_v4l2_c_g_dv_timings(struct file *file, void *priv,
				       struct v4l2_dv_timings *timings)
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)video_drvdata(file);
	struct forward_v4l2_io *io = iodev->io;

	forward_v4l2_fill_dv_timings(&io->timings, timings);

	return 0;
}

static int forward_v4l2_c_dv_timings_cap(struct file *file, void *priv,
					 struct v4l2_dv_timings_cap *cap)
{
	cap->type = V4L2_DV_BT_656_1120;
	cap->bt.capabilities = V4L2_DV_BT_CAP_INTERLACED | V4L2_DV_BT_CAP_PROGRESSIVE;
	cap->bt.max_height = 4096;
	cap->bt.max_width = 8192;
	cap->bt.min_height = 480;
	cap->bt.min_width = 720;
	cap->bt.max_pixelclock = 594000000;
	cap->bt.min_pixelclock = 13500000;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 9, 0)
	cap->bt.standards = V4L2_DV_BT_STD_SDI | V4L2_DV_BT_STD_CEA861;
#else
	cap->bt.standards = V4L2_DV_BT_STD_CEA861;
#endif

	return 0;
}

static int forward_v4l2_c_log_status(struct file *file, void *priv)
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)video_drvdata(file);
	struct forward_v4l2_io *io = iodev->io;
	struct device *dev = &iodev->vdev.dev;
	char prefix[64];

	snprintf(prefix, 64, "%s %s", dev_driver_string(dev), dev_name(dev));
	v4l2_ctrl_handler_log_status(&iodev->ctl, prefix);
	if (io->state == FORWARD_IO_TX)
		dev_info(dev, "Frame repeats: %d\n", io->stats.frame_drops);
	else
		dev_info(dev, "Frame drops: %d\n", io->stats.frame_drops);
	dev_info(dev, "Time without signal present: %dms\n", io->stats.no_signal_time);
	dev_info(dev, "HW frame discontinuous: %d\n", io->stats.hw_noseq_count);
	dev_info(dev, "SW frame discontinuous: %d\n", io->stats.sw_noseq_count);
	dev_info(dev, "Max IRQ period: %lluns\n", io->stats.max_irq_period);
	dev_info(dev, "Max processing delay: %lluns\n", io->stats.max_proc_delay);
	dev_info(dev, "Max processing time: %lluns\n", io->stats.max_proc_time);
	dev_info(dev, "Max processing period: %lluns\n", io->stats.max_proc_period);
	dev_info(dev, "Checksum/decode errors: %llu\n", io->stats.data_errors);

	return 0;
}

static int forward_v4l2_subscribe_event(struct v4l2_fh *fh,
					const struct v4l2_event_subscription *sub)
{
	struct forward_v4l2_io_dev *iodev =
		(struct forward_v4l2_io_dev *)video_get_drvdata(fh->vdev);
	struct forward_v4l2_io *io = iodev->io;

	if (io->state == FORWARD_IO_RX) {
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 16, 0)
		if (sub->type == V4L2_EVENT_SOURCE_CHANGE)
			return v4l2_event_subscribe(fh, sub, FORWARD_V4L2_EVENT_QUEUE_SIZE, NULL);
#endif
	}

	switch (sub->type) {
	case V4L2_EVENT_VSYNC:
		return v4l2_event_subscribe(fh, sub, FORWARD_V4L2_EVENT_QUEUE_SIZE, NULL);
	case V4L2_EVENT_FRAME_SYNC:
		return v4l2_event_subscribe(fh, sub, FORWARD_V4L2_EVENT_QUEUE_SIZE, NULL);
	case V4L2_EVENT_FORWARD_TIMESTAMP:
		return v4l2_event_subscribe(fh, sub, FORWARD_V4L2_EVENT_QUEUE_SIZE, NULL);
	default:
		return v4l2_ctrl_subscribe_event(fh, sub);
	}

	return v4l2_ctrl_subscribe_event(fh, sub);
}

static long forward_v4l2_c_default(struct file *file, void *fh, bool valid_prio, unsigned int cmd,
				   void *arg)
{
	struct forward_v4l2_io_dev *iodev = (struct forward_v4l2_io_dev *)video_drvdata(file);
	struct forward_v4l2_io *io = iodev->io;

	switch (cmd) {
	case VIDIOC_FORWARD_GET_IO_INDEX:
		*((int *)arg) = io->index;
		break;
	default:
		return -ENOTTY;
	}
	return 0;
}

static const struct v4l2_ioctl_ops forward_v4l2_ioctl_ops = {
	.vidioc_querycap = forward_v4l2_c_querycap,

	.vidioc_g_parm = forward_v4l2_c_g_parm,
	.vidioc_s_parm = forward_v4l2_c_s_parm,

	.vidioc_streamon = vb2_ioctl_streamon,
	.vidioc_streamoff = vb2_ioctl_streamoff,

	.vidioc_enum_input = forward_v4l2_c_enum_input,
	.vidioc_g_input = forward_v4l2_c_g_input,
	.vidioc_s_input = forward_v4l2_c_s_input,

	.vidioc_enum_output = forward_v4l2_c_enum_output,
	.vidioc_g_output = forward_v4l2_c_g_output,
	.vidioc_s_output = forward_v4l2_c_s_output,

	.vidioc_try_fmt_vid_cap = forward_v4l2_c_try_fmt,
	.vidioc_s_fmt_vid_cap = forward_v4l2_c_s_fmt,
	.vidioc_g_fmt_vid_cap = forward_v4l2_c_g_fmt,
	.vidioc_enum_fmt_vid_cap = forward_v4l2_c_enum_fmt,

	.vidioc_try_fmt_vbi_cap = forward_v4l2_c_try_fmt,
	.vidioc_s_fmt_vbi_cap = forward_v4l2_c_s_fmt,
	.vidioc_g_fmt_vbi_cap = forward_v4l2_c_g_fmt,

	.vidioc_try_fmt_vid_out = forward_v4l2_c_try_fmt,
	.vidioc_s_fmt_vid_out = forward_v4l2_c_s_fmt,
	.vidioc_g_fmt_vid_out = forward_v4l2_c_g_fmt,
	.vidioc_enum_fmt_vid_out = forward_v4l2_c_enum_fmt,

	.vidioc_try_fmt_vbi_out = forward_v4l2_c_try_fmt,
	.vidioc_s_fmt_vbi_out = forward_v4l2_c_s_fmt,
	.vidioc_g_fmt_vbi_out = forward_v4l2_c_g_fmt,

	.vidioc_enum_frameintervals = forward_v4l2_c_enum_frameintervals,

	.vidioc_s_dv_timings = forward_v4l2_c_s_dv_timings,
	.vidioc_g_dv_timings = forward_v4l2_c_g_dv_timings,
	.vidioc_enum_dv_timings = forward_v4l2_c_enum_dv_timings,
	.vidioc_query_dv_timings = forward_v4l2_c_query_dv_timings,
	.vidioc_dv_timings_cap = forward_v4l2_c_dv_timings_cap,

	.vidioc_reqbufs = vb2_ioctl_reqbufs,
	.vidioc_create_bufs = vb2_ioctl_create_bufs,
	.vidioc_querybuf = vb2_ioctl_querybuf,
	.vidioc_qbuf = vb2_ioctl_qbuf,
	.vidioc_dqbuf = vb2_ioctl_dqbuf,
	.vidioc_expbuf = vb2_ioctl_expbuf,

	.vidioc_log_status = forward_v4l2_c_log_status,
	.vidioc_subscribe_event = forward_v4l2_subscribe_event,
	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,

	.vidioc_default = forward_v4l2_c_default,
};

static int forward_v4l2_open(struct file *filp)
{
	return v4l2_fh_open(filp);
}

static int forward_v4l2_release(struct file *filp)
{
	return vb2_fop_release(filp);
}

static const struct v4l2_file_operations forward_v4l2_fops = { .owner = THIS_MODULE,
							       .open = forward_v4l2_open,
							       .release = forward_v4l2_release,
							       .read = vb2_fop_read,
							       .poll = vb2_fop_poll,
							       .unlocked_ioctl = video_ioctl2,
							       .mmap = vb2_fop_mmap };

static void forward_v4l2_io_reset_stats(struct forward_v4l2_io *io)
{
	io->stats.frame_drops = 0;
	io->stats.no_signal_time = 0;
	io->stats.hw_noseq_count = 0;
	io->stats.sw_noseq_count = 0;
	io->stats.max_irq_period = 0;
	io->stats.max_proc_delay = 0;
	io->stats.max_proc_time = 0;
	io->stats.max_proc_period = 0;
	io->stats.data_errors = 0;
	io->stats.last_irq_time = 0;
	io->stats.last_proc_time = 0;
	io->stats.alsa_resyncs = 0;
}

#define FORWARD_V4L2_IO_ATTR_RO(_name, _printf)                                          \
	static ssize_t forward_v4l2_io_stats_show_##_name(                               \
		struct device *dev, struct device_attribute *attr, char *buf)            \
	{                                                                                \
		struct video_device *vdev = container_of(dev, struct video_device, dev); \
		struct forward_v4l2_io_dev *iodev =                                      \
			container_of(vdev, struct forward_v4l2_io_dev, vdev);            \
		return scnprintf(buf, PAGE_SIZE, _printf, iodev->io->stats._name);       \
	}                                                                                \
	static DEVICE_ATTR(_name, S_IRUGO, forward_v4l2_io_stats_show_##_name, NULL);

FORWARD_V4L2_IO_ATTR_RO(frame_drops, "%d\n");
FORWARD_V4L2_IO_ATTR_RO(no_signal_time, "%d\n");
FORWARD_V4L2_IO_ATTR_RO(hw_noseq_count, "%d\n");
FORWARD_V4L2_IO_ATTR_RO(sw_noseq_count, "%d\n");
FORWARD_V4L2_IO_ATTR_RO(max_irq_period, "%llu\n");
FORWARD_V4L2_IO_ATTR_RO(max_proc_delay, "%llu\n");
FORWARD_V4L2_IO_ATTR_RO(max_proc_time, "%llu\n");
FORWARD_V4L2_IO_ATTR_RO(max_proc_period, "%llu\n");
FORWARD_V4L2_IO_ATTR_RO(data_errors, "%llu\n");
FORWARD_V4L2_IO_ATTR_RO(alsa_last_hw_samples, "%u\n");
FORWARD_V4L2_IO_ATTR_RO(alsa_last_sw_samples, "%u\n");
FORWARD_V4L2_IO_ATTR_RO(alsa_hw_samples, "%llu\n");
FORWARD_V4L2_IO_ATTR_RO(alsa_sw_samples, "%llu\n");
FORWARD_V4L2_IO_ATTR_RO(alsa_expected_samples, "%llu\n");
FORWARD_V4L2_IO_ATTR_RO(alsa_resyncs, "%u\n");

static ssize_t forward_v4l2_io_stats_reset(struct device *dev, struct device_attribute *attr,
					   const char *buf, size_t count)
{
	struct video_device *vdev = container_of(dev, struct video_device, dev);
	struct forward_v4l2_io_dev *iodev = container_of(vdev, struct forward_v4l2_io_dev, vdev);
	bool reset = 0;

	if (kstrtobool(buf, &reset) < 0)
		return -EINVAL;

	if (reset)
		forward_v4l2_io_reset_stats(iodev->io);

	return count;
}
static DEVICE_ATTR(reset, S_IWUSR | S_IWGRP, NULL, forward_v4l2_io_stats_reset);

static struct attribute *forward_v4l2_io_stats_attrs[] = { &dev_attr_frame_drops.attr,
							   &dev_attr_no_signal_time.attr,
							   &dev_attr_hw_noseq_count.attr,
							   &dev_attr_sw_noseq_count.attr,
							   &dev_attr_max_irq_period.attr,
							   &dev_attr_max_proc_delay.attr,
							   &dev_attr_max_proc_time.attr,
							   &dev_attr_max_proc_period.attr,
							   &dev_attr_data_errors.attr,
							   &dev_attr_alsa_last_hw_samples.attr,
							   &dev_attr_alsa_last_sw_samples.attr,
							   &dev_attr_alsa_hw_samples.attr,
							   &dev_attr_alsa_sw_samples.attr,
							   &dev_attr_alsa_expected_samples.attr,
							   &dev_attr_alsa_resyncs.attr,
							   &dev_attr_reset.attr,
							   NULL };

static struct attribute_group forward_v4l2_io_stats_group = {
	.name = "stats",
	.attrs = forward_v4l2_io_stats_attrs,
};

static void forward_v4l2_io_deinit_dev(struct forward_v4l2_io_dev *iodev)
{
	video_unregister_device(&iodev->vdev);
	forward_v4l2_ctrl_deinit(iodev);
	vb2_queue_release(&iodev->queue);
}

static int forward_v4l2_io_init_dev(struct forward_v4l2_io_dev *iodev)
{
	int status = 0;
	struct vb2_queue *q;
	struct video_device *vdev;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 16, 0)
	enum vfl_devnode_type devnode_type = 0;
#else
	int devnode_type = 0;
#endif

	mutex_init(&iodev->lock);
	spin_lock_init(&iodev->q_slock);
	INIT_LIST_HEAD(&iodev->buffers);

	q = &iodev->queue;
	q->type = iodev->type;
	q->io_modes = VB2_MMAP;
	q->drv_priv = iodev;
	q->buf_struct_size = sizeof(struct forward_v4l2_buffer);
	q->ops = &forward_v4l2_qops;
	q->mem_ops = &vb2_vmalloc_memops;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(3, 15, 0)
	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(6, 8, 0)
	q->min_queued_buffers = FORWARD_V4L2_MIN_BUFFERS;
#else
	q->min_buffers_needed = FORWARD_V4L2_MIN_BUFFERS;
#endif
#else
	q->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
#endif
	q->lock = &iodev->lock;

	status = vb2_queue_init(q);
	if (status)
		return status;

	vdev = &iodev->vdev;

	switch (iodev->type) {
	case V4L2_BUF_TYPE_VBI_CAPTURE:
	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
		snprintf(vdev->name, sizeof(vdev->name), "%s IN%d", iodev->io->v4l2->v4l2_dev.name,
			 iodev->io->number);
		vdev->vfl_dir = VFL_DIR_RX;
		break;
	case V4L2_BUF_TYPE_VBI_OUTPUT:
	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
		snprintf(vdev->name, sizeof(vdev->name), "%s OUT%d", iodev->io->v4l2->v4l2_dev.name,
			 iodev->io->number);
		vdev->vfl_dir = VFL_DIR_TX;
		break;
	default:
		break;
	}

#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 7, 0)
	switch (iodev->type) {
	case V4L2_BUF_TYPE_VBI_CAPTURE:
		vdev->device_caps = V4L2_CAP_VBI_CAPTURE | V4L2_CAP_STREAMING;
		break;
	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
		vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
		break;
	case V4L2_BUF_TYPE_VBI_OUTPUT:
		vdev->device_caps = V4L2_CAP_VBI_OUTPUT | V4L2_CAP_STREAMING;
		break;
	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
		vdev->device_caps = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
		break;
	default:
		break;
	}
#endif

	switch (iodev->type) {
	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
#if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 7, 0)
		devnode_type = VFL_TYPE_VIDEO;
#else
		devnode_type = VFL_TYPE_GRABBER;
#endif
		break;
	case V4L2_BUF_TYPE_VBI_OUTPUT:
	case V4L2_BUF_TYPE_VBI_CAPTURE:
		devnode_type = VFL_TYPE_VBI;
		break;
	default:
		break;
	}

	vdev->release = video_device_release_empty;
	vdev->fops = &forward_v4l2_fops;
	vdev->ioctl_ops = &forward_v4l2_ioctl_ops;
	vdev->lock = &iodev->lock;
	vdev->queue = q;
	vdev->v4l2_dev = &iodev->io->v4l2->v4l2_dev;
	vdev->tvnorms = 0;
	video_set_drvdata(vdev, iodev);

	switch (iodev->type) {
	case V4L2_BUF_TYPE_VBI_CAPTURE:
	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
		forward_v4l2_ctrl_init_cap(iodev);
		break;
	case V4L2_BUF_TYPE_VBI_OUTPUT:
	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
		forward_v4l2_ctrl_init_out(iodev);
		break;
	default:
		break;
	}

	status = video_register_device(vdev, devnode_type, -1);

	if (status)
		return status;

#if LINUX_VERSION_CODE >= KERNEL_VERSION(4, 14, 0)
	status = device_add_group(&vdev->dev, &forward_v4l2_io_stats_group);
#endif

	return 0;
}

static void forward_v4l2_io_process_bypass(struct work_struct *w)
{
	struct delayed_work *dwork = container_of(w, struct delayed_work, work);
	struct forward_v4l2_io *io = container_of(dwork, struct forward_v4l2_io, bypass_work);

	if (io->v4l2->ops->toggle_bypass)
		io->v4l2->ops->toggle_bypass(io, !io->bypass_disabled,
					     V4L2_FORWARD_BYPASS_WATCHDOG_PERIOD_MS);

	if (io->bypass_disabled)
		schedule_delayed_work(dwork,
				      HZ * (V4L2_FORWARD_BYPASS_WATCHDOG_PERIOD_MS / 2) / 1000);
}

void forward_v4l2_io_deinit(struct forward_v4l2_io *io)
{
	if (io->irq.private) {
		forward_irq_listener_remove(io->v4l2->dev, &io->irq);
		io->irq.private = NULL;
	}

	mutex_lock(&io->lock);
	io->ready = false;
	mutex_unlock(&io->lock);

	if (io->state == FORWARD_IO_DISABLED) {
		return;
	}

	if (io->v4l2->ops->toggle_bypass)
		cancel_delayed_work_sync(&io->bypass_work);

	if (io->v4l2->ops->fini_io)
		io->v4l2->ops->fini_io(io);

	forward_v4l2_io_deinit_dev(&io->vbi);
	forward_v4l2_io_deinit_dev(&io->video);

	if (io->genlock) {
		forward_v4l2_genlock_deinit(io->genlock);
		devm_kfree(io->v4l2->dev->dev, io->genlock);
		io->genlock = NULL;
	}

	devm_kfree(io->v4l2->dev->dev, io->mapped_buffers);
	io->mapped_buffers = NULL;
}

int forward_v4l2_io_init(struct forward_v4l2_io *io)
{
	int status = 0;

	if (io->state == FORWARD_IO_DISABLED)
		return 0;

	mutex_lock(&io->lock);

	// Zero forward_v4l2_io_dev struct because v4l2/kobject expects empty objects
	memset(&io->video, 0, sizeof(struct forward_v4l2_io_dev));
	memset(&io->vbi, 0, sizeof(struct forward_v4l2_io_dev));

	io->video.io = io;
	io->video.type = (io->state == FORWARD_IO_RX) ? V4L2_BUF_TYPE_VIDEO_CAPTURE :
							V4L2_BUF_TYPE_VIDEO_OUTPUT;
	io->vbi.io = io;
	io->vbi.type = (io->state == FORWARD_IO_RX) ? V4L2_BUF_TYPE_VBI_CAPTURE :
						      V4L2_BUF_TYPE_VBI_OUTPUT;

	INIT_DELAYED_WORK(&io->bypass_work, forward_v4l2_io_process_bypass);

	if (default_mute)
		io->mute = 1;
	else
		io->mute = 0;

	if (io->v4l2->ops->genlock_switch && (io->state == FORWARD_IO_TX)) {
		io->genlock = devm_kzalloc(io->v4l2->dev->dev, sizeof(struct forward_v4l2_genlock),
					   GFP_KERNEL);

		if (!io->genlock)
			goto fail;

		io->genlock->io = io;
		forward_v4l2_genlock_init(io->genlock);
	}

	status = forward_v4l2_io_init_dev(&io->video);
	if (status)
		goto fail;

	status = forward_v4l2_io_init_dev(&io->vbi);
	if (status)
		goto fail;

	io->hw_prev_buffer = -1;

	if (io->v4l2->ops->init_io) {
		status = io->v4l2->ops->init_io(io);
		if (status)
			goto fail;
	}

	if (io->v4l2->ops->toggle_mute)
		io->v4l2->ops->toggle_mute(io, io->mute);
	io->manual_mute = false;

	forward_v4l2_enum_timings(io, 0, &io->timings);
	io->v4l2->ops->timings_query(io, &io->timings);
	io->v4l2->ops->timings_set(io, &io->timings);
	forward_v4l2_fill_pix_fmt(&io->timings, &io->format);
	forward_v4l2_fill_vbi_fmt(&io->timings, &io->vbi_format);

	if (io->v4l2->ops->check_fmt)
		io->v4l2->ops->check_fmt(io, &io->format);
	if (io->v4l2->ops->set_fmt)
		io->v4l2->ops->set_fmt(io, &io->format);

	io->hw_timings = io->timings;
	if (io->hw_timings.type == FORWARD_TIMING_SDI)
		io->hw_interlaced = io->hw_timings.sdi.interlaced;
	else if (io->hw_timings.type == FORWARD_TIMING_HDMI)
		io->hw_interlaced = io->hw_timings.hdmi.bt.interlaced;

	io->mapped_buffers = devm_kzalloc(
		io->v4l2->dev->dev,
		io->v4l2->ops->hw_buffers * sizeof(struct forward_v4l2_io_buffers), GFP_KERNEL);

	io->ready = true;
	io->streaming = false;
	io->toggle_streaming = false;

	forward_v4l2_io_reset_stats(io);

	forward_irq_listener_init(&io->irq);
	io->irq.type = FORWARD_IRQ_LISTENER_WORKQUEUE | FORWARD_IRQ_LISTENER_CALLBACK;
	io->irq.mask = io->v4l2->ops->irq_mask(io);
	io->irq.func = &forward_v4l2_io_irq_func;
	io->irq.wq_func = &forward_v4l2_io_irq_wq_func;
	io->irq.private = io;
	io->irq.work_queue = io->index;
	io->irq.priority = FORWARD_V4L2_IRQ_PRIORITY;
	forward_irq_listener_add(io->v4l2->dev, &io->irq);

	mutex_unlock(&io->lock);

	return 0;

fail:
	mutex_unlock(&io->lock);
	forward_v4l2_io_deinit(io);
	return status;
}
