 +
 +	if (on) {
 +		if (!atomic_read(&priv->use_count))
-+			reg8_write(client, 0x15, 0x9b);		/* enable CSI output, VC is set accordingly to Link number, BIT7 magic must be set */
++			reg8_write(client, 0x69, priv->links_mask ^ 0x0f); /* unmask CSI forwarding from detected links */
 +		atomic_inc(&priv->use_count);
 +	} else {
 +		atomic_dec(&priv->use_count);
 +		if (!atomic_read(&priv->use_count))
-+			reg8_write(client, 0x15, 0x13);		/* disable CSI output, VC is set accordingly to Link number */
++			reg8_write(client, 0x69, 0x0f);		/* mask CSI forwarding from all links */
 +	}
 +
 +	return 0;

+	usleep_range(2000, 2500);				/* wait 2ms after any change of reverse channel settings */
+	reg8_write(client, 0x00, 0xe0 | mask);			/* enable GMSL link for CAMx */
+	reg8_write(client, 0x0a, (mask << 4) | mask);		/* enable reverse control for CAMx */
+	reg8_write(client, 0x69, mask ^ 0x0f);			/* unmask link for CAMx */
+	reg8_write(client, 0x1b, mask);				/* enable equalizer for CAMx */
+	usleep_range(2000, 2500);				/* wait 2ms after any change of reverse channel settings */
+

+	struct ti964_ti913_priv *priv = v4l2_get_subdevdata(sd);
+	struct i2c_client *client = priv->client;
+
+	reg8_write(client, 0x33, ((priv->lanes - 1) ^ 0x3) << 4 | 0x1); /* enable CSI output, set CSI lane count, non-continuous CSI mode */
+
+	return 0;
+}
