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

#define GENLOCK_UNLOCK_OFFSET_THRESHOLD 5280

static void forward_v4l2_genlock_process_analog(struct forward_v4l2_analog_in *a)
{
	int result;

	if (!a)
		return;

	if (a->mode != a->new_mode) {
		result = a->v4l2->ops->analog_rx_set_mode(a->v4l2, a->new_mode);
		if (result) {
			a->new_mode = a->mode;
			forward_err(a->v4l2->dev, "Cannot switch analog rx mode: %d", result);
		} else {
			if (a->new_mode != V4L_FORWARD_ANALOG_RX_MODE_GENLOCK)
				a->timings.type = FORWARD_TIMING_INVALID;
			a->mode = a->new_mode;
		}
	}

	if (a->mode == V4L_FORWARD_ANALOG_RX_MODE_GENLOCK) {
		struct forward_v4l2_timings new_tim;
		if (!a->v4l2->ops->analog_rx_video_query(a->v4l2, &new_tim)) {
			a->no_signal = false;
			a->timings_changed =
				!forward_v4l2_timings_equal(&a->timings, &new_tim, false);
			a->timings = new_tim;
		} else {
			a->no_signal = true;
		}
	}
}

void forward_v4l2_genlock_process(struct forward_v4l2 *v4l2)
{
	int i;
	bool need_analog = false;
	struct forward_v4l2_analog_in *a = v4l2->analog;
	int status;

	forward_v4l2_genlock_process_analog(a);

	for (i = 0; i < v4l2->dev->cfg.io_count; i++) {
		struct forward_v4l2_genlock *gl = v4l2->io[i].genlock;
		if (!gl)
			continue;
		if (gl->source == V4L_FORWARD_GENLOCK_SRC_ANALOG)
			need_analog = true;
	}

	if (a && (a->mode == V4L_FORWARD_ANALOG_RX_MODE_GENLOCK) && !a->no_signal && need_analog) {
		if (!a->non_master || a->timings_changed) {
			status = v4l2->ops->genlock_set_global_mode(v4l2, false, &a->timings);
			if (status)
				forward_err(v4l2->dev,
					    "Cannot configure board for analog genlock: %d",
					    status);
			a->timings_changed = false;
			a->non_master = true;
		}
	} else if (!need_analog && a->non_master) {
		status = v4l2->ops->genlock_set_global_mode(v4l2, true, NULL);
		if (status)
			forward_err(v4l2->dev, "Cannot configure board for master: %d", status);
		a->non_master = false;
	}
}

static void forward_v4l2_genlock_switch(struct forward_v4l2_genlock *gl)
{
	struct forward_v4l2_io *io = gl->io;
	struct forward_v4l2 *v4l2 = io->v4l2;
	struct forward_v4l2_dev_ops *ops = v4l2->ops;
	struct forward_dev *dev = v4l2->dev;
	struct forward_v4l2_analog_in *analog = v4l2->analog;
	bool no_signal;
	unsigned long sflags;

	if (gl->new_source == V4L_FORWARD_GENLOCK_SRC_ANALOG) {
		no_signal = analog->no_signal;
	} else if (gl->new_source >= V4L_FORWARD_GENLOCK_SRC_IN0) {
		int index = (int)gl->new_source - (int)V4L_FORWARD_GENLOCK_SRC_IN0;
		if (index < dev->cfg.io_count)
			no_signal = ops->timings_query(&v4l2->io[index], NULL);
	}

	if ((gl->new_source != V4L_FORWARD_GENLOCK_SRC_MASTER) &&
	    (gl->state != V4L_FORWARD_GENLOCK_NO_INPUT_SIGNAL) && no_signal) {
		forward_warn(dev, "OUT%d: No genlock source signal, failing back\n", io->number);
		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)) {
		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->source = gl->new_source;
	gl->state = V4L_FORWARD_GENLOCK_LOCKING;
	spin_unlock_irqrestore(&gl->lock, sflags);
}

void forward_v4l2_genlock_process_io(struct forward_v4l2_genlock *gl)
{
	struct forward_v4l2_dev_ops *ops = gl->io->v4l2->ops;
	unsigned long sflags;
	bool no_signal;

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

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

	if ((gl->source == gl->new_source) && (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;
	}

	if (gl->source == V4L_FORWARD_GENLOCK_SRC_ANALOG) {
		no_signal = gl->io->v4l2->analog->no_signal;
	} else if (gl->source >= V4L_FORWARD_GENLOCK_SRC_IN0) {
		int index = (int)gl->source - (int)V4L_FORWARD_GENLOCK_SRC_IN0;
		if (index < gl->io->v4l2->dev->cfg.io_count)
			no_signal = ops->timings_query(&gl->io->v4l2->io[index], NULL);
	}

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

void forward_v4l2_genlock_process_out_irq(struct forward_v4l2_genlock *gl, bool field)
{
	struct forward_v4l2_dev_ops *ops = gl->io->v4l2->ops;
	u32 in_time, out_time, now;
	u64 now_sys;
	bool in_time_valid;
	s32 offset;
	unsigned long sflags;

	if (gl->source == V4L_FORWARD_GENLOCK_SRC_ANALOG) {
		ops->analog_rx_timestamp(gl->io->v4l2, &in_time_valid, &in_time, &now, &now_sys);
	} else if (gl->source >= V4L_FORWARD_GENLOCK_SRC_IN0) {
		int index = (int)gl->source - (int)V4L_FORWARD_GENLOCK_SRC_IN0;
		if (index < gl->io->v4l2->dev->cfg.io_count)
			in_time = gl->io->v4l2->io[index].frame_hw_time;
	}

	out_time = gl->io->frame_hw_time;
	offset = (s32)(out_time - in_time);

	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, gl->user_offset - offset, true);
				gl->state = V4L_FORWARD_GENLOCK_LOCKED;
			}
		} else if ((gl->state == V4L_FORWARD_GENLOCK_LOCKED) &&
			   ((offset - gl->user_offset) > GENLOCK_UNLOCK_OFFSET_THRESHOLD)) {
			gl->state = V4L_FORWARD_GENLOCK_LOCKING;
			gl->stable_ref_count = 0;
		} else {
			gl->stable_ref_count = 0;
		}
		gl->ref_offset = offset;
	}
	spin_unlock_irqrestore(&gl->lock, sflags);
}

void forward_v4l2_genlock_deinit(struct forward_v4l2_genlock *gl)
{
}

int forward_v4l2_genlock_init(struct forward_v4l2_genlock *gl)
{
	spin_lock_init(&gl->lock);
	gl->state = V4L_FORWARD_GENLOCK_MASTER;
	forward_v4l2_genlock_switch(gl);

	return 0;
}
