/*
   forward-v4l2-fd722.h - v4l2 driver for SoftLab-NSK FD722 video board

   Copyright (C) 2017 - 2023 Konstantin Oblaukhov <oblaukhov@sl.iae.nsk.su>

   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.h"
#include "forward-v4l2-io.h"

#include "forward-pll.h"

#include "fd722_reg.h"

#include "forward-v4l2-ioctl.h"
#include "forward-v4l2-genlock.h"

#include <linux/delay.h>

#define FD722_BUFFER_NUMBER_IN(in, irq) (((irq) >> ((in)*4 + 17)) & 0x7)
#define FD722_BUFFER_NUMBER_OUT(out, irq) (((irq) >> ((out)*2 + 5)) & 0x1)
#define FD722_BUFFER_NEXT(buf, offset, count) ((((unsigned int)(buf)) + (offset)) % (count))
#define FD722_NUMBER_IN_BUFFERS (8)
#define FD722_NUMBER_OUT_BUFFERS (2)

// 50 buffers per second at 270 Mbit/s
#define FD722_ASI_IN_BUFFER_SIZE (270000000ULL / 10 / 50)
#define FD722_ASI_IN_BUFFER_SIZE_RAW (270000000ULL / 8 / 50)

#define FD722_GENLOCK_ANALOG_DELAY 300
#define FD722_GENLOCK_SD_NTSC_DELAY 27953
#define FD722_GENLOCK_SD_PAL_DELAY 364
#define FD722_GENLOCK_HD_DELAY 189
#define FD722_GENLOCK_3G_DELAY 97

#define FD722_YC_SWAP_REVISION 861
#define FD722_ASI_PERIOD_REVISION 871
#define FD722_IS_VERSION_5(ver) (((ver >> 16) & 0xF) >= 6)

static void fd722_check_pll_configuration(struct forward_v4l2 *v4l2)
{
	FD722_ClockFrequency freq;
	bool clk_valid;
	bool clk_m;
	int i;
	bool any_m = false;
	struct forward_pll *pll = v4l2->dev->pll;

	if ((v4l2->genlock->source != V4L_FORWARD_GENLOCK_SRC_MASTER) ||
	    (v4l2->genlock->new_source != V4L_FORWARD_GENLOCK_SRC_MASTER))
		return;

	if (!pll)
		return;

	mutex_lock(&v4l2->dev->lock);

	freq = FD722_ClockFrequency_R(v4l2->dev->csr);
	clk_valid = (freq.frequency > 148200) && (freq.frequency < 148800);
	clk_m = (148500 - freq.frequency) > 75;

	for (i = 0; i < 2; i++) {
		struct forward_v4l2_timings *t = &v4l2->io[i + 2].timings;

		if (!clk_valid)
			clk_m = v4l2->io[i + 2].timings.sdi.m;

		if (t->sdi.m) {
			any_m = true;
			break;
		}
	}

	if ((clk_m != any_m) || !clk_valid) {
		struct forward_pll_mode *mode;
		mode = pll->ops->find_mode(pll, 0, 0, any_m, 148500000, any_m);

		if (!mode)
			return;

		pll->ops->switch_mode(pll, mode);
		pll->ops->calibrate(pll);

		for (i = 0; i < 2; i++) {
			FD722_VideoOutCS ocs;
			ocs = FD722_VideoOutCS_R(v4l2->dev->csr, i);
			ocs.clockM = clk_m ? 1 : 0;
			FD722_VideoOutCS_W(v4l2->dev->csr, i, ocs);
		}
	}

	mutex_unlock(&v4l2->dev->lock);
}

static u32 fd722_irq_mask(const struct forward_v4l2_io *io)
{
	if (io->index < 2)
		return (1 << (io->index * 4 + 16));
	else
		return (1 << ((io->index - 2) * 2 + 4));
}

inline bool fd722_irq_info_in(struct forward_v4l2_io *io, u32 irq, int *cur_buf, int *prev_buf,
			      int *next_buf)
{
	bool interlaced = io->timings.sdi.interlaced || io->asi;

	if (!(irq & (1 << (io->index * 4 + 16))))
		return false;

	*cur_buf = FD722_BUFFER_NUMBER_IN(io->index, irq);
	*prev_buf = FD722_BUFFER_NEXT(*cur_buf, interlaced ? -1 : -2, FD722_NUMBER_IN_BUFFERS);
	*next_buf = FD722_BUFFER_NEXT(*cur_buf, interlaced ? 1 : 2, FD722_NUMBER_IN_BUFFERS);

	return true;
}

inline bool fd722_irq_info_out(struct forward_v4l2_io *io, u32 irq, int *cur_buf, int *prev_buf,
			       int *next_buf)
{
	if (!(irq & (1 << ((io->index - 2) * 2 + 4))))
		return false;

	*cur_buf = FD722_BUFFER_NUMBER_OUT((io->index - 2), irq);
	*prev_buf = FD722_BUFFER_NEXT(*cur_buf, -1, FD722_NUMBER_OUT_BUFFERS);
	*next_buf = FD722_BUFFER_NEXT(*cur_buf, 1, FD722_NUMBER_OUT_BUFFERS);

	return true;
}

inline bool fd722_irq_info(struct forward_v4l2_io *io, u32 irq, int *cur_buf, int *prev_buf,
			   int *next_buf)
{
	if (io->state == FORWARD_IO_TX)
		return fd722_irq_info_out(io, irq, cur_buf, prev_buf, next_buf);
	else
		return fd722_irq_info_in(io, irq, cur_buf, prev_buf, next_buf);
}

static void fd722_region_info(const struct forward_v4l2_io *io, u32 *address, size_t *size)
{
	if (io->state == FORWARD_IO_TX) {
		*size = 0x01000000UL;
		*address = (io->index - 2) * 0x01000000UL + 0x05000000UL;
	} else {
		*size = 0x02000000UL;
		*address = io->index * 0x02000000UL;
	}
}

static void fd722_buffer_info(const struct forward_v4l2_io *io, int buffer, u32 *address,
			      size_t *size)
{
	if (io->state == FORWARD_IO_TX) {
		*size = 0x00800000UL;
		*address = (io->index - 2) * 0x01000000UL + buffer * *size + 0x05000000UL;
	} else {
		*size = 0x00400000UL;
		*address = io->index * 0x02000000UL + (buffer * *size);
	}
}

static bool fd722_timings_valid(const struct forward_v4l2_io *io,
				const struct forward_v4l2_timings *t)
{
	if (t->type != FORWARD_TIMING_SDI)
		return false;

	if ((t->sdi.mode != FORWARD_SDI_MODE_3G_SDI) && (t->sdi.mode != FORWARD_SDI_MODE_HD_SDI) &&
	    (t->sdi.mode != FORWARD_SDI_MODE_SD_SDI)) {
		return false;
	}

	return true;
}

static int fd722_timings_query(const struct forward_v4l2_io *io, struct forward_v4l2_timings *t)
{
	struct forward_dev *dev = io->v4l2->dev;
	enum forward_sdi_mode mode;
	FD722_VideoInCS ics = FD722_VideoInCS_R(dev->csr, io->index);
	FD722_VideoInLine line;
	FD722_VideoInPixel pixel;
	FD722_VideoInVPID vpid = FD722_VideoInVPID_R(dev->csr, io->index);
	FD722_VideoInFrameSize framesize = FD722_VideoInFrameSize_R(dev->csr, io->index);
	u32 pixel_clk;
	int width, height, active_width, active_height;
	bool result;

	if (io->state != FORWARD_IO_RX)
		return -ENOTSUPP;

	if (!ics.cd)
		return -ENOLINK;

	if (!ics.modeLocked || !ics.formatLocked)
		return -ENOLCK;

	switch (ics.mode) {
	case 0:
		mode = FORWARD_SDI_MODE_HD_SDI;
		break;
	case 1:
		mode = FORWARD_SDI_MODE_SD_SDI;
		break;
	case 2:
		mode = FORWARD_SDI_MODE_3G_SDI;
		break;
	default:
		mode = FORWARD_SDI_MODE_INVALID;
		break;
	}

	if (ics.mode == 3) {
		int i = 0;
		for (i = 0; i < forward_v4l2_timings_count; i++) {
			if (fd722_timings_valid(io, &forward_v4l2_timings[i])) {
				*t = forward_v4l2_timings[i];
				t->sdi.flags |= FORWARD_SDI_TIMINGS_F_ASI;
				break;
			}
		}
		return 0;
	}

	line = FD722_VideoInLine_R(dev->csr, io->index);
	pixel = FD722_VideoInPixel_R(dev->csr, io->index);
	vpid = FD722_VideoInVPID_R(dev->csr, io->index);
	if (!framesize.size)
		framesize.size = 1;
	width = pixel.totalPixels;
	height = line.totalLines;
	active_width = pixel.activePixels;
	active_height = line.activeLines;

	pixel_clk = 27000000ULL * width * height / framesize.size;

	result = forward_v4l2_timing_guess_sdi(mode, !ics.progressive, width, height, active_width,
					       active_height, vpid.vpid, pixel_clk, t);

	return result ? 0 : -ERANGE;
}

inline int fd722_timings_set_in(const struct forward_v4l2_io *io,
				const struct forward_v4l2_timings *t)
{
	if (!fd722_timings_valid(io, t))
		return -ERANGE;

	return 0;
}

inline int fd722_timings_set_out(const struct forward_v4l2_io *io,
				 const struct forward_v4l2_timings *t)
{
	// TODO: ASI
	struct forward_dev *dev = io->v4l2->dev;
	int index = io->index - 2;
	FD722_VideoOutCS ocs;
	FD722_VideoOutLine line;
	FD722_VideoOutPixel pixel;
	FD722_VideoOutStart start;
	FD722_VideoOutStop stop;
	FD722_VideoOutField field;
	FD722_VideoOutVPID vpid;
	bool asi = t->sdi.flags & FORWARD_SDI_TIMINGS_F_ASI;

	memset(&ocs, 0, sizeof(FD722_VideoOutCS));

	ocs.reset = 1;
	FD722_VideoOutCS_W(dev->csr, index, ocs);

	ocs.levelB = 0;

	switch (t->sdi.mode) {
	case FORWARD_SDI_MODE_SD_SDI:
		ocs.mode = 1;
		break;
	case FORWARD_SDI_MODE_HD_SDI:
		ocs.mode = 0;
		break;
	default:
		ocs.mode = 2;
		break;
	}

	if (asi)
		ocs.mode = 3;

	ocs.freeRunning = 1;
	ocs.progressive = (asi || t->sdi.interlaced) ? 0 : 1;

	if (asi) {
		line.totalLines = 540;
		pixel.totalPixels = 1000 >> io->asi_period;
		pixel.activePixels = 720 >> io->asi_period;
		start.startOdd = 11;
		start.startEven = 281;
		stop.stopOdd = 267;
		stop.stopEven = 537;
		field.switchOdd = 539;
		field.switchEven = 269;
		vpid.vpid = 0x00000000;
	} else {
		line.totalLines = t->sdi.height;
		pixel.totalPixels = t->sdi.width;
		pixel.activePixels = t->sdi.active_width;
		start.startOdd = t->sdi.active_starts[0];
		start.startEven = t->sdi.active_starts[1];
		stop.stopOdd = t->sdi.active_stops[0];
		stop.stopEven = t->sdi.active_stops[1];
		field.switchOdd = t->sdi.field_switch[0];
		field.switchEven = t->sdi.field_switch[1];
		vpid.vpid = forward_v4l2_timings_vpid(t, false, false, io->widescreen);
	}

	FD722_VideoOutLine_W(dev->csr, index, line);
	FD722_VideoOutPixel_W(dev->csr, index, pixel);
	FD722_VideoOutStart_W(dev->csr, index, start);
	FD722_VideoOutStop_W(dev->csr, index, stop);
	FD722_VideoOutField_W(dev->csr, index, field);
	FD722_VideoOutVPID_W(dev->csr, index, vpid);
	FD722_VideoOutCS_W(dev->csr, index, ocs);

	if (asi)
		ocs.playbackRaw = 1;
	else {
		bool gl_correct =
			forward_v4l2_timings_fps_divisible(t, &io->v4l2->genlock->timings);
		ocs.freeRunning = (io->genlock && gl_correct) ? 0 : 1;
	}

	ocs.syncSource = io->genlock_sync_gen;

	FD722_VideoOutCS_W(dev->csr, index, ocs);

	ocs.reset = 0;
	FD722_VideoOutCS_W(dev->csr, index, ocs);

	fd722_check_pll_configuration(io->v4l2);

	return 0;
}

static int fd722_timings_set(const struct forward_v4l2_io *io, const struct forward_v4l2_timings *t)
{
	if (io->state == FORWARD_IO_TX)
		return fd722_timings_set_out(io, t);
	else
		return fd722_timings_set_in(io, t);
}

static bool fd722_timings_changed(const struct forward_v4l2_io *io,
				  const struct forward_v4l2_timings *old,
				  struct forward_v4l2_timings *new)
{
	if (fd722_timings_query(io, new))
		return false;

	if ((old->sdi.flags & FORWARD_SDI_TIMINGS_F_ASI) ?
		    !(new->sdi.flags &FORWARD_SDI_TIMINGS_F_ASI) :
		    (new->sdi.flags &FORWARD_SDI_TIMINGS_F_ASI))
		return true;

	if (old->sdi.mode != new->sdi.mode)
		return true;

	return !forward_v4l2_timings_equal(new, old, true);
}

static void fd722_check_fmt(const struct forward_v4l2_io *io, struct v4l2_pix_format *f)
{
	struct forward_dev *dev = io->v4l2->dev;
	FD722_VideoInCS ics;

	if ((f->pixelformat != V4L2_PIX_FMT_MPEG) && (f->pixelformat != V4L2_PIX_FMT_UYVY) &&
	    (f->pixelformat != V4L2_PIX_FMT_YUYV) && (f->pixelformat != V4L2_PIX_FMT_V210) &&
	    (f->pixelformat != V4L2_PIX_FMT_SL_RAW) &&
	    (f->pixelformat != V4L2_PIX_FMT_SL_RAW_ASI)) {
		f->pixelformat = V4L2_PIX_FMT_UYVY;
		f->bytesperline = f->width * 2;
		f->sizeimage = f->height * f->bytesperline;
	}

	if (f->pixelformat == V4L2_PIX_FMT_MPEG) {
		f->bytesperline = 0;
		if ((dev->fw_version & 0xFFFF) >= FD722_ASI_PERIOD_REVISION)
			f->sizeimage = FD722_ASI_IN_BUFFER_SIZE >> (io->asi_period);
		else
			f->sizeimage = FD722_ASI_IN_BUFFER_SIZE;
	} else if (f->pixelformat == V4L2_PIX_FMT_SL_RAW_ASI) {
		f->bytesperline = 0;
		if ((dev->fw_version & 0xFFFF) >= FD722_ASI_PERIOD_REVISION)
			f->sizeimage = FD722_ASI_IN_BUFFER_SIZE_RAW >> (io->asi_period);
		else
			f->sizeimage = FD722_ASI_IN_BUFFER_SIZE_RAW;
	}

	if (io->state != FORWARD_IO_RX)
		return;

	ics = FD722_VideoInCS_R(dev->csr, io->index);

	if (!ics.cd || !ics.formatLocked || !ics.modeLocked)
		return;

	if (ics.mode == 3) {
		if (f->pixelformat == V4L2_PIX_FMT_SL_RAW)
			f->pixelformat = V4L2_PIX_FMT_SL_RAW_ASI;
		else if (f->pixelformat != V4L2_PIX_FMT_SL_RAW_ASI)
			f->pixelformat = V4L2_PIX_FMT_MPEG;

		if (f->pixelformat == V4L2_PIX_FMT_MPEG) {
			f->bytesperline = 0;
			if ((dev->fw_version & 0xFFFF) >= FD722_ASI_PERIOD_REVISION)
				f->sizeimage = FD722_ASI_IN_BUFFER_SIZE >> (io->asi_period);
			else
				f->sizeimage = FD722_ASI_IN_BUFFER_SIZE;
		} else if (f->pixelformat == V4L2_PIX_FMT_SL_RAW) {
			f->bytesperline = 0;
			if ((dev->fw_version & 0xFFFF) >= FD722_ASI_PERIOD_REVISION)
				f->sizeimage = FD722_ASI_IN_BUFFER_SIZE_RAW >> (io->asi_period);
			else
				f->sizeimage = FD722_ASI_IN_BUFFER_SIZE_RAW;
		}
	} else {
		if (f->pixelformat == V4L2_PIX_FMT_SL_RAW_ASI)
			f->pixelformat = V4L2_PIX_FMT_SL_RAW;

		if (f->pixelformat == V4L2_PIX_FMT_MPEG)
			f->pixelformat = V4L2_PIX_FMT_UYVY;

		if (f->pixelformat == V4L2_PIX_FMT_V210) {
			f->bytesperline = f->width * 8 / 3;
			f->bytesperline = ((f->bytesperline - 1) / 128 + 1) * 128;
		} else
			f->bytesperline = f->width * 2;

		f->sizeimage = f->height * f->bytesperline;
	}
}

static int fd722_enum_fmt(const struct forward_v4l2_io *io, int index, u32 *pix_fmt, char *desc,
			  size_t desc_len)
{
	(void)io;

	if (io->state == FORWARD_IO_RX) {
		switch (index) {
		case 0:
			*pix_fmt = V4L2_PIX_FMT_UYVY;
			snprintf(desc, desc_len, "UYVY 4:2:2");
			break;
		case 1:
			*pix_fmt = V4L2_PIX_FMT_YUYV;
			snprintf(desc, desc_len, "YUYV 4:2:2");
			break;
		case 2:
			*pix_fmt = V4L2_PIX_FMT_MPEG;
			snprintf(desc, desc_len, "MPEG-TS");
			break;
		case 3:
			*pix_fmt = V4L2_PIX_FMT_V210;
			snprintf(desc, desc_len, "YUYV 4:2:2 v210");
			break;
		case 4:
			*pix_fmt = V4L2_PIX_FMT_SL_RAW;
			snprintf(desc, desc_len, "SoftLab-NSK Raw");
			break;
		case 5:
			*pix_fmt = V4L2_PIX_FMT_SL_RAW_ASI;
			snprintf(desc, desc_len, "SoftLab-NSK Raw ASI");
			break;
		default:
			return -EINVAL;
		}
	} else {
		switch (index) {
		case 0:
			*pix_fmt = V4L2_PIX_FMT_UYVY;
			snprintf(desc, desc_len, "UYVY 4:2:2");
			break;
		case 1:
			*pix_fmt = V4L2_PIX_FMT_MPEG;
			snprintf(desc, desc_len, "MPEG-TS");
			break;
		case 2:
			*pix_fmt = V4L2_PIX_FMT_V210;
			snprintf(desc, desc_len, "YUYV 4:2:2 v210");
			break;
		case 3:
			*pix_fmt = V4L2_PIX_FMT_SL_RAW;
			snprintf(desc, desc_len, "SoftLab-NSK Raw");
			break;
		case 4:
			*pix_fmt = V4L2_PIX_FMT_SL_RAW_ASI;
			snprintf(desc, desc_len, "SoftLab-NSK Raw ASI");
			break;
		default:
			return -EINVAL;
		}
	}
	return 0;
}

static int fd722_update_meta(struct forward_v4l2_io *io)
{
	struct forward_dev *dev = io->v4l2->dev;

	if (io->state == FORWARD_IO_RX) {
		FD722_VideoInVPID vpid = FD722_VideoInVPID_R(dev->csr, io->index);

		if (((vpid.vpid >> 24) & 0xFF) == 0x81)
			io->widescreen = (vpid.vpid & 0x8000) ? true : false;
		else
			io->widescreen = true;
	} else {
		FD722_VideoOutVPID vpid = FD722_VideoOutVPID_R(dev->csr, io->index - 2);

		if (((vpid.vpid >> 24) & 0xFF) == 0x81) {
			if (io->widescreen)
				vpid.vpid |= 0x8000;
			else
				vpid.vpid &= ~(0x8000);

			FD722_VideoOutVPID_W(dev->csr, io->index - 2, vpid);
		}
	}

	return 0;
}

static void fd722_update_timecode(struct forward_v4l2_io *io, u64 timecode, u16 ddb)
{
	uint32_t *reg = io->v4l2->dev->csr + FD722_VideoOutATC_A(io->index - 2);
	iowrite32((0 << 30) | ((timecode >> 0) & 0x3FFFFFFF), reg);
	iowrite32((1 << 30) | ((timecode >> 30) & 0x3FFFFFFF), reg);
	iowrite32((2 << 30) | ((timecode >> 60) & 0xF) | (((u32)ddb << 4) & 0xFFF0), reg);
}

static void fd722_start(struct forward_v4l2_io *io)
{
	struct forward_dev *dev = io->v4l2->dev;

	if (io->state == FORWARD_IO_RX) {
		FD722_VideoInCS ics = FD722_VideoInCS_R(dev->csr, io->index);

		ics.test = 0;
		ics.vbi10b = 1;
		ics.vis10b = (io->format.pixelformat == V4L2_PIX_FMT_V210) ? 1 : 0;
		ics.vbiTop = io->vbi_en ? 1 : 0;
		ics.vbiBottom = 0;
		ics.captureRaw = (io->asi || io->raw) ? 1 : 0;
		if ((dev->fw_version & 0xFFFF) >= FD722_ASI_PERIOD_REVISION)
			ics.asiPeriod = io->asi_period;
		ics.ycSwap = (io->format.pixelformat == V4L2_PIX_FMT_YUYV) ? 1 : 0;

		if ((io->format.pixelformat == V4L2_PIX_FMT_YUYV) &&
		    (dev->fw_version & 0xFFFF) < FD722_YC_SWAP_REVISION) {
			io->format_conv = FORWARD_V4L2_CONV_UYVY_YUYV;
		} else if (io->format.pixelformat == V4L2_PIX_FMT_V210) {
			io->format_conv = FORWARD_V4L2_CONV_10BIT_V210;
		} else {
			io->format_conv = FORWARD_V4L2_CONV_NONE;
		}

		FD722_VideoInCS_W(dev->csr, io->index, ics);
	} else {
		FD722_VideoOutCS ocs = FD722_VideoOutCS_R(dev->csr, io->index - 2);
		FD722_VideoOutOddDataCount oddC;
		FD722_VideoOutEvenDataCount evenC;
		struct forward_v4l2_timings *t = &io->timings;

		ocs.playbackRaw = (io->asi || io->raw) ? 1 : 0;
		ocs.vbiTop = io->vbi_en ? 1 : 0;
		ocs.vbiBottom = 0;
		ocs.vis10b = (io->format.pixelformat == V4L2_PIX_FMT_V210) ? 1 : 0;
		ocs.vbi10b = 1;
		ocs.dataPresent = 1;
		ocs.test = 0;
		ocs.clone = 0;
		ocs.atcEnable = (io->atc_type != V4L2_FORWARD_TIMECODE_DISABLED) ? 1 : 0;
		ocs.atcType = (io->atc_type == V4L2_FORWARD_TIMECODE_LTC) ? 0 : 1;

		if (io->asi) {
			oddC.count = 675000;
			evenC.count = 675000;
		} else if (io->raw) {
			oddC.count = (t->sdi.field_switch[1] + 1) * t->sdi.width * 20 / 8;
			evenC.count = (t->sdi.field_switch[0] - t->sdi.field_switch[1]) *
				      t->sdi.width * 20 / 8;
		} else {
			if (io->format.pixelformat != V4L2_PIX_FMT_V210) {
				oddC.count = t->sdi.active_height[0] * t->sdi.active_width * 2;
				evenC.count = t->sdi.active_height[1] * t->sdi.active_width * 2;
			} else {
				oddC.count = t->sdi.active_height[0] * t->sdi.active_width * 20 / 8;
				evenC.count =
					t->sdi.active_height[1] * t->sdi.active_width * 20 / 8;
			}

			if (io->vbi_en) {
				oddC.count += t->sdi.topVBIs[0] * t->sdi.active_width * 20 / 8;
				evenC.count += t->sdi.topVBIs[1] * t->sdi.active_width * 20 / 8;
			}
		}

		FD722_VideoOutOddDataCount_W(dev->csr, io->index - 2, oddC);
		FD722_VideoOutEvenDataCount_W(dev->csr, io->index - 2, evenC);

		if (io->format.pixelformat == V4L2_PIX_FMT_V210) {
			io->format_conv = FORWARD_V4L2_CONV_10BIT_V210;
		} else {
			io->format_conv = FORWARD_V4L2_CONV_NONE;
		}

		FD722_VideoOutCS_W(dev->csr, io->index - 2, ocs);
	}
	dev->cfg.toggle_streaming(dev, io->index, true);
}

static void fd722_stop(struct forward_v4l2_io *io)
{
	struct forward_dev *dev = io->v4l2->dev;

	if (io->state == FORWARD_IO_TX) {
		FD722_VideoOutCS ocs = FD722_VideoOutCS_R(dev->csr, io->index - 2);
		ocs.atcEnable = 0;
		FD722_VideoOutCS_W(dev->csr, io->index - 2, ocs);
	}

	dev->cfg.toggle_streaming(dev, io->index, false);
}

static bool fd722_signal_present(const struct forward_v4l2_io *io)
{
	struct forward_dev *dev = io->v4l2->dev;
	FD722_VideoInCS ics = FD722_VideoInCS_R(dev->csr, io->index);
	return ics.cd ? true : false;
}

static u32 fd722_hw_timestamp(struct forward_v4l2 *v4l2)
{
	struct forward_dev *dev = v4l2->dev;
	return FD722_VideoClockCounter_R(dev->csr).counter;
}

static s64 fd722_clock_deviation(struct forward_v4l2 *v4l2)
{
	struct forward_dev *dev = v4l2->dev;
	FD722_ClockDeviation dv = FD722_ClockDeviation_R(dev->csr);
	u64 deviation = dv.deviation * 1000000000000ULL / 2818572288ULL;

	return dv.direction ? -deviation : deviation;
}

static s64 fd722_set_clock_deviation(struct forward_v4l2 *v4l2, s64 ppt)
{
	struct forward_dev *dev = v4l2->dev;
	if (FD722_IS_VERSION_5(dev->fw_version)) {
		return v4l2->dev->pll->ops->tune(v4l2->dev->pll, ppt);
	} else {
		FD722_ClockDeviation dv;
		u64 value = ((ppt < 0) ? -ppt : ppt) * 2818572288ULL / 1000000000000ULL;

		dv.deviation = (value > 1290555) ? 1290555 : value;
		dv.direction = (ppt < 0) ? 1 : 0;

		FD722_ClockDeviation_W(dev->csr, dv);

		value = dv.deviation * 1000000000000ULL / 2818572288ULL;
		return (ppt < 0) ? -value : value;
	}
}

static u32 fd722_io_timestamp(const struct forward_v4l2_io *io)
{
	struct forward_dev *dev = io->v4l2->dev;
	u32 ts;
	if (io->state == FORWARD_IO_RX)
		ts = FD722_VideoInIRQRefTime_R(dev->csr, io->index).time;
	else
		ts = 0;
	return ts;
}

static int fd722_genlock_query(const struct forward_v4l2_genlock *gl,
			       enum v4l2_forward_genlock_source src, struct forward_v4l2_timings *t)
{
	struct forward_v4l2 *v4l2 = gl->v4l2;

	if (src == V4L_FORWARD_GENLOCK_SRC_MASTER) {
		*t = v4l2->io[2].timings;
		return 0;
	} else if (src == V4L_FORWARD_GENLOCK_SRC_IN0)
		return fd722_timings_query(&v4l2->io[0], t);
	else if (src == V4L_FORWARD_GENLOCK_SRC_IN1)
		return fd722_timings_query(&v4l2->io[1], t);
	else if (src == V4L_FORWARD_GENLOCK_SRC_ANALOG) {
		FD722_AnalogSync cnt = FD722_AnalogSync_R(v4l2->dev->csr);

		if ((cnt.hsync > 1024) || (cnt.vsync > 1000000) || (cnt.hsync == 0) ||
		    (cnt.vsync == 0))
			return -ENOLINK;

		return forward_v4l2_timing_guess_analog(13500000 / cnt.hsync,
							13500000000 / cnt.vsync, t) ?
			       0 :
			       -ERANGE;
	} else
		return -EINVAL;
}

static int fd722_genlock_switch(const struct forward_v4l2_genlock *gl,
				enum v4l2_forward_genlock_source src,
				struct forward_v4l2_timings *t)
{
	struct forward_v4l2 *v4l2 = gl->v4l2;
	struct forward_pll *pll = v4l2->dev->pll;
	FD722_SyncGenerator sg = { 0 };
	FD722_SyncGeneratorField sgf = { 0 };
	struct forward_pll_mode *mode = NULL;
	bool v5 = FD722_IS_VERSION_5(v4l2->dev->fw_version);

	if (src == V4L_FORWARD_GENLOCK_SRC_MASTER) {
		mode = pll->ops->find_mode(pll, 0, 0, false, 148500000, false);
		pll->ops->switch_mode(pll, mode);
		if (v5)
			pll->ops->select_input(pll, 0, true, false);
		else
			pll->ops->select_input(pll, 1, false, false);
		pll->ops->calibrate(pll);
	} else {
		int pll_pix_freq = 0;
		int pll_h_freq = 0;
		FD722_Genlock gl = FD722_Genlock_R(v4l2->dev->csr);

		pll_h_freq = forward_v4l2_pixel_clock(t) / t->sdi.width;
		if (src == V4L_FORWARD_GENLOCK_SRC_ANALOG) {
			gl.mode = (t->sdi.mode == FORWARD_SDI_MODE_SD_SDI) ? 1 : 0;
		} else if ((src == V4L_FORWARD_GENLOCK_SRC_IN0) ||
			   (src == V4L_FORWARD_GENLOCK_SRC_IN1)) {
			if ((t->sdi.mode == FORWARD_SDI_MODE_SD_SDI) ||
			    (t->sdi.flags & FORWARD_SDI_TIMINGS_F_ASI))
				pll_pix_freq = v5 ? 13500000 : 6750000;
			else if (t->sdi.mode == FORWARD_SDI_MODE_HD_SDI)
				pll_pix_freq = 74250000;
			else if (t->sdi.mode == FORWARD_SDI_MODE_3G_SDI)
				pll_pix_freq = 148500000;
			gl.mode = (src == V4L_FORWARD_GENLOCK_SRC_IN1) ? 3 : 2;
		}

		mode = pll->ops->find_mode(pll, pll_pix_freq, pll_h_freq, t->sdi.m, 148500000,
					   t->sdi.m);
		if (!mode)
			return -EINVAL;

		FD722_Genlock_W(v4l2->dev->csr, gl);
		pll->ops->switch_mode(pll, mode);

		if (v5) {
			int input = 0;
			switch (src) {
			case V4L_FORWARD_GENLOCK_SRC_ANALOG:
				input = 0;
				break;
			case V4L_FORWARD_GENLOCK_SRC_IN0:
				input = 1;
				break;
			case V4L_FORWARD_GENLOCK_SRC_IN1:
				input = 2;
				break;
			default:
				input = 0;
				break;
			}
			pll->ops->select_input(pll, input, false, false);
			pll->ops->calibrate(pll);
		} else {
			if ((src == V4L_FORWARD_GENLOCK_SRC_IN0) ||
			    (src == V4L_FORWARD_GENLOCK_SRC_IN1)) {
				// Hack to avoid PLL->IN->PLL loop stuck
				FD722_VideoInCS cs;
				int count = 0;

				// Force Master clock
				pll->ops->select_input(pll, 1, false, false);
				pll->ops->calibrate(pll);

				// Wait for input
				do {
					msleep(100);
					cs = FD722_VideoInCS_R(
						v4l2->dev->csr,
						(src == V4L_FORWARD_GENLOCK_SRC_IN1) ? 1 : 0);
					count++;
				} while ((!cs.modeLocked) && (count < 100));

				// Switch to input clock
				pll->ops->select_input(pll, 0, false, true);
			} else {
				pll->ops->select_input(pll, 0, false, true);
				pll->ops->calibrate(pll);
			}
		}
	}

	sg.frameSize = t->sdi.width * t->sdi.height;
	if (t->sdi.mode == FORWARD_SDI_MODE_SD_SDI)
		sg.frameSize *= 11;
	else if (t->sdi.mode == FORWARD_SDI_MODE_HD_SDI)
		sg.frameSize *= 2;
	if (t->sdi.interlaced)
		sgf.fieldSize = sg.frameSize / 2;
	else
		sgf.fieldSize = sg.frameSize;

	if (!sg.frameSize || !sgf.fieldSize) {
		sg.frameSize = 5940000;
		sgf.fieldSize = 2970000;
	}

	FD722_SyncGenerator_W(v4l2->dev->csr, 0, sg);
	FD722_SyncGeneratorField_W(v4l2->dev->csr, 0, sgf);

	return 0;
}

u32 fd722_genlock_sync_irq_mask(const struct forward_v4l2_genlock *gl, int num)
{
	return num ? (1 << 10) : (1 << 8);
}

bool fd722_genlock_sync_irq_info(const struct forward_v4l2_genlock *gl, int num, u32 irq,
				 bool *field, s32 *gl_offset)
{
	struct forward_v4l2 *v4l2 = gl->v4l2;
	u32 ref_time, sync_time;

	if (!(irq & (num ? (1 << 10) : (1 << 8))))
		return false;

	*field = (irq & (num ? (1 << 11) : (1 << 9))) ? true : false;
	sync_time = FD722_SyncGeneratorTime_R(v4l2->dev->csr, num).time;

	if (gl->source == V4L_FORWARD_GENLOCK_SRC_IN0)
		ref_time = FD722_VideoInIRQRefTime_R(v4l2->dev->csr, 0).time;
	else if (gl->source == V4L_FORWARD_GENLOCK_SRC_IN1)
		ref_time = FD722_VideoInIRQRefTime_R(v4l2->dev->csr, 1).time;
	else if (gl->source == V4L_FORWARD_GENLOCK_SRC_ANALOG)
		ref_time = FD722_AnalogVSyncTime_R(v4l2->dev->csr).time;

	*gl_offset = (s32)(sync_time - ref_time);

	return true;
}

void fd722_genlock_sync_offset(const struct forward_v4l2_genlock *gl, int num, s32 offset,
			       bool comp_delay)
{
	s32 frame_size = gl->timings.sdi.height * gl->timings.sdi.width;
	FD722_SyncGeneratorPhase ph;
	s32 sync_gen_offset = 0;

	if (gl->source == V4L_FORWARD_GENLOCK_SRC_ANALOG) {
		if (gl->timings.sdi.height == 1125)
			sync_gen_offset =
				-(gl->timings.sdi.width - gl->timings.sdi.active_width - 192);
		else if (gl->timings.sdi.height == 750)
			sync_gen_offset =
				-(gl->timings.sdi.width - gl->timings.sdi.active_width - 260);
		else if (gl->timings.sdi.height == 625)
			sync_gen_offset = -12 - 46;
		else if (gl->timings.sdi.height == 525)
			sync_gen_offset = -16 - 44;

		sync_gen_offset += -gl->timings.sdi.width;
		if (!gl->timings.sdi.interlaced)
			sync_gen_offset += -gl->timings.sdi.width;
	}

	if (gl->timings.sdi.mode == FORWARD_SDI_MODE_SD_SDI) {
		frame_size *= 11;
		sync_gen_offset *= 11;
	} else if (gl->timings.sdi.mode == FORWARD_SDI_MODE_HD_SDI) {
		frame_size *= 2;
		sync_gen_offset *= 2;
	}

	if (gl->source == V4L_FORWARD_GENLOCK_SRC_ANALOG)
		sync_gen_offset += -FD722_GENLOCK_ANALOG_DELAY;
	else {
		if (gl->timings.sdi.mode == FORWARD_SDI_MODE_SD_SDI)
			sync_gen_offset += (gl->timings.sdi.height == 525) ?
						   -FD722_GENLOCK_SD_NTSC_DELAY :
						   -FD722_GENLOCK_SD_PAL_DELAY;
		else if (gl->timings.sdi.mode == FORWARD_SDI_MODE_HD_SDI)
			sync_gen_offset += -FD722_GENLOCK_HD_DELAY;
		else
			sync_gen_offset += -FD722_GENLOCK_3G_DELAY;
	}

	if (!comp_delay)
		sync_gen_offset = 0;

	offset += sync_gen_offset;

	if (offset > frame_size)
		offset = offset % frame_size;
	if (offset < 0)
		offset = (offset % frame_size) + frame_size;

	ph.phase = offset;
	FD722_SyncGeneratorPhase_W(gl->v4l2->dev->csr, num, ph);
}

void fd722_genlock_sync_output(const struct forward_v4l2_genlock *gl, struct forward_v4l2_io *io)
{
	FD722_VideoOutCS cs = FD722_VideoOutCS_R(io->v4l2->dev->csr, (io->index - 2));
	bool correct = forward_v4l2_timings_fps_divisible(&gl->timings, &io->timings);
	cs.freeRunning = (io->genlock && correct) ? 0 : 1;
	cs.syncSource = io->genlock_sync_gen;
	FD722_VideoOutCS_W(io->v4l2->dev->csr, (io->index - 2), cs);
}

struct forward_v4l2_dev_ops fd722_ops = {
	.irq_mask = fd722_irq_mask,
	.irq_info = fd722_irq_info,
	.region_info = fd722_region_info,
	.buffer_info = fd722_buffer_info,
	.timings_valid = fd722_timings_valid,
	.timings_set = fd722_timings_set,
	.timings_query = fd722_timings_query,
	.timings_changed = fd722_timings_changed,
	.check_fmt = fd722_check_fmt,
	.enum_fmt = fd722_enum_fmt,
	.update_meta = fd722_update_meta,
	.update_timecode = fd722_update_timecode,
	.start = fd722_start,
	.stop = fd722_stop,
	.signal_present = fd722_signal_present,
	.hw_buffers = FD722_NUMBER_IN_BUFFERS,
	.vbi_support = true,

	.io_timestamp = fd722_io_timestamp,
	.hw_timestamp = fd722_hw_timestamp,
	.clock_deviation = fd722_clock_deviation,
	.set_clock_deviation = fd722_set_clock_deviation,

	.genlock_query = fd722_genlock_query,
	.genlock_switch = fd722_genlock_switch,
	.genlock_sync_irq_mask = fd722_genlock_sync_irq_mask,
	.genlock_sync_irq_info = fd722_genlock_sync_irq_info,
	.genlock_sync_offset = fd722_genlock_sync_offset,
	.genlock_sync_output = fd722_genlock_sync_output,
};
