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

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

static void forward_v4l2_genlock_irq(void *priv, u32 irq, u64 timestamp)
{
	struct forward_v4l2_genlock *gl = priv;
	struct forward_v4l2_dev_ops *ops = gl->v4l2->ops;
	bool field;
	s32 offset;
	unsigned long sflags;

	ops->genlock_sync_irq_info(gl, 0, irq, &field, &offset);

	spin_lock_irqsave(&gl->lock, sflags);
	if (!field) {
		if (gl->state == V4L_FORWARD_GENLOCK_LOCKING) {
			int delta = (int)gl->ref_offset - (int)offset;
			if (abs(delta) <= 1)
				gl->stable_ref_count++;
			else
				gl->stable_ref_count = 0;

			if (gl->stable_ref_count > 100) {
				ops->genlock_sync_offset(gl, 0, gl->user_offset - offset, true);
				gl->state = V4L_FORWARD_GENLOCK_LOCKED;
			}
		} else {
			gl->stable_ref_count = 0;
		}
		gl->ref_offset = offset;
	}
	spin_unlock_irqrestore(&gl->lock, sflags);
}

static void forward_v4l2_genlock_switch(struct forward_v4l2_genlock *gl)
{
	struct forward_v4l2_dev_ops *ops = gl->v4l2->ops;
	struct forward_dev *dev = gl->v4l2->dev;
	struct forward_v4l2_timings new_timings;
	unsigned long sflags;

	if (ops->genlock_query(gl, gl->new_source, &new_timings)) {
		forward_warn(dev, "No genlock source signal, failing back\n");
		//gl->new_source = gl->source;
		spin_lock_irqsave(&gl->lock, sflags);
		gl->state = V4L_FORWARD_GENLOCK_NO_INPUT_SIGNAL;
		spin_unlock_irqrestore(&gl->lock, sflags);
		return;
	}

	if (ops->genlock_switch(gl, gl->new_source, &new_timings)) {
		forward_warn(dev, "Failed to configure hardware for genlock, failing back\n");
		gl->new_source = gl->source;
		return;
	}

	spin_lock_irqsave(&gl->lock, sflags);
	gl->timings = new_timings;
	gl->source = gl->new_source;
	gl->state = V4L_FORWARD_GENLOCK_LOCKING;
	spin_unlock_irqrestore(&gl->lock, sflags);

	// Asked by Komar
	if (gl->source != V4L_FORWARD_GENLOCK_SRC_MASTER) {
		int i = 0;

		for (i = 0; i < dev->cfg.io_count; i++) {
			if (gl->v4l2->io[i].state == FORWARD_IO_TX) {
				gl->v4l2->io[i].genlock = true;
				if (ops->genlock_sync_output)
					ops->genlock_sync_output(gl, &gl->v4l2->io[i]);
			}
		}
	}
}

void forward_v4l2_genlock_process(struct forward_v4l2_genlock *gl)
{
	struct forward_v4l2_dev_ops *ops = gl->v4l2->ops;
	unsigned long sflags;

	if (gl->source != gl->new_source)
		forward_v4l2_genlock_switch(gl);

	if (ops->genlock_query(gl, gl->source, &gl->in_timings))
		gl->in_timings_valid = false;
	else
		gl->in_timings_valid = true;

	if (gl->source == V4L_FORWARD_GENLOCK_SRC_MASTER) {
		spin_lock_irqsave(&gl->lock, sflags);
		gl->state = V4L_FORWARD_GENLOCK_MASTER;
		spin_unlock_irqrestore(&gl->lock, sflags);
		return;
	}

	spin_lock_irqsave(&gl->lock, sflags);
	if (!gl->in_timings_valid && (gl->state != V4L_FORWARD_GENLOCK_NO_INPUT_SIGNAL))
		gl->state = V4L_FORWARD_GENLOCK_HOLDOVER;
	else if (gl->in_timings_valid && (gl->state == V4L_FORWARD_GENLOCK_HOLDOVER))
		gl->state = V4L_FORWARD_GENLOCK_LOCKING;
	spin_unlock_irqrestore(&gl->lock, sflags);

	if (gl->in_timings_valid &&
	    !forward_v4l2_timings_equal(&gl->in_timings, &gl->timings, true))
		forward_v4l2_genlock_switch(gl);

	if (gl->user_offset != gl->new_user_offset) {
		ops->genlock_sync_offset(gl, 0, gl->new_user_offset - gl->user_offset, false);
		gl->user_offset = gl->new_user_offset;
	}
}

void forward_v4l2_genlock_deinit(struct forward_v4l2_genlock *gl)
{
	if (gl->listener.private)
		forward_irq_listener_remove(gl->v4l2->dev, &gl->listener);
	gl->listener.private = NULL;
}

int forward_v4l2_genlock_init(struct forward_v4l2_genlock *gl)
{
	struct forward_v4l2_dev_ops *ops = gl->v4l2->ops;

	spin_lock_init(&gl->lock);

	gl->state = V4L_FORWARD_GENLOCK_MASTER;

	forward_irq_listener_init(&gl->listener);
	gl->listener.type = FORWARD_IRQ_LISTENER_CALLBACK;
	gl->listener.mask = ops->genlock_sync_irq_mask(gl, 0);
	gl->listener.func = &forward_v4l2_genlock_irq;
	gl->listener.private = gl;
	forward_irq_listener_add(gl->v4l2->dev, &gl->listener);

	return 0;
}
