From ffca9b4224bda6b101c8747f2853c64737d88bf3 Mon Sep 17 00:00:00 2001
From: Konstantin Oblaukhov <oblaukhov.konstantin@gmail.com>
Date: Wed, 1 Mar 2023 10:11:21 +0700
Subject: [PATCH 01/13] Fix small bug with devices without v4l2.

---
 forward-v4l2/forward-v4l2-core.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/forward-v4l2/forward-v4l2-core.c b/forward-v4l2/forward-v4l2-core.c
index f4d9f1e..ffda81b 100644
--- a/forward-v4l2/forward-v4l2-core.c
+++ b/forward-v4l2/forward-v4l2-core.c
@@ -104,6 +104,8 @@ static int forward_v4l2_probe(struct forward_dev *dev, void **private)
 	v4l2->dev = dev;
 	*private = v4l2;
 
+	INIT_DELAYED_WORK(&v4l2->work, forward_v4l2_work);
+
 	switch (dev->type) {
 	case FORWARD_FD788:
 		v4l2->ops = &fd788_ops;
@@ -128,8 +130,6 @@ static int forward_v4l2_probe(struct forward_dev *dev, void **private)
 
 	snprintf(v4l2->v4l2_dev.name, sizeof(v4l2->v4l2_dev.name), "%s", v4l2->dev->name);
 
-	INIT_DELAYED_WORK(&v4l2->work, forward_v4l2_work);
-
 	result = v4l2_device_register(v4l2->dev->dev, &v4l2->v4l2_dev);
 	if (result)
 		goto fail;
-- 
2.39.1


From e2b2c4cc73574975329d82cede141438e8d7344b Mon Sep 17 00:00:00 2001
From: Konstantin Oblaukhov <oblaukhov.konstantin@gmail.com>
Date: Sat, 11 Mar 2023 21:14:43 +0700
Subject: [PATCH 02/13] Fix Ubuntu DKMS updates.

---
 dkms.conf | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/dkms.conf b/dkms.conf
index 86efcf8..dca6c47 100644
--- a/dkms.conf
+++ b/dkms.conf
@@ -12,3 +12,5 @@ DEST_MODULE_LOCATION[1]="/extra"
 BUILT_MODULE_NAME[2]="forward-alsa"
 BUILT_MODULE_LOCATION[2]="forward-alsa/"
 DEST_MODULE_LOCATION[2]="/extra"
+
+AUTOINSTALL="yes"
-- 
2.39.1


From cb7c32d8251173e654fc2aa1b7b640ab8e8a3b76 Mon Sep 17 00:00:00 2001
From: Konstantin Oblaukhov <oblaukhov.konstantin@gmail.com>
Date: Sat, 11 Mar 2023 21:16:12 +0700
Subject: [PATCH 03/13] PLL Abstraction.

---
 forward-alsa/forward-alsa-core.c              |   1 +
 .../forward-passthrough-core.c                |   1 +
 .../forward-passthrough-fd722.c               |  27 +--
 .../forward-passthrough-fd788.c               |  27 +--
 forward-v4l2/forward-v4l2-core.c              |   1 +
 forward-v4l2/forward-v4l2-fd722.c             |  55 ++---
 forward-v4l2/forward-v4l2-fd788.c             |  55 ++---
 forward/forward-core.c                        |  14 ++
 forward/forward-fd722.c                       |  14 +-
 forward/forward-fd788.c                       |  15 +-
 forward/forward-si532x.c                      | 206 ++++++++++++------
 .../forward-si532x.h                          |  16 +-
 include/forward-fd788.h                       |  31 ---
 include/forward-pll.h                         |  53 +++++
 include/forward-si532x.h                      |  60 -----
 include/forward.h                             |   2 +
 16 files changed, 305 insertions(+), 273 deletions(-)
 rename include/forward-fd722.h => forward/forward-si532x.h (74%)
 delete mode 100644 include/forward-fd788.h
 create mode 100644 include/forward-pll.h
 delete mode 100644 include/forward-si532x.h

diff --git a/forward-alsa/forward-alsa-core.c b/forward-alsa/forward-alsa-core.c
index c835be8..831c46e 100644
--- a/forward-alsa/forward-alsa-core.c
+++ b/forward-alsa/forward-alsa-core.c
@@ -209,6 +209,7 @@ static void forward_alsa_io_changed(struct forward_dev *dev, struct forward_io *
 
 static struct forward_extension forward_alsa_ext = {
 	.name = FORWARD_ALSA_MODULE_NAME,
+	.version = FORWARD_VERSION_CODE,
 	.probe = forward_alsa_probe,
 	.reprobe = forward_alsa_reprobe,
 	.remove = forward_alsa_remove,
diff --git a/forward-passthrough/forward-passthrough-core.c b/forward-passthrough/forward-passthrough-core.c
index cfbe176..7cde52d 100644
--- a/forward-passthrough/forward-passthrough-core.c
+++ b/forward-passthrough/forward-passthrough-core.c
@@ -499,6 +499,7 @@ static void forward_passthrough_io_changed(struct forward_dev *dev, struct forwa
  */
 static struct forward_extension forward_passthrough_ext = {
 	.name = FORWARD_PASSTHROUGH_MODULE_NAME,
+	.version = FORWARD_VERSION_CODE,
 	.probe = forward_passthrough_probe,
 	.reprobe = forward_passthrough_reprobe,
 	.remove = forward_passthrough_remove,
diff --git a/forward-passthrough/forward-passthrough-fd722.c b/forward-passthrough/forward-passthrough-fd722.c
index a4c1594..2ff93ce 100644
--- a/forward-passthrough/forward-passthrough-fd722.c
+++ b/forward-passthrough/forward-passthrough-fd722.c
@@ -19,8 +19,7 @@
 */
 
 #include "forward-passthrough.h"
-#include "forward-si532x.h"
-#include "forward-fd722.h"
+#include "forward-pll.h"
 #include "fd722_reg.h"
 
 #include <linux/types.h>
@@ -278,11 +277,11 @@ static void fd722_configure_io(struct forward_passthrough_io *io)
 static void fd722_configure_genlock(struct forward_passthrough_io *io, bool analog)
 {
 	struct forward_passthrough *pass = io->pass;
-	struct forward_si532x *pll = &((struct forward_fd722 *)pass->dev->cfg.private)->pll;
+	struct forward_pll *pll = pass->dev->pll;
 	FD722_SyncGenerator sg = { 0 };
 	FD722_SyncGeneratorField sgf = { 0 };
 	struct forward_passthrough_format *fmt = &io->format;
-	struct forward_si532x_mode mode = { 0 };
+	struct forward_pll_mode *mode;
 	FD722_Genlock gl;
 	int pll_in_freq = 0;
 	int i;
@@ -303,25 +302,21 @@ static void fd722_configure_genlock(struct forward_passthrough_io *io, bool anal
 		gl.mode = io->in_index ? 3 : 2;
 	}
 
-	if (!forward_si532x_find_mode(pll_in_freq, fmt->m, &mode))
+	mode = pll->ops->find_mode(pll, pll_in_freq, 0, 148500000, fmt->m);
+	if (!mode)
 		return;
 
 	FD722_Genlock_W(pass->dev->csr, gl);
 
-	forward_si532x_load_defaults(pll);
-	forward_si532x_configure_factors(pll, mode.bwsel, mode.n1_hs, mode.nc1_ls, mode.n2_hs,
-					 mode.n2_ls, mode.n31, mode.n32);
-
+	pll->ops->switch_mode(pll, mode);
 	if (!analog) {
 		// Hack to avoid PLL->IN->PLL loop stuck
 		FD722_VideoInCS cs;
 		int count = 0;
 
 		// Force Master clock
-		forward_si532x_select_input(pll, 0);
-		forward_si532x_write_reg(pll, 0x04, 0x12);
-		forward_si532x_write_reg(pll, 0x03, 0x55);
-		forward_si532x_calibrate(pll);
+		pll->ops->select_input(pll, 1, false, false);
+		pll->ops->calibrate(pll);
 
 		// Wait for input
 		do {
@@ -331,10 +326,10 @@ static void fd722_configure_genlock(struct forward_passthrough_io *io, bool anal
 		} while ((!cs.modeLocked) && (count < 100));
 
 		// Switch to input clock
-		forward_si532x_write_reg(pll, 0x04, 0x92);
+		pll->ops->select_input(pll, 0, false, true);
 	} else {
-		forward_si532x_select_input(pll, 0);
-		forward_si532x_calibrate(pll);
+		pll->ops->select_input(pll, 0, false, true);
+		pll->ops->calibrate(pll);
 	}
 
 	sg.frameSize = fmt->total_width * fmt->total_height;
diff --git a/forward-passthrough/forward-passthrough-fd788.c b/forward-passthrough/forward-passthrough-fd788.c
index 19ff1e1..af4fcad 100644
--- a/forward-passthrough/forward-passthrough-fd788.c
+++ b/forward-passthrough/forward-passthrough-fd788.c
@@ -19,8 +19,7 @@
 */
 
 #include "forward-passthrough.h"
-#include "forward-si532x.h"
-#include "forward-fd788.h"
+#include "forward-pll.h"
 #include "fd788_reg.h"
 
 #include <linux/types.h>
@@ -256,11 +255,11 @@ static void fd788_configure_io(struct forward_passthrough_io *io)
 static void fd788_configure_genlock(struct forward_passthrough_io *io, bool analog)
 {
 	struct forward_passthrough *pass = io->pass;
-	struct forward_si532x *pll = &((struct forward_fd788 *)pass->dev->cfg.private)->pll;
+	struct forward_pll *pll = pass->dev->pll;
 	FD788_SyncGenerator sg = { 0 };
 	FD788_SyncGeneratorField sgf = { 0 };
 	struct forward_passthrough_format *fmt = &io->format;
-	struct forward_si532x_mode mode = { 0 };
+	struct forward_pll_mode *mode;
 	FD788_Genlock gl;
 	int pll_in_freq = 0;
 	int i;
@@ -281,25 +280,21 @@ static void fd788_configure_genlock(struct forward_passthrough_io *io, bool anal
 		gl.mode = io->in_index ? 3 : 2;
 	}
 
-	if (!forward_si532x_find_mode(pll_in_freq, fmt->m, &mode))
+	mode = pll->ops->find_mode(pll, pll_in_freq, 0, 148500000, fmt->m);
+	if (!mode)
 		return;
 
 	FD788_Genlock_W(pass->dev->csr, gl);
 
-	forward_si532x_load_defaults(pll);
-	forward_si532x_configure_factors(pll, mode.bwsel, mode.n1_hs, mode.nc1_ls, mode.n2_hs,
-					 mode.n2_ls, mode.n31, mode.n32);
-
+	pll->ops->switch_mode(pll, mode);
 	if (!analog) {
 		// Hack to avoid PLL->IN->PLL loop stuck
 		FD788_VideoInCS cs;
 		int count = 0;
 
 		// Force Master clock
-		forward_si532x_select_input(pll, 0);
-		forward_si532x_write_reg(pll, 0x04, 0x12);
-		forward_si532x_write_reg(pll, 0x03, 0x55);
-		forward_si532x_calibrate(pll);
+		pll->ops->select_input(pll, 1, false, false);
+		pll->ops->calibrate(pll);
 
 		// Wait for input
 		do {
@@ -309,10 +304,10 @@ static void fd788_configure_genlock(struct forward_passthrough_io *io, bool anal
 		} while ((!cs.modeLocked) && (count < 100));
 
 		// Switch to input clock
-		forward_si532x_write_reg(pll, 0x04, 0x92);
+		pll->ops->select_input(pll, 0, false, true);
 	} else {
-		forward_si532x_select_input(pll, 0);
-		forward_si532x_calibrate(pll);
+		pll->ops->select_input(pll, 0, false, true);
+		pll->ops->calibrate(pll);
 	}
 
 	sg.frameSize = fmt->total_width * fmt->total_height;
diff --git a/forward-v4l2/forward-v4l2-core.c b/forward-v4l2/forward-v4l2-core.c
index ffda81b..c9273e2 100644
--- a/forward-v4l2/forward-v4l2-core.c
+++ b/forward-v4l2/forward-v4l2-core.c
@@ -204,6 +204,7 @@ static void forward_v4l2_io_changed(struct forward_dev *dev, struct forward_io *
 
 static struct forward_extension forward_v4l2_ext = {
 	.name = FORWARD_V4L2_MODULE_NAME,
+	.version = FORWARD_VERSION_CODE,
 	.probe = forward_v4l2_probe,
 	.reprobe = forward_v4l2_reprobe,
 	.remove = forward_v4l2_remove,
diff --git a/forward-v4l2/forward-v4l2-fd722.c b/forward-v4l2/forward-v4l2-fd722.c
index 151f067..4f1b5b2 100644
--- a/forward-v4l2/forward-v4l2-fd722.c
+++ b/forward-v4l2/forward-v4l2-fd722.c
@@ -21,10 +21,10 @@
 #include "forward-v4l2.h"
 #include "forward-v4l2-io.h"
 
+#include "forward-pll.h"
+
 #include "fd722_reg.h"
 
-#include "forward-si532x.h"
-#include "forward-fd722.h"
 #include "forward-v4l2-ioctl.h"
 #include "forward-v4l2-genlock.h"
 
@@ -56,12 +56,15 @@ static void fd722_check_pll_configuration(struct forward_v4l2 *v4l2)
 	bool clk_m;
 	int i;
 	bool any_m = false;
-	struct forward_si532x *pll = &((struct forward_fd722 *)v4l2->dev->cfg.private)->pll;
+	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);
@@ -81,12 +84,14 @@ static void fd722_check_pll_configuration(struct forward_v4l2 *v4l2)
 	}
 
 	if ((clk_m != any_m) || !clk_valid) {
-		forward_si532x_load_defaults(pll);
-		if (!any_m)
-			forward_si532x_configure_factors(pll, 10, 9, 4, 11, 288, 4, 16);
-		else
-			forward_si532x_configure_factors(pll, 8, 9, 4, 10, 1800, 250, 91);
-		forward_si532x_calibrate(pll);
+		struct forward_pll_mode *mode;
+		mode = pll->ops->find_mode(pll, 0, 0, 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;
@@ -720,17 +725,17 @@ static int fd722_genlock_switch(const struct forward_v4l2_genlock *gl,
 				struct forward_v4l2_timings *t)
 {
 	struct forward_v4l2 *v4l2 = gl->v4l2;
-	struct forward_si532x *pll = &((struct forward_fd722 *)v4l2->dev->cfg.private)->pll;
+	struct forward_pll *pll = v4l2->dev->pll;
 	FD722_SyncGenerator sg = { 0 };
 	FD722_SyncGeneratorField sgf = { 0 };
+	struct forward_pll_mode *mode = NULL;
 
 	if (src == V4L_FORWARD_GENLOCK_SRC_MASTER) {
-		forward_si532x_load_defaults(pll);
-		forward_si532x_configure_factors(pll, 10, 9, 4, 11, 288, 4, 16);
-		forward_si532x_select_input(pll, 1);
-		forward_si532x_calibrate(pll);
+		mode = pll->ops->find_mode(pll, 0, 0, 148500000, false);
+		pll->ops->switch_mode(pll, mode);
+		pll->ops->select_input(pll, 1, false, false);
+		pll->ops->calibrate(pll);
 	} else {
-		struct forward_si532x_mode mode = { 0 };
 		int pll_in_freq = 0;
 		FD722_Genlock gl = FD722_Genlock_R(v4l2->dev->csr);
 
@@ -749,14 +754,12 @@ static int fd722_genlock_switch(const struct forward_v4l2_genlock *gl,
 			gl.mode = (src == V4L_FORWARD_GENLOCK_SRC_IN1) ? 3 : 2;
 		}
 
-		if (!forward_si532x_find_mode(pll_in_freq, t->sdi.m, &mode))
+		mode = pll->ops->find_mode(pll, pll_in_freq, 0, 148500000, t->sdi.m);
+		if (!mode)
 			return -EINVAL;
 
 		FD722_Genlock_W(v4l2->dev->csr, gl);
-
-		forward_si532x_load_defaults(pll);
-		forward_si532x_configure_factors(pll, mode.bwsel, mode.n1_hs, mode.nc1_ls,
-						 mode.n2_hs, mode.n2_ls, mode.n31, mode.n32);
+		pll->ops->switch_mode(pll, mode);
 
 		if ((src == V4L_FORWARD_GENLOCK_SRC_IN0) || (src == V4L_FORWARD_GENLOCK_SRC_IN1)) {
 			// Hack to avoid PLL->IN->PLL loop stuck
@@ -764,10 +767,8 @@ static int fd722_genlock_switch(const struct forward_v4l2_genlock *gl,
 			int count = 0;
 
 			// Force Master clock
-			forward_si532x_select_input(pll, 0);
-			forward_si532x_write_reg(pll, 0x04, 0x12);
-			forward_si532x_write_reg(pll, 0x03, 0x55);
-			forward_si532x_calibrate(pll);
+			pll->ops->select_input(pll, 1, false, false);
+			pll->ops->calibrate(pll);
 
 			// Wait for input
 			do {
@@ -779,10 +780,10 @@ static int fd722_genlock_switch(const struct forward_v4l2_genlock *gl,
 			} while ((!cs.modeLocked) && (count < 100));
 
 			// Switch to input clock
-			forward_si532x_write_reg(pll, 0x04, 0x92);
+			pll->ops->select_input(pll, 0, false, true);
 		} else {
-			forward_si532x_select_input(pll, 0);
-			forward_si532x_calibrate(pll);
+			pll->ops->select_input(pll, 0, false, true);
+			pll->ops->calibrate(pll);
 		}
 	}
 
diff --git a/forward-v4l2/forward-v4l2-fd788.c b/forward-v4l2/forward-v4l2-fd788.c
index a107eca..6f5725e 100644
--- a/forward-v4l2/forward-v4l2-fd788.c
+++ b/forward-v4l2/forward-v4l2-fd788.c
@@ -22,9 +22,10 @@
 #include "forward-v4l2-io.h"
 #include "forward-v4l2-genlock.h"
 
+#include "forward-pll.h"
+
 #include <linux/delay.h>
 
-#include "forward-fd788.h"
 #include "fd788_reg.h"
 #include "forward-v4l2-ioctl.h"
 
@@ -53,12 +54,15 @@ static void fd788_check_pll_configuration(struct forward_v4l2 *v4l2)
 	bool clk_m;
 	int i;
 	bool any_m = false;
-	struct forward_si532x *pll = &((struct forward_fd788 *)v4l2->dev->cfg.private)->pll;
+	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 = FD788_ClockFrequency_R(v4l2->dev->csr);
@@ -86,12 +90,15 @@ static void fd788_check_pll_configuration(struct forward_v4l2 *v4l2)
 	}
 
 	if ((clk_m != any_m) || !clk_valid) {
-		forward_si532x_load_defaults(pll);
-		if (!any_m)
-			forward_si532x_configure_factors(pll, 10, 9, 4, 11, 288, 4, 16);
-		else
-			forward_si532x_configure_factors(pll, 8, 9, 4, 10, 1800, 250, 91);
-		forward_si532x_calibrate(pll);
+		struct forward_pll_mode *mode;
+		mode = pll->ops->find_mode(pll, 0, 0, 148500000, any_m);
+
+		if (!mode)
+			return;
+
+		pll->ops->switch_mode(pll, mode);
+		pll->ops->calibrate(pll);
+
 		for (i = 0; i < 8; i++) {
 			FD788_VideoOutCS ocs;
 			if (v4l2->io[i].state != FORWARD_IO_TX)
@@ -901,17 +908,17 @@ static int fd788_genlock_switch(const struct forward_v4l2_genlock *gl,
 				struct forward_v4l2_timings *t)
 {
 	struct forward_v4l2 *v4l2 = gl->v4l2;
-	struct forward_si532x *pll = &((struct forward_fd788 *)v4l2->dev->cfg.private)->pll;
+	struct forward_pll *pll = v4l2->dev->pll;
 	FD788_SyncGenerator sg = { 0 };
 	FD788_SyncGeneratorField sgf = { 0 };
+	struct forward_pll_mode *mode = NULL;
 
 	if (src == V4L_FORWARD_GENLOCK_SRC_MASTER) {
-		forward_si532x_load_defaults(pll);
-		forward_si532x_configure_factors(pll, 10, 9, 4, 11, 288, 4, 16);
-		forward_si532x_select_input(pll, 1);
-		forward_si532x_calibrate(pll);
+		mode = pll->ops->find_mode(pll, 0, 0, 148500000, false);
+		pll->ops->switch_mode(pll, mode);
+		pll->ops->select_input(pll, 1, false, false);
+		pll->ops->calibrate(pll);
 	} else {
-		struct forward_si532x_mode mode = { 0 };
 		int pll_in_freq = 0;
 		FD788_Genlock gl = FD788_Genlock_R(v4l2->dev->csr);
 
@@ -930,14 +937,12 @@ static int fd788_genlock_switch(const struct forward_v4l2_genlock *gl,
 			gl.mode = (src == V4L_FORWARD_GENLOCK_SRC_IN1) ? 3 : 2;
 		}
 
-		if (!forward_si532x_find_mode(pll_in_freq, t->sdi.m, &mode))
+		mode = pll->ops->find_mode(pll, pll_in_freq, 0, 148500000, t->sdi.m);
+		if (!mode)
 			return -EINVAL;
 
 		FD788_Genlock_W(v4l2->dev->csr, gl);
-
-		forward_si532x_load_defaults(pll);
-		forward_si532x_configure_factors(pll, mode.bwsel, mode.n1_hs, mode.nc1_ls,
-						 mode.n2_hs, mode.n2_ls, mode.n31, mode.n32);
+		pll->ops->switch_mode(pll, mode);
 
 		if ((src == V4L_FORWARD_GENLOCK_SRC_IN0) || (src == V4L_FORWARD_GENLOCK_SRC_IN1)) {
 			// Hack to avoid PLL->IN->PLL loop stuck
@@ -945,10 +950,8 @@ static int fd788_genlock_switch(const struct forward_v4l2_genlock *gl,
 			int count = 0;
 
 			// Force Master clock
-			forward_si532x_select_input(pll, 0);
-			forward_si532x_write_reg(pll, 0x04, 0x12);
-			forward_si532x_write_reg(pll, 0x03, 0x55);
-			forward_si532x_calibrate(pll);
+			pll->ops->select_input(pll, 1, false, false);
+			pll->ops->calibrate(pll);
 
 			// Wait for input
 			do {
@@ -960,10 +963,10 @@ static int fd788_genlock_switch(const struct forward_v4l2_genlock *gl,
 			} while ((!cs.modeLocked) && (count < 100));
 
 			// Switch to input clock
-			forward_si532x_write_reg(pll, 0x04, 0x92);
+			pll->ops->select_input(pll, 0, false, true);
 		} else {
-			forward_si532x_select_input(pll, 0);
-			forward_si532x_calibrate(pll);
+			pll->ops->select_input(pll, 0, false, true);
+			pll->ops->calibrate(pll);
 		}
 	}
 
diff --git a/forward/forward-core.c b/forward/forward-core.c
index 883e53f..f4bd17c 100644
--- a/forward/forward-core.c
+++ b/forward/forward-core.c
@@ -664,6 +664,16 @@ int forward_register_extension(struct forward_extension *ext)
 {
 	struct forward_dev *dev;
 
+	if (ext->version != FORWARD_VERSION_CODE) {
+		printk(KERN_ERR FORWARD_MODULE_NAME
+		       ": cannot register extension %s - wrong verion (%d.%d.%d, should be %d.%d.%d)\n",
+		       ext->name, (ext->version >> 16) & 0xFF, (ext->version >> 8) & 0xFF,
+		       (ext->version >> 0) & 0xFF, FORWARD_VERSION_MAJOR, FORWARD_VERSION_MINOR,
+		       FORWARD_VERSION_PATCH);
+
+		return -EINVAL;
+	}
+
 	mutex_lock(&forward_extensions_lock);
 	list_add_tail(&ext->node, &forward_extensions);
 	mutex_unlock(&forward_extensions_lock);
@@ -787,7 +797,11 @@ static struct platform_driver forward_plat_driver = {
 	},
 };
 
+#if LINUX_VERSION_CODE >= KERNEL_VERSION(6, 2, 0)
+static char *forward_devnode(const struct device *dev, umode_t *mode)
+#else
 static char *forward_devnode(struct device *dev, umode_t *mode)
+#endif
 {
 	size_t size = strlen(dev_name(dev)) + sizeof(FORWARD_CLASS_NAME) + 2;
 	char *result = kmalloc(size, GFP_KERNEL);
diff --git a/forward/forward-fd722.c b/forward/forward-fd722.c
index be7af05..6ce45b0 100644
--- a/forward/forward-fd722.c
+++ b/forward/forward-fd722.c
@@ -21,7 +21,6 @@
 #include "forward.h"
 #include "forward-vdma.h"
 
-#include "forward-fd722.h"
 #include "forward-si532x.h"
 #include "fd722_reg.h"
 
@@ -66,25 +65,16 @@ static bool fd722_i2c_xfer(struct forward_dev *dev, u8 bus, bool read, uint8_t a
 
 static int fd722_init(struct forward_dev *dev)
 {
-	struct forward_fd722 *fd722 =
-		devm_kzalloc(dev->parent_dev, sizeof(struct forward_fd722), GFP_KERNEL);
-	if (!fd722)
-		return -ENOMEM;
-
-	dev->cfg.private = fd722;
 	dev->i2c_xfer = &fd722_i2c_xfer;
 
-	forward_si532x_init(dev, &fd722->pll, 0, FD722_PLL_ADDRESS);
+	forward_si532x_init(dev, &dev->pll, 0, FD722_PLL_ADDRESS, 2);
 
 	return 0;
 }
 
 static void fd722_fini(struct forward_dev *dev)
 {
-	struct forward_fd722 *fd722 = dev->cfg.private;
-	if (dev->cfg.private)
-		devm_kfree(dev->parent_dev, fd722);
-	dev->cfg.private = NULL;
+	forward_si532x_fini(dev, &dev->pll);
 }
 
 static void fd722_read_id(struct forward_dev *dev)
diff --git a/forward/forward-fd788.c b/forward/forward-fd788.c
index ffafab7..008dc77 100644
--- a/forward/forward-fd788.c
+++ b/forward/forward-fd788.c
@@ -21,7 +21,7 @@
 #include "forward.h"
 #include "forward-vdma.h"
 
-#include "forward-fd788.h"
+#include "forward-si532x.h"
 #include "fd788_reg.h"
 
 #include <linux/delay.h>
@@ -70,25 +70,16 @@ static bool fd788_i2c_xfer(struct forward_dev *dev, u8 bus, bool read, uint8_t a
 
 static int fd788_init(struct forward_dev *dev)
 {
-	struct forward_fd788 *fd788 =
-		devm_kzalloc(dev->parent_dev, sizeof(struct forward_fd788), GFP_KERNEL);
-	if (!fd788)
-		return -ENOMEM;
-
-	dev->cfg.private = fd788;
 	dev->i2c_xfer = &fd788_i2c_xfer;
 
-	forward_si532x_init(dev, &fd788->pll, 0, FD788_PLL_ADDRESS);
+	forward_si532x_init(dev, &dev->pll, 0, FD788_PLL_ADDRESS, 2);
 
 	return 0;
 }
 
 static void fd788_fini(struct forward_dev *dev)
 {
-	struct forward_fd788 *fd788 = dev->cfg.private;
-	if (dev->cfg.private)
-		devm_kfree(dev->parent_dev, fd788);
-	dev->cfg.private = NULL;
+	forward_si532x_fini(dev, &dev->pll);
 }
 
 static void fd788_read_id(struct forward_dev *dev)
diff --git a/forward/forward-si532x.c b/forward/forward-si532x.c
index c02f110..568d8e9 100644
--- a/forward/forward-si532x.c
+++ b/forward/forward-si532x.c
@@ -18,7 +18,26 @@
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 */
 
-#include "forward-si532x.h"
+#include "forward-pll.h"
+
+struct forward_si532x {
+	struct forward_pll pll;
+
+	u8 bus;
+	u8 address;
+};
+
+struct forward_si532x_mode {
+	int in_freq;
+	bool m;
+	u8 bwsel;
+	u8 n1_hs;
+	u32 nc1_ls;
+	u8 n2_hs;
+	u32 n2_ls;
+	u32 n31;
+	u32 n32;
+};
 
 const struct forward_si532x_mode forward_si532x_modes[] = {
 	{ 13500000 / 200, false, 5, 9, 4, 11, 7200, 1, 400 },
@@ -50,32 +69,26 @@ const struct forward_si532x_mode forward_si532x_modes[] = {
 	{ 148500000, true, 8, 9, 4, 10, 1800, 500, 91 },
 };
 
+const struct forward_si532x_mode si532x_default_mode = { -1, false, 10, 9, 4, 11, 288, 44, 16 };
+const struct forward_si532x_mode si532x_default_mode_m = { -1, true, 8, 9, 4, 10, 1800, 250, 91 };
+
 const int forward_si532x_modes_count =
 	sizeof(forward_si532x_modes) / sizeof(struct forward_si532x_mode);
 
-int forward_si532x_init(struct forward_dev *dev, struct forward_si532x *pll, u8 bus, u8 address)
+static int si532x_write_reg(struct forward_si532x *si532x, u8 reg, u8 data)
 {
-	pll->dev = dev;
-	pll->bus = bus;
-	pll->address = address;
-	return 0;
+	return si532x->pll.dev->i2c_xfer(si532x->pll.dev, si532x->bus, false, si532x->address, reg,
+					 &data);
 }
-EXPORT_SYMBOL(forward_si532x_init);
-
-int forward_si532x_write_reg(struct forward_si532x *pll, u8 reg, u8 data)
-{
-	return pll->dev->i2c_xfer(pll->dev, pll->bus, false, pll->address, reg, &data);
-}
-EXPORT_SYMBOL(forward_si532x_write_reg);
 
-int forward_si532x_read_reg(struct forward_si532x *pll, u8 reg, u8 *data)
+static int si532x_read_reg(struct forward_si532x *si532x, u8 reg, u8 *data)
 {
-	return pll->dev->i2c_xfer(pll->dev, pll->bus, true, pll->address, reg, data);
+	return si532x->pll.dev->i2c_xfer(si532x->pll.dev, si532x->bus, true, si532x->address, reg,
+					 data);
 }
-EXPORT_SYMBOL(forward_si532x_read_reg);
 
-int forward_si532x_configure_factors(struct forward_si532x *pll, u8 bwsel, u8 n1_hs, u32 nc1_ls,
-				     u8 n2_hs, u32 n2_ls, u32 n31, u32 n32)
+static int si532x_configure_factors(struct forward_si532x *si532x, u8 bwsel, u8 n1_hs, u32 nc1_ls,
+				    u8 n2_hs, u32 n2_ls, u32 n31, u32 n32)
 {
 	u32 reg_value;
 
@@ -100,84 +113,153 @@ int forward_si532x_configure_factors(struct forward_si532x *pll, u8 bwsel, u8 n1
 	if ((n32 < 1) || (n32 > (1 << 19)))
 		return -EINVAL;
 
-	forward_si532x_write_reg(pll, 0x02, (bwsel << 4) | (0x2));
-	forward_si532x_write_reg(pll, 0x19, ((n1_hs - 4) << 5));
+	si532x_write_reg(si532x, 0x02, (bwsel << 4) | (0x2));
+	si532x_write_reg(si532x, 0x19, ((n1_hs - 4) << 5));
 
 	reg_value = nc1_ls - 1;
-	forward_si532x_write_reg(pll, 0x1F, (reg_value >> 16) & 0x0F);
-	forward_si532x_write_reg(pll, 0x20, (reg_value >> 8) & 0xFF);
-	forward_si532x_write_reg(pll, 0x21, reg_value & 0xFF);
+	si532x_write_reg(si532x, 0x1F, (reg_value >> 16) & 0x0F);
+	si532x_write_reg(si532x, 0x20, (reg_value >> 8) & 0xFF);
+	si532x_write_reg(si532x, 0x21, reg_value & 0xFF);
 
 	reg_value = n2_ls - 1;
-	forward_si532x_write_reg(pll, 0x28, ((n2_hs - 4) << 5) | ((reg_value >> 16) & 0x0F));
-	forward_si532x_write_reg(pll, 0x29, (reg_value >> 8) & 0xFF);
-	forward_si532x_write_reg(pll, 0x2A, reg_value & 0xFF);
+	si532x_write_reg(si532x, 0x28, ((n2_hs - 4) << 5) | ((reg_value >> 16) & 0x0F));
+	si532x_write_reg(si532x, 0x29, (reg_value >> 8) & 0xFF);
+	si532x_write_reg(si532x, 0x2A, reg_value & 0xFF);
 
 	reg_value = n31 - 1;
-	forward_si532x_write_reg(pll, 0x2B, (reg_value >> 16) & 0x07);
-	forward_si532x_write_reg(pll, 0x2C, (reg_value >> 8) & 0xFF);
-	forward_si532x_write_reg(pll, 0x2D, reg_value & 0xFF);
+	si532x_write_reg(si532x, 0x2B, (reg_value >> 16) & 0x07);
+	si532x_write_reg(si532x, 0x2C, (reg_value >> 8) & 0xFF);
+	si532x_write_reg(si532x, 0x2D, reg_value & 0xFF);
 
 	reg_value = n32 - 1;
-	forward_si532x_write_reg(pll, 0x2E, (reg_value >> 16) & 0x07);
-	forward_si532x_write_reg(pll, 0x2F, (reg_value >> 8) & 0xFF);
-	forward_si532x_write_reg(pll, 0x30, reg_value & 0xFF);
+	si532x_write_reg(si532x, 0x2E, (reg_value >> 16) & 0x07);
+	si532x_write_reg(si532x, 0x2F, (reg_value >> 8) & 0xFF);
+	si532x_write_reg(si532x, 0x30, reg_value & 0xFF);
 
 	return 0;
 }
-EXPORT_SYMBOL(forward_si532x_configure_factors);
 
-int forward_si532x_load_defaults(struct forward_si532x *pll)
+static int si532x_load_defaults(struct forward_si532x *si532x)
 {
 	// PLL mode, CLK always on, no bypass
-	forward_si532x_write_reg(pll, 0x00, 0x34);
+	si532x_write_reg(si532x, 0x00, 0x34);
 	// CLKIN1 > CLKIN2 priority
-	forward_si532x_write_reg(pll, 0x01, 0xE1);
+	//si532x_write_reg(si532x, 0x01, 0xE1);
 	// No output disabling during calibration, CLKIN2 selected
-	forward_si532x_write_reg(pll, 0x03, 0x55);
+	si532x_write_reg(si532x, 0x03, 0x55);
 	// Automatic clock selection, default hold history
-	forward_si532x_write_reg(pll, 0x04, 0x92);
+	//si532x_write_reg(si532x, 0x04, 0x92);
 	// LVDS outputs
-	forward_si532x_write_reg(pll, 0x06, 0x0F);
+	si532x_write_reg(si532x, 0x06, 0x0F);
 	// External crystal as reference
-	forward_si532x_write_reg(pll, 0x07, 0x28);
+	si532x_write_reg(si532x, 0x07, 0x28);
 	// CLKOUT1 enabled, CLKOUT2 disabled
-	forward_si532x_write_reg(pll, 0x0A, 0x08);
+	si532x_write_reg(si532x, 0x0A, 0x08);
 	// Frequency offset and phase error detector
-	forward_si532x_write_reg(pll, 0x13, 0x2F);
+	si532x_write_reg(si532x, 0x13, 0x2F);
 	// Enable CK2_BAD, LOL, INT pins
-	forward_si532x_write_reg(pll, 0x14, 0x3B);
+	si532x_write_reg(si532x, 0x14, 0x3B);
 	// Disable clock selection pins
-	forward_si532x_write_reg(pll, 0x15, 0xFC);
+	si532x_write_reg(si532x, 0x15, 0xFC);
 	// Enable fast lock
-	forward_si532x_write_reg(pll, 0x89, 0x01);
+	si532x_write_reg(si532x, 0x89, 0x01);
 
 	return 0;
 }
-EXPORT_SYMBOL(forward_si532x_load_defaults);
 
-void forward_si532x_calibrate(struct forward_si532x *pll)
+static struct forward_pll_mode *si532x_find_mode(struct forward_pll *pll, s64 in_freq,
+						 s64 common_in_freq, s64 out_freq, bool m)
 {
-	forward_si532x_write_reg(pll, 0x88, 0x40);
-}
-EXPORT_SYMBOL(forward_si532x_calibrate);
+	int i;
 
-void forward_si532x_select_input(struct forward_si532x *pll, int input)
-{
-	forward_si532x_write_reg(pll, 0x01, (input == 1) ? 0xE1 : 0xE4);
-}
-EXPORT_SYMBOL(forward_si532x_select_input);
+	if (out_freq != 148500000)
+		return NULL;
+
+	if (in_freq <= 0) {
+		if (!m)
+			return (struct forward_pll_mode *)&si532x_default_mode;
+		else
+			return (struct forward_pll_mode *)&si532x_default_mode_m;
+	}
 
-bool forward_si532x_find_mode(int in_freq, bool m, struct forward_si532x_mode *mode)
-{
-	int i;
 	for (i = 0; i < forward_si532x_modes_count; i++) {
 		const struct forward_si532x_mode *cur = &forward_si532x_modes[i];
 		if ((cur->in_freq == in_freq) && (cur->m == m)) {
-			*mode = *cur;
-			return true;
+			return (struct forward_pll_mode *)cur;
 		}
 	}
-	return false;
+	return NULL;
+}
+
+static int si532x_switch_mode(struct forward_pll *pll, struct forward_pll_mode *mode)
+{
+	struct forward_si532x *si532x = container_of(pll, struct forward_si532x, pll);
+	struct forward_si532x_mode *m = (struct forward_si532x_mode *)mode;
+
+	if (mode == NULL)
+		return -EINVAL;
+
+	si532x_load_defaults(si532x);
+	si532x_configure_factors(si532x, m->bwsel, m->n1_hs, m->nc1_ls, m->n2_hs, m->n2_ls, m->n31,
+				 m->n32);
+
+	return 0;
+}
+
+static int si532x_select_input(struct forward_pll *pll, int input, bool master, bool autoswitch)
+{
+	struct forward_si532x *si532x = container_of(pll, struct forward_si532x, pll);
+
+	si532x_write_reg(si532x, 0x04, autoswitch ? 0x92 : 0x12);
+	si532x_write_reg(si532x, 0x03, (input == 1) ? 0x55 : 0x15);
+	si532x_write_reg(si532x, 0x01, (input == 1) ? 0xE1 : 0xE4);
+
+	return 0;
+}
+
+int si532x_tune(struct forward_pll *pll, s64 ppt)
+{
+	return -ENOTSUPP;
+}
+
+void si532x_calibrate(struct forward_pll *pll)
+{
+	struct forward_si532x *si532x = container_of(pll, struct forward_si532x, pll);
+	si532x_write_reg(si532x, 0x88, 0x40);
+}
+
+static struct forward_pll_ops si5324_ops = { .find_mode = si532x_find_mode,
+					     .switch_mode = si532x_switch_mode,
+					     .select_input = si532x_select_input,
+					     .tune = si532x_tune,
+					     .calibrate = si532x_calibrate };
+
+int forward_si532x_init(struct forward_dev *dev, struct forward_pll **pll, u8 bus, u8 address,
+			int num_inputs)
+{
+	struct forward_si532x *si532x = kzalloc(sizeof(struct forward_si532x), GFP_KERNEL);
+	if (!si532x)
+		return -ENOMEM;
+
+	si532x->pll.dev = dev;
+	si532x->pll.features = FORWARD_PLL_FEATURE_FALLBACK;
+	si532x->pll.num_inputs = num_inputs;
+	si532x->pll.ops = &si5324_ops;
+	si532x->bus = bus;
+	si532x->address = address;
+
+	*pll = &si532x->pll;
+
+	return 0;
+}
+EXPORT_SYMBOL(forward_si532x_init);
+
+void forward_si532x_fini(struct forward_dev *dev, struct forward_pll **pll)
+{
+	if (pll) {
+		struct forward_si532x *si532x = container_of(*pll, struct forward_si532x, pll);
+		kfree(si532x);
+		*pll = NULL;
+	}
 }
-EXPORT_SYMBOL(forward_si532x_find_mode);
+EXPORT_SYMBOL(forward_si532x_fini);
diff --git a/include/forward-fd722.h b/forward/forward-si532x.h
similarity index 74%
rename from include/forward-fd722.h
rename to forward/forward-si532x.h
index ce2b4a2..6246ccf 100644
--- a/include/forward-fd722.h
+++ b/forward/forward-si532x.h
@@ -1,5 +1,5 @@
 /*
-   forward-fd722.h - FD722 driver for SoftLab-NSK Forward video boards
+   forward-si532x.h - si532x PLL driver for SoftLab-NSK Forward boards
 
    Copyright (C) 2017 - 2023 Konstantin Oblaukhov <oblaukhov@sl.iae.nsk.su>
 
@@ -18,14 +18,8 @@
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 */
 
-#ifndef FORWARD_FD722_H
-#define FORWARD_FD722_H
+#include "forward-pll.h"
 
-#include "forward.h"
-#include "forward-si532x.h"
-
-struct forward_fd722 {
-	struct forward_si532x pll;
-};
-
-#endif
+int forward_si532x_init(struct forward_dev *dev, struct forward_pll **pll, u8 bus, u8 address,
+			int num_inputs);
+void forward_si532x_fini(struct forward_dev *dev, struct forward_pll **pll);
diff --git a/include/forward-fd788.h b/include/forward-fd788.h
deleted file mode 100644
index eb6c51b..0000000
--- a/include/forward-fd788.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
-   forward-fd788.h - FD788 driver 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.
-*/
-
-#ifndef FORWARD_FD788_H
-#define FORWARD_FD788_H
-
-#include "forward.h"
-#include "forward-si532x.h"
-
-struct forward_fd788 {
-	struct forward_si532x pll;
-};
-
-#endif
diff --git a/include/forward-pll.h b/include/forward-pll.h
new file mode 100644
index 0000000..a23d080
--- /dev/null
+++ b/include/forward-pll.h
@@ -0,0 +1,53 @@
+/*
+   forward-pll.h - PLL drivers for SoftLab-NSK Forward 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.
+*/
+
+#ifndef FORWARD_PLL_H
+#define FORWARD_PLL_H
+
+#include <linux/kernel.h>
+
+#include "forward.h"
+
+struct forward_pll_mode;
+struct forward_pll;
+
+#define FORWARD_PLL_FEATURE_MASTER (1 << 0)
+#define FORWARD_PLL_FEATURE_FALLBACK (1 << 1)
+#define FORWARD_PLL_FEATURE_TUNE (1 << 2)
+
+struct forward_pll_ops {
+	struct forward_pll_mode *(*find_mode)(struct forward_pll *pll, s64 in_freq,
+					      s64 common_in_freq, s64 out_freq, bool m);
+	int (*switch_mode)(struct forward_pll *pll, struct forward_pll_mode *mode);
+	int (*select_input)(struct forward_pll *pll, int input, bool master, bool autoswitch);
+	int (*tune)(struct forward_pll *pll, s64 ppt);
+	void (*calibrate)(struct forward_pll *pll);
+};
+
+struct forward_pll {
+	struct forward_dev *dev;
+
+	int num_inputs;
+	int features;
+
+	struct forward_pll_ops *ops;
+};
+
+#endif
diff --git a/include/forward-si532x.h b/include/forward-si532x.h
deleted file mode 100644
index a68764a..0000000
--- a/include/forward-si532x.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
-   forward-si532x.h - si532x PLL driver for SoftLab-NSK Forward 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.
-*/
-
-#ifndef FORWARD_si532x_H
-#define FORWARD_si532x_H
-
-#include <linux/kernel.h>
-
-#include "forward.h"
-
-struct forward_si532x {
-	struct forward_dev *dev;
-
-	u8 bus;
-	u8 address;
-};
-
-struct forward_si532x_mode {
-	int in_freq;
-	bool m;
-	u8 bwsel;
-	u8 n1_hs;
-	u32 nc1_ls;
-	u8 n2_hs;
-	u32 n2_ls;
-	u32 n31;
-	u32 n32;
-};
-
-int forward_si532x_init(struct forward_dev *dev, struct forward_si532x *pll, u8 bus, u8 address);
-
-int forward_si532x_write_reg(struct forward_si532x *pll, u8 reg, u8 data);
-int forward_si532x_read_reg(struct forward_si532x *pll, u8 reg, u8 *data);
-
-int forward_si532x_configure_factors(struct forward_si532x *pll, u8 bwsel, u8 n1_hs, u32 nc1_ls,
-				     u8 n2_hs, u32 n2_ls, u32 n31, u32 n32);
-int forward_si532x_load_defaults(struct forward_si532x *pll);
-void forward_si532x_calibrate(struct forward_si532x *pll);
-void forward_si532x_select_input(struct forward_si532x *pll, int input);
-
-bool forward_si532x_find_mode(int in_freq, bool m, struct forward_si532x_mode *mode);
-
-#endif
diff --git a/include/forward.h b/include/forward.h
index 2d517a8..8fcbe0c 100644
--- a/include/forward.h
+++ b/include/forward.h
@@ -141,6 +141,7 @@ struct forward_dev {
 	spinlock_t extensions_lock;
 
 	struct forward_vdma *vdma;
+	struct forward_pll *pll;
 	struct forward_io *io;
 
 	const struct attribute_group *attr_groups[3];
@@ -148,6 +149,7 @@ struct forward_dev {
 
 struct forward_extension {
 	const char *name;
+	const u32 version;
 	int (*probe)(struct forward_dev *dev, void **private);
 	int (*reprobe)(struct forward_dev *dev, void **private);
 	void (*remove)(struct forward_dev *dev, void **private);
-- 
2.39.1


From ca4cd9f091086fbb38bbfc3611f1d3ec35bee130 Mon Sep 17 00:00:00 2001
From: Konstantin Oblaukhov <oblaukhov.konstantin@gmail.com>
Date: Sat, 11 Mar 2023 23:18:53 +0700
Subject: [PATCH 04/13] Si5342 driver for FD922.

---
 forward-v4l2/forward-v4l2-fd922.c |  76 +----
 forward/Makefile                  |   3 +-
 forward/forward-fd922.c           |  14 +
 forward/forward-si532x.c          |  28 +-
 forward/forward-si534x.c          | 454 ++++++++++++++++++++++++++++++
 forward/forward-si534x.h          |  24 ++
 6 files changed, 511 insertions(+), 88 deletions(-)
 create mode 100644 forward/forward-si534x.c
 create mode 100644 forward/forward-si534x.h

diff --git a/forward-v4l2/forward-v4l2-fd922.c b/forward-v4l2/forward-v4l2-fd922.c
index 950fd9c..56b7621 100644
--- a/forward-v4l2/forward-v4l2-fd922.c
+++ b/forward-v4l2/forward-v4l2-fd922.c
@@ -23,6 +23,7 @@
 
 #include <linux/delay.h>
 #include "forward-v4l2-ioctl.h"
+#include "forward-pll.h"
 
 #include "fd922_reg.h"
 
@@ -37,74 +38,6 @@
 
 #define FD922_ASI_PERIOD_REVISION 54
 
-inline void si53xx_select_bank(struct forward_dev *dev, u16 address)
-{
-	u8 bank = ((address >> 8) & 0xFF);
-	u8 tx[4] = { 0x00, 0x01, 0x40, bank };
-	dev->spi_xfer(dev, 4, tx, 32, NULL, 0);
-}
-
-static void si53xx_read_data(struct forward_dev *dev, u16 address, u8 *data, u8 count)
-{
-	u8 tx[2 + 8 * 2];
-	u8 rx[2 + 8 * 2];
-	int i;
-
-	si53xx_select_bank(dev, address);
-
-	tx[0] = 0x00;
-	tx[1] = (address & 0xFF);
-	for (i = 0; i < count; i++) {
-		tx[2 + i * 2] = 0xA0;
-		tx[2 + i * 2 + 1] = 0x00;
-	}
-	dev->spi_xfer(dev, 4, tx, (2 + count * 2) * 8, rx, (2 + count * 2) * 8);
-	for (i = 0; i < count; i++)
-		data[i] = rx[2 + i * 2 + 1];
-}
-
-static void si53xx_write_data(struct forward_dev *dev, u16 address, const u8 *data, u8 count)
-{
-	uint8_t tx[2 + 8 * 2];
-	int i;
-
-	si53xx_select_bank(dev, address);
-
-	tx[0] = 0x00;
-	tx[1] = (address & 0xFF);
-	for (i = 0; i < count; i++) {
-		tx[2 + i * 2] = 0x60;
-		tx[2 + i * 2 + 1] = data[i];
-	}
-	dev->spi_xfer(dev, 4, tx, (2 + count * 2) * 8, NULL, 0);
-}
-
-static void si53xx_write_n_div(struct forward_dev *dev, int channel, u64 num, u64 denom)
-{
-	u16 num_a, denom_a, up_a;
-	u8 data[8];
-	num_a = (channel == 0) ? 0x302 : 0x30d;
-	denom_a = (channel == 0) ? 0x308 : 0x313;
-	up_a = (channel == 0) ? 0x30c : 0x317;
-
-	data[0] = (num >> 0) & 0xff;
-	data[1] = (num >> 8) & 0xff;
-	data[2] = (num >> 16) & 0xff;
-	data[3] = (num >> 24) & 0xff;
-	data[4] = (num >> 32) & 0xff;
-	data[5] = (num >> 40) & 0xff;
-	si53xx_write_data(dev, num_a, data, 6);
-
-	data[0] = (denom >> 0) & 0xff;
-	data[1] = (denom >> 8) & 0xff;
-	data[2] = (denom >> 16) & 0xff;
-	data[3] = (denom >> 24) & 0xff;
-	si53xx_write_data(dev, denom_a, data, 4);
-
-	data[0] = 0xff;
-	si53xx_write_data(dev, up_a, data, 1);
-}
-
 static u32 fd922_irq_mask(const struct forward_v4l2_io *io)
 {
 	return (1 << (io->index * 3));
@@ -694,12 +627,7 @@ static s64 fd922_clock_deviation(struct forward_v4l2 *v4l2)
 
 static s64 fd922_set_clock_deviation(struct forward_v4l2 *v4l2, s64 ppt)
 {
-	u32 denom = 0xc8000000 + 0xc8000000LL * ppt / 1000000000000LL;
-
-	si53xx_write_n_div(v4l2->dev, 0, 0x2328000000ULL, denom);
-	si53xx_write_n_div(v4l2->dev, 1, 0x2331000000ULL, denom);
-
-	return 0;
+	return v4l2->dev->pll->ops->tune(v4l2->dev->pll, ppt);
 }
 
 struct forward_v4l2_dev_ops fd922_ops = {
diff --git a/forward/Makefile b/forward/Makefile
index 38ad428..f6a23e3 100644
--- a/forward/Makefile
+++ b/forward/Makefile
@@ -3,7 +3,8 @@ obj-m := forward.o
 forward-objs := forward-core.o forward-irq.o forward-ctl.o \
                 forward-vdma.o forward-fd788.o forward-fd722.o \
                 forward-fd720.o forward-fd922.o forward-fdio.o \
-                forward-fd940.o forward-fd2110.o forward-si532x.o
+                forward-fd940.o forward-fd2110.o forward-si532x.o \
+                forward-si534x.o
 
 SRC ?= $(PWD)
 KERNEL_SRC ?= /lib/modules/$(shell uname -r)/build
diff --git a/forward/forward-fd922.c b/forward/forward-fd922.c
index 1726d5a..f991803 100644
--- a/forward/forward-fd922.c
+++ b/forward/forward-fd922.c
@@ -20,6 +20,7 @@
 
 #include "forward.h"
 #include "forward-vdma.h"
+#include "forward-si534x.h"
 
 #include "fd922_reg.h"
 
@@ -80,11 +81,24 @@ static bool fd922_spi_xfer(struct forward_dev *dev, u8 bus, u8 *tx, int tx_bits,
 
 static int fd922_init(struct forward_dev *dev)
 {
+	struct forward_pll_mode *mode;
+
 	dev->spi_xfer = &fd922_spi_xfer;
+
+	forward_si534x_init(dev, &dev->pll, 4, 3);
+	mode = dev->pll->ops->find_mode(dev->pll, 0, 0, 148500000, false);
+	if (mode) {
+		printk("TEST SWITCH\n");
+		dev->pll->ops->switch_mode(dev->pll, mode);
+		dev->pll->ops->select_input(dev->pll, 0, true, false);
+		dev->pll->ops->calibrate(dev->pll);
+	}
+
 	return 0;
 }
 static void fd922_fini(struct forward_dev *dev)
 {
+	forward_si534x_fini(dev, &dev->pll);
 }
 
 static void fd922_read_id(struct forward_dev *dev)
diff --git a/forward/forward-si532x.c b/forward/forward-si532x.c
index 568d8e9..385a2ac 100644
--- a/forward/forward-si532x.c
+++ b/forward/forward-si532x.c
@@ -81,11 +81,11 @@ static int si532x_write_reg(struct forward_si532x *si532x, u8 reg, u8 data)
 					 &data);
 }
 
-static int si532x_read_reg(struct forward_si532x *si532x, u8 reg, u8 *data)
-{
-	return si532x->pll.dev->i2c_xfer(si532x->pll.dev, si532x->bus, true, si532x->address, reg,
-					 data);
-}
+//static int si532x_read_reg(struct forward_si532x *si532x, u8 reg, u8 *data)
+//{
+//	return si532x->pll.dev->i2c_xfer(si532x->pll.dev, si532x->bus, true, si532x->address, reg,
+//					 data);
+//}
 
 static int si532x_configure_factors(struct forward_si532x *si532x, u8 bwsel, u8 n1_hs, u32 nc1_ls,
 				    u8 n2_hs, u32 n2_ls, u32 n31, u32 n32)
@@ -217,22 +217,24 @@ static int si532x_select_input(struct forward_pll *pll, int input, bool master,
 	return 0;
 }
 
-int si532x_tune(struct forward_pll *pll, s64 ppt)
+static int si532x_tune(struct forward_pll *pll, s64 ppt)
 {
 	return -ENOTSUPP;
 }
 
-void si532x_calibrate(struct forward_pll *pll)
+static void si532x_calibrate(struct forward_pll *pll)
 {
 	struct forward_si532x *si532x = container_of(pll, struct forward_si532x, pll);
 	si532x_write_reg(si532x, 0x88, 0x40);
 }
 
-static struct forward_pll_ops si5324_ops = { .find_mode = si532x_find_mode,
-					     .switch_mode = si532x_switch_mode,
-					     .select_input = si532x_select_input,
-					     .tune = si532x_tune,
-					     .calibrate = si532x_calibrate };
+static struct forward_pll_ops si532x_ops = {
+	.find_mode = si532x_find_mode,
+	.switch_mode = si532x_switch_mode,
+	.select_input = si532x_select_input,
+	.tune = si532x_tune,
+	.calibrate = si532x_calibrate,
+};
 
 int forward_si532x_init(struct forward_dev *dev, struct forward_pll **pll, u8 bus, u8 address,
 			int num_inputs)
@@ -244,7 +246,7 @@ int forward_si532x_init(struct forward_dev *dev, struct forward_pll **pll, u8 bu
 	si532x->pll.dev = dev;
 	si532x->pll.features = FORWARD_PLL_FEATURE_FALLBACK;
 	si532x->pll.num_inputs = num_inputs;
-	si532x->pll.ops = &si5324_ops;
+	si532x->pll.ops = &si532x_ops;
 	si532x->bus = bus;
 	si532x->address = address;
 
diff --git a/forward/forward-si534x.c b/forward/forward-si534x.c
new file mode 100644
index 0000000..086e2b3
--- /dev/null
+++ b/forward/forward-si534x.c
@@ -0,0 +1,454 @@
+/*
+   forward-si534x.c - si534x PLL driver for SoftLab-NSK Forward 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-pll.h"
+#include <linux/delay.h>
+
+struct forward_si534x {
+	struct forward_pll pll;
+
+	u8 bus;
+	u8 bank;
+};
+
+struct forward_si534x_mode {
+	int in_freq;
+	int pfd_freq;
+	bool m;
+	u64 Pn[3];
+	u32 Pd[3];
+	u64 Mn;
+	u32 Md;
+	u64 Bslow;
+	u64 Bfast;
+	u8 oof_div_sel[3];
+	u32 oof_ratio_ref[3];
+	u32 lol_clr_delay;
+	u32 hold_15m_cyc_count;
+	u16 pfd_en_delay;
+};
+/*
+const struct forward_si532x_mode forward_si532x_modes[] = {
+	{ 13500000 / 200, false, 5, 9, 4, 11, 7200, 1, 400 },
+	{ 13500000 / 200, true, 3, 9, 4, 11, 36000, 5, 2002 },
+	{ 13500000 / 240, false, 5, 9, 4, 11, 8640, 1, 480 },
+	{ 13500000 / 400, false, 5, 9, 4, 11, 14400, 1, 800 },
+	{ 13500000 / 400, true, 2, 9, 4, 11, 72000, 5, 4004 },
+	{ 13500000 / 480, false, 4, 9, 4, 11, 17280, 1, 960 },
+	{ 13500000 / 500, false, 4, 9, 4, 11, 18000, 1, 1000 },
+	{ 13500000 / 500, true, 4, 9, 4, 11, 18000, 1, 1001 },
+	{ 13500000 / 400, false, 5, 9, 4, 11, 14400, 1, 800 },
+	{ 13500000 / 400, true, 2, 9, 4, 11, 72000, 5, 4004 },
+	{ 13500000 / 480, false, 4, 9, 4, 11, 17280, 1, 960 },
+	{ 13500000 / 300, false, 5, 9, 4, 11, 10800, 1, 600 },
+	{ 13500000 / 300, true, 3, 9, 4, 11, 54000, 5, 3003 },
+	{ 13500000 / 360, false, 5, 9, 4, 11, 12960, 1, 720 },
+	{ 13500000 / 600, false, 4, 9, 4, 11, 21600, 1, 1200 },
+	{ 13500000 / 600, true, 2, 9, 4, 11, 108000, 5, 6006 },
+	{ 13500000 / 720, false, 4, 9, 4, 11, 25920, 1, 1440 },
+	{ 13500000 / 750, false, 4, 9, 4, 11, 27000, 1, 1500 },
+	{ 13500000 / 750, true, 3, 9, 4, 11, 54000, 2, 3003 },
+	{ 13500000 / 864, false, 3, 9, 4, 11, 31104, 1, 1728 },
+	{ 13500000 / 858, false, 3, 9, 4, 11, 30888, 1, 1716 },
+	{ 13500000 / 858, false, 3, 9, 4, 11, 30888, 1, 1716 },
+	{ 6750000, false, 10, 9, 4, 11, 288, 4, 16 },
+	{ 74250000, false, 10, 9, 4, 11, 288, 44, 16 },
+	{ 74250000, true, 8, 9, 4, 10, 1800, 250, 91 },
+	{ 148500000, false, 10, 9, 4, 11, 252, 77, 14 },
+	{ 148500000, true, 8, 9, 4, 10, 1800, 500, 91 },
+};
+*/
+const struct forward_si534x_mode si534x_default_mode = { -1,
+							 -1,
+							 false,
+							 { 1, 2640, 2640 },
+							 { 1, 1, 1 },
+							 0x00B9A000000000,
+							 0x80000000,
+							 0x07070F0F170F,
+							 0x0707090A2B18,
+							 { 1, 12, 12 },
+							 { 0x1111111, 0x1600000, 0x1600000 },
+							 0x0000E1AD,
+							 0x00E77C,
+							 0x03B3 };
+
+/*
+const int forward_si532x_modes_count =
+	sizeof(forward_si532x_modes) / sizeof(struct forward_si532x_mode);
+*/
+
+/*static int si532x_write_reg(struct forward_si532x *si532x, u8 reg, u8 data)
+{
+	return si532x->pll.dev->i2c_xfer(si532x->pll.dev, si532x->bus, false, si532x->address, reg,
+					 &data);
+}
+
+static int si532x_read_reg(struct forward_si532x *si532x, u8 reg, u8 *data)
+{
+	return si532x->pll.dev->i2c_xfer(si532x->pll.dev, si532x->bus, true, si532x->address, reg,
+					 data);
+}*/
+
+/*
+static int si532x_configure_factors(struct forward_si532x *si532x, u8 bwsel, u8 n1_hs, u32 nc1_ls,
+				    u8 n2_hs, u32 n2_ls, u32 n31, u32 n32)
+{
+	u32 reg_value;
+
+	if (bwsel > 15)
+		return -EINVAL;
+
+	if (n1_hs < 4 || n1_hs > 11)
+		return -EINVAL;
+
+	if (((nc1_ls != 1) && (nc1_ls % 2)) || (nc1_ls < 1) || (nc1_ls > (1 << 20)))
+		return -EINVAL;
+
+	if (n2_hs < 4 || n2_hs > 11)
+		return -EINVAL;
+
+	if (((n2_ls != 1) && (n2_ls % 2)) || (n2_ls < 1) || (n2_ls > (1 << 20)))
+		return -EINVAL;
+
+	if ((n31 < 1) || (n31 > (1 << 19)))
+		return -EINVAL;
+
+	if ((n32 < 1) || (n32 > (1 << 19)))
+		return -EINVAL;
+
+	si532x_write_reg(si532x, 0x02, (bwsel << 4) | (0x2));
+	si532x_write_reg(si532x, 0x19, ((n1_hs - 4) << 5));
+
+	reg_value = nc1_ls - 1;
+	si532x_write_reg(si532x, 0x1F, (reg_value >> 16) & 0x0F);
+	si532x_write_reg(si532x, 0x20, (reg_value >> 8) & 0xFF);
+	si532x_write_reg(si532x, 0x21, reg_value & 0xFF);
+
+	reg_value = n2_ls - 1;
+	si532x_write_reg(si532x, 0x28, ((n2_hs - 4) << 5) | ((reg_value >> 16) & 0x0F));
+	si532x_write_reg(si532x, 0x29, (reg_value >> 8) & 0xFF);
+	si532x_write_reg(si532x, 0x2A, reg_value & 0xFF);
+
+	reg_value = n31 - 1;
+	si532x_write_reg(si532x, 0x2B, (reg_value >> 16) & 0x07);
+	si532x_write_reg(si532x, 0x2C, (reg_value >> 8) & 0xFF);
+	si532x_write_reg(si532x, 0x2D, reg_value & 0xFF);
+
+	reg_value = n32 - 1;
+	si532x_write_reg(si532x, 0x2E, (reg_value >> 16) & 0x07);
+	si532x_write_reg(si532x, 0x2F, (reg_value >> 8) & 0xFF);
+	si532x_write_reg(si532x, 0x30, reg_value & 0xFF);
+
+	return 0;
+}
+*/
+/*
+static int si532x_load_defaults(struct forward_si532x *si532x)
+{
+	// PLL mode, CLK always on, no bypass
+	si532x_write_reg(si532x, 0x00, 0x34);
+	// CLKIN1 > CLKIN2 priority
+	//si532x_write_reg(si532x, 0x01, 0xE1);
+	// No output disabling during calibration, CLKIN2 selected
+	si532x_write_reg(si532x, 0x03, 0x55);
+	// Automatic clock selection, default hold history
+	//si532x_write_reg(si532x, 0x04, 0x92);
+	// LVDS outputs
+	si532x_write_reg(si532x, 0x06, 0x0F);
+	// External crystal as reference
+	si532x_write_reg(si532x, 0x07, 0x28);
+	// CLKOUT1 enabled, CLKOUT2 disabled
+	si532x_write_reg(si532x, 0x0A, 0x08);
+	// Frequency offset and phase error detector
+	si532x_write_reg(si532x, 0x13, 0x2F);
+	// Enable CK2_BAD, LOL, INT pins
+	si532x_write_reg(si532x, 0x14, 0x3B);
+	// Disable clock selection pins
+	si532x_write_reg(si532x, 0x15, 0xFC);
+	// Enable fast lock
+	si532x_write_reg(si532x, 0x89, 0x01);
+
+	return 0;
+}
+*/
+inline void si534x_select_bank(struct forward_si534x *si534x, u16 addr)
+{
+	u8 bank = ((addr >> 8) & 0xFF);
+	u8 tx[4] = { 0x00, 0x01, 0x40, bank };
+
+	if (si534x->bank != bank)
+		si534x->pll.dev->spi_xfer(si534x->pll.dev, si534x->bus, tx, 32, NULL, 0);
+	si534x->bank = bank;
+}
+
+inline void si534x_write_data(struct forward_si534x *si534x, u16 addr, const u8 *data, u8 count)
+{
+	u8 tx[2 + 16];
+	int i;
+
+	si534x_select_bank(si534x, addr);
+
+	tx[0] = 0x00;
+	tx[1] = (addr & 0xFF);
+	for (i = 0; i < count; i++) {
+		tx[2 + i * 2] = 0x60;
+		tx[2 + i * 2 + 1] = data[i];
+	}
+
+	si534x->pll.dev->spi_xfer(si534x->pll.dev, si534x->bus, tx, (2 + count * 2) * 8, NULL, 0);
+}
+
+//inline void si534x_read_data(struct forward_si534x *si534x, u16 addr, u8* data, u8 count)
+//{
+//	u8 tx[2 + 16];
+//	u8 rx[2 + 16];
+//	int i;
+//
+//	si534x_select_bank(si534x, addr);
+//
+//	tx[0] = 0x00;
+//	tx[1] = (addr & 0xFF);
+//	for (i = 0; i < count; i++) {
+//		tx[2 + i * 2] = 0xA0;
+//		tx[2 + i * 2 + 1] = 0x00;
+//	}
+//	si534x->pll.dev->spi_xfer(si534x->pll.dev, si534x->bus, tx, (2 + count * 2) * 8, rx, (2 + count * 2) * 8);
+//	for (i = 0; i < count; i++)
+//		data[i] = rx[2 + i * 2 + 1];
+//}
+
+static void si534x_write_reg_8(struct forward_si534x *si534x, u16 addr, u8 data)
+{
+	si534x_write_data(si534x, addr, &data, 1);
+}
+
+//static void si534x_read_reg_8(struct forward_si534x *si534x, u16 addr, u8 *data)
+//{
+//	si534x_read_data(si534x, addr, data, 1);
+//}
+
+static void si534x_write_reg_16(struct forward_si534x *si534x, u16 addr, u16 data)
+{
+	si534x_write_data(si534x, addr, (u8 *)&data, 2);
+}
+
+//static void si534x_read_reg_16(struct forward_si534x *si534x, u16 addr, u16 *data)
+//{
+//	si534x_read_data(si534x, addr, (u8*)data, 2);
+//}
+
+static void si534x_write_reg_24(struct forward_si534x *si534x, u16 addr, u32 data)
+{
+	si534x_write_data(si534x, addr, (u8 *)&data, 3);
+}
+
+//static void si534x_read_reg_24(struct forward_si534x *si534x, u16 addr, u32 *data)
+//{
+//	si534x_read_data(si534x, addr, (u8*)data, 3);
+//}
+
+static void si534x_write_reg_32(struct forward_si534x *si534x, u16 addr, u32 data)
+{
+	si534x_write_data(si534x, addr, (u8 *)&data, 4);
+}
+
+//static void si534x_read_reg_32(struct forward_si534x *si534x, u16 addr, u32 *data)
+//{
+//	si534x_read_data(si534x, addr, (u8*)data, 4);
+//}
+
+static void si534x_write_reg_48(struct forward_si534x *si534x, u16 addr, u64 data)
+{
+	si534x_write_data(si534x, addr, (u8 *)&data, 6);
+}
+
+//static void si534x_read_reg_48(struct forward_si534x *si534x, u16 addr, u64 *data)
+//{
+//	si534x_read_data(si534x, addr, (u8*)data, 6);
+//}
+
+static void si534x_write_reg_56(struct forward_si534x *si534x, u16 addr, u64 data)
+{
+	si534x_write_data(si534x, addr, (u8 *)&data, 7);
+}
+
+//static void si534x_read_reg_56(struct forward_si534x *si534x, u16 addr, u64 *data)
+//{
+//	si534x_read_data(si534x, addr, (u8*)data, 7);
+//}
+
+static struct forward_pll_mode *si534x_find_mode(struct forward_pll *pll, s64 in_freq,
+						 s64 common_in_freq, s64 out_freq, bool m)
+{
+	int i;
+
+	if (out_freq != 148500000)
+		return NULL;
+
+	if (in_freq <= 0) {
+		//if (!m)
+		return (struct forward_pll_mode *)&si534x_default_mode;
+		//else
+		//	return (struct forward_pll_mode *)&si532x_default_mode_m;
+	}
+
+	/*for (i = 0; i < forward_si532x_modes_count; i++) {
+		const struct forward_si532x_mode *cur = &forward_si532x_modes[i];
+		if ((cur->in_freq == in_freq) && (cur->m == m)) {
+			return (struct forward_pll_mode *)cur;
+		}
+	}*/
+	return NULL;
+}
+
+static int si534x_switch_mode(struct forward_pll *pll, struct forward_pll_mode *mode)
+{
+	struct forward_si534x *si534x = container_of(pll, struct forward_si534x, pll);
+	struct forward_si534x_mode *m = (struct forward_si534x_mode *)mode;
+
+	si534x_write_reg_8(si534x, 0x0B24, 0xC0);
+	si534x_write_reg_8(si534x, 0x0B25, 0x00);
+	si534x_write_reg_8(si534x, 0x0B40, 0x01);
+
+	msleep(300);
+
+	si534x_write_reg_8(si534x, 0x0041, m->oof_div_sel[0]);
+	si534x_write_reg_8(si534x, 0x0042, m->oof_div_sel[1]);
+	si534x_write_reg_8(si534x, 0x0043, m->oof_div_sel[2]);
+
+	si534x_write_reg_32(si534x, 0x005A, m->oof_ratio_ref[0]);
+	si534x_write_reg_32(si534x, 0x005E, m->oof_ratio_ref[1]);
+	si534x_write_reg_32(si534x, 0x0062, m->oof_ratio_ref[2]);
+	si534x_write_reg_32(si534x, 0x00A9, m->lol_clr_delay);
+
+	si534x_write_reg_48(si534x, 0x0208, m->Pn[0]);
+	si534x_write_reg_32(si534x, 0x020E, m->Pd[0]);
+	si534x_write_reg_48(si534x, 0x0212, m->Pn[1]);
+	si534x_write_reg_32(si534x, 0x0218, m->Pd[1]);
+	si534x_write_reg_48(si534x, 0x021C, m->Pn[2]);
+	si534x_write_reg_32(si534x, 0x0222, m->Pd[2]);
+	si534x_write_reg_8(si534x, 0x0230, 0x07);
+
+	si534x_write_reg_48(si534x, 0x0508, m->Bslow);
+	si534x_write_reg_48(si534x, 0x050E, m->Bfast);
+
+	si534x_write_reg_56(si534x, 0x0515, m->Mn);
+	si534x_write_reg_32(si534x, 0x051C, m->Md);
+	si534x_write_reg_32(si534x, 0x0520, 0x1);
+
+	si534x_write_reg_24(si534x, 0x0532, m->hold_15m_cyc_count);
+	si534x_write_reg_16(si534x, 0x0589, m->pfd_en_delay);
+
+	si534x_write_reg_8(si534x, 0x001C, 0x01);
+	si534x_write_reg_8(si534x, 0x0B40, 0x00);
+	si534x_write_reg_8(si534x, 0x0B24, 0xC3);
+	si534x_write_reg_8(si534x, 0x0B25, 0xC2);
+
+	return 0;
+}
+
+static int si534x_select_input(struct forward_pll *pll, int input, bool master, bool autoswitch)
+{
+	struct forward_si534x *si534x = container_of(pll, struct forward_si534x, pll);
+
+	si534x_write_reg_8(si534x, 0x052A, (input << 1) | 0x1);
+
+	if (autoswitch) {
+		switch (input) {
+		case 0:
+			si534x_write_reg_16(si534x, 0x0538, 0x0321);
+			break;
+		case 1:
+			si534x_write_reg_16(si534x, 0x0538, 0x0213);
+			break;
+		case 2:
+			si534x_write_reg_16(si534x, 0x0538, 0x0123);
+			break;
+		}
+		si534x_write_reg_8(si534x, 0x0536, 0x6);
+	} else
+		si534x_write_reg_8(si534x, 0x0536, 0x4);
+
+	si534x_write_reg_8(si534x, 0x0535, master ? 1 : 0);
+
+	return 0;
+}
+
+static int si534x_tune(struct forward_pll *pll, s64 ppt)
+{
+	struct forward_si534x *si534x = container_of(pll, struct forward_si534x, pll);
+	u32 denom = 0xc8000000 + 0xc8000000LL * ppt / 1000000000000LL;
+
+	printk("si534x_tune(): %llu/%llu\n", 0x2328000000ULL, denom);
+
+	si534x_write_reg_48(si534x, 0x302, 0x2328000000ULL);
+	si534x_write_reg_32(si534x, 0x308, denom);
+	si534x_write_reg_48(si534x, 0x30d, 0x2331000000ULL);
+	si534x_write_reg_32(si534x, 0x313, denom);
+	si534x_write_reg_32(si534x, 0x338, 2);
+
+	return 0;
+}
+
+static void si534x_calibrate(struct forward_pll *pll)
+{
+	struct forward_si534x *si534x = container_of(pll, struct forward_si534x, pll);
+	si534x_write_reg_8(si534x, 0x001C, 0x4);
+}
+
+static struct forward_pll_ops si534x_ops = {
+	.find_mode = si534x_find_mode,
+	.switch_mode = si534x_switch_mode,
+	.select_input = si534x_select_input,
+	.tune = si534x_tune,
+	.calibrate = si534x_calibrate,
+};
+
+int forward_si534x_init(struct forward_dev *dev, struct forward_pll **pll, u8 bus, int num_inputs)
+{
+	struct forward_si534x *si534x = kzalloc(sizeof(struct forward_si534x), GFP_KERNEL);
+
+	if (!si534x)
+		return -ENOMEM;
+
+	si534x->pll.dev = dev;
+	si534x->pll.features = FORWARD_PLL_FEATURE_FALLBACK | FORWARD_PLL_FEATURE_MASTER |
+			       FORWARD_PLL_FEATURE_TUNE;
+	si534x->pll.num_inputs = num_inputs;
+	si534x->pll.ops = &si534x_ops;
+	si534x->bus = bus;
+	si534x->bank = 0xFF;
+
+	*pll = &si534x->pll;
+
+	return 0;
+}
+EXPORT_SYMBOL(forward_si534x_init);
+
+void forward_si534x_fini(struct forward_dev *dev, struct forward_pll **pll)
+{
+	if (pll) {
+		struct forward_si534x *si534x = container_of(*pll, struct forward_si534x, pll);
+		kfree(si534x);
+		*pll = NULL;
+	}
+}
+EXPORT_SYMBOL(forward_si534x_fini);
diff --git a/forward/forward-si534x.h b/forward/forward-si534x.h
new file mode 100644
index 0000000..538281d
--- /dev/null
+++ b/forward/forward-si534x.h
@@ -0,0 +1,24 @@
+/*
+   forward-si534x.h - si534x PLL driver for SoftLab-NSK Forward 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-pll.h"
+
+int forward_si534x_init(struct forward_dev *dev, struct forward_pll **pll, u8 bus, int num_inputs);
+void forward_si534x_fini(struct forward_dev *dev, struct forward_pll **pll);
-- 
2.39.1


From 955ede7ea86d7d3ffaf390b78406578f1244520f Mon Sep 17 00:00:00 2001
From: Konstantin Oblaukhov <oblaukhov.konstantin@gmail.com>
Date: Wed, 15 Mar 2023 17:19:02 +0700
Subject: [PATCH 05/13] Genlock for FD922.

---
 forward-v4l2/forward-v4l2-fd722.c   |   6 +-
 forward-v4l2/forward-v4l2-fd788.c   |   6 +-
 forward-v4l2/forward-v4l2-fd922.c   | 215 ++++++++++
 forward-v4l2/forward-v4l2-genlock.c |   3 +-
 forward/forward-fd922.c             |   7 -
 forward/forward-si532x.c            |  11 +-
 forward/forward-si534x.c            | 605 +++++++++++++++++++++-------
 include/forward-pll.h               |   4 +-
 8 files changed, 685 insertions(+), 172 deletions(-)

diff --git a/forward-v4l2/forward-v4l2-fd722.c b/forward-v4l2/forward-v4l2-fd722.c
index 4f1b5b2..d5f1809 100644
--- a/forward-v4l2/forward-v4l2-fd722.c
+++ b/forward-v4l2/forward-v4l2-fd722.c
@@ -85,7 +85,7 @@ static void fd722_check_pll_configuration(struct forward_v4l2 *v4l2)
 
 	if ((clk_m != any_m) || !clk_valid) {
 		struct forward_pll_mode *mode;
-		mode = pll->ops->find_mode(pll, 0, 0, 148500000, any_m);
+		mode = pll->ops->find_mode(pll, 0, 0, any_m, 148500000, any_m);
 
 		if (!mode)
 			return;
@@ -731,7 +731,7 @@ static int fd722_genlock_switch(const struct forward_v4l2_genlock *gl,
 	struct forward_pll_mode *mode = NULL;
 
 	if (src == V4L_FORWARD_GENLOCK_SRC_MASTER) {
-		mode = pll->ops->find_mode(pll, 0, 0, 148500000, false);
+		mode = pll->ops->find_mode(pll, 0, 0, false, 148500000, false);
 		pll->ops->switch_mode(pll, mode);
 		pll->ops->select_input(pll, 1, false, false);
 		pll->ops->calibrate(pll);
@@ -754,7 +754,7 @@ static int fd722_genlock_switch(const struct forward_v4l2_genlock *gl,
 			gl.mode = (src == V4L_FORWARD_GENLOCK_SRC_IN1) ? 3 : 2;
 		}
 
-		mode = pll->ops->find_mode(pll, pll_in_freq, 0, 148500000, t->sdi.m);
+		mode = pll->ops->find_mode(pll, pll_in_freq, 0, t->sdi.m, 148500000, t->sdi.m);
 		if (!mode)
 			return -EINVAL;
 
diff --git a/forward-v4l2/forward-v4l2-fd788.c b/forward-v4l2/forward-v4l2-fd788.c
index 6f5725e..d86b292 100644
--- a/forward-v4l2/forward-v4l2-fd788.c
+++ b/forward-v4l2/forward-v4l2-fd788.c
@@ -91,7 +91,7 @@ static void fd788_check_pll_configuration(struct forward_v4l2 *v4l2)
 
 	if ((clk_m != any_m) || !clk_valid) {
 		struct forward_pll_mode *mode;
-		mode = pll->ops->find_mode(pll, 0, 0, 148500000, any_m);
+		mode = pll->ops->find_mode(pll, 0, 0, any_m, 148500000, any_m);
 
 		if (!mode)
 			return;
@@ -914,7 +914,7 @@ static int fd788_genlock_switch(const struct forward_v4l2_genlock *gl,
 	struct forward_pll_mode *mode = NULL;
 
 	if (src == V4L_FORWARD_GENLOCK_SRC_MASTER) {
-		mode = pll->ops->find_mode(pll, 0, 0, 148500000, false);
+		mode = pll->ops->find_mode(pll, 0, 0, false, 148500000, false);
 		pll->ops->switch_mode(pll, mode);
 		pll->ops->select_input(pll, 1, false, false);
 		pll->ops->calibrate(pll);
@@ -937,7 +937,7 @@ static int fd788_genlock_switch(const struct forward_v4l2_genlock *gl,
 			gl.mode = (src == V4L_FORWARD_GENLOCK_SRC_IN1) ? 3 : 2;
 		}
 
-		mode = pll->ops->find_mode(pll, pll_in_freq, 0, 148500000, t->sdi.m);
+		mode = pll->ops->find_mode(pll, pll_in_freq, 0, t->sdi.m, 148500000, t->sdi.m);
 		if (!mode)
 			return -EINVAL;
 
diff --git a/forward-v4l2/forward-v4l2-fd922.c b/forward-v4l2/forward-v4l2-fd922.c
index 56b7621..937a738 100644
--- a/forward-v4l2/forward-v4l2-fd922.c
+++ b/forward-v4l2/forward-v4l2-fd922.c
@@ -25,6 +25,8 @@
 #include "forward-v4l2-ioctl.h"
 #include "forward-pll.h"
 
+#include "forward-v4l2-genlock.h"
+
 #include "fd922_reg.h"
 
 #define FD922_BUFFER_NUMBER(in, irq) (((irq) >> ((in)*3 + 1)) & 0x3)
@@ -32,6 +34,12 @@
 #define FD922_NUMBER_IN_BUFFERS (4)
 #define FD922_NUMBER_OUT_BUFFERS (2)
 
+#define FD922_GENLOCK_ANALOG_DELAY 300
+#define FD922_GENLOCK_SD_NTSC_DELAY 27953
+#define FD922_GENLOCK_SD_PAL_DELAY 364
+#define FD922_GENLOCK_HD_DELAY 189
+#define FD922_GENLOCK_3G_DELAY 97
+
 // 50 buffers per second at 270 Mbit/s
 #define FD922_ASI_IN_BUFFER_SIZE (270000000ULL / 10 / 50)
 #define FD922_ASI_IN_BUFFER_SIZE_RAW (270000000ULL / 8 / 50)
@@ -630,6 +638,206 @@ static s64 fd922_set_clock_deviation(struct forward_v4l2 *v4l2, s64 ppt)
 	return v4l2->dev->pll->ops->tune(v4l2->dev->pll, ppt);
 }
 
+static int fd922_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 fd922_timings_query(&v4l2->io[0], t);
+	else if (src == V4L_FORWARD_GENLOCK_SRC_IN1)
+		return fd922_timings_query(&v4l2->io[1], t);
+	else if (src == V4L_FORWARD_GENLOCK_SRC_ANALOG) {
+		FD922_AnalogSync cnt = FD922_AnalogSync_R(v4l2->dev->csr);
+
+		if ((cnt.hsync > 2048) || (cnt.vsync > 1000000) || (cnt.hsync == 0) ||
+		    (cnt.vsync == 0))
+			return -ENOLINK;
+
+		return forward_v4l2_timing_guess_analog(18000000 / cnt.hsync,
+							18000000000 / cnt.vsync, t) ?
+			       0 :
+			       -ERANGE;
+	} else
+		return -EINVAL;
+}
+
+static int fd922_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;
+	FD922_SyncGenerator sg = { 0 };
+	FD922_SyncGeneratorField sgf = { 0 };
+	struct forward_pll_mode *mode = NULL;
+
+	if (src == V4L_FORWARD_GENLOCK_SRC_MASTER) {
+		mode = pll->ops->find_mode(pll, 0, 0, false, 148500000, false);
+		pll->ops->switch_mode(pll, mode);
+		pll->ops->select_input(pll, 0, true, false);
+		pll->ops->calibrate(pll);
+	} else {
+		unsigned int pll_pix_freq = 0;
+		unsigned int pll_h_freq = 0;
+		int pll_input = 0;
+
+		FD922_Genlock gl = FD922_Genlock_R(v4l2->dev->csr);
+
+		pll_pix_freq = forward_v4l2_pixel_clock(t);
+		pll_h_freq = pll_pix_freq / t->sdi.width;
+		if (t->sdi.mode == FORWARD_SDI_MODE_6G_SDI) {
+			pll_pix_freq /= 2;
+			pll_h_freq /= 2;
+		} else if (t->sdi.mode == FORWARD_SDI_MODE_12G_SDI) {
+			pll_pix_freq /= 4;
+			pll_h_freq /= 2;
+		}
+		if (src == V4L_FORWARD_GENLOCK_SRC_ANALOG) {
+			pll_input = 0;
+			gl.mode = (t->sdi.mode == FORWARD_SDI_MODE_SD_SDI) ? 1 : 0;
+		} else {
+			pll_input = (src == V4L_FORWARD_GENLOCK_SRC_IN0) ? 1 : 2;
+			gl.mode = (src == V4L_FORWARD_GENLOCK_SRC_IN0) ? 2 : 3;
+		}
+
+		gl.m = t->sdi.m ? true : false;
+
+		mode = pll->ops->find_mode(pll, pll_pix_freq, pll_h_freq, t->sdi.m, 148500000,
+					   false);
+		if (!mode)
+			return -EINVAL;
+
+		FD922_Genlock_W(v4l2->dev->csr, gl);
+		pll->ops->switch_mode(pll, mode);
+
+		pll->ops->select_input(pll, pll_input, false, false);
+		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;
+	}
+	sg.m = t->sdi.m ? 1 : 0;
+
+	FD922_SyncGenerator_W(v4l2->dev->csr, 0, sg);
+	FD922_SyncGeneratorField_W(v4l2->dev->csr, 0, sgf);
+
+	return 0;
+}
+
+u32 fd922_genlock_sync_irq_mask(const struct forward_v4l2_genlock *gl, int num)
+{
+	return num ? (1 << 14) : (1 << 12);
+}
+
+bool fd922_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 = 0, sync_time;
+
+	if (!(irq & (num ? (1 << 14) : (1 << 12))))
+		return false;
+
+	*field = (irq & (num ? (1 << 15) : (1 << 13))) ? true : false;
+	sync_time = FD922_SyncGeneratorTime_R(v4l2->dev->csr, num).time;
+
+	if (gl->source == V4L_FORWARD_GENLOCK_SRC_IN0)
+		ref_time = FD922_VideoInIRQRefTime_R(v4l2->dev->csr, 0).time;
+	else if (gl->source == V4L_FORWARD_GENLOCK_SRC_IN1)
+		ref_time = FD922_VideoInIRQRefTime_R(v4l2->dev->csr, 1).time;
+	else if (gl->source == V4L_FORWARD_GENLOCK_SRC_ANALOG)
+		ref_time = FD922_AnalogVSyncTime_R(v4l2->dev->csr).time;
+
+	*gl_offset = (s32)(sync_time - ref_time);
+
+	return true;
+}
+
+void fd922_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;
+	FD922_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 += -FD922_GENLOCK_ANALOG_DELAY;
+	else {
+		if (gl->timings.sdi.mode == FORWARD_SDI_MODE_SD_SDI)
+			sync_gen_offset += (gl->timings.sdi.height == 525) ?
+						   -FD922_GENLOCK_SD_NTSC_DELAY :
+						   -FD922_GENLOCK_SD_PAL_DELAY;
+		else if (gl->timings.sdi.mode == FORWARD_SDI_MODE_HD_SDI)
+			sync_gen_offset += -FD922_GENLOCK_HD_DELAY;
+		else
+			sync_gen_offset += -FD922_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;
+	FD922_SyncGeneratorPhase_W(gl->v4l2->dev->csr, num, ph);
+}
+
+void fd922_genlock_sync_output(const struct forward_v4l2_genlock *gl, struct forward_v4l2_io *io)
+{
+	FD922_VideoOutCS cs = FD922_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;
+	cs.clockM = gl->timings.sdi.m ? 1 : 0;
+	FD922_VideoOutCS_W(io->v4l2->dev->csr, (io->index - 2), cs);
+}
+
 struct forward_v4l2_dev_ops fd922_ops = {
 	.irq_mask = fd922_irq_mask,
 	.irq_info = fd922_irq_info,
@@ -653,4 +861,11 @@ struct forward_v4l2_dev_ops fd922_ops = {
 	.hw_timestamp = fd922_hw_timestamp,
 	.clock_deviation = fd922_clock_deviation,
 	.set_clock_deviation = fd922_set_clock_deviation,
+
+	.genlock_query = fd922_genlock_query,
+	.genlock_switch = fd922_genlock_switch,
+	.genlock_sync_irq_mask = fd922_genlock_sync_irq_mask,
+	.genlock_sync_irq_info = fd922_genlock_sync_irq_info,
+	.genlock_sync_offset = fd922_genlock_sync_offset,
+	.genlock_sync_output = fd922_genlock_sync_output,
 };
diff --git a/forward-v4l2/forward-v4l2-genlock.c b/forward-v4l2/forward-v4l2-genlock.c
index 83ef2d1..320d8a8 100644
--- a/forward-v4l2/forward-v4l2-genlock.c
+++ b/forward-v4l2/forward-v4l2-genlock.c
@@ -35,7 +35,8 @@ static void forward_v4l2_genlock_irq(void *priv, u32 irq, u64 timestamp)
 	spin_lock_irqsave(&gl->lock, sflags);
 	if (!field) {
 		if (gl->state == V4L_FORWARD_GENLOCK_LOCKING) {
-			if (gl->ref_offset == offset)
+			int delta = (int)gl->ref_offset - (int)offset;
+			if (abs(delta) <= 1)
 				gl->stable_ref_count++;
 			else
 				gl->stable_ref_count = 0;
diff --git a/forward/forward-fd922.c b/forward/forward-fd922.c
index f991803..9722575 100644
--- a/forward/forward-fd922.c
+++ b/forward/forward-fd922.c
@@ -86,13 +86,6 @@ static int fd922_init(struct forward_dev *dev)
 	dev->spi_xfer = &fd922_spi_xfer;
 
 	forward_si534x_init(dev, &dev->pll, 4, 3);
-	mode = dev->pll->ops->find_mode(dev->pll, 0, 0, 148500000, false);
-	if (mode) {
-		printk("TEST SWITCH\n");
-		dev->pll->ops->switch_mode(dev->pll, mode);
-		dev->pll->ops->select_input(dev->pll, 0, true, false);
-		dev->pll->ops->calibrate(dev->pll);
-	}
 
 	return 0;
 }
diff --git a/forward/forward-si532x.c b/forward/forward-si532x.c
index 385a2ac..07f1c98 100644
--- a/forward/forward-si532x.c
+++ b/forward/forward-si532x.c
@@ -167,16 +167,16 @@ static int si532x_load_defaults(struct forward_si532x *si532x)
 	return 0;
 }
 
-static struct forward_pll_mode *si532x_find_mode(struct forward_pll *pll, s64 in_freq,
-						 s64 common_in_freq, s64 out_freq, bool m)
+static struct forward_pll_mode *si532x_find_mode(struct forward_pll *pll, s64 in_pix_freq,
+						 s64 in_h_freq, bool in_m, s64 out_freq, bool out_m)
 {
 	int i;
 
 	if (out_freq != 148500000)
 		return NULL;
 
-	if (in_freq <= 0) {
-		if (!m)
+	if ((in_pix_freq <= 0) && (in_h_freq <= 0)) {
+		if (!in_m)
 			return (struct forward_pll_mode *)&si532x_default_mode;
 		else
 			return (struct forward_pll_mode *)&si532x_default_mode_m;
@@ -184,7 +184,8 @@ static struct forward_pll_mode *si532x_find_mode(struct forward_pll *pll, s64 in
 
 	for (i = 0; i < forward_si532x_modes_count; i++) {
 		const struct forward_si532x_mode *cur = &forward_si532x_modes[i];
-		if ((cur->in_freq == in_freq) && (cur->m == m)) {
+		if (((in_h_freq <= 0) || (cur->in_freq == in_h_freq)) &&
+		    ((in_pix_freq <= 0) || (cur->in_freq == in_pix_freq)) && (cur->m == in_m)) {
 			return (struct forward_pll_mode *)cur;
 		}
 	}
diff --git a/forward/forward-si534x.c b/forward/forward-si534x.c
index 086e2b3..c56f3b3 100644
--- a/forward/forward-si534x.c
+++ b/forward/forward-si534x.c
@@ -29,9 +29,10 @@ struct forward_si534x {
 };
 
 struct forward_si534x_mode {
-	int in_freq;
-	int pfd_freq;
-	bool m;
+	int in_freq_p;
+	int in_freq_h;
+	bool in_m;
+	bool out_m;
 	u64 Pn[3];
 	u32 Pd[3];
 	u64 Mn;
@@ -44,40 +45,390 @@ struct forward_si534x_mode {
 	u32 hold_15m_cyc_count;
 	u16 pfd_en_delay;
 };
-/*
-const struct forward_si532x_mode forward_si532x_modes[] = {
-	{ 13500000 / 200, false, 5, 9, 4, 11, 7200, 1, 400 },
-	{ 13500000 / 200, true, 3, 9, 4, 11, 36000, 5, 2002 },
-	{ 13500000 / 240, false, 5, 9, 4, 11, 8640, 1, 480 },
-	{ 13500000 / 400, false, 5, 9, 4, 11, 14400, 1, 800 },
-	{ 13500000 / 400, true, 2, 9, 4, 11, 72000, 5, 4004 },
-	{ 13500000 / 480, false, 4, 9, 4, 11, 17280, 1, 960 },
-	{ 13500000 / 500, false, 4, 9, 4, 11, 18000, 1, 1000 },
-	{ 13500000 / 500, true, 4, 9, 4, 11, 18000, 1, 1001 },
-	{ 13500000 / 400, false, 5, 9, 4, 11, 14400, 1, 800 },
-	{ 13500000 / 400, true, 2, 9, 4, 11, 72000, 5, 4004 },
-	{ 13500000 / 480, false, 4, 9, 4, 11, 17280, 1, 960 },
-	{ 13500000 / 300, false, 5, 9, 4, 11, 10800, 1, 600 },
-	{ 13500000 / 300, true, 3, 9, 4, 11, 54000, 5, 3003 },
-	{ 13500000 / 360, false, 5, 9, 4, 11, 12960, 1, 720 },
-	{ 13500000 / 600, false, 4, 9, 4, 11, 21600, 1, 1200 },
-	{ 13500000 / 600, true, 2, 9, 4, 11, 108000, 5, 6006 },
-	{ 13500000 / 720, false, 4, 9, 4, 11, 25920, 1, 1440 },
-	{ 13500000 / 750, false, 4, 9, 4, 11, 27000, 1, 1500 },
-	{ 13500000 / 750, true, 3, 9, 4, 11, 54000, 2, 3003 },
-	{ 13500000 / 864, false, 3, 9, 4, 11, 31104, 1, 1728 },
-	{ 13500000 / 858, false, 3, 9, 4, 11, 30888, 1, 1716 },
-	{ 13500000 / 858, false, 3, 9, 4, 11, 30888, 1, 1716 },
-	{ 6750000, false, 10, 9, 4, 11, 288, 4, 16 },
-	{ 74250000, false, 10, 9, 4, 11, 288, 44, 16 },
-	{ 74250000, true, 8, 9, 4, 10, 1800, 250, 91 },
-	{ 148500000, false, 10, 9, 4, 11, 252, 77, 14 },
-	{ 148500000, true, 8, 9, 4, 10, 1800, 500, 91 },
+
+const struct forward_si534x_mode forward_si534x_modes[] = {
+	{ 13500000,
+	  15625,
+	  false,
+	  false,
+	  { 1, 864, 864 },
+	  { 1, 1, 1 },
+	  0x014E2000000000,
+	  0x80000000,
+	  0x03070F0F170F,
+	  0x0307090A2B18,
+	  { 0, 10, 10 },
+	  { 0x12F684C, 0x1000000, 0x1000000 },
+	  0x00018AC7,
+	  0x01A0AB,
+	  0x06AC },
+	{ 13500000,
+	  15734,
+	  false,
+	  false,
+	  { 1, 858, 858 },
+	  { 1, 1, 1 },
+	  0x014BCE00000000,
+	  0x80000000,
+	  0x03070F0F170F,
+	  0x0307090A2B18,
+	  { 0, 10, 10 },
+	  { 0x1318776, 0x1000000, 0x1000000 },
+	  0x0000C942,
+	  0x019DC6,
+	  0x06A0 },
+	// 720p24
+	{ 74250000,
+	  18000,
+	  false,
+	  false,
+	  { 1, 4125, 4125 },
+	  { 1, 1, 1 },
+	  0x01220A00000000,
+	  0x80000000,
+	  0x07070F0F1810,
+	  0x0707090A2C19,
+	  { 0, 12, 12 },
+	  { 0x15D867C, 0x1600000, 0x1600000 },
+	  0x0000AF6C,
+	  0x0169B1,
+	  0x05C9 },
+	// 720p23.97
+	{ 74250000,
+	  18000,
+	  true,
+	  false,
+	  { 1, 4125, 4125 },
+	  { 1, 1, 1 },
+	  0x016AE950000000,
+	  0xA0000000,
+	  0x07070F0F1810,
+	  0x0707090A2C19,
+	  { 0, 12, 12 },
+	  { 0x15D2D19, 0x15FA5FA, 0x15FA5FA },
+	  0x0000AF9A,
+	  0x016A0E,
+	  0x05CB },
+	{ 74250000,
+	  18000,
+	  true,
+	  true,
+	  { 1, 4125, 4125 },
+	  { 1, 1, 1 },
+	  0x016AE950000000,
+	  0xA0000000,
+	  0x07070F0F1810,
+	  0x0707090A2C19,
+	  { 0, 12, 12 },
+	  { 0x15D2D19, 0x15FA5FA, 0x15FA5FA },
+	  0x0000AF9A,
+	  0x016A0E,
+	  0x05CB },
+	// 720p25
+	{ 74250000,
+	  18750,
+	  false,
+	  false,
+	  { 1, 3960, 3960 },
+	  { 1, 1, 1 },
+	  0x01167000000000,
+	  0x80000000,
+	  0x07070F0F1810,
+	  0x0707090A2C19,
+	  { 0, 12, 12 },
+	  { 0x16C16C1, 0x1600000, 0x1600000 },
+	  0x0000A83F,
+	  0x015B39,
+	  0x058F },
+	// 720p50
+	{ 74250000,
+	  37500,
+	  false,
+	  false,
+	  { 2, 3960, 3960 },
+	  { 1, 1, 1 },
+	  0x01167000000000,
+	  0x80000000,
+	  0x07070F0F1810,
+	  0x0707090A2C19,
+	  { 1, 12, 12 },
+	  { 0x16C16C1, 0x1600000, 0x1600000 },
+	  0x0000A83F,
+	  0x015B39,
+	  0x058F },
+	// 720p30
+	{ 74250000,
+	  22500,
+	  false,
+	  false,
+	  { 1, 3300, 3300 },
+	  { 1, 1, 1 },
+	  0x00E80800000000,
+	  0x80000000,
+	  0x07070E0E1910,
+	  0x070708092D19,
+	  { 0, 12, 12 },
+	  { 0x1B4E81B, 0x1600000, 0x1600000 },
+	  0x00008B89,
+	  0x01215B,
+	  0x04A1 },
+	// 720p60
+	{ 74250000,
+	  45000,
+	  false,
+	  false,
+	  { 2, 3300, 3300 },
+	  { 1, 1, 1 },
+	  0x00E80800000000,
+	  0x80000000,
+	  0x07070E0E1910,
+	  0x070708092D19,
+	  { 1, 12, 12 },
+	  { 0x1B4E81B, 0x1600000, 0x1600000 },
+	  0x00008B89,
+	  0x01215B,
+	  0x04A1 },
+	// 720p29.97
+	{ 74250000,
+	  22500,
+	  true,
+	  false,
+	  { 1, 3300, 3300 },
+	  { 1, 1, 1 },
+	  0x01225440000000,
+	  0xA0000000,
+	  0x07070E0E1910,
+	  0x070708092D19,
+	  { 0, 12, 12 },
+	  { 0x1B4785F, 0x15FA5FA, 0x15FA5FA },
+	  0x00008BAE,
+	  0x0121A5,
+	  0x04A2 },
+	{ 74250000,
+	  22500,
+	  true,
+	  true,
+	  { 1, 3300, 3300 },
+	  { 1, 1, 1 },
+	  0x01225440000000,
+	  0xA0000000,
+	  0x07070E0E1910,
+	  0x070708092D19,
+	  { 0, 12, 12 },
+	  { 0x1B4785F, 0x15FA5FA, 0x15FA5FA },
+	  0x00008BAE,
+	  0x0121A5,
+	  0x04A2 },
+	// 720p59.94
+	{ 74250000,
+	  22500,
+	  true,
+	  false,
+	  { 2, 3300, 3300 },
+	  { 1, 1, 1 },
+	  0x01225440000000,
+	  0xA0000000,
+	  0x07070E0E1910,
+	  0x070708092D19,
+	  { 1, 12, 12 },
+	  { 0x1B4785F, 0x15FA5FA, 0x15FA5FA },
+	  0x00008BAE,
+	  0x0121A5,
+	  0x04A2 },
+	{ 74250000,
+	  22500,
+	  true,
+	  true,
+	  { 2, 3300, 3300 },
+	  { 1, 1, 1 },
+	  0x01225440000000,
+	  0xA0000000,
+	  0x07070E0E1910,
+	  0x070708092D19,
+	  { 1, 12, 12 },
+	  { 0x1B4785F, 0x15FA5FA, 0x15FA5FA },
+	  0x00008BAE,
+	  0x0121A5,
+	  0x04A2 },
+	// 1080p24
+	{ 74250000,
+	  27000,
+	  false,
+	  false,
+	  { 1, 2750, 2750 },
+	  { 1, 1, 1 },
+	  0x00C15C00000000,
+	  0x80000000,
+	  0x07070F0F170F,
+	  0x0707090A2B18,
+	  { 1, 12, 12 },
+	  { 0x10624DD, 0x1600000, 0x1600000 },
+	  0x0000EB3E,
+	  0x00F121,
+	  0x03DB },
+	// 1080p23.98
+	{ 74250000,
+	  27000,
+	  true,
+	  false,
+	  { 1, 2750, 2750 },
+	  { 1, 1, 1 },
+	  0x00C18D80000000,
+	  0x80000000,
+	  0x07070F0F170F,
+	  0x0707090A2B18,
+	  { 1, 12, 12 },
+	  { 0x105E1D2, 0x15FA5FA, 0x15FA5FA },
+	  0x0000EB7C,
+	  0x00F15F,
+	  0x03DC },
+	{ 74250000,
+	  27000,
+	  true,
+	  true,
+	  { 1, 2750, 2750 },
+	  { 1, 1, 1 },
+	  0x00C18D80000000,
+	  0x80000000,
+	  0x07070F0F170F,
+	  0x0707090A2B18,
+	  { 1, 12, 12 },
+	  { 0x105E1D2, 0x15FA5FA, 0x15FA5FA },
+	  0x0000EB7C,
+	  0x00F15F,
+	  0x03DC },
+	// 1080p25/i50
+	{ 74250000,
+	  28125,
+	  false,
+	  false,
+	  { 1, 2640, 2640 },
+	  { 1, 1, 1 },
+	  0x00B9A000000000,
+	  0x80000000,
+	  0x07070F0F170F,
+	  0x0707090A2B18,
+	  { 1, 12, 12 },
+	  { 0x1111111, 0x1600000, 0x1600000 },
+	  0x0000E1AD,
+	  0x00E77C,
+	  0x03B3 },
+	// 1080p50
+	{ 148500000,
+	  56250,
+	  false,
+	  false,
+	  { 2, 5280, 5280 },
+	  { 1, 1, 1 },
+	  0x00B9A000000000,
+	  0x80000000,
+	  0x07070F0F170F,
+	  0x0707090A2B18,
+	  { 2, 13, 13 },
+	  { 0x1111111, 0x1600000, 0x1600000 },
+	  0x0000E1AD,
+	  0x00E77C,
+	  0x03B3 },
+	// 1080p30/i60
+	{ 74250000,
+	  33750,
+	  false,
+	  false,
+	  { 1, 2200, 2200 },
+	  { 1, 1, 1 },
+	  0x009AB000000000,
+	  0x80000000,
+	  0x07030F0F1810,
+	  0x070308092E1A,
+	  { 1, 12, 12 },
+	  { 0x147AE14, 0x1600000, 0x1600000 },
+	  0x0000BB65,
+	  0x00C0E7,
+	  0x0315 },
+	// 1080p60
+	{ 148500000,
+	  67500,
+	  false,
+	  false,
+	  { 2, 4400, 4400 },
+	  { 1, 1, 1 },
+	  0x009AB000000000,
+	  0x80000000,
+	  0x07030F0F1810,
+	  0x070308092E1A,
+	  { 2, 13, 13 },
+	  { 0x147AE14, 0x1600000, 0x1600000 },
+	  0x0000BB65,
+	  0x00C0E7,
+	  0x0315 },
+	// 1080p29.97/i59.94
+	{ 74250000,
+	  33750,
+	  true,
+	  false,
+	  { 1, 2200, 2200 },
+	  { 1, 1, 1 },
+	  0x00C18D80000000,
+	  0xA0000000,
+	  0x07030F0F1810,
+	  0x070308092E1A,
+	  { 1, 12, 12 },
+	  { 0x1475A47, 0x15FA5FA, 0x15FA5FA },
+	  0x0000BB96,
+	  0x00C119,
+	  0x0316 },
+	{ 74250000,
+	  33750,
+	  true,
+	  true,
+	  { 1, 2200, 2200 },
+	  { 1, 1, 1 },
+	  0x00C18D80000000,
+	  0xA0000000,
+	  0x07030F0F1810,
+	  0x070308092E1A,
+	  { 1, 12, 12 },
+	  { 0x1475A47, 0x15FA5FA, 0x15FA5FA },
+	  0x0000BB96,
+	  0x00C119,
+	  0x0316 },
+	// 1080p59.94
+	{ 148500000,
+	  67500,
+	  true,
+	  false,
+	  { 2, 4400, 4400 },
+	  { 1, 1, 1 },
+	  0x00C18D80000000,
+	  0xA0000000,
+	  0x07030F0F1810,
+	  0x070308092E1A,
+	  { 2, 13, 13 },
+	  { 0x1475A47, 0x15FA5FA, 0x15FA5FA },
+	  0x0000BB96,
+	  0x00C119,
+	  0x0316 },
+	{ 148500000,
+	  67500,
+	  true,
+	  true,
+	  { 2, 4400, 4400 },
+	  { 1, 1, 1 },
+	  0x00C18D80000000,
+	  0xA0000000,
+	  0x07030F0F1810,
+	  0x070308092E1A,
+	  { 2, 13, 13 },
+	  { 0x1475A47, 0x15FA5FA, 0x15FA5FA },
+	  0x0000BB96,
+	  0x00C119,
+	  0x0316 },
 };
-*/
+
 const struct forward_si534x_mode si534x_default_mode = { -1,
 							 -1,
 							 false,
+							 false,
 							 { 1, 2640, 2640 },
 							 { 1, 1, 1 },
 							 0x00B9A000000000,
@@ -89,106 +440,40 @@ const struct forward_si534x_mode si534x_default_mode = { -1,
 							 0x0000E1AD,
 							 0x00E77C,
 							 0x03B3 };
+const struct forward_si534x_mode si534x_default_mode_m = { -1,
+							   -1,
+							   true,
+							   false,
+							   { 1, 2200, 2200 },
+							   { 1, 1, 1 },
+							   0x00C18D80000000,
+							   0xA0000000,
+							   0x07030F0F1810,
+							   0x070308092E1A,
+							   { 1, 12, 12 },
+							   { 0x1475A47, 0x15FA5FA, 0x15FA5FA },
+							   0x0000BB96,
+							   0x00C119,
+							   0x0316 };
+
+const struct forward_si534x_mode si534x_default_mode_m_m = { -1,
+							     -1,
+							     true,
+							     true,
+							     { 1, 2200, 2200 },
+							     { 1, 1, 1 },
+							     0x00C18D80000000,
+							     0xA0000000,
+							     0x07030F0F1810,
+							     0x070308092E1A,
+							     { 1, 12, 12 },
+							     { 0x1475A47, 0x15FA5FA, 0x15FA5FA },
+							     0x0000BB96,
+							     0x00C119,
+							     0x0316 };
+const int forward_si534x_modes_count =
+	sizeof(forward_si534x_modes) / sizeof(struct forward_si534x_mode);
 
-/*
-const int forward_si532x_modes_count =
-	sizeof(forward_si532x_modes) / sizeof(struct forward_si532x_mode);
-*/
-
-/*static int si532x_write_reg(struct forward_si532x *si532x, u8 reg, u8 data)
-{
-	return si532x->pll.dev->i2c_xfer(si532x->pll.dev, si532x->bus, false, si532x->address, reg,
-					 &data);
-}
-
-static int si532x_read_reg(struct forward_si532x *si532x, u8 reg, u8 *data)
-{
-	return si532x->pll.dev->i2c_xfer(si532x->pll.dev, si532x->bus, true, si532x->address, reg,
-					 data);
-}*/
-
-/*
-static int si532x_configure_factors(struct forward_si532x *si532x, u8 bwsel, u8 n1_hs, u32 nc1_ls,
-				    u8 n2_hs, u32 n2_ls, u32 n31, u32 n32)
-{
-	u32 reg_value;
-
-	if (bwsel > 15)
-		return -EINVAL;
-
-	if (n1_hs < 4 || n1_hs > 11)
-		return -EINVAL;
-
-	if (((nc1_ls != 1) && (nc1_ls % 2)) || (nc1_ls < 1) || (nc1_ls > (1 << 20)))
-		return -EINVAL;
-
-	if (n2_hs < 4 || n2_hs > 11)
-		return -EINVAL;
-
-	if (((n2_ls != 1) && (n2_ls % 2)) || (n2_ls < 1) || (n2_ls > (1 << 20)))
-		return -EINVAL;
-
-	if ((n31 < 1) || (n31 > (1 << 19)))
-		return -EINVAL;
-
-	if ((n32 < 1) || (n32 > (1 << 19)))
-		return -EINVAL;
-
-	si532x_write_reg(si532x, 0x02, (bwsel << 4) | (0x2));
-	si532x_write_reg(si532x, 0x19, ((n1_hs - 4) << 5));
-
-	reg_value = nc1_ls - 1;
-	si532x_write_reg(si532x, 0x1F, (reg_value >> 16) & 0x0F);
-	si532x_write_reg(si532x, 0x20, (reg_value >> 8) & 0xFF);
-	si532x_write_reg(si532x, 0x21, reg_value & 0xFF);
-
-	reg_value = n2_ls - 1;
-	si532x_write_reg(si532x, 0x28, ((n2_hs - 4) << 5) | ((reg_value >> 16) & 0x0F));
-	si532x_write_reg(si532x, 0x29, (reg_value >> 8) & 0xFF);
-	si532x_write_reg(si532x, 0x2A, reg_value & 0xFF);
-
-	reg_value = n31 - 1;
-	si532x_write_reg(si532x, 0x2B, (reg_value >> 16) & 0x07);
-	si532x_write_reg(si532x, 0x2C, (reg_value >> 8) & 0xFF);
-	si532x_write_reg(si532x, 0x2D, reg_value & 0xFF);
-
-	reg_value = n32 - 1;
-	si532x_write_reg(si532x, 0x2E, (reg_value >> 16) & 0x07);
-	si532x_write_reg(si532x, 0x2F, (reg_value >> 8) & 0xFF);
-	si532x_write_reg(si532x, 0x30, reg_value & 0xFF);
-
-	return 0;
-}
-*/
-/*
-static int si532x_load_defaults(struct forward_si532x *si532x)
-{
-	// PLL mode, CLK always on, no bypass
-	si532x_write_reg(si532x, 0x00, 0x34);
-	// CLKIN1 > CLKIN2 priority
-	//si532x_write_reg(si532x, 0x01, 0xE1);
-	// No output disabling during calibration, CLKIN2 selected
-	si532x_write_reg(si532x, 0x03, 0x55);
-	// Automatic clock selection, default hold history
-	//si532x_write_reg(si532x, 0x04, 0x92);
-	// LVDS outputs
-	si532x_write_reg(si532x, 0x06, 0x0F);
-	// External crystal as reference
-	si532x_write_reg(si532x, 0x07, 0x28);
-	// CLKOUT1 enabled, CLKOUT2 disabled
-	si532x_write_reg(si532x, 0x0A, 0x08);
-	// Frequency offset and phase error detector
-	si532x_write_reg(si532x, 0x13, 0x2F);
-	// Enable CK2_BAD, LOL, INT pins
-	si532x_write_reg(si532x, 0x14, 0x3B);
-	// Disable clock selection pins
-	si532x_write_reg(si532x, 0x15, 0xFC);
-	// Enable fast lock
-	si532x_write_reg(si532x, 0x89, 0x01);
-
-	return 0;
-}
-*/
 inline void si534x_select_bank(struct forward_si534x *si534x, u16 addr)
 {
 	u8 bank = ((addr >> 8) & 0xFF);
@@ -295,27 +580,32 @@ static void si534x_write_reg_56(struct forward_si534x *si534x, u16 addr, u64 dat
 //	si534x_read_data(si534x, addr, (u8*)data, 7);
 //}
 
-static struct forward_pll_mode *si534x_find_mode(struct forward_pll *pll, s64 in_freq,
-						 s64 common_in_freq, s64 out_freq, bool m)
+static struct forward_pll_mode *si534x_find_mode(struct forward_pll *pll, s64 in_pix_freq,
+						 s64 in_h_freq, bool in_m, s64 out_freq, bool out_m)
 {
 	int i;
 
 	if (out_freq != 148500000)
 		return NULL;
 
-	if (in_freq <= 0) {
-		//if (!m)
-		return (struct forward_pll_mode *)&si534x_default_mode;
-		//else
-		//	return (struct forward_pll_mode *)&si532x_default_mode_m;
+	if ((in_pix_freq <= 0) && (in_h_freq <= 0)) {
+		if (!in_m)
+			return (struct forward_pll_mode *)&si534x_default_mode;
+		else if (!out_m)
+			return (struct forward_pll_mode *)&si534x_default_mode_m;
+		else
+			return (struct forward_pll_mode *)&si534x_default_mode_m_m;
 	}
 
-	/*for (i = 0; i < forward_si532x_modes_count; i++) {
-		const struct forward_si532x_mode *cur = &forward_si532x_modes[i];
-		if ((cur->in_freq == in_freq) && (cur->m == m)) {
+	for (i = 0; i < forward_si534x_modes_count; i++) {
+		const struct forward_si534x_mode *cur = &forward_si534x_modes[i];
+		if (((in_pix_freq < 0) || (cur->in_freq_p == in_pix_freq)) &&
+		    ((in_h_freq < 0) || (cur->in_freq_h == in_h_freq)) && (cur->in_m == in_m) &&
+		    (cur->out_m == out_m)) {
 			return (struct forward_pll_mode *)cur;
 		}
-	}*/
+	}
+
 	return NULL;
 }
 
@@ -326,7 +616,7 @@ static int si534x_switch_mode(struct forward_pll *pll, struct forward_pll_mode *
 
 	si534x_write_reg_8(si534x, 0x0B24, 0xC0);
 	si534x_write_reg_8(si534x, 0x0B25, 0x00);
-	si534x_write_reg_8(si534x, 0x0B40, 0x01);
+	si534x_write_reg_8(si534x, 0x0540, 0x01);
 
 	msleep(300);
 
@@ -349,18 +639,35 @@ static int si534x_switch_mode(struct forward_pll *pll, struct forward_pll_mode *
 
 	si534x_write_reg_48(si534x, 0x0508, m->Bslow);
 	si534x_write_reg_48(si534x, 0x050E, m->Bfast);
+	si534x_write_reg_8(si534x, 0x0514, 0x01);
 
 	si534x_write_reg_56(si534x, 0x0515, m->Mn);
 	si534x_write_reg_32(si534x, 0x051C, m->Md);
-	si534x_write_reg_32(si534x, 0x0520, 0x1);
+	si534x_write_reg_8(si534x, 0x0520, 0x1);
 
 	si534x_write_reg_24(si534x, 0x0532, m->hold_15m_cyc_count);
 	si534x_write_reg_16(si534x, 0x0589, m->pfd_en_delay);
 
+	//si534x_write_reg_24(si534x, 0x0250, 0x00);
+	//si534x_write_reg_24(si534x, 0x0253, 0x00);
+
+	if (m->out_m) {
+		si534x_write_reg_48(si534x, 0x0302, 0x2331000000ULL);
+		si534x_write_reg_32(si534x, 0x0308, 0xc8000000);
+		si534x_write_reg_8(si534x, 0x030c, 0x1);
+	} else {
+		si534x_write_reg_48(si534x, 0x0302, 0x2328000000ULL);
+		si534x_write_reg_32(si534x, 0x0308, 0xc8000000);
+		si534x_write_reg_8(si534x, 0x030c, 0x1);
+	}
+	si534x_write_reg_48(si534x, 0x030d, 0x2331000000ULL);
+	si534x_write_reg_32(si534x, 0x0313, 0xc8000000);
+	si534x_write_reg_8(si534x, 0x0317, 0x1);
+
 	si534x_write_reg_8(si534x, 0x001C, 0x01);
-	si534x_write_reg_8(si534x, 0x0B40, 0x00);
+	si534x_write_reg_8(si534x, 0x0540, 0x00);
 	si534x_write_reg_8(si534x, 0x0B24, 0xC3);
-	si534x_write_reg_8(si534x, 0x0B25, 0xC2);
+	si534x_write_reg_8(si534x, 0x0B25, 0x02);
 
 	return 0;
 }
@@ -397,11 +704,7 @@ static int si534x_tune(struct forward_pll *pll, s64 ppt)
 	struct forward_si534x *si534x = container_of(pll, struct forward_si534x, pll);
 	u32 denom = 0xc8000000 + 0xc8000000LL * ppt / 1000000000000LL;
 
-	printk("si534x_tune(): %llu/%llu\n", 0x2328000000ULL, denom);
-
-	si534x_write_reg_48(si534x, 0x302, 0x2328000000ULL);
 	si534x_write_reg_32(si534x, 0x308, denom);
-	si534x_write_reg_48(si534x, 0x30d, 0x2331000000ULL);
 	si534x_write_reg_32(si534x, 0x313, denom);
 	si534x_write_reg_32(si534x, 0x338, 2);
 
diff --git a/include/forward-pll.h b/include/forward-pll.h
index a23d080..683f8f9 100644
--- a/include/forward-pll.h
+++ b/include/forward-pll.h
@@ -33,8 +33,8 @@ struct forward_pll;
 #define FORWARD_PLL_FEATURE_TUNE (1 << 2)
 
 struct forward_pll_ops {
-	struct forward_pll_mode *(*find_mode)(struct forward_pll *pll, s64 in_freq,
-					      s64 common_in_freq, s64 out_freq, bool m);
+	struct forward_pll_mode *(*find_mode)(struct forward_pll *pll, s64 in_pix_freq,
+					      s64 in_h_freq, bool in_m, s64 out_freq, bool out_m);
 	int (*switch_mode)(struct forward_pll *pll, struct forward_pll_mode *mode);
 	int (*select_input)(struct forward_pll *pll, int input, bool master, bool autoswitch);
 	int (*tune)(struct forward_pll *pll, s64 ppt);
-- 
2.39.1


From d784a519bd5a0f23937440a4ea28aad7e04c7690 Mon Sep 17 00:00:00 2001
From: "Konstantin K. Oblaukhov" <oblaukhov@sl.iae.nsk.su>
Date: Thu, 16 Mar 2023 15:36:10 +0700
Subject: [PATCH 06/13] Genlock for FD722v5.0

---
 forward-v4l2/forward-v4l2-fd722.c | 103 +++++++++++++++++++-----------
 forward-v4l2/forward-v4l2-fd922.c |   2 +
 forward/forward-core.c            |   1 +
 forward/forward-fd722.c           |  74 ++++++++++++++++++++-
 forward/forward-fd922.c           |   2 -
 forward/forward-si532x.c          |   4 +-
 forward/forward-si534x.c          |   8 +--
 include/fd722_reg.h               |  23 +++++++
 include/forward-pll.h             |   2 +-
 9 files changed, 171 insertions(+), 48 deletions(-)

diff --git a/forward-v4l2/forward-v4l2-fd722.c b/forward-v4l2/forward-v4l2-fd722.c
index d5f1809..d4c67b8 100644
--- a/forward-v4l2/forward-v4l2-fd722.c
+++ b/forward-v4l2/forward-v4l2-fd722.c
@@ -48,6 +48,7 @@
 
 #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)
 {
@@ -669,17 +670,20 @@ static s64 fd722_clock_deviation(struct forward_v4l2 *v4l2)
 static s64 fd722_set_clock_deviation(struct forward_v4l2 *v4l2, s64 ppt)
 {
 	struct forward_dev *dev = v4l2->dev;
-	FD722_ClockDeviation dv;
-	u64 value = ((ppt < 0) ? -ppt : ppt) * 2818572288ULL / 1000000000000ULL;
-
-	dv.deviation = (value > 1290555) ? 1290555 : value;
-	dv.direction = (ppt < 0) ? 1 : 0;
+	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;
 
-	FD722_ClockDeviation_W(dev->csr, dv);
+		dv.deviation = (value > 1290555) ? 1290555 : value;
+		dv.direction = (ppt < 0) ? 1 : 0;
 
-	value = dv.deviation * 1000000000000ULL / 2818572288ULL;
+		FD722_ClockDeviation_W(dev->csr, dv);
 
-	return (ppt < 0) ? -value : value;
+		value = dv.deviation * 1000000000000ULL / 2818572288ULL;
+		return (ppt < 0) ? -value : value;
+	}
 }
 
 static u32 fd722_io_timestamp(const struct forward_v4l2_io *io)
@@ -729,61 +733,88 @@ static int fd722_genlock_switch(const struct forward_v4l2_genlock *gl,
 	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);
-		pll->ops->select_input(pll, 1, false, false);
+		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_in_freq = 0;
+		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) {
-			pll_in_freq = forward_v4l2_pixel_clock(t) / t->sdi.width;
 			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_in_freq = 6750000;
+				pll_pix_freq = v5 ? 13500000 : 6750000;
 			else if (t->sdi.mode == FORWARD_SDI_MODE_HD_SDI)
-				pll_in_freq = 74250000;
+				pll_pix_freq = 74250000;
 			else if (t->sdi.mode == FORWARD_SDI_MODE_3G_SDI)
-				pll_in_freq = 148500000;
+				pll_pix_freq = 148500000;
 			gl.mode = (src == V4L_FORWARD_GENLOCK_SRC_IN1) ? 3 : 2;
 		}
 
-		mode = pll->ops->find_mode(pll, pll_in_freq, 0, t->sdi.m, 148500000, t->sdi.m);
+		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 ((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);
+		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);
-
-			// 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);
+			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);
+			}
 		}
 	}
 
diff --git a/forward-v4l2/forward-v4l2-fd922.c b/forward-v4l2/forward-v4l2-fd922.c
index 937a738..f0acc00 100644
--- a/forward-v4l2/forward-v4l2-fd922.c
+++ b/forward-v4l2/forward-v4l2-fd922.c
@@ -765,6 +765,8 @@ bool fd922_genlock_sync_irq_info(const struct forward_v4l2_genlock *gl, int num,
 		ref_time = FD922_AnalogVSyncTime_R(v4l2->dev->csr).time;
 
 	*gl_offset = (s32)(sync_time - ref_time);
+	if (gl->timings.sdi.m)
+		*gl_offset = (s64)*gl_offset * 1000LL / 1001LL;
 
 	return true;
 }
diff --git a/forward/forward-core.c b/forward/forward-core.c
index f4bd17c..6ed978e 100644
--- a/forward/forward-core.c
+++ b/forward/forward-core.c
@@ -64,6 +64,7 @@ static const struct pci_device_id forward_pci_ids[] = {
 	{ PCI_DEVICE(0x1C1F, 0x001C), .driver_data = FORWARD_FD922 },
 	{ PCI_DEVICE(0x1C1F, 0x001F), .driver_data = FORWARD_FD940 },
 	{ PCI_DEVICE(0x1C1F, 0x0020), .driver_data = FORWARD_FD2110 },
+	{ PCI_DEVICE(0x1C1F, 0x0021), .driver_data = FORWARD_FD722 },
 	{},
 };
 
diff --git a/forward/forward-fd722.c b/forward/forward-fd722.c
index 6ce45b0..1e86574 100644
--- a/forward/forward-fd722.c
+++ b/forward/forward-fd722.c
@@ -22,6 +22,7 @@
 #include "forward-vdma.h"
 
 #include "forward-si532x.h"
+#include "forward-si534x.h"
 #include "fd722_reg.h"
 
 #include <linux/delay.h>
@@ -63,18 +64,85 @@ static bool fd722_i2c_xfer(struct forward_dev *dev, u8 bus, bool read, uint8_t a
 	return !i2c.cs && !i2c.nack;
 }
 
+static bool fd722_spi_xfer(struct forward_dev *dev, u8 bus, u8 *tx, int tx_bits, u8 *rx,
+			   int rx_bits)
+{
+	int bits = max(tx_bits, rx_bits);
+	int word_count = (bits - 1) / 16 + 1;
+	int last_word_bits = ((bits - 1) % 16) + 1;
+	bool last8 = last_word_bits <= 8;
+	int i;
+	FD722_SPI ctl;
+
+	ctl.bus = bus;
+	ctl.start = 0;
+	ctl.cs = 1;
+	FD722_SPI_W(dev->csr, ctl);
+
+	for (i = 0; i < word_count; i++) {
+		if (tx && ((i * 16) < tx_bits)) {
+			uint8_t cur_tx_bits = ((tx_bits - i * 16 - 1) % 16) + 1;
+			if (cur_tx_bits <= 8) {
+				ctl.data = tx[i * 2] << (16 - cur_tx_bits);
+			} else {
+				ctl.data = (tx[i * 2] << 8) | (tx[i * 2 + 1] << (16 - cur_tx_bits));
+			}
+		} else {
+			ctl.data = 0xFFFF;
+		}
+
+		ctl.bus = bus;
+		ctl.cs = 1;
+		ctl.start = 1;
+		ctl.bit8 = ((i == word_count - 1) && last8) ? 0 : 1;
+		FD722_SPI_W(dev->csr, ctl);
+
+		do {
+			ctl = FD722_SPI_R(dev->csr);
+		} while (ctl.start);
+
+		if (rx && ((i * 16) < rx_bits)) {
+			uint8_t cur_rx_bits = ((rx_bits - i * 16 - 1) % 16) + 1;
+			uint16_t data = ctl.data;
+			if (cur_rx_bits <= 8) {
+				rx[i * 2] = data & 0xFF;
+			} else {
+				rx[i * 2] = (data >> 8) & 0xFF;
+				rx[i * 2 + 1] = data & 0xFF;
+			}
+		}
+	}
+
+	ctl.bus = bus;
+	ctl.start = 0;
+	ctl.cs = 0;
+
+	FD722_SPI_W(dev->csr, ctl);
+
+	return true;
+}
+
 static int fd722_init(struct forward_dev *dev)
 {
-	dev->i2c_xfer = &fd722_i2c_xfer;
+	FD722_DynamicVersion v = FD722_DynamicVersion_R(dev->csr);
 
-	forward_si532x_init(dev, &dev->pll, 0, FD722_PLL_ADDRESS, 2);
+	if (v.minor >= 6) {
+		dev->spi_xfer = &fd722_spi_xfer;
+		forward_si534x_init(dev, &dev->pll, 1, 3);
+	} else {
+		dev->i2c_xfer = &fd722_i2c_xfer;
+		forward_si532x_init(dev, &dev->pll, 0, FD722_PLL_ADDRESS, 2);
+	}
 
 	return 0;
 }
 
 static void fd722_fini(struct forward_dev *dev)
 {
-	forward_si532x_fini(dev, &dev->pll);
+	if (((dev->fw_version >> 16) & 0xF) >= 6)
+		forward_si534x_fini(dev, &dev->pll);
+	else
+		forward_si532x_fini(dev, &dev->pll);
 }
 
 static void fd722_read_id(struct forward_dev *dev)
diff --git a/forward/forward-fd922.c b/forward/forward-fd922.c
index 9722575..0128a5d 100644
--- a/forward/forward-fd922.c
+++ b/forward/forward-fd922.c
@@ -81,8 +81,6 @@ static bool fd922_spi_xfer(struct forward_dev *dev, u8 bus, u8 *tx, int tx_bits,
 
 static int fd922_init(struct forward_dev *dev)
 {
-	struct forward_pll_mode *mode;
-
 	dev->spi_xfer = &fd922_spi_xfer;
 
 	forward_si534x_init(dev, &dev->pll, 4, 3);
diff --git a/forward/forward-si532x.c b/forward/forward-si532x.c
index 07f1c98..0a38051 100644
--- a/forward/forward-si532x.c
+++ b/forward/forward-si532x.c
@@ -218,9 +218,9 @@ static int si532x_select_input(struct forward_pll *pll, int input, bool master,
 	return 0;
 }
 
-static int si532x_tune(struct forward_pll *pll, s64 ppt)
+static s64 si532x_tune(struct forward_pll *pll, s64 ppt)
 {
-	return -ENOTSUPP;
+	return 0;
 }
 
 static void si532x_calibrate(struct forward_pll *pll)
diff --git a/forward/forward-si534x.c b/forward/forward-si534x.c
index c56f3b3..6cca68b 100644
--- a/forward/forward-si534x.c
+++ b/forward/forward-si534x.c
@@ -599,8 +599,8 @@ static struct forward_pll_mode *si534x_find_mode(struct forward_pll *pll, s64 in
 
 	for (i = 0; i < forward_si534x_modes_count; i++) {
 		const struct forward_si534x_mode *cur = &forward_si534x_modes[i];
-		if (((in_pix_freq < 0) || (cur->in_freq_p == in_pix_freq)) &&
-		    ((in_h_freq < 0) || (cur->in_freq_h == in_h_freq)) && (cur->in_m == in_m) &&
+		if (((in_pix_freq <= 0) || (cur->in_freq_p == in_pix_freq)) &&
+		    ((in_h_freq <= 0) || (cur->in_freq_h == in_h_freq)) && (cur->in_m == in_m) &&
 		    (cur->out_m == out_m)) {
 			return (struct forward_pll_mode *)cur;
 		}
@@ -699,7 +699,7 @@ static int si534x_select_input(struct forward_pll *pll, int input, bool master,
 	return 0;
 }
 
-static int si534x_tune(struct forward_pll *pll, s64 ppt)
+static s64 si534x_tune(struct forward_pll *pll, s64 ppt)
 {
 	struct forward_si534x *si534x = container_of(pll, struct forward_si534x, pll);
 	u32 denom = 0xc8000000 + 0xc8000000LL * ppt / 1000000000000LL;
@@ -708,7 +708,7 @@ static int si534x_tune(struct forward_pll *pll, s64 ppt)
 	si534x_write_reg_32(si534x, 0x313, denom);
 	si534x_write_reg_32(si534x, 0x338, 2);
 
-	return 0;
+	return ppt;
 }
 
 static void si534x_calibrate(struct forward_pll *pll)
diff --git a/include/fd722_reg.h b/include/fd722_reg.h
index 91180cc..b19b7ad 100644
--- a/include/fd722_reg.h
+++ b/include/fd722_reg.h
@@ -102,6 +102,29 @@ FD722_VAuxMonitor __always_inline FD722_VAuxMonitor_R(uint32_t *csr)
 	return *((FD722_VAuxMonitor *)&r);
 }
 
+typedef struct _FD722_SPI {
+	uint32_t data : 16;
+	uint32_t _unused2316 : 8;
+	uint32_t cs : 1;
+	uint32_t bit8 : 1;
+	uint32_t start : 1;
+	uint32_t bus : 1;
+	uint32_t _unused3128 : 4;
+} __attribute__((packed)) FD722_SPI;
+
+#define FD722_SPI_A (0x86)
+
+FD722_SPI __always_inline FD722_SPI_R(uint32_t *csr)
+{
+	uint32_t r = __read_reg(csr, FD722_SPI_A);
+	return *((FD722_SPI *)&r);
+}
+
+void __always_inline FD722_SPI_W(uint32_t *csr, FD722_SPI v)
+{
+	__write_reg(csr, FD722_SPI_A, *((uint32_t *)&v));
+}
+
 typedef struct _FD722_I2C {
 	uint32_t data : 8;
 	uint32_t subAddress : 8;
diff --git a/include/forward-pll.h b/include/forward-pll.h
index 683f8f9..98a665e 100644
--- a/include/forward-pll.h
+++ b/include/forward-pll.h
@@ -37,7 +37,7 @@ struct forward_pll_ops {
 					      s64 in_h_freq, bool in_m, s64 out_freq, bool out_m);
 	int (*switch_mode)(struct forward_pll *pll, struct forward_pll_mode *mode);
 	int (*select_input)(struct forward_pll *pll, int input, bool master, bool autoswitch);
-	int (*tune)(struct forward_pll *pll, s64 ppt);
+	s64 (*tune)(struct forward_pll *pll, s64 ppt);
 	void (*calibrate)(struct forward_pll *pll);
 };
 
-- 
2.39.1


From 4c1125014fc2114ff23e001ec45092a20a4987a9 Mon Sep 17 00:00:00 2001
From: Konstantin Oblaukhov <oblaukhov.konstantin@gmail.com>
Date: Fri, 17 Mar 2023 16:04:45 +0700
Subject: [PATCH 07/13] Device hot reboot.

---
 dkms.conf                      |   2 +-
 forward-v4l2/forward-v4l2-io.c |   2 +-
 forward/forward-core.c         | 113 ++++++++++++++++++++++++++-------
 forward/forward-ctl.c          |  31 +++++++--
 forward/forward-fd720.c        |   9 +++
 forward/forward-fd722.c        |   9 +++
 forward/forward-fd788.c        |   9 +++
 include/fd720_reg.h            |  20 ++++++
 include/fd788_reg.h            |  29 +++++++++
 include/forward-version.h      |   2 +-
 include/forward.h              |   3 +
 11 files changed, 197 insertions(+), 32 deletions(-)

diff --git a/dkms.conf b/dkms.conf
index dca6c47..a2f143e 100644
--- a/dkms.conf
+++ b/dkms.conf
@@ -1,5 +1,5 @@
 PACKAGE_NAME="forward"
-PACKAGE_VERSION="1.5.0"
+PACKAGE_VERSION="1.5.1"
 
 BUILT_MODULE_NAME[0]="forward"
 BUILT_MODULE_LOCATION[0]="forward/"
diff --git a/forward-v4l2/forward-v4l2-io.c b/forward-v4l2/forward-v4l2-io.c
index 2e8a83c..c4241e0 100644
--- a/forward-v4l2/forward-v4l2-io.c
+++ b/forward-v4l2/forward-v4l2-io.c
@@ -1163,7 +1163,7 @@ static int forward_v4l2_c_querycap(struct file *file, void *priv, struct v4l2_ca
 		snprintf((char *)cap->bus_info, sizeof(cap->bus_info), "PCI:%s",
 			 pci_name(io->v4l2->dev->pci_dev));
 	}
-	cap->version = LINUX_VERSION_CODE;
+	cap->version = FORWARD_VERSION_CODE;
 
 	switch (iodev->type) {
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
diff --git a/forward/forward-core.c b/forward/forward-core.c
index 6ed978e..f33ae6a 100644
--- a/forward/forward-core.c
+++ b/forward/forward-core.c
@@ -33,6 +33,7 @@
 #include <linux/ctype.h>
 #include <linux/export.h>
 #include <linux/of.h>
+#include <linux/delay.h>
 
 #define FORWARD_MODULE_NAME "forward"
 
@@ -448,7 +449,7 @@ static void forward_fini_io(struct forward_dev *dev, int io)
 	dev->io[io].dev = NULL;
 }
 
-static void forward_remove_common(struct forward_dev *dev)
+static void forward_fini_device(struct forward_dev *dev)
 {
 	unsigned long sflags;
 	int i;
@@ -481,6 +482,92 @@ static void forward_remove_common(struct forward_dev *dev)
 	forward_vdma_remove(dev);
 
 	forward_irq_remove(dev);
+}
+
+static int forward_init_device(struct forward_dev *dev)
+{
+	int result;
+	int i;
+
+	result = forward_irq_probe(dev);
+	if (result)
+		goto fail;
+
+	result = forward_vdma_probe(dev);
+	if (result)
+		goto fail;
+
+	if (dev->cfg.io_number != 0) {
+		dev->io = devm_kzalloc(dev->parent_dev,
+				       dev->cfg.io_count * sizeof(struct forward_io), GFP_KERNEL);
+
+		for (i = 0; i < dev->cfg.io_count; i++) {
+			result = forward_init_io(dev, i);
+			if (result)
+				goto fail;
+		}
+	}
+
+	forward_vdma_enable(dev);
+
+fail:
+	return result;
+}
+
+int forward_reboot_device(struct forward_dev *dev)
+{
+	struct forward_extension *ext;
+	int status;
+
+	if (!dev->cfg.reboot) {
+		forward_warn(dev, "Device reboot unsupported");
+		return -ENOTSUPP;
+	}
+
+	mutex_lock(&dev->lock);
+
+	if (forward_ref_read(dev) != 1) {
+		forward_warn(dev, "Cannot reboot device, device is busy");
+		mutex_unlock(&dev->lock);
+		return -EBUSY;
+	}
+
+	forward_fini_device(dev);
+
+	forward_fini_registers(dev);
+
+	dev->cfg.reboot(dev);
+
+	// TODO: Platform dev remap
+	if (dev->pci_dev) {
+		pci_save_state(dev->pci_dev);
+	}
+
+	msleep(5000);
+
+	if (dev->pci_dev) {
+		pci_restore_state(dev->pci_dev);
+		pci_set_master(dev->pci_dev);
+	}
+
+	forward_init_registers(dev);
+
+	status = forward_init_device(dev);
+
+	mutex_unlock(&dev->lock);
+
+	mutex_lock(&forward_extensions_lock);
+	list_for_each_entry (ext, &forward_extensions, node)
+		forward_ext_bind(dev, ext);
+	mutex_unlock(&forward_extensions_lock);
+
+	return status;
+}
+EXPORT_SYMBOL(forward_reboot_device);
+
+static void forward_remove_common(struct forward_dev *dev)
+{
+	forward_fini_device(dev);
 
 	dev->cfg.fini(dev);
 
@@ -497,7 +584,6 @@ static void forward_remove_common(struct forward_dev *dev)
 
 static int forward_probe_common(struct forward_dev *dev)
 {
-	int i;
 	int result = 0;
 	struct forward_extension *ext;
 
@@ -552,33 +638,16 @@ static int forward_probe_common(struct forward_dev *dev)
 	list_add_tail(&dev->list, &forward_devices);
 	mutex_unlock(&forward_devices_lock);
 
+	forward_parse_mode(dev, FORWARD_DEFAULT_MODE);
+
 	result = forward_ctl_probe(dev);
 	if (result)
 		goto fail;
 
-	result = forward_irq_probe(dev);
+	result = forward_init_device(dev);
 	if (result)
 		goto fail;
 
-	result = forward_vdma_probe(dev);
-	if (result)
-		goto fail;
-
-	if (dev->cfg.io_number != 0) {
-		dev->io = devm_kzalloc(dev->parent_dev,
-				       dev->cfg.io_count * sizeof(struct forward_io), GFP_KERNEL);
-
-		for (i = 0; i < dev->cfg.io_count; i++) {
-			result = forward_init_io(dev, i);
-			if (result)
-				goto fail;
-		}
-	}
-
-	forward_vdma_enable(dev);
-
-	forward_parse_mode(dev, FORWARD_DEFAULT_MODE);
-
 	forward_info(dev, "Added successfully\n");
 
 	mutex_unlock(&dev->lock);
diff --git a/forward/forward-ctl.c b/forward/forward-ctl.c
index c699bbc..603dd1f 100644
--- a/forward/forward-ctl.c
+++ b/forward/forward-ctl.c
@@ -384,6 +384,26 @@ static ssize_t forward_attr_store_io_config(struct device *dev, struct device_at
 	return count;
 }
 
+static ssize_t forward_attr_store_reboot(struct device *dev, struct device_attribute *attr,
+					 const char *buf, size_t count)
+{
+	struct forward_dev *fdev = dev_get_drvdata(dev);
+	int status;
+
+	if (count < 1)
+		return -EINVAL;
+
+	if (buf[0] == '1')
+		status = forward_reboot_device(fdev);
+	else if (buf[0] == '0')
+		status = 0;
+	else
+		status = -EINVAL;
+
+	return status ? status : count;
+}
+
+static DEVICE_ATTR(reboot, S_IWUSR, NULL, forward_attr_store_reboot);
 static DEVICE_ATTR(software_id, S_IRUGO, forward_attr_show_software_id, NULL);
 static DEVICE_ATTR(hardware_id, S_IRUGO, forward_attr_show_hardware_id, NULL);
 static DEVICE_ATTR(name, S_IRUGO, forward_attr_show_name, NULL);
@@ -394,13 +414,10 @@ static DEVICE_ATTR(io_config, S_IWUSR | S_IWGRP | S_IRUGO, forward_attr_show_io_
 		   forward_attr_store_io_config);
 
 static struct attribute *forward_dev_attrs[] = {
-	&dev_attr_software_id.attr,
-	&dev_attr_hardware_id.attr,
-	&dev_attr_name.attr,
-	&dev_attr_mode.attr,
-	&dev_attr_extensions.attr,
-	&dev_attr_io_config.attr,
-	NULL,
+	&dev_attr_software_id.attr, &dev_attr_hardware_id.attr,
+	&dev_attr_name.attr,	    &dev_attr_mode.attr,
+	&dev_attr_extensions.attr,  &dev_attr_io_config.attr,
+	&dev_attr_reboot.attr,	    NULL,
 };
 
 static struct attribute_group forward_dev_attr_group = {
diff --git a/forward/forward-fd720.c b/forward/forward-fd720.c
index 12c13b0..f0216f7 100644
--- a/forward/forward-fd720.c
+++ b/forward/forward-fd720.c
@@ -37,6 +37,14 @@ static void fd720_fini(struct forward_dev *dev)
 {
 }
 
+static void fd720_reboot(struct forward_dev *dev)
+{
+	FD720_GlobalCSR g = FD720_GlobalCSR_R(dev->csr);
+	g.reboot = 1;
+	FD720_GlobalCSR_W(dev->csr, g);
+	// Board dies in ~2-5s
+}
+
 static void fd720_read_id(struct forward_dev *dev)
 {
 	int cap_addr = pci_find_ext_capability(dev->pci_dev, PCI_EXT_CAP_ID_DSN);
@@ -232,6 +240,7 @@ struct forward_dev_config fd720_cfg = {
 
 	.init = fd720_init,
 	.fini = fd720_fini,
+	.reboot = fd720_reboot,
 
 	.read_id = fd720_read_id,
 
diff --git a/forward/forward-fd722.c b/forward/forward-fd722.c
index 1e86574..a7081ba 100644
--- a/forward/forward-fd722.c
+++ b/forward/forward-fd722.c
@@ -145,6 +145,14 @@ static void fd722_fini(struct forward_dev *dev)
 		forward_si532x_fini(dev, &dev->pll);
 }
 
+static void fd722_reboot(struct forward_dev *dev)
+{
+	FD722_GlobalCSR g = FD722_GlobalCSR_R(dev->csr);
+	g.reboot = 1;
+	FD722_GlobalCSR_W(dev->csr, g);
+	// Board dies in ~2-5s
+}
+
 static void fd722_read_id(struct forward_dev *dev)
 {
 	int cap_addr = pci_find_ext_capability(dev->pci_dev, PCI_EXT_CAP_ID_DSN);
@@ -363,6 +371,7 @@ struct forward_dev_config fd722_cfg = {
 
 	.init = fd722_init,
 	.fini = fd722_fini,
+	.reboot = fd722_reboot,
 
 	.read_id = fd722_read_id,
 
diff --git a/forward/forward-fd788.c b/forward/forward-fd788.c
index 008dc77..5bf1916 100644
--- a/forward/forward-fd788.c
+++ b/forward/forward-fd788.c
@@ -82,6 +82,14 @@ static void fd788_fini(struct forward_dev *dev)
 	forward_si532x_fini(dev, &dev->pll);
 }
 
+static void fd788_reboot(struct forward_dev *dev)
+{
+	FD788_GlobalCSR g = FD788_GlobalCSR_R(dev->csr);
+	g.reboot = 1;
+	FD788_GlobalCSR_W(dev->csr, g);
+	// Board dies in ~2-5s
+}
+
 static void fd788_read_id(struct forward_dev *dev)
 {
 	int cap_addr = pci_find_ext_capability(dev->pci_dev, PCI_EXT_CAP_ID_DSN);
@@ -305,6 +313,7 @@ struct forward_dev_config fd788_cfg = {
 
 	.init = fd788_init,
 	.fini = fd788_fini,
+	.reboot = fd788_reboot,
 
 	.read_id = fd788_read_id,
 
diff --git a/include/fd720_reg.h b/include/fd720_reg.h
index 3f75a1b..dcf71cb 100644
--- a/include/fd720_reg.h
+++ b/include/fd720_reg.h
@@ -257,6 +257,26 @@ void __always_inline FD720_VideoInColorConversion_W(uint32_t *csr, unsigned int
 	__write_reg(csr, FD720_VideoInColorConversion_A(n), *((uint32_t *)&v));
 }
 
+typedef struct _FD720_GlobalCSR {
+	uint32_t videoReset : 1;
+	uint32_t icapRW : 1;
+	uint32_t reboot : 1;
+	uint32_t _unused3103 : 29;
+} __attribute__((packed)) FD720_GlobalCSR;
+
+#define FD720_GlobalCSR_A (0x80)
+
+FD720_GlobalCSR __always_inline FD720_GlobalCSR_R(uint32_t *csr)
+{
+	uint32_t r = __read_reg(csr, FD720_GlobalCSR_A);
+	return *((FD720_GlobalCSR *)&r);
+}
+
+void __always_inline FD720_GlobalCSR_W(uint32_t *csr, FD720_GlobalCSR v)
+{
+	__write_reg(csr, FD720_GlobalCSR_A, *((uint32_t *)&v));
+}
+
 typedef struct _FD720_TemperatureMonitor {
 	uint32_t temperature : 10;
 	uint32_t maxTemperature : 10;
diff --git a/include/fd788_reg.h b/include/fd788_reg.h
index 87f9ab0..ab670c2 100644
--- a/include/fd788_reg.h
+++ b/include/fd788_reg.h
@@ -27,6 +27,35 @@
 #define __read_reg(csr, addr) ioread32(&csr[addr])
 #define __write_reg(csr, addr, v) iowrite32(v, &csr[addr])
 
+typedef struct _FD788_GlobalCSR {
+	uint32_t videoReset : 1;
+	uint32_t vpllReset : 1;
+	uint32_t reconfigure : 1;
+	uint32_t vpllClkSelect : 1;
+	uint32_t icapRW : 1;
+	uint32_t reboot : 1;
+	uint32_t _unused1506 : 10;
+	uint32_t vpllLocked : 1;
+	uint32_t qpll0Locked : 1;
+	uint32_t qpll1Locked : 1;
+	uint32_t vpllClk1Bad : 1;
+	uint32_t vpllClk2Bad : 1;
+	uint32_t _unused3121 : 11;
+} __attribute__((packed)) FD788_GlobalCSR;
+
+#define FD788_GlobalCSR_A (0xC0)
+
+FD788_GlobalCSR __always_inline FD788_GlobalCSR_R(uint32_t *csr)
+{
+	uint32_t r = __read_reg(csr, FD788_GlobalCSR_A);
+	return *((FD788_GlobalCSR *)&r);
+}
+
+void __always_inline FD788_GlobalCSR_W(uint32_t *csr, FD788_GlobalCSR v)
+{
+	__write_reg(csr, FD788_GlobalCSR_A, *((uint32_t *)&v));
+}
+
 typedef struct _FD788_TemperatureMonitor {
 	uint32_t temperature : 10;
 	uint32_t maxTemperature : 10;
diff --git a/include/forward-version.h b/include/forward-version.h
index 5379007..490ed18 100644
--- a/include/forward-version.h
+++ b/include/forward-version.h
@@ -26,7 +26,7 @@
 
 #define FORWARD_VERSION_MAJOR 1
 #define FORWARD_VERSION_MINOR 5
-#define FORWARD_VERSION_PATCH 0
+#define FORWARD_VERSION_PATCH 1
 
 #define _FORWARD_VERSION_STR_HELPER(x) #x
 #define _FORWARD_VERSION_STR(x) _FORWARD_VERSION_STR_HELPER(x)
diff --git a/include/forward.h b/include/forward.h
index 8fcbe0c..36cedff 100644
--- a/include/forward.h
+++ b/include/forward.h
@@ -70,6 +70,7 @@ struct forward_dev_config {
 
 	int (*init)(struct forward_dev *dev);
 	void (*fini)(struct forward_dev *dev);
+	void (*reboot)(struct forward_dev *dev);
 
 	void (*read_id)(struct forward_dev *dev);
 
@@ -178,6 +179,8 @@ int forward_io_ref_read(struct forward_io *io);
 int forward_switch_mode(struct forward_dev *dev, const char *mode_string);
 bool forward_has_mode(struct forward_dev *dev, const char *mode);
 
+int forward_reboot_device(struct forward_dev *dev);
+
 int forward_switch_io(struct forward_io *io, enum forward_io_state state);
 
 int forward_toggle_streaming(struct forward_io *io, bool enabled);
-- 
2.39.1


From 27f2d81cb5fafd2934389fab56baf0b4368f9f46 Mon Sep 17 00:00:00 2001
From: Konstantin Oblaukhov <oblaukhov.konstantin@gmail.com>
Date: Mon, 10 Apr 2023 22:34:22 +0700
Subject: [PATCH 08/13] Initial FD788 v2.0 support.

---
 forward/forward-core.c  |  1 +
 forward/forward-fd788.c | 73 ++++++++++++++++++++++++++++++++++++++---
 include/fd788_reg.h     | 32 +++++++++++++++++-
 3 files changed, 101 insertions(+), 5 deletions(-)

diff --git a/forward/forward-core.c b/forward/forward-core.c
index f33ae6a..b6c3314 100644
--- a/forward/forward-core.c
+++ b/forward/forward-core.c
@@ -66,6 +66,7 @@ static const struct pci_device_id forward_pci_ids[] = {
 	{ PCI_DEVICE(0x1C1F, 0x001F), .driver_data = FORWARD_FD940 },
 	{ PCI_DEVICE(0x1C1F, 0x0020), .driver_data = FORWARD_FD2110 },
 	{ PCI_DEVICE(0x1C1F, 0x0021), .driver_data = FORWARD_FD722 },
+	{ PCI_DEVICE(0x1C1F, 0x0022), .driver_data = FORWARD_FD788 },
 	{},
 };
 
diff --git a/forward/forward-fd788.c b/forward/forward-fd788.c
index 5bf1916..1a75946 100644
--- a/forward/forward-fd788.c
+++ b/forward/forward-fd788.c
@@ -22,6 +22,7 @@
 #include "forward-vdma.h"
 
 #include "forward-si532x.h"
+#include "forward-si534x.h"
 #include "fd788_reg.h"
 
 #include <linux/delay.h>
@@ -68,18 +69,82 @@ static bool fd788_i2c_xfer(struct forward_dev *dev, u8 bus, bool read, uint8_t a
 	return !i2c.cs && !i2c.nack;
 }
 
+static bool fd788_spi_xfer(struct forward_dev *dev, u8 bus, u8 *tx, int tx_bits, u8 *rx,
+			   int rx_bits)
+{
+	int bits = max(tx_bits, rx_bits);
+	int word_count = (bits - 1) / 32 + 1;
+	int byte_count = (bits - 1) / 8 + 1;
+	int last_word_bits = bits % 32;
+
+	FD788_SPIControl ctl;
+	FD788_SPIData data;
+	int i;
+
+	ctl.bus = bus + 1;
+	ctl.rxBits = rx_bits;
+	ctl.txBits = tx_bits;
+	FD788_SPIControl_W(dev->csr, ctl);
+
+	for (i = 0; i < word_count; i++) {
+		int j;
+
+		if ((i * 8) < tx_bits) {
+			u32 value = 0;
+			if (tx) {
+				for (j = 0; j < 4; j++) {
+					if (i * 4 + j < byte_count)
+						value |= tx[i * 4 + j] << ((3 - j) * 8);
+				}
+			} else
+				value = 0xFFFFFFFF;
+			data.data = value;
+			FD788_SPIData_W(dev->csr, data);
+		}
+
+		if ((i * 8) < rx_bits) {
+			data = FD788_SPIData_R(dev->csr);
+			if (i == (word_count - 1))
+				data.data <<= (32 - last_word_bits) % 32;
+
+			if (rx) {
+				for (j = 0; j < 4; j++) {
+					if (i * 4 + j < byte_count)
+						rx[i * 4 + j] = (data.data >> ((3 - j) * 8)) & 0xFF;
+				}
+			}
+		}
+	}
+
+	ctl.bus = 0;
+	ctl.rxBits = 0;
+	ctl.txBits = 0;
+	FD788_SPIControl_W(dev->csr, ctl);
+
+	return true;
+}
+
 static int fd788_init(struct forward_dev *dev)
 {
-	dev->i2c_xfer = &fd788_i2c_xfer;
+	FD788_DynamicVersion v = FD788_DynamicVersion_R(dev->csr);
 
-	forward_si532x_init(dev, &dev->pll, 0, FD788_PLL_ADDRESS, 2);
+	if (v.hardware >= 2) {
+		dev->spi_xfer = &fd788_spi_xfer;
+		forward_si534x_init(dev, &dev->pll, 1, 3);
+	} else {
+		dev->i2c_xfer = &fd788_i2c_xfer;
+		forward_si532x_init(dev, &dev->pll, 0, FD788_PLL_ADDRESS, 2);
+	}
 
 	return 0;
 }
 
 static void fd788_fini(struct forward_dev *dev)
 {
-	forward_si532x_fini(dev, &dev->pll);
+	if (((dev->fw_version >> 24) & 0xF) >= 2)
+		forward_si534x_fini(dev, &dev->pll);
+	else
+		forward_si532x_fini(dev, &dev->pll);
 }
 
 static void fd788_reboot(struct forward_dev *dev)
@@ -112,7 +177,7 @@ static void fd788_read_id(struct forward_dev *dev)
 		dev->hard_id = (u64)dev->soft_id;
 	}
 
-	dev->fw_version = (v.major << 24) | (v.minor << 16) | v.revision;
+	dev->fw_version = (v.hardware << 24) | (v.minor << 16) | v.revision;
 }
 
 static void fd788_enable_interrupts(struct forward_dev *dev, u32 interrupts)
diff --git a/include/fd788_reg.h b/include/fd788_reg.h
index ab670c2..52f8158 100644
--- a/include/fd788_reg.h
+++ b/include/fd788_reg.h
@@ -205,6 +205,36 @@ FD788_SoftID __always_inline FD788_SoftID_R(uint32_t *csr)
 	return *((FD788_SoftID *)&r);
 }
 
+typedef struct _FD788_SPIControl {
+	uint32_t txBits : 12;
+	uint32_t rxBits : 12;
+	uint32_t bus : 8;
+} __attribute__((packed)) FD788_SPIControl;
+
+#define FD788_SPIControl_A (0xD2)
+
+void __always_inline FD788_SPIControl_W(uint32_t *csr, FD788_SPIControl v)
+{
+	__write_reg(csr, FD788_SPIControl_A, *((uint32_t *)&v));
+}
+
+typedef struct _FD788_SPIData {
+	uint32_t data : 32;
+} __attribute__((packed)) FD788_SPIData;
+
+#define FD788_SPIData_A (0xD3)
+
+FD788_SPIData __always_inline FD788_SPIData_R(uint32_t *csr)
+{
+	uint32_t r = __read_reg(csr, FD788_SPIData_A);
+	return *((FD788_SPIData *)&r);
+}
+
+void __always_inline FD788_SPIData_W(uint32_t *csr, FD788_SPIData v)
+{
+	__write_reg(csr, FD788_SPIData_A, *((uint32_t *)&v));
+}
+
 typedef struct _FD788_StaticVersion {
 	uint32_t revision : 16;
 	uint32_t minor : 8;
@@ -520,7 +550,7 @@ FD788_AnalogVSyncTime __always_inline FD788_AnalogVSyncTime_R(uint32_t *csr)
 typedef struct _FD788_DynamicVersion {
 	uint32_t revision : 16;
 	uint32_t minor : 8;
-	uint32_t major : 4;
+	uint32_t hardware : 4;
 	uint32_t project : 4;
 } __attribute__((packed)) FD788_DynamicVersion;
 
-- 
2.39.1


From d00f49abf6a642a13b0536341b9f7611b7e238be Mon Sep 17 00:00:00 2001
From: Konstantin Oblaukhov <oblaukhov.konstantin@gmail.com>
Date: Tue, 11 Apr 2023 12:02:00 +0700
Subject: [PATCH 09/13] Genlock support for FD788v2.

---
 forward-v4l2/forward-v4l2-fd788.c | 104 +++++++++++++++++++-----------
 1 file changed, 68 insertions(+), 36 deletions(-)

diff --git a/forward-v4l2/forward-v4l2-fd788.c b/forward-v4l2/forward-v4l2-fd788.c
index d86b292..91e550d 100644
--- a/forward-v4l2/forward-v4l2-fd788.c
+++ b/forward-v4l2/forward-v4l2-fd788.c
@@ -47,6 +47,8 @@
 #define FD788_YC_SWAP_REVISION 849
 #define FD788_ASI_PERIOD_REVISION 873
 
+#define FD788_IS_VERSION_2(ver) (((ver >> 24) & 0xF) >= 2)
+
 static void fd788_check_pll_configuration(struct forward_v4l2 *v4l2)
 {
 	FD788_ClockFrequency freq;
@@ -855,17 +857,20 @@ static s64 fd788_clock_deviation(struct forward_v4l2 *v4l2)
 static s64 fd788_set_clock_deviation(struct forward_v4l2 *v4l2, s64 ppt)
 {
 	struct forward_dev *dev = v4l2->dev;
-	FD788_ClockDeviation dv;
-	u64 value = ((ppt < 0) ? -ppt : ppt) * 2818578888ULL / 1000000000000ULL;
-
-	dv.deviation = (value > 1290555) ? 1290555 : value;
-	dv.direction = (ppt < 0) ? 1 : 0;
+	if (FD788_IS_VERSION_2(dev->fw_version)) {
+		return v4l2->dev->pll->ops->tune(v4l2->dev->pll, ppt);
+	} else {
+		FD788_ClockDeviation dv;
+		u64 value = ((ppt < 0) ? -ppt : ppt) * 2818572288ULL / 1000000000000ULL;
 
-	FD788_ClockDeviation_W(dev->csr, dv);
+		dv.deviation = (value > 1290555) ? 1290555 : value;
+		dv.direction = (ppt < 0) ? 1 : 0;
 
-	value = dv.deviation * 1000000000000ULL / 2818578888ULL;
+		FD788_ClockDeviation_W(dev->csr, dv);
 
-	return (ppt < 0) ? -value : value;
+		value = dv.deviation * 1000000000000ULL / 2818572288ULL;
+		return (ppt < 0) ? -value : value;
+	}
 }
 
 static int fd788_genlock_query(const struct forward_v4l2_genlock *gl,
@@ -912,61 +917,88 @@ static int fd788_genlock_switch(const struct forward_v4l2_genlock *gl,
 	FD788_SyncGenerator sg = { 0 };
 	FD788_SyncGeneratorField sgf = { 0 };
 	struct forward_pll_mode *mode = NULL;
+	bool v2 = FD788_IS_VERSION_2(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);
-		pll->ops->select_input(pll, 1, false, false);
+		if (v2)
+			pll->ops->select_input(pll, 0, true, false);
+		else
+			pll->ops->select_input(pll, 1, false, false);
 		pll->ops->calibrate(pll);
 	} else {
-		int pll_in_freq = 0;
+		int pll_pix_freq = 0;
+		int pll_h_freq = 0;
 		FD788_Genlock gl = FD788_Genlock_R(v4l2->dev->csr);
 
+		pll_h_freq = forward_v4l2_pixel_clock(t) / t->sdi.width;
 		if (src == V4L_FORWARD_GENLOCK_SRC_ANALOG) {
-			pll_in_freq = forward_v4l2_pixel_clock(t) / t->sdi.width;
 			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_in_freq = 6750000;
+				pll_pix_freq = v2 ? 13500000 : 6750000;
 			else if (t->sdi.mode == FORWARD_SDI_MODE_HD_SDI)
-				pll_in_freq = 74250000;
+				pll_pix_freq = 74250000;
 			else if (t->sdi.mode == FORWARD_SDI_MODE_3G_SDI)
-				pll_in_freq = 148500000;
+				pll_pix_freq = 148500000;
 			gl.mode = (src == V4L_FORWARD_GENLOCK_SRC_IN1) ? 3 : 2;
 		}
 
-		mode = pll->ops->find_mode(pll, pll_in_freq, 0, t->sdi.m, 148500000, t->sdi.m);
+		mode = pll->ops->find_mode(pll, pll_pix_freq, pll_h_freq, t->sdi.m, 148500000,
+					   t->sdi.m);
 		if (!mode)
 			return -EINVAL;
 
 		FD788_Genlock_W(v4l2->dev->csr, gl);
 		pll->ops->switch_mode(pll, mode);
 
-		if ((src == V4L_FORWARD_GENLOCK_SRC_IN0) || (src == V4L_FORWARD_GENLOCK_SRC_IN1)) {
-			// Hack to avoid PLL->IN->PLL loop stuck
-			FD788_VideoInCS cs;
-			int count = 0;
-
-			// Force Master clock
-			pll->ops->select_input(pll, 1, false, false);
+		if (v2) {
+			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);
-
-			// Wait for input
-			do {
-				msleep(100);
-				cs = FD788_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);
+			if ((src == V4L_FORWARD_GENLOCK_SRC_IN0) ||
+			    (src == V4L_FORWARD_GENLOCK_SRC_IN1)) {
+				// Hack to avoid PLL->IN->PLL loop stuck
+				FD788_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 = FD788_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);
+			}
 		}
 	}
 
-- 
2.39.1


From f3d1efb8114c4c7256f40ec16019026b736d99ca Mon Sep 17 00:00:00 2001
From: Konstantin Oblaukhov <oblaukhov.konstantin@gmail.com>
Date: Sat, 22 Apr 2023 23:09:31 +0700
Subject: [PATCH 10/13] Fix PLL not locking on SDI.

---
 forward/forward-si534x.c | 94 ++++++++++++++++++++--------------------
 1 file changed, 47 insertions(+), 47 deletions(-)

diff --git a/forward/forward-si534x.c b/forward/forward-si534x.c
index 6cca68b..289584f 100644
--- a/forward/forward-si534x.c
+++ b/forward/forward-si534x.c
@@ -56,10 +56,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x014E2000000000,
 	  0x80000000,
 	  0x03070F0F170F,
-	  0x0307090A2B18,
+	  0x03070B0C2716,
 	  { 0, 10, 10 },
 	  { 0x12F684C, 0x1000000, 0x1000000 },
-	  0x00018AC7,
+	  0x0003CB07,
 	  0x01A0AB,
 	  0x06AC },
 	{ 13500000,
@@ -71,10 +71,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x014BCE00000000,
 	  0x80000000,
 	  0x03070F0F170F,
-	  0x0307090A2B18,
+	  0x03070B0C2716,
 	  { 0, 10, 10 },
 	  { 0x1318776, 0x1000000, 0x1000000 },
-	  0x0000C942,
+	  0x0003C997,
 	  0x019DC6,
 	  0x06A0 },
 	// 720p24
@@ -87,10 +87,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x01220A00000000,
 	  0x80000000,
 	  0x07070F0F1810,
-	  0x0707090A2C19,
+	  0x07070B0C2817,
 	  { 0, 12, 12 },
 	  { 0x15D867C, 0x1600000, 0x1600000 },
-	  0x0000AF6C,
+	  0x0003AFC1,
 	  0x0169B1,
 	  0x05C9 },
 	// 720p23.97
@@ -103,10 +103,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x016AE950000000,
 	  0xA0000000,
 	  0x07070F0F1810,
-	  0x0707090A2C19,
+	  0x07070B0C2817,
 	  { 0, 12, 12 },
 	  { 0x15D2D19, 0x15FA5FA, 0x15FA5FA },
-	  0x0000AF9A,
+	  0x0003AFEF,
 	  0x016A0E,
 	  0x05CB },
 	{ 74250000,
@@ -118,7 +118,7 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x016AE950000000,
 	  0xA0000000,
 	  0x07070F0F1810,
-	  0x0707090A2C19,
+	  0x07070B0C2817,
 	  { 0, 12, 12 },
 	  { 0x15D2D19, 0x15FA5FA, 0x15FA5FA },
 	  0x0000AF9A,
@@ -134,10 +134,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x01167000000000,
 	  0x80000000,
 	  0x07070F0F1810,
-	  0x0707090A2C19,
+	  0x07070B0C2817,
 	  { 0, 12, 12 },
 	  { 0x16C16C1, 0x1600000, 0x1600000 },
-	  0x0000A83F,
+	  0x0003A894,
 	  0x015B39,
 	  0x058F },
 	// 720p50
@@ -150,10 +150,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x01167000000000,
 	  0x80000000,
 	  0x07070F0F1810,
-	  0x0707090A2C19,
+	  0x07070B0C2817,
 	  { 1, 12, 12 },
 	  { 0x16C16C1, 0x1600000, 0x1600000 },
-	  0x0000A83F,
+	  0x0003A894,
 	  0x015B39,
 	  0x058F },
 	// 720p30
@@ -166,10 +166,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x00E80800000000,
 	  0x80000000,
 	  0x07070E0E1910,
-	  0x070708092D19,
+	  0x07070B0C2716,
 	  { 0, 12, 12 },
 	  { 0x1B4E81B, 0x1600000, 0x1600000 },
-	  0x00008B89,
+	  0x00038BDF,
 	  0x01215B,
 	  0x04A1 },
 	// 720p60
@@ -182,10 +182,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x00E80800000000,
 	  0x80000000,
 	  0x07070E0E1910,
-	  0x070708092D19,
+	  0x07070B0C2716,
 	  { 1, 12, 12 },
 	  { 0x1B4E81B, 0x1600000, 0x1600000 },
-	  0x00008B89,
+	  0x00038BDF,
 	  0x01215B,
 	  0x04A1 },
 	// 720p29.97
@@ -198,10 +198,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x01225440000000,
 	  0xA0000000,
 	  0x07070E0E1910,
-	  0x070708092D19,
+	  0x07070B0C2716,
 	  { 0, 12, 12 },
 	  { 0x1B4785F, 0x15FA5FA, 0x15FA5FA },
-	  0x00008BAE,
+	  0x00038C04,
 	  0x0121A5,
 	  0x04A2 },
 	{ 74250000,
@@ -213,10 +213,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x01225440000000,
 	  0xA0000000,
 	  0x07070E0E1910,
-	  0x070708092D19,
+	  0x07070B0C2716,
 	  { 0, 12, 12 },
 	  { 0x1B4785F, 0x15FA5FA, 0x15FA5FA },
-	  0x00008BAE,
+	  0x00038C04,
 	  0x0121A5,
 	  0x04A2 },
 	// 720p59.94
@@ -229,10 +229,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x01225440000000,
 	  0xA0000000,
 	  0x07070E0E1910,
-	  0x070708092D19,
+	  0x07070B0C2716,
 	  { 1, 12, 12 },
 	  { 0x1B4785F, 0x15FA5FA, 0x15FA5FA },
-	  0x00008BAE,
+	  0x00038C04,
 	  0x0121A5,
 	  0x04A2 },
 	{ 74250000,
@@ -244,10 +244,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x01225440000000,
 	  0xA0000000,
 	  0x07070E0E1910,
-	  0x070708092D19,
+	  0x07070B0C2716,
 	  { 1, 12, 12 },
 	  { 0x1B4785F, 0x15FA5FA, 0x15FA5FA },
-	  0x00008BAE,
+	  0x00038C04,
 	  0x0121A5,
 	  0x04A2 },
 	// 1080p24
@@ -260,10 +260,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x00C15C00000000,
 	  0x80000000,
 	  0x07070F0F170F,
-	  0x0707090A2B18,
+	  0x07070B0C2716,
 	  { 1, 12, 12 },
 	  { 0x10624DD, 0x1600000, 0x1600000 },
-	  0x0000EB3E,
+	  0x0003EB94,
 	  0x00F121,
 	  0x03DB },
 	// 1080p23.98
@@ -276,10 +276,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x00C18D80000000,
 	  0x80000000,
 	  0x07070F0F170F,
-	  0x0707090A2B18,
+	  0x07070B0C2716,
 	  { 1, 12, 12 },
 	  { 0x105E1D2, 0x15FA5FA, 0x15FA5FA },
-	  0x0000EB7C,
+	  0x0003EBD1,
 	  0x00F15F,
 	  0x03DC },
 	{ 74250000,
@@ -291,10 +291,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x00C18D80000000,
 	  0x80000000,
 	  0x07070F0F170F,
-	  0x0707090A2B18,
+	  0x07070B0C2716,
 	  { 1, 12, 12 },
 	  { 0x105E1D2, 0x15FA5FA, 0x15FA5FA },
-	  0x0000EB7C,
+	  0x0003EBD1,
 	  0x00F15F,
 	  0x03DC },
 	// 1080p25/i50
@@ -307,10 +307,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x00B9A000000000,
 	  0x80000000,
 	  0x07070F0F170F,
-	  0x0707090A2B18,
+	  0x07070B0C2716,
 	  { 1, 12, 12 },
 	  { 0x1111111, 0x1600000, 0x1600000 },
-	  0x0000E1AD,
+	  0x0003E202,
 	  0x00E77C,
 	  0x03B3 },
 	// 1080p50
@@ -323,10 +323,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x00B9A000000000,
 	  0x80000000,
 	  0x07070F0F170F,
-	  0x0707090A2B18,
+	  0x07070B0C2716,
 	  { 2, 13, 13 },
 	  { 0x1111111, 0x1600000, 0x1600000 },
-	  0x0000E1AD,
+	  0x0003E202,
 	  0x00E77C,
 	  0x03B3 },
 	// 1080p30/i60
@@ -339,10 +339,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x009AB000000000,
 	  0x80000000,
 	  0x07030F0F1810,
-	  0x070308092E1A,
+	  0x07030B0C2817,
 	  { 1, 12, 12 },
 	  { 0x147AE14, 0x1600000, 0x1600000 },
-	  0x0000BB65,
+	  0x0003BBBA,
 	  0x00C0E7,
 	  0x0315 },
 	// 1080p60
@@ -355,10 +355,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x009AB000000000,
 	  0x80000000,
 	  0x07030F0F1810,
-	  0x070308092E1A,
+	  0x07030B0C2817,
 	  { 2, 13, 13 },
 	  { 0x147AE14, 0x1600000, 0x1600000 },
-	  0x0000BB65,
+	  0x0003BBBA,
 	  0x00C0E7,
 	  0x0315 },
 	// 1080p29.97/i59.94
@@ -371,10 +371,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x00C18D80000000,
 	  0xA0000000,
 	  0x07030F0F1810,
-	  0x070308092E1A,
+	  0x07030B0C2817,
 	  { 1, 12, 12 },
 	  { 0x1475A47, 0x15FA5FA, 0x15FA5FA },
-	  0x0000BB96,
+	  0x0003BBEB,
 	  0x00C119,
 	  0x0316 },
 	{ 74250000,
@@ -386,10 +386,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x00C18D80000000,
 	  0xA0000000,
 	  0x07030F0F1810,
-	  0x070308092E1A,
+	  0x07030B0C2817,
 	  { 1, 12, 12 },
 	  { 0x1475A47, 0x15FA5FA, 0x15FA5FA },
-	  0x0000BB96,
+	  0x0003BBEB,
 	  0x00C119,
 	  0x0316 },
 	// 1080p59.94
@@ -402,10 +402,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x00C18D80000000,
 	  0xA0000000,
 	  0x07030F0F1810,
-	  0x070308092E1A,
+	  0x07030B0C2817,
 	  { 2, 13, 13 },
 	  { 0x1475A47, 0x15FA5FA, 0x15FA5FA },
-	  0x0000BB96,
+	  0x0003BBEB,
 	  0x00C119,
 	  0x0316 },
 	{ 148500000,
@@ -417,10 +417,10 @@ const struct forward_si534x_mode forward_si534x_modes[] = {
 	  0x00C18D80000000,
 	  0xA0000000,
 	  0x07030F0F1810,
-	  0x070308092E1A,
+	  0x07030B0C2817,
 	  { 2, 13, 13 },
 	  { 0x1475A47, 0x15FA5FA, 0x15FA5FA },
-	  0x0000BB96,
+	  0x0003BBEB,
 	  0x00C119,
 	  0x0316 },
 };
-- 
2.39.1


From 16fac53fb7cf23867adff8c5d1b055f321bd5e16 Mon Sep 17 00:00:00 2001
From: Konstantin Oblaukhov <oblaukhov.konstantin@gmail.com>
Date: Wed, 26 Apr 2023 17:30:05 +0700
Subject: [PATCH 11/13] FD722v5 and FD788v2 support.

---
 .../forward-passthrough-fd722.c               | 62 ++++++++++-------
 .../forward-passthrough-fd788.c               | 68 ++++++++++++-------
 2 files changed, 81 insertions(+), 49 deletions(-)

diff --git a/forward-passthrough/forward-passthrough-fd722.c b/forward-passthrough/forward-passthrough-fd722.c
index 2ff93ce..1db7cd1 100644
--- a/forward-passthrough/forward-passthrough-fd722.c
+++ b/forward-passthrough/forward-passthrough-fd722.c
@@ -37,6 +37,8 @@
 #define FD722_GENLOCK_HD_DELAY 189
 #define FD722_GENLOCK_3G_DELAY 97
 
+#define FD722_IS_VERSION_5(ver) (((ver >> 16) & 0xF) >= 6)
+
 static u32 fd722_irq_mask(struct forward_passthrough_io *io)
 {
 	return (1 << (io->in_index * 4 + 16)) | (1 << ((io->out_index - 2) * 2 + 4));
@@ -283,8 +285,9 @@ static void fd722_configure_genlock(struct forward_passthrough_io *io, bool anal
 	struct forward_passthrough_format *fmt = &io->format;
 	struct forward_pll_mode *mode;
 	FD722_Genlock gl;
-	int pll_in_freq = 0;
+	int pll_in_h_freq = 0, pll_in_p_freq = 0;
 	int i;
+	bool v5 = FD722_IS_VERSION_5(io->pass->dev->fw_version);
 
 	if (!io->configured)
 		return;
@@ -292,44 +295,57 @@ static void fd722_configure_genlock(struct forward_passthrough_io *io, bool anal
 	gl = FD722_Genlock_R(pass->dev->csr);
 
 	if (analog) {
-		pll_in_freq = fmt->pixelclock / fmt->total_width;
+		pll_in_h_freq = fmt->pixelclock / fmt->total_width;
 		gl.mode = 0;
 	} else {
 		if (io->format.pixelclock == 13500000)
-			pll_in_freq = 6750000;
+			pll_in_p_freq = v5 ? 13500000 : 6750000;
 		else
-			pll_in_freq = (int)io->format.pixelclock;
+			pll_in_p_freq = (int)io->format.pixelclock;
 		gl.mode = io->in_index ? 3 : 2;
 	}
 
-	mode = pll->ops->find_mode(pll, pll_in_freq, 0, 148500000, fmt->m);
+	mode = pll->ops->find_mode(pll, pll_in_p_freq, pll_in_h_freq, fmt->m, 148500000, fmt->m);
 	if (!mode)
 		return;
 
 	FD722_Genlock_W(pass->dev->csr, gl);
 
 	pll->ops->switch_mode(pll, mode);
-	if (!analog) {
-		// 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);
+	if (v5) {
+		int input = 0;
+		if (analog)
+			input = 0;
+		else 
+			input = io->in_index ? 2 : 1;
+		pll->ops->select_input(pll, input, false, true);
 		pll->ops->calibrate(pll);
-
-		// Wait for input
-		do {
-			msleep(100);
-			cs = FD722_VideoInCS_R(pass->dev->csr, io->in_index);
-			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);
+		if (!analog) {
+			// 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(
+					io->pass->dev->csr,
+					io->in_index ? 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 = fmt->total_width * fmt->total_height;
diff --git a/forward-passthrough/forward-passthrough-fd788.c b/forward-passthrough/forward-passthrough-fd788.c
index af4fcad..0ce7224 100644
--- a/forward-passthrough/forward-passthrough-fd788.c
+++ b/forward-passthrough/forward-passthrough-fd788.c
@@ -36,6 +36,8 @@
 #define FD788_GENLOCK_HD_DELAY 193
 #define FD788_GENLOCK_3G_DELAY 101
 
+#define FD788_IS_VERSION_2(ver) (((ver >> 24) & 0xF) >= 2)
+
 static u32 fd788_irq_mask(struct forward_passthrough_io *io)
 {
 	return (1 << (io->in_index * 3)) | (1 << (io->out_index * 3));
@@ -253,7 +255,7 @@ static void fd788_configure_io(struct forward_passthrough_io *io)
 }
 
 static void fd788_configure_genlock(struct forward_passthrough_io *io, bool analog)
-{
+{	
 	struct forward_passthrough *pass = io->pass;
 	struct forward_pll *pll = pass->dev->pll;
 	FD788_SyncGenerator sg = { 0 };
@@ -261,8 +263,9 @@ static void fd788_configure_genlock(struct forward_passthrough_io *io, bool anal
 	struct forward_passthrough_format *fmt = &io->format;
 	struct forward_pll_mode *mode;
 	FD788_Genlock gl;
-	int pll_in_freq = 0;
+	int pll_in_h_freq = 0, pll_in_p_freq = 0;
 	int i;
+	bool v2 = FD788_IS_VERSION_2(io->pass->dev->fw_version);
 
 	if (!io->configured)
 		return;
@@ -270,44 +273,57 @@ static void fd788_configure_genlock(struct forward_passthrough_io *io, bool anal
 	gl = FD788_Genlock_R(pass->dev->csr);
 
 	if (analog) {
-		pll_in_freq = fmt->pixelclock / fmt->total_width;
+		pll_in_h_freq = fmt->pixelclock / fmt->total_width;
 		gl.mode = 0;
 	} else {
 		if (io->format.pixelclock == 13500000)
-			pll_in_freq = 6750000;
+			pll_in_p_freq = v2 ? 13500000 : 6750000;
 		else
-			pll_in_freq = (int)io->format.pixelclock;
+			pll_in_p_freq = (int)io->format.pixelclock;
 		gl.mode = io->in_index ? 3 : 2;
 	}
 
-	mode = pll->ops->find_mode(pll, pll_in_freq, 0, 148500000, fmt->m);
+	mode = pll->ops->find_mode(pll, pll_in_p_freq, pll_in_h_freq, fmt->m, 148500000, fmt->m);
 	if (!mode)
 		return;
 
 	FD788_Genlock_W(pass->dev->csr, gl);
 
 	pll->ops->switch_mode(pll, mode);
-	if (!analog) {
-		// Hack to avoid PLL->IN->PLL loop stuck
-		FD788_VideoInCS cs;
-		int count = 0;
 
-		// Force Master clock
-		pll->ops->select_input(pll, 1, false, false);
+	if (v2) {
+		int input = 0;
+		if (analog)
+			input = 0;
+		else 
+			input = io->in_index ? 2 : 1;
+		pll->ops->select_input(pll, input, false, true);
 		pll->ops->calibrate(pll);
-
-		// Wait for input
-		do {
-			msleep(100);
-			cs = FD788_VideoInCS_R(pass->dev->csr, io->in_index);
-			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);
+		if (!analog) {
+			// Hack to avoid PLL->IN->PLL loop stuck
+			FD788_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 = FD788_VideoInCS_R(
+					io->pass->dev->csr,
+					io->in_index ? 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 = fmt->total_width * fmt->total_height;
@@ -330,10 +346,10 @@ static void fd788_configure_genlock(struct forward_passthrough_io *io, bool anal
 
 	for (i = 0; i < pass->ops->num_io; i++) {
 		struct forward_passthrough_io *o = &pass->io[i];
-		FD788_VideoOutCS cs = FD788_VideoOutCS_R(pass->dev->csr, o->out_index);
+		FD788_VideoOutCS cs = FD788_VideoOutCS_R(pass->dev->csr, (o->out_index - 2));
 		cs.syncSource = 0;
 		cs.freeRunning = 0;
-		FD788_VideoOutCS_W(pass->dev->csr, o->out_index, cs);
+		FD788_VideoOutCS_W(pass->dev->csr, (o->out_index - 2), cs);
 	}
 }
 
-- 
2.39.1


From 86759a64439d806e938a816b483d2f08a31e357e Mon Sep 17 00:00:00 2001
From: Konstantin Oblaukhov <oblaukhov.konstantin@gmail.com>
Date: Wed, 26 Apr 2023 21:51:54 +0700
Subject: [PATCH 12/13] Fix FD788 passthrough.

---
 forward-passthrough/forward-passthrough-fd722.c |  6 ++----
 forward-passthrough/forward-passthrough-fd788.c | 12 +++++-------
 2 files changed, 7 insertions(+), 11 deletions(-)

diff --git a/forward-passthrough/forward-passthrough-fd722.c b/forward-passthrough/forward-passthrough-fd722.c
index 1db7cd1..6595bbd 100644
--- a/forward-passthrough/forward-passthrough-fd722.c
+++ b/forward-passthrough/forward-passthrough-fd722.c
@@ -317,7 +317,7 @@ static void fd722_configure_genlock(struct forward_passthrough_io *io, bool anal
 		int input = 0;
 		if (analog)
 			input = 0;
-		else 
+		else
 			input = io->in_index ? 2 : 1;
 		pll->ops->select_input(pll, input, false, true);
 		pll->ops->calibrate(pll);
@@ -334,9 +334,7 @@ static void fd722_configure_genlock(struct forward_passthrough_io *io, bool anal
 			// Wait for input
 			do {
 				msleep(100);
-				cs = FD722_VideoInCS_R(
-					io->pass->dev->csr,
-					io->in_index ? 1 : 0);
+				cs = FD722_VideoInCS_R(io->pass->dev->csr, io->in_index ? 1 : 0);
 				count++;
 			} while ((!cs.modeLocked) && (count < 100));
 
diff --git a/forward-passthrough/forward-passthrough-fd788.c b/forward-passthrough/forward-passthrough-fd788.c
index 0ce7224..c72a769 100644
--- a/forward-passthrough/forward-passthrough-fd788.c
+++ b/forward-passthrough/forward-passthrough-fd788.c
@@ -255,7 +255,7 @@ static void fd788_configure_io(struct forward_passthrough_io *io)
 }
 
 static void fd788_configure_genlock(struct forward_passthrough_io *io, bool analog)
-{	
+{
 	struct forward_passthrough *pass = io->pass;
 	struct forward_pll *pll = pass->dev->pll;
 	FD788_SyncGenerator sg = { 0 };
@@ -295,7 +295,7 @@ static void fd788_configure_genlock(struct forward_passthrough_io *io, bool anal
 		int input = 0;
 		if (analog)
 			input = 0;
-		else 
+		else
 			input = io->in_index ? 2 : 1;
 		pll->ops->select_input(pll, input, false, true);
 		pll->ops->calibrate(pll);
@@ -312,9 +312,7 @@ static void fd788_configure_genlock(struct forward_passthrough_io *io, bool anal
 			// Wait for input
 			do {
 				msleep(100);
-				cs = FD788_VideoInCS_R(
-					io->pass->dev->csr,
-					io->in_index ? 1 : 0);
+				cs = FD788_VideoInCS_R(io->pass->dev->csr, io->in_index ? 1 : 0);
 				count++;
 			} while ((!cs.modeLocked) && (count < 100));
 
@@ -346,10 +344,10 @@ static void fd788_configure_genlock(struct forward_passthrough_io *io, bool anal
 
 	for (i = 0; i < pass->ops->num_io; i++) {
 		struct forward_passthrough_io *o = &pass->io[i];
-		FD788_VideoOutCS cs = FD788_VideoOutCS_R(pass->dev->csr, (o->out_index - 2));
+		FD788_VideoOutCS cs = FD788_VideoOutCS_R(pass->dev->csr, o->out_index);
 		cs.syncSource = 0;
 		cs.freeRunning = 0;
-		FD788_VideoOutCS_W(pass->dev->csr, (o->out_index - 2), cs);
+		FD788_VideoOutCS_W(pass->dev->csr, o->out_index, cs);
 	}
 }
 
-- 
2.39.1


From df36847c923624f55bea6324050b8fdc02d95e90 Mon Sep 17 00:00:00 2001
From: Konstantin Oblaukhov <oblaukhov.konstantin@gmail.com>
Date: Wed, 26 Apr 2023 22:15:37 +0700
Subject: [PATCH 13/13] Fix passthrough not building.

---
 Makefile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Makefile b/Makefile
index bc42742..d2e46cf 100644
--- a/Makefile
+++ b/Makefile
@@ -1,4 +1,4 @@
-obj-y += forward/ forward-v4l2/ forward-alsa/
+obj-y += forward/ forward-v4l2/ forward-alsa/ forward-passthrough/
 
 SRC ?= $(PWD)
 KERNEL_SRC ?= /lib/modules/$(shell uname -r)/build
-- 
2.39.1

