From fbf26f561d6e3d5629b10ab7e0eb737fc417bf02 Mon Sep 17 00:00:00 2001
From: Konstantin Oblaukhov <oblaukhov.konstantin@gmail.com>
Date: Thu, 27 Apr 2023 11:30:56 +0700
Subject: [PATCH 1/2] Separate threads for each IO.

---
 .../forward-passthrough-core.c                | 111 +++++++++++-------
 forward-passthrough/forward-passthrough.h     |   4 +-
 2 files changed, 73 insertions(+), 42 deletions(-)

diff --git a/forward-passthrough/forward-passthrough-core.c b/forward-passthrough/forward-passthrough-core.c
index 7cde52d..5b8d140 100644
--- a/forward-passthrough/forward-passthrough-core.c
+++ b/forward-passthrough/forward-passthrough-core.c
@@ -309,34 +309,60 @@ static void forward_passthrough_process(struct forward_passthrough_io *io, u32 i
  * IO processing thread
  */
 static int forward_passthrough_thread(void *p)
+{
+	struct forward_passthrough_io *io = p;
+	struct forward_passthrough *pass = io->pass;
+	struct forward_irq_listener listener;
+
+	mutex_lock(&io->lock);
+	io->a_out_groups = 1;
+	pass->ops->configure_io(io);
+	mutex_unlock(&io->lock);
+
+	forward_irq_listener_init(&listener);
+	listener.type = FORWARD_IRQ_LISTENER_WAIT;
+	listener.mask = pass->ops->irq_mask(io);
+	forward_irq_listener_add(pass->dev, &listener);
+
+	while (!kthread_should_stop()) {
+		u32 irq = forward_irq_listener_wait_kthread(&listener, true, HZ / 10);
+
+		if (kthread_should_stop())
+			break;
+
+		mutex_lock(&io->lock);
+		forward_passthrough_process(io, irq);
+		mutex_unlock(&io->lock);
+	}
+
+	mutex_lock(&io->lock);
+	forward_passthrough_toggle_streaming(io, false);
+	mutex_unlock(&io->lock);
+
+	forward_irq_listener_remove(pass->dev, &listener);
+
+	return 0;
+}
+
+/**
+ * Genlock thread
+ */
+static int forward_passthrough_genlock_thread(void *p)
 {
 	struct forward_passthrough *pass = p;
 	struct forward_irq_listener listener;
-	const struct forward_passthrough_dev_ops *ops = pass->ops;
-	int i;
+	bool configured = false;
 	u64 last_genlock_check = ktime_get_ns();
-	u32 gl_old_offset = 0;
-	bool genlocked = false;
+	s32 gl_old_offset = 0;
 
 	// Genlock master selection
 	pass->master = &pass->io[0];
 	pass->analog_master = false;
-
-	// Setup I/O = query input format and set same format on output
-	for (i = 0; i < ops->num_io; i++) {
-		pass->io[i].a_out_groups = 1;
-		pass->ops->configure_io(&pass->io[i]);
-	}
-
-	// Enable genlock
-	pass->ops->configure_genlock(pass->master, pass->analog_master);
+	pass->genlocked = false;
 
 	forward_irq_listener_init(&listener);
 	listener.type = FORWARD_IRQ_LISTENER_WAIT;
-	listener.mask = 0;
-	for (i = 0; i < pass->ops->num_io; i++)
-		listener.mask |= pass->ops->irq_mask(&pass->io[i]);
-	listener.mask |= pass->ops->genlock_irq_mask(&pass->io[i]);
+	listener.mask = pass->ops->genlock_irq_mask(pass->master);
 	forward_irq_listener_add(pass->dev, &listener);
 
 	while (!kthread_should_stop()) {
@@ -347,38 +373,28 @@ static int forward_passthrough_thread(void *p)
 		if (kthread_should_stop())
 			break;
 
-		if (!pass->master->configured) {
-			pass->ops->configure_io(pass->master);
-			pass->ops->configure_genlock(pass->master, false);
-		}
-
-		for (i = 0; i < ops->num_io; i++) {
-			struct forward_passthrough_io *io = &pass->io[i];
-			mutex_lock(&io->lock);
-			forward_passthrough_process(io, irq);
-			mutex_unlock(&io->lock);
-		}
+		// Delay because genlock irq comes a bit (microseconds) earlier than input
+		// This cause race condition on offset readings.
+		msleep(1);
 
 		if (pass->ops->genlock_irq_info(pass->master, irq, &gl_field, &gl_offset)) {
 			if ((ktime_to_ms(listener.time - last_genlock_check) > 1000) && !gl_field) {
-				if (!genlocked && (gl_old_offset == gl_offset)) {
+				if (!pass->genlocked && (gl_old_offset == gl_offset)) {
 					pass->ops->genlock_offset(pass->master, -gl_offset);
-					genlocked = true;
+					pass->genlocked = true;
 				}
 				if (abs(gl_old_offset - gl_offset) > 1000) {
-					genlocked = false;
+					pass->genlocked = false;
 				}
 				gl_old_offset = gl_offset;
 				last_genlock_check = listener.time;
 			}
 		}
-	}
 
-	for (i = 0; i < pass->ops->num_io; i++) {
-		struct forward_passthrough_io *io = &pass->io[i];
-		mutex_lock(&io->lock);
-		forward_passthrough_toggle_streaming(io, false);
-		mutex_unlock(&io->lock);
+		if (!configured && pass->master->configured) {
+			pass->ops->configure_genlock(pass->master, pass->analog_master);
+			configured = true;
+		}
 	}
 
 	forward_irq_listener_remove(pass->dev, &listener);
@@ -392,10 +408,19 @@ static int forward_passthrough_thread(void *p)
 static void forward_passthrough_remove(struct forward_dev *dev, void **private)
 {
 	struct forward_passthrough *pass = *private;
+	const struct forward_passthrough_dev_ops *ops = pass->ops;
+	int i;
 
-	if (pass->thread) {
-		kthread_stop(pass->thread);
-		pass->thread = NULL;
+	if (pass->gl_thread) {
+		kthread_stop(pass->gl_thread);
+		pass->gl_thread = NULL;
+	}
+
+	for (i = 0; i < ops->num_io; i++) {
+		if (pass->threads[i]) {
+			kthread_stop(pass->threads[i]);
+			pass->threads[i] = NULL;
+		}
 	}
 
 	if (pass->io) {
@@ -460,7 +485,11 @@ static int forward_passthrough_probe(struct forward_dev *dev, void **private)
 		}
 	}
 
-	pass->thread = kthread_run(forward_passthrough_thread, pass, "fd-%llu-pt", dev->soft_id);
+	for (i = 0; i < ops->num_io; i++)
+		pass->threads[i] = kthread_run(forward_passthrough_thread, &pass->io[i],
+					       "fd-%llu-pt-%d", dev->soft_id, i);
+	pass->gl_thread = kthread_run(forward_passthrough_genlock_thread, pass, "fd-%llu-pt-gl",
+				      dev->soft_id);
 
 	forward_info(dev, "Created %d passthrough devices: \n", ops->num_io);
 	for (i = 0; i < ops->num_io; i++)
diff --git a/forward-passthrough/forward-passthrough.h b/forward-passthrough/forward-passthrough.h
index 31555ce..8a5292c 100644
--- a/forward-passthrough/forward-passthrough.h
+++ b/forward-passthrough/forward-passthrough.h
@@ -154,8 +154,10 @@ struct forward_passthrough {
 	struct forward_passthrough_io *io;
 	struct forward_passthrough_io *master;
 	bool analog_master;
+	bool genlocked;
 
-	struct task_struct *thread;
+	struct task_struct *threads[FORWARD_MAX_IO / 2];
+	struct task_struct *gl_thread;
 };
 
 /**
-- 
2.39.1

